function [tg,theta] = tgmo2(tmap,ntex,radius,norient,varargin)
% function [tg,theta] = tgmo2(tmap,ntex,radius,norient,...)
%
% Compute the texture gradient at a single scale and multiple
% orientations.
%
% INPUT
%	tmap		Texton map, values in [1,ntex].
%	ntex		Number of textons.
%	radius		Radius of disc for texture gradient.
%	norient		Number of orientation at which to compute
%			the texture gradient.
%	'smooth'	Smoothing method, one of
%			{'gaussian','savgol','none'}, default 'none'.
%	'sigma'		Sigma for smoothing, default to radius.
%	'tsim'		Texton similarity matrix.  If not
%			provided, then use chi-squared.
%
% OUTPUT
%	tg		Size [h w norient] array of tg images.
%	theta		Vector of disc orientations (which are
%			orthogonal to the texture gradient).
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

% process options
smooth = 'none';
sigma = radius;
usechi2 = true;
for i = 1:2:numel(varargin),
  opt = varargin{i};
  if ~ischar(opt), error('option names not a string'); end
  if i==numel(varargin), error(sprintf('option ''%s'' has no value',opt)); end
  val = varargin{i+1};
  switch opt,
   case 'smooth',
    switch val,
     case {'none','gaussian','savgol'}, smooth=val;
     otherwise, error(sprintf('invalid option smooth=''%s''',val));
    end
   case 'sigma', sigma=val;
   case 'tsim', tsim=val; usechi2=false;
   otherwise, error(sprintf('invalid option ''%s''',opt));
  end
end

radius = max(1,radius);
norient = max(1,norient);
theta = (0:norient-1)/norient*pi;

% check texton labels
if any(tmap~=round(tmap)),
  error('texton labels not integral');
end
if min(tmap(:)) < 1 | max(tmap(:))>ntex,
  error(sprintf('texton labels out of range [1,%d]',ntex));
end

% radius of discrete disc
wr = floor(radius);

% count number of pixels in a disc
[u,v] = meshgrid(-wr:wr,-wr:wr);
gamma = mod(atan2(v,u),2*pi);
mask = (u.^2 + v.^2 <= radius^2);
mask(wr+1,wr+1) = 0; % mask out center pixel to remove bias
count = sum(mask(:));

% determine which pie slice each pixel falls into
% 0=masked [1,2*norient]=slice
slice = 1 + floor(gamma/(pi/norient));
slice = slice .* mask;

[h,w] = size(tmap);
tg = zeros(h,w,norient);
fwrite(2,'[');
for x = 1:w,
  fwrite(2,'.');
  for y = 1:h,
    pie = zeros(ntex,2*norient);
    for u = -wr:wr,
      xi = x + u;
      if xi<1 | xi>w, continue; end
      for v = -wr:wr,
        yi = y + v;
        if yi<1 | yi>h, continue; end
        s = slice(v+wr+1,u+wr+1);
        if s==0, continue; end % masked out
        t = tmap(yi,xi);
        pie(t,s) = pie(t,s) + 1;
      end
    end
    pie = pie .* (2/count); % normalize
    % initialize left/right histograms
    lhist = sum(pie(:,1:norient),2);
    rhist = sum(pie(:,norient+1:end),2);
    % spin the disc to compute tg at each orientation
    for i = 1:norient,
      if usechi2,
        chi = (lhist-rhist).^2 ./ (lhist+rhist+eps);
        tg(y,x,i) = 0.5*sum(chi);
      else
        lrdiff = abs(lhist-rhist);
        tg(y,x,i) = lrdiff' * tsim * lrdiff;
      end
      if i<norient,
        inc = pie(:,norient+i) - pie(:,i);
        lhist = lhist + inc;
        rhist = rhist - inc;
      end
    end
  end
end
fprintf(2,']\n');

for i = 1:norient,
  switch smooth,
   case 'gaussian',
    f = oeFilter([sigma .5],3,theta(i)+pi/2);
    tg(:,:,i) = applyFilter(f,tg(:,:,i));
   case 'savgol',
    a = fitparab(tg(:,:,i),sigma,sigma/4,theta(i));
    tg(:,:,i) = max(0,a);
  end
end


function [kHz] = THz2kHz(THz)
% Convert frequency from terahertz to kilohertz.
% Chad A. Greene 2012
kHz = THz*1e+9;


function test_failed = test_libltfat_solvehermitiansystem(varargin)
test_failed = 0;

fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];

Larr =    [301, 9,11,110, 9, 8, 11, 10, 301];

    complexstring = 'complex';
    funname_init = makelibraryname('hermsystemsolver_init',flags.complexity,1);
    funname_execute = makelibraryname('hermsystemsolver_execute',flags.complexity,1);
    funname_done = makelibraryname('hermsystemsolver_done',flags.complexity,1);

    p = libpointer();
    calllib('libltfat',funname_init,max(Larr),p);

for Lidx = 1:numel(Larr)
    L = Larr(Lidx);

    D = randn(L,flags.complexity)+1i*randn(L,flags.complexity);
   % D(:,1) = D(:,end);
    A = D*D';
    Amessedup = A;
    for n=2:size(A,2)
        for m=1:n-1
              Amessedup(m,n) = randn(1);
        end
    end
    Aint = complex2interleaved(Amessedup);
    APtr = libpointer(dataPtr,Aint);

    b = randn(L,1,flags.complexity)+ 1i*randn(L,1,flags.complexity);
    bint = complex2interleaved(b);
    bPtr = libpointer(dataPtr,bint);

    tic
    trueres = A\b;
    toc

    tic
    status = calllib('libltfat',funname_execute,p,APtr,L,bPtr);
    toc

    res = norm(trueres - interleaved2complex(bPtr.Value));

    [test_failed,fail]=ltfatdiditfail(res+status,test_failed,1e-8);
    fprintf(['SOLVEHERM L:%3i, %s %s %s %s\n'],L,flags.complexity,complexstring,ltfatstatusstring(status),fail);

end

    calllib('libltfat',funname_done,p);


function [ gd,filtertype ] = gsp_jtv_design_can_dual(g,filtertype)
%GSP_JTV_DESIGN_CAN_DUAL This function returns the canonical dual of the time-vertex filterbank g
%   Usage:  [gd,filtertype] = gsp_jtv_design_can_dual( g,filtertype );
%
%   Inputs parameters:
%       g          : cell array of time-vertex filters
%       filtertype : Filter domain (ts,js,ts-array,js-array)
%
%   Ouputs parameters:
%       g          : cell array of canonical dual time-vertex filters
%       filtertype : Filter domain (ts,js)
%
%   This function returns the canonical dual of the time-vertex filterbank g
%

% Author: Francesco Grassi
% Date:   September 2016

Nf = size(g,1);
gd = cell(Nf,1);

for n = 1:Nf
    gd{n} = @(x,t) can_dual(g,filtertype,n,x,t);
end

switch filtertype
    case {'ts','ts-array'}
        filtertype = 'ts';
    case {'js','js-array'}
        filtertype = 'js';
end

end

function sol = can_dual(g,ft,n,x,t)

if ~isvector(x);x=x(:,1);end
if ~isvector(t);t=t(1,:);end

sol = gsp_jtv_evaluate_can_dual( g,ft,x,t,n );

end


function [P,Uinit,output] = cp_als(X,R,varargin)
%CP_ALS Compute a CP decomposition of any type of tensor.
%
%   P = CP_ALS(X,R) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating least-squares
%   algorithm.  The input X can be a tensor, sptensor, ktensor, or
%   ttensor. The result P is a ktensor.
%
%   P = CP_ALS(X,R,'param',value,...) specifies optional parameters and
%   values. Valid parameters and their default values are:
%      'tol' - Tolerance on difference in fit {1.0e-4}
%      'maxiters' - Maximum number of iterations {50}
%      'dimorder' - Order to loop through dimensions {1:ndims(A)}
%      'init' - Initial guess [{'random'}|'nvecs'|cell array]
%      'printitn' - Print fit every n iterations; 0 for no printing {1}
%
%   [P,U0] = CP_ALS(...) also returns the initial guess.
%
%   [P,U0,out] = CP_ALS(...) also returns additional output that contains
%   the input parameters.
%
%   Note: The "fit" is defined as 1 - norm(X-full(P))/norm(X) and is
%   loosely the proportion of the data described by the CP model, i.e., a
%   fit of 1 is perfect.
%
%   Examples:
%   X = sptenrand([5 4 3], 10);
%   P = cp_als(X,2);
%   P = cp_als(X,2,'dimorder',[3 2 1]);
%   P = cp_als(X,2,'dimorder',[3 2 1],'init','nvecs');
%   U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of P
%   [P,U0,out] = cp_als(X,2,'dimorder',[3 2 1],'init',U0);
%   P = cp_als(X,2,out.params); %<-- Same params as previous run
%
%   See also KTENSOR, TENSOR, SPTENSOR, TTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Extract number of dimensions and norm of X.
N = ndims(X);
normX = norm(X);

%% Set algorithm parameters from input or by using defaults
params = inputParser;
params.addParamValue('tol',1e-4,@isscalar);
params.addParamValue('maxiters',50,@(x) isscalar(x) & x > 0);
params.addParamValue('dimorder',1:N,@(x) isequal(sort(x),1:N));
params.addParamValue('init', 'random', @(x) (iscell(x) || ismember(x,{'random','nvecs'})));
params.addParamValue('printitn',1,@isscalar);
params.parse(varargin{:});

%% Copy from params object
fitchangetol = params.Results.tol;
maxiters = params.Results.maxiters;
dimorder = params.Results.dimorder;
init = params.Results.init;
printitn = params.Results.printitn;

%% Error checking

%% Set up and error checking on initial guess for U.
if iscell(init)
    Uinit = init;
    if numel(Uinit) ~= N
        error('OPTS.init does not have %d cells',N);
    end
    for n = dimorder(2:end);
        if ~isequal(size(Uinit{n}),[size(X,n) R])
            error('OPTS.init{%d} is the wrong size',n);
        end
    end
else
    % Observe that we don't need to calculate an initial guess for the
    % first index in dimorder because that will be solved for in the first
    % inner iteration.
    if strcmp(init,'random')
        Uinit = cell(N,1);
        for n = dimorder(2:end)
            Uinit{n} = rand(size(X,n),R);
        end
    elseif strcmp(init,'nvecs') || strcmp(init,'eigs')
        Uinit = cell(N,1);
        for n = dimorder(2:end)
            Uinit{n} = nvecs(X,n,R);
        end
    else
        error('The selected initialization method is not supported');
    end
end

%% Set up for iterations - initializing U and the fit.
U = Uinit;
fit = 0;

if printitn>0
  fprintf('\nCP_ALS:\n');
end

%% Main Loop: Iterate until convergence

if (isa(X,'sptensor') || isa(X,'tensor')) && (exist('cpals_core','file') == 3)

    %fprintf('Using C++ code\n');
    [lambda,U] = cpals_core(X, Uinit, fitchangetol, maxiters, dimorder);
    P = ktensor(lambda,U);

else

    for iter = 1:maxiters

        fitold = fit;

        % Iterate over all N modes of the tensor
        for n = dimorder(1:end)

            % Calculate Unew = X_(n) * khatrirao(all U except n, 'r').
            Unew = mttkrp(X,U,n);

            % Compute the matrix of coefficients for linear system
            Y = ones(R,R);
            for i = [1:n-1,n+1:N]
                Y = Y .* (U{i}'*U{i});
            end

            % Need to figure out which of the following lines works better.
            % Meanwhile, we'll stick witht he line from TTB 2.2 since that is
            % what seems to work best based on preliminary testing.
            %Unew = Unew * pinv(Y); %<- Line from TTB 2.3.
            Unew = (Y \ Unew')'; %<- Line from TTB 2.2.

            % Normalize each vector to prevent singularities in coefmatrix
            if iter == 1
                lambda = sqrt(sum(Unew.^2,1))'; %2-norm
            else
                lambda = max( max(Unew,[],1), 1 )'; %max-norm
            end
            Unew = Unew * spdiags(1./lambda,0,R,R);
            if issparse(Unew)
                U{n} = full(Unew);   % for the case R=1
            else
                U{n} = Unew;
            end
        end

        P = ktensor(lambda,U);
        normresidual = sqrt( normX^2 + norm(P)^2 - 2 * innerprod(X,P) );
        fit = 1 - (normresidual / normX); %fraction explained by model
        fitchange = abs(fitold - fit);

        if mod(iter,printitn)==0
            fprintf(' Iter %2d: fit = %e fitdelta = %7.1e\n', iter, fit, fitchange);
        end

        % Check for convergence
        if (iter > 1) && (fitchange < fitchangetol)
            break;
        end
    end
end

%% Clean up final result
% Arrange the final tensor so that the columns are normalized.
P = arrange(P);
% Fix the signs
P = fixsigns(P);

if printitn>0
  normresidual = sqrt( normX^2 + norm(P)^2 - 2 * innerprod(X,P) );
  fit = 1 - (normresidual / normX); %fraction explained by model
  fprintf(' Final fit = %e \n', fit);
end

output = struct;
output.params = params.Results;
output.iters = iter;


function [d dt pred] = bfs(A,u,varargin)
% BFS Compute the breadth first search order.
%
% [d dt pred] = bfs(A,u) returns the distance to each vertex (d) and the
% discover time (dt) in a breadth first search starting from vertex u.
%    d(i) = dt(i) = -1 if vertex i is not reachable from vertex u.
% pred is the predecessor array.  pred(i) = 0 if vertex (i)
% is in a component not reachable from u and i != u.
%
% This method works on directed graphs.
% The runtime is O(V+E).
%
% ... = bfs(A,u,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   options.target: a special vertex that will stop the search when hit
%       [{'none'} | any vertex number besides the u]
%
% Note: this function does not depend upon the non-zero values of A, but
% only uses the non-zero structure of A.
%
% Example:
%    load graphs/bfs_example.mat
%    d = bfs(A,1)
%
% See also DFS

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2006-04-19: Initial version
%  2006-05-31: Added full2sparse check
%  2007-04-19: Added target option
%  2008-10-07: Changed options parsing
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct('target', 'none');
options = merge_options(options,varargin{:});

if check, check_matlab_bgl(A,struct()); end

if strcmp(options.target,'none')
    target = 0; % a flag used to denote "no target" to the mex
elseif isa(options.target, 'double')
    target = options.target;
else
    error('matlab_bgl:invalidParameter', ...
        'options.target is not ''none'' or a vertex number.');
end

if (trans)
    A = A';
end

[d dt pred] = bfs_mex(A,u,target);


function s = trunc_singular(s, tol, relative, maxrank)
% REL_TRUNC_SINGULAR Helper routine to truncate singular values

%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

    if ~exist('relative','var'),    relative = true;     end
    if ~exist('maxrank','var'), maxrank = length(s); end

    summ = cumsum(s.^2,'reverse');

    if relative
        s = find(summ > tol^2, 1, 'last');
        if isempty(s), s = 1; end
    else
        s = find(summ > tol^2*summ(1), 1, 'last');
        if isempty(s), s = 1; end
    end

    s = min([s, maxrank, length(s)]);

end


function [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,optname,x0,LB,UB,varargin)
% RMSEARCH: Randomly multistarted overlay to several optimizers
% usage: [xfinal,ffinal,exitflag,xstart] = RMSEARCH(fun,optname,x0,LB,UB)
% usage: [xfinal,ffinal,exitflag,xstart] = RMSEARCH(fun,optname,x0,LB,UB,prop1,val1,prop2,val2,...)
%
% This tool is useful when little information is available
% for good starting values for an optimization, or when many
% local solutions exist. It is also useful to map out the various
% basins of attaction of each local solution.
%
% RMSEARCH is normally used for bounded optimization problems,
% but with care can be used when some variables are unbounded.
% Whenever possible I'd still recommend the use of both lower
% and upper bounds if you can possibly do so. Restriction of
% the solution space is an important way to improve the
% performance of any optimization.
%
%
% arguments: (Input)
%  fun - name of a function, inline function, or a function handle
%
%  optname - character string - the name of the
%        optimizer to be used. Valid options are
%
%        'fminsearchbnd', 'fminsearchcon', 'fmincon'
%        'fminbnd', 'fzero', 'lsqnonlin', 'fminsearch'
%
%        No default is allowed, but the function name may
%        be shortened as long as the shortening is unambiguous.
%        Thus 'fminb' or 'fz' are acceptable, but 'fm' is not.
%
%        Note that fzero and fminbnd, since they work in 1-d
%        will search EVERY bracketed interval they find that
%        contains either a root or a minimizer (as appropriate.)
%
%        Remember to set the options structure properly for
%        fmincon, turning off the largescale optimizer as
%        appropriate. Otherwise you will see many warning
%        messages generated, one for each call to the optimizer.
%
%  x0 -  starting value. Used to determine the dimension and
%        shape of the parameter vector. Also used to determine
%        the distribution of the random starts when the bounds
%        are incompletely specified.
%
%        No default is allowed.
%
%        If both lower and upper bounds are supplied for a
%        parameter, then the distribution for the multi-starts
%        is chosen to be uniform between those bounds. If no
%        upper bound or no lower bound, then the distribution
%        follows a two parameter exponential distribution, with
%        mean at x0(i). If both upper and lower bounds are
%        missing (i.e., they are +/- inf), then that parameter
%        will be normally distribued with mean at x0(i).
%
%  LB -  Lower bounds for the parameters, as used by fmincon,
%        fminsearchbnd, lsqnonlin, etc.
%
%        LB must be the same size as x0, and LB(i)<=UB(i)
%
%        When no lower bound exists for a variable, use -inf
%
%  UB -  Upper bounds for the parameters, as used by fmincon,
%        fminsearchbnd, lsqnonlin, etc.
%
%        UB must be the same size as x0, and LB(i)<=UB(i)
%
%        When no upper bound exists for a variable, use +inf
%
%
% Additional inputs must be in the form of property/value pairs.
%   Properties are character strings. They may be shortened
%   to the extent that they are unambiguous. Properties are
%   not case sensitive. Valid property names are:
%
%    'Plot', 'Nonlcon', 'FractionUsed', 'InitialSample',
%    'OverSample', 'A', 'B' 'Options'
%
%   All properties have default values, chosen as intelligently
%   as I could manage. Values that are character strings may
%   also be unambiguously shortened. The legal values for each
%   property are:
%
%   'Plot' - specifies if a plot is generated {'on', 'off'}
%         A plot can only be generated for 1 or 2 dimensional
%         problems. A warning is generated for more than 2
%         variables if plot is turned on.
%
%         DEFAULT: 'off'
%
%   'Nonlcon' - Nonlinear constraint function or function handle
%         Only used for 'fmincon' or 'fminsearchcon'. Other
%         optimizers will cause an error if this is provided.
%
%         DEFAULT: ''
%
%   'FractionUsed' - Fractional amount of the total sample that
%         are used as multiple starts for the optimizer. It must
%         lie in the half open interval (0,1].
%
%         DEFAULT: 1  (100% of the samples are used as starting
%         points.
%
%         Note: FractionUsed is ignored for fminbnd and fzero.
%
%   'InitialSample' - Size of the total initial random sample
%         generated.
%
%         DEFAULT: 100  (100 points are generated)
%
%   'OverSample' - Over-sampling ratio. If linear inequality or
%         or nonlinear inequality constraints are involved, a
%         simple sampling strategy within the bounds may fail
%         to produce as many feasible points as are requested.
%
%         Thus if 100 points are requested in the initial sample,
%         a 20 to 1 oversampling ratio will generate a total of
%         2000 initial samples that are all within the simple bound
%         constraints. All of these points are compared to the
%         inequality constraints, rejecting all of those that fail.
%         The first InitialSample set of points are retained, only
%         these points are considered as starting points for an
%         optimization.
%
%         DEFAULT: 20  (a 20 to 1 over-sampling ratio)
%
%   'A', 'B' - Linear Inequality constraint matrix (A) and right
%         hand side vector (B), used only for 'fmincon' or
%         'fminsearchcon'. Other optimizers will cause an error
%         if these are provided.
%
%   'Options' - options structure for the specified optimizer.
%
%        default: optimset('optname')
%
%        For fminsearchbnd or fminsearchcon, the default is
%        optimset('fminsearch')
%
% arguments: (Output)
%  xfinal - final solutions found for each xstart point. Each
%         row of xfinal is one solution.
%
%  ffinal - objective function at xfinal.
%
%         For lsqnonlin or lsqcurvefit, ffinal is the sum
%         of squares.
%
%  exitflag - exitflags for each "solution" found
%
%  xstart - starting value used for each point in xfinal
%
%
% Example usage:
%  Find all zeros of besselj(2,x) betweeen x=0 and x=50. Note
%  that x0 is actually ignored in this case. Generate a plot of
%  the sample points and the solutions found.
%
%  fun = @(x) besselj(2,x);
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,'fzero',...
%       10,0,50,'initialsample',500,'plot','on')
%
% Example usage:
%  Find local minimizers of besselj(2,x) betweeen x=0 and x=200
%  Generate a plot of the sample points and the solutions found.
%
%  fun = @(x) besselj(2,x);
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,'fminbnd',...
%       10,0,200,'initialsample',500,'plot','on')
%
% Example usage:
%  Find local minimizers of the peaks function. Generate
%  a plot of the sample points and the solutions found. Use
%  fminsearchbnd as the optimizer.
%
%  fun = inline('peaks(x(1),x(2))','x');
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun,'fminsearchbnd',...
%      [0 0],[-5 -5],[5 5],'initialsample',100,'plot','on')
%
% Example usage:
%  Find the local minimizers of the peaks function within a circle
%  of radius 2 around the origin. Generate a plot of the sample points
%  and the solutions found. Use fmincon as the optimizer, setting the
%  appropriate options for its use. Note the trick with deal for nonlcon
%  inside an anonymous function.
%
%  fun = inline('peaks(x(1),x(2))','x');
%  opts = optimset('fmincon');
%  opts.LargeScale = 'off';
%  opts.Display = 'none';
%  nonlcon = @(x) deal(norm(x) - 2,[]);
%  [xfinal,ffinal,exitflag,xstart] = rmsearch(fun, ...
%     'fmincon',[0 0],[-2 -2],[2 2],'initialsample',100, ...
%     'plot','on','options',opts,'nonlcon',nonlcon)
%
%
% See also: fminsearchbnd, fmincon, fminsearchcon, fzero, fminbnd, lsqnonlin
%
%
% Author: John D'Errico
% E-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 1/1/07

% set defaults
par.Plot = 'off';
par.Nonlcon = '';
par.FractionUsed = 1;
par.InitialSample = 100;
par.OverSample = 20;
par.A = [];
par.B = [];
par.OptName = optname;
par.Options = [];

na = length(varargin);
if (rem(na,2)==1)
  error 'Property/value pairs must come as PAIRS of arguments.'
elseif na>0
  par = parse_pv_pairs(par,varargin);
end

% shape of x0, dimension of the problem
if (nargin<3) || isempty(x0)
  error 'x0 must be provided.'
else
  par.Nx0 = size(x0);
  x0 = x0(:);
  par.Dim = prod(par.Nx0);
end

% checks for validity of parameters, any problems
par = check_params(par);

% Was fun a string, or an inline/anonymous function?
if (nargin<1)
  help rmsearch
  return
elseif isempty(fun)
  error 'fun was not supplied.'
elseif ischar(fun)
  % a character function name
  fun = str2func(fun);
end

% Check the bounds
if (nargin<4) || isempty(LB)
  LB = repmat(-inf,par.Dim,1);
else
  LB = LB(:);
end
if (nargin<5) || isempty(UB)
  UB = repmat(inf,par.Dim,1);
else
  UB = UB(:);
end
% are they the same size as X0?
if length(LB)~=par.Dim
  error 'Lower bound array was inconsistent in size with x0'
end
if length(UB)~=par.Dim
  error 'Upper bound array was inconsistent in size with x0'
end

% Were there any unusable inequalities provided?
if ~ismember(par.OptName,{'fmincon','fminsearchcon'})
  if par.UseLC
    error(['Linear inequalities provided are not usable by ',par,OptName])
  elseif par.UseNLC
    error(['Nonlinear inequalities provided are not usable by ',par,OptName])
  end
end

% are any of the bounds inconsistent?
if any(LB>UB)
  error 'It is required that: LB <= UB'
end

% Choose the distributions used to sample
% from for each variable.
LBinf=isinf(LB);
UBinf=isinf(UB);
for i = 1:par.Dim
  if LBinf(i) && UBinf(i)
    % both are unbounded, use a normal
    vardist(i).dist = 'normal';
    vardist(i).mean = x0(i);
    vardist(i).sd = 1;
  elseif ~LBinf(i) && UBinf(i)
    % only lower bound, use an exponential
    vardist(i).dist = 'lower';
    vardist(i).lambda = x0(i) - LB(i);
    if vardist(i).lambda<=0
      vardist(i).lambda = 0.5;
    else
      vardist(i).lambda = 0.5 ./(vardist(i).lambda);
    end
    vardist(i).shift = LB(i);
  elseif LBinf(i) && ~UBinf(i)
    % only upper bound, use an exponential
    vardist(i).dist = 'upper';
    vardist(i).lambda = UB(i) - x0(i);
    if vardist(i).lambda <= 0
      vardist(i).lambda = 0.5;
    else
      vardist(i).lambda = 0.5 ./(vardist(i).lambda);
    end
    vardist(i).shift = UB(i);
  else
    % dual bounds, use a uniform
    vardist(i).dist = 'uniform';
    vardist(i).a = LB(i);
    vardist(i).b = UB(i);
  end
end

% Generate the intial random samples. Choose enough
% of them in case there are constraints to worry about.
if par.UseLC || par.UseNLC
  % oversampling for rejection
  xinitial = rand(par.OverSample*par.InitialSample,par.Dim);
else
  % No need to oversample
  xinitial = rand(par.InitialSample,par.Dim);
end

% generate a sample with the specified distributions
for i = 1:par.Dim
  switch vardist(i).dist
    case 'uniform'
      % uniform between two bounds
      xinitial(:,i) = vardist(i).a + ...
        (vardist(i).b - vardist(i).a)*xinitial(:,i);
    case 'normal'
      % unbounded - use a normal
      xinitial(:,i) = vardist(i).mean + ...
        vardist(i).sd*randn(par.InitialSample,1);
    case 'lower'
      % lower bound, use an exponential
      xinitial(:,i) = vardist(i).shift - ...
        log(xinitial(:,i))/vardist(i).lambda;
    case 'upper'
      % upper bound, use an exponential
      xinitial(:,i) = vardist(i).shift + ...
        log(xinitial(:,i))/vardist(i).lambda;
  end
end

% if 1-d problem, fminbnd and fzero will both do best
% if any bounds are included in the sample
if (par.Dim == 1)
  if ~isinf(LB) && ~isinf(UB)
    xinitial = [LB;UB;xinitial];
  elseif  (~isinf(LB) && isinf(UB))
    xinitial = [LB;xinitial];
  elseif (isinf(LB) && ~isinf(UB))
    xinitial = [UB;xinitial];
  end
end

% Do we need to reject any samples?
if par.UseLC
  k = ((repmat(par.B,1,size(xinitial,1)) - par.A*xinitial') < 0);
  k = any(k,1);
  xinitial(k,:) = [];
end
if size(xinitial,1)<1
  error 'Linear inequalities too strict. No feasible points were identified.'
end
if par.UseNLC
  % test any nonlinear constraints
  nx = size(xinitial,1);
  k = true(nx,1);
  for i = 1:nx
    [cineq,ceq] = par.Nonlcon(xinitial(i,:)); %#ok
    k(i) = all(cineq>0);
  end
  % reject those that failed
  xinitial(k,:) = [];
end
if size(xinitial,1)<1
  error 'Nonlinear constraints too strict. No feasible points were identified.'
end

% Some points got through, but not as many as requested
xsize = size(xinitial,1);
if xsize < par.InitialSample
  warning('RMSEARCH:FeasiblePoints',['Only ',num2str(xsize),' feasible points were found, ', ...
    num2str(par.InitialSample),' were requested. Insufficiently Oversampled.'])
end
% keep as many samples as we need if oversampled
xinitial = xinitial(1:min(xsize,par.InitialSample),:);
xsize = size(xinitial,1);

% Evaluate each sample through our objective function.
finitial = zeros(xsize,1);
for i = 1:xsize
  if ~strcmp(par.OptName,'lsqnonlin')
    % A simple optimizer:
    % 'fminbnd', 'fzero', 'fmincon', 'fminsearchbnd',
    % 'fminsearchcon','fminsearch'
    finitial(i) = fun(reshape(xinitial(i,:),par.Nx0));
  else
    % lsqnonlin, so we need to form the sum of squares
    f = fun(reshape(xinitial(i,:),par.Nx0));
    finitial(i) = sum(f.^2);
  end
end

% At which of these sample points will we
% start the indicated optimizer? Was it a 1-d
% problem? If so, then both fzero and fminbnd
% use a bracketed interval.
xfinal = zeros(size(xinitial));
ffinal = zeros(size(finitial));
if (par.Dim==1) && ismember(par.OptName,{'fzero', 'fminbnd'})
  switch par.OptName
    case 'fzero'
      % fzero - sort on x first
      [xinitial,tags] = sort(xinitial);
      finitial = finitial(tags);

      % find zero crossings in f
      ind = 1:(xsize-1);
      k = find((sign(finitial(ind)) .* sign(finitial(ind+1))) <= 0);

      % any crossings found?
      if isempty(k)
        error 'No zero crossings found in this sample for fzero'
      end

      xfinal = zeros(length(k),1);
      ffinal = xfinal;
      exitflag = xfinal;
      xstart = xfinal;
      fstart = xfinal;
      % loop over the candidate intervals
      for i = 1:length(k)
        [xfinal(i),ffinal(i),exitflag(i)] = fzero(fun, ...
          [xinitial(k(i)),xinitial(k(i)+1)],par.Options);

        % store the better of the two points in each bracket
        if abs(finitial(k(i))) <= abs(finitial(k(i)+1))
          xstart(i) = xinitial(k(i));
          fstart(i) = finitial(k(i));
        else
          xstart(i) = xinitial(k(i)+1);
          fstart(i) = finitial(k(i)+1);
        end
      end

    case 'fminbnd'
      % fminbnd - sort on x first
      [xinitial,tags] = sort(xinitial);
      finitial = finitial(tags);

      % find local minima in f
      ind = 2:(xsize-1);
      k = 1 + find((finitial(ind) <= finitial(ind-1)) & ...
        (finitial(ind) <= finitial(ind+1)));

      % do we look in the first interval?
      if finitial(1) < finitial(2)
        k = union(2,k);
      end
      % or the last one?
      if finitial(end) < finitial(end-1)
        k = union(xsize-1,k);
      end

      % any local minima found?
      xfinal = zeros(length(k),1);
      ffinal = xfinal;
      exitflag = xfinal;
      xstart = xfinal;
      fstart = xfinal;
      % loop over the candidate intervals
      for i = 1:length(k)
        [xfinal(i),ffinal(i),exitflag(i)] = fminbnd(fun, ...
          xinitial(k(i)-1),xinitial(k(i)+1),par.Options);

        % store in xstart
        xstart(i) = xinitial(k(i));
        fstart(i) = finitial(k(i));
      end
  end
else
  % it must be a multivariable minimization, or only one
  % variable, but not with a bracketed optimizer.

  % sort on f first in increasing order
  [finitial,tags] = sort(finitial);
  xinitial = xinitial(tags,:);

  if isempty(par.FractionUsed) || (par.FractionUsed==1)
    % use all the points as starting values
    nk = xsize;
  else
    nk = min(xsize,ceil(xsize*par.FractionUsed));
  end

  % any local minima found?
  xfinal = zeros(nk,par.Dim);
  ffinal = zeros(nk,1);
  exitflag = ffinal;
  xstart = xfinal;
  fstart = ffinal;
  % loop over the candidates chosen
  for i = 1:nk
    % store the start points in xstart and
    % fstart for plotting later
    xstart(i,:) = xinitial(i,:);
    fstart(i) = finitial(i);

    % but which optimizer?
    switch par.OptName
      case 'lsqnonlin'
        [xfinal(i,:),ffinal(i),residual,exitflag(i)] = ...
          lsqnonlin(fun,xinitial(i,:),LB,UB,par.Options); %#ok
      case 'fminsearchbnd'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fminsearchbnd(fun,xinitial(i,:),LB,UB,par.Options);
      case 'fminsearchcon'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fminsearchcon(fun,xinitial(i,:),LB,UB, ...
          par.A,par.B,par.Nonlcon,par.Options);
      case 'fminsearch'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fminsearch(fun,xinitial(i,:),par.Options);
      case 'fmincon'
        [xfinal(i,:),ffinal(i),exitflag(i)] = ...
          fmincon(fun,xinitial(i,:),par.A,par.B, ...
          [],[],LB,UB,par.Nonlcon,par.Options);
    end
  end
end

% do we plot the results?
if strcmp(par.Plot,'on') && par.Dim <= 2
  switch par.Dim
    case 1
      % its a 1-d problem
      figure
      plot([xstart';xfinal'],[fstart';ffinal'],'g-')
      hold on
      plot(xfinal,ffinal,'ro')
      plot(xinitial,finitial,'b+')
      plot([xinitial(1),xinitial(end)],[0 0],'k:')
      hold off
      xlabel 'x'
      ylabel 'fun(x)'
      title(['Random multi-started search: ',par.OptName])

    case 2
      % a 2-d problem
      figure
      C = finitial-min(finitial);
      C = C/max(C);
      colormap hsv
      scatter3(xinitial(:,1),xinitial(:,2),finitial,50,C,'+')
      hold on
      plot3([xstart(:,1)';xfinal(:,1)'], ...
        [xstart(:,2)';xfinal(:,2)'],[fstart';ffinal'],'g-')
      plot3(xfinal(:,1),xfinal(:,2),ffinal,'ro')
      hold off
      xlabel 'x'
      ylabel 'y'
      zlabel 'fun([x,y])'
      title(['Random multi-started search: ',par.OptName])

  end
end

% ============================================
%     end of mainline rmsearch
% ============================================

% ============================================
% subfunction - check_params
% ============================================
function par = check_params(par)
% check the parameters for acceptability
%
% Defaults
% par.Plot = 'off'
% par.Nonlcon = '';
% par.FractionUsed = 1;
% par.InitialSample = 100;
% par.OptName = optname;

% Nonlcon == '' by default
if ~isempty(par.Nonlcon)
  if ischar(par.Nonlcon)
    par.Nonlcon = str2func(par.Nonlcon);
  end
  par.UseNLC = true;
else
  par.UseNLC = false;
end

% FractionUsed == 1 by default
if isempty(par.FractionUsed)
  par.FractionUsed = 1;
elseif (length(par.FractionUsed)>1) || (par.FractionUsed<=0) || (par.FractionUsed>1)
  error 'FractionUsed must be scalar, in the interval (0,1]'
end

% InitialSample == 100 by default
if isempty(par.InitialSample)
  par.InitialSample = 100;
elseif (length(par.InitialSample)>1) || (par.InitialSample<=0)
  error 'InitialSample must be positive and scalar'
else
  par.InitialSample = ceil(par.InitialSample);
end

% OverSample == 20 by default
if isempty(par.OverSample)
  par.OverSample = 20;
elseif (length(par.OverSample)>1) || (par.OverSample<1)
  error 'OverSample must be positive and scalar, >= 1'
end

% OptName is char
valid = {'fmincon', 'fminsearchcon', 'fminsearchbnd', ...
  'fminbnd', 'fzero', 'lsqnonlin', 'fminsearch'};
if isempty(par.OptName) || ~ischar(par.OptName)
  error 'Invalid OptName: Must be non-empty character'
end
ind = strmatch(par.OptName,valid,'exact');
if (length(ind)==1)
  par.OptName = valid{ind};
else
  ind = strmatch(par.OptName,valid);
  if isempty(ind) || (length(ind)>1)
    error(['Invalid OptName: ',par.OptName])
  else
    par.OptName = valid{ind};
  end
end
% 2+ dimensions for a 1-d optimizer?
if (par.Dim>1) && ismember(par.OptName,{'fminbnd', 'fzero'})
  error '1-d optimizer specified, but more than 1 variable to optimize'
end

% Options is a struct
if isempty(par.Options)
  switch par.OptName
    case 'fmincon'
      par.options = optimset('fmincon');
    case 'fzero'
      par.options = optimset('fzero');
    case 'fminbnd'
      par.options = optimset('fminbnd');
    case 'lsqnonlin'
      par.options = optimset('lsqnonlin');
    case {'fminsearch', 'fminsearchbnd', 'fminsearchcon'}
      par.options = optimset('fminbnd');
  end
elseif ~isstruct(par.Options)
  error 'If provided, Options must be a struct generated by optimset'
end

% Plot is char
valid = {'on', 'off'};
if isempty(par.Plot)
  error 'Invalid Plot: Must be character or empty'
end
ind = find(strncmpi(par.Plot,valid,length(par.Plot)));
if (length(ind)==1)
  par.Plot = valid{ind};
else
  error(['Invalid Plot: ',par.Plot])
end
if strcmp(par.Plot,'on') && (par.Dim>3)
  % No plots available for 4 or more variables
  Warning 'Sorry, plots are not generated for 4 or more variables.'
  par.Plot = 'off';
end

% A & B are both empty by default
if ~isempty(par.A) && (size(par.A,2)~=par.Dim)
  error 'A must have the same number of columns as the # of variables'
end
if ~isempty(par.B) && (size(par.A,1)~=size(par.B,1))
  error 'B must have the same number of rows as A'
end
if isempty(par.A)
  par.UseLC = false;
else
  par.UseLC = true;
end

% ============================================
% Included subfunction - parse_pv_pairs
% ============================================
function params=parse_pv_pairs(params,pv_pairs)
% parse_pv_pairs: parses sets of property value pairs, allows defaults
% usage: params=parse_pv_pairs(default_params,pv_pairs)
%
% arguments: (input)
%  default_params - structure, with one field for every potential
%             property/value pair. Each field will contain the default
%             value for that property. If no default is supplied for a
%             given property, then that field must be empty.
%
%  pv_array - cell array of property/value pairs.
%             Case is ignored when comparing properties to the list
%             of field names. Also, any unambiguous shortening of a
%             field/property name is allowed.
%
% arguments: (output)
%  params   - parameter struct that reflects any updated property/value
%             pairs in the pv_array.
%
% Example usage:
% First, set default values for the parameters. Assume we
% have four parameters that we wish to use optionally in
% the function examplefun.
%
%  - 'viscosity', which will have a default value of 1
%  - 'volume', which will default to 1
%  - 'pie' - which will have default value 3.141592653589793
%  - 'description' - a text field, left empty by default
%
% The first argument to examplefun is one which will always be
% supplied.
%
%   function examplefun(dummyarg1,varargin)
%   params.Viscosity = 1;
%   params.Volume = 1;
%   params.Pie = 3.141592653589793
%
%   params.Description = '';
%   params=parse_pv_pairs(params,varargin);
%   params
%
% Use examplefun, overriding the defaults for 'pie', 'viscosity'
% and 'description'. The 'volume' parameter is left at its default.
%
%   examplefun(rand(10),'vis',10,'pie',3,'Description','Hello world')
%
% params =
%     Viscosity: 10
%        Volume: 1
%           Pie: 3
%   Description: 'Hello world'
%
% Note that capitalization was ignored, and the property 'viscosity'
% was truncated as supplied. Also note that the order the pairs were
% supplied was arbitrary.

npv = length(pv_pairs);
n = npv/2;

if n~=floor(n)
  error 'Property/value pairs must come in PAIRS.'
end
if n<=0
  % just return the defaults
  return
end

if ~isstruct(params)
  error 'No structure for defaults was supplied'
end

% there was at least one pv pair. process any supplied
propnames = fieldnames(params);
lpropnames = lower(propnames);
for i=1:n
  p_i = lower(pv_pairs{2*i-1});
  v_i = pv_pairs{2*i};

  ind = strmatch(p_i,lpropnames,'exact');
  if isempty(ind)
    ind = find(strncmp(p_i,lpropnames,length(p_i)));
    if isempty(ind)
      error(['No matching property found for: ',pv_pairs{2*i-1}])
    elseif length(ind)>1
      error(['Ambiguous property name: ',pv_pairs{2*i-1}])
    end
  end
  p_i = propnames{ind};

  % override the corresponding default in params
  params = setfield(params,p_i,v_i); %#ok

end

% end % parse_pv_pairs


function [init_factors, init_factors_opts] = generate_init_factors(V, rank, options)
% Initialization algorithm
%
% Created by H.Kasai on May 21, 2019
%
% Change log:
%
%
%   May  21, 2019 (Hiroyuki Kasai): Created the initial version
%
%   Oct. 14, 2020 (H.Huangi): Added 'LPinit'
%
%   June. 20, 2022 (Hiroyuki Kasai): Modified to handle the case where one
%                                   factorization matrix is input, and
%                                   the other is not.

    % V = WH + R

    m = size(V, 1);
    n = size(V, 2);

    init_factors_opts = [];

    % initialize
    generate_wh_init = true;
    generate_r_init = true;

    %% generate W and H
    if isfield(options, 'x_init')
        if isfield(options.x_init, 'W') && isfield(options.x_init, 'H')
            init_factors.W = options.x_init.W;
            init_factors.H = options.x_init.H;
            generate_wh_init = false;
        elseif isfield(options.x_init, 'W') && ~isfield(options.x_init, 'H')
            init_factors.W = options.x_init.W;
            init_factors.H = rand(rank, n);
            generate_wh_init = false;
        elseif ~isfield(options.x_init, 'W') && isfield(options.x_init, 'H')
            init_factors.W = rand(m, rank);
            init_factors.H = options.x_init.H;
            generate_wh_init = false;
        end

        if isfield(options.x_init, 'R')
            init_factors.R = options.x_init.R;
            generate_r_init = false;
        end
    end

    if isfield(options, 'special_init_factors')

        if generate_wh_init || generate_r_init
            [init_factors, init_factors_opts] = options.special_init_factors(V, rank, generate_wh_init, init_factors_opts, options);
        end

    else

        if generate_wh_init

            if ~isfield(options, 'init_alg')
                alg = 'random';
            else
                alg = options.init_alg;
            end

            switch(alg)

                case 'LPinit'

                    H = LPinitSemiNMF(V, rank);
                    W = V * pinv(H);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'random'

                    W = rand(m, rank);
                    H = rand(rank, n);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'ones'

                    W = ones(m, rank);
                    H = ones(rank, n);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'semi_random' % for SemiNMF

                    H = rand(rank, n);
                    W = V / H; % V * inv(H)

                    init_factors.W = W;
                    init_factors.H = H;

                case 'symm'

                    W = ones(m, rank);

                    init_factors.W = W;
                    init_factors.H = W';

                case 'symm_mean'

                    % make sure that entries of H fall into the interval [0, 2*sqrt(m/k)],
                    % where 'm' is the average of all entries of V.
                    % See https://github.com/dakuang/symnmf

                    W = 2 * full(sqrt(mean(mean(V)) / rank)) * rand(m, rank);

                    init_factors.W = W;
                    init_factors.H = W';

                case 'NNDSVD'

                    [W, H] = NNDSVD(abs(V), rank, 0);

                    init_factors.W = W;
                    init_factors.H = H;

                case 'kmeans'

                    [label, center] = litekmeans(V',rank, 'maxIter', 10);
                    center = max(0,center);
                    W = center';
                    WTW = W'*W;
                    WTW = max(WTW, WTW');
                    WTX = W'*V;
                    H = max(0, WTW\WTX);

                    init_factors.W = W;
                    init_factors.H = H;

                case {'prob_expectation', 'prob_random'}

                    T = tn_vector();

                    if strcmp(alg, 'prob_random')
                        E = exponential_dist();
                    end

                    exp_U   = options.exp_U;
                    exp_V   = options.exp_V;
                    var_U   = options.var_U;
                    var_V   = options.var_V;
                    mu_U    = options.mu_U;
                    mu_V    = options.mu_V;
                    tau_U   = options.tau_U;
                    tau_V   = options.tau_V;

                    for k = 1 : rank
                        for i = 1 : m
                            tau_U(i, k) = 1;

                            if options.ard
                                hyperparam = options.exp_lambdak(k);
                            else
                                hyperparam = options.hyperparams.lambdaU(i, k);
                            end

                            if strcmp(alg, 'prob_random')
                                mu_U(i, k) = E.exponential_draw(hyperparam);
                            else
                                mu_U(i, k) = 1.0/hyperparam;
                            end
                        end
                    end

                    for k = 1 : rank
                        for j = 1 : n
                            tau_V(j, k) = 1;

                            if options.ard
                                hyperparam = options.exp_lambdak(k);
                            else
                                hyperparam = options.hyperparams.lambdaV(j, k);
                            end

                            if strcmp(alg, 'prob_random')
                                mu_V(j, k) = E.exponential_draw(hyperparam);
                            else
                                mu_V(j, k) = 1.0/hyperparam;
                            end
                        end
                    end

                    for k = 1 : rank
                        [exp_U, var_U] = update_exp(T, exp_U, var_U, mu_U, tau_U, k);
                    end

                    for k = 1 : rank
                        [exp_V, var_V] = update_exp(T, exp_V, var_V, mu_V, tau_V, k);
                    end

                    init_factors.W = exp_U;
                    init_factors.H = exp_V';
                    init_factors_opts.var_W = var_U;
                    init_factors_opts.var_H = var_V';
                    init_factors_opts.mu_W  = mu_U;
                    init_factors_opts.mu_H  = mu_V';
                    init_factors_opts.tau_W = tau_U;
                    init_factors_opts.tau_H = tau_V';

                otherwise

                    % do random initialization

                    W = rand(m, rank);
                    H = rand(rank, n);

                    init_factors.W = W;
                    init_factors.H = H;

            end

        end

        if isfield(options, 'norm_w')
            if options.norm_w ~= 0
                % normalize W
                init_factors.W = normalize_W(init_factors.W, options.norm_w);
            end
        end

        if isfield(options, 'norm_h')
            if options.norm_h ~= 0
                % normalize H
                init_factors.H = normalize_H(init_factors.H, options.norm_h);
            end
        end

        %% generate R
        if generate_r_init
            if isfield(options, 'x_init_robust')
                if options.x_init_robust
                    if isfield(options, 'x_init')
                        if isfield(options.x_init, 'R')
                            init_factors.R = options.x_init.R;
                            generate_r_init = false;
                        end
                    end

                    if generate_r_init
                        init_factors.R = rand(m, n);
                    end
                else
                    init_factors.R = zeros(m, n);
                end
            else
                init_factors.R = zeros(m, n);
            end
        end

    end
end

function [exp_A, var_A] = update_exp(T, exp_A, var_A, mu_A, tau_A, k)

    exp_A(:,k) = T.expectation(mu_A(:,k), tau_A(:,k));
    var_A(:,k) = T.variance(mu_A(:,k), tau_A(:,k));

end


% -*- INTERNAL UNDOCUMENTED FUNCTION -*-
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2015 Rafael Vazquez
%
% 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 3 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 should have received a copy of the GNU General Public License
% along with Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [Jinv, geo_inv_der2] = geopdes_inv_der2__ (geo_map_jac, geo_map_der2)

vsize = size (geo_map_jac);
vsize(end+1:4) = 1;

rdim = vsize(1);
ndim = vsize(2);

Jinv = geopdes_inv__ (geo_map_jac);
Jinv = reshape (Jinv, [ndim, rdim, vsize(3:end)]);
Jsize = size (Jinv);
Jsize(end+1:4) = 1;

Jinv1 = reshape (Jinv, [1, Jsize]);
Jinv2 = permute (Jinv1, [1 3 2 4:numel(Jsize)+1]);
Jinv3 = permute (Jinv1, [3 2 1 4:numel(Jsize)+1]);

geo_inv_der2 = zeros ([ndim, rdim, rdim, vsize(3:end)]);

for alp = 1:ndim
  for idim = 1:rdim
    for jdim = 1:rdim
      aux1 = sum (bsxfun (@times, geo_map_der2, Jinv1(:,:,idim,:,:)), 2);
      aux2 = sum (bsxfun (@times, aux1, Jinv2(:,jdim,:,:,:)), 3);
      aux3 = sum (bsxfun (@times, aux2, Jinv3(:,alp,:,:,:)), 1);

      geo_inv_der2(alp,idim,jdim,:,:) = -reshape (aux3, vsize(3:end));
    end
  end
end

end

%!test
%!
%! %% x = u^2 + u + 1
%! %% y = v^2 + v + 1
%!
%! xufun = @(u, v) (2*u + 1);
%! xvfun = @(u, v) (0*u);
%! yufun = @(u, v) (0*u);
%! yvfun = @(u, v) (2*v + 1);
%!
%! xuufun = @(u, v) (2+0*u);
%! xuvfun = @(u, v) (0*u);
%! xvvfun = @(u, v) (0*u);
%!
%! yuufun = @(u, v) (0*u);
%! yuvfun = @(u, v) (0*u);
%! yvvfun = @(u, v) (2+0*v);
%!
%! uxfun = @(u, v) (1./(2*u+1));
%! vyfun = @(u, v) (1./(2*v+1));
%! uxxfun = @(u, v) (-2./(2*u+1).^3);
%! vyyfun = @(u, v) (-2./(2*v+1).^3);
%!
%! u = linspace (0,1, 10);
%! v = linspace (0,1, 10);
%! %u = rand (1, 10);
%! %v = rand (1, 10);
%! geo_map_jac(1,1,:) = xufun(u,v);
%! geo_map_jac(1,2,:) = xvfun(u,v);
%! geo_map_jac(2,1,:) = yufun(u,v);
%! geo_map_jac(2,2,:) = yvfun(u,v);
%!
%! geo_map_der2(1,1,1,:) = xuufun(u,v);
%! geo_map_der2(1,1,2,:) = xuvfun(u,v);
%! geo_map_der2(1,2,1,:) = xuvfun(u,v);
%! geo_map_der2(1,2,2,:) = xvvfun(u,v);
%! geo_map_der2(2,1,1,:) = yuufun(u,v);
%! geo_map_der2(2,1,2,:) = yuvfun(u,v);
%! geo_map_der2(2,2,1,:) = yuvfun(u,v);
%! geo_map_der2(2,2,2,:) = yvvfun(u,v);
%!
%! [Jinv, geo_inv_der2] = geopdes_inv_der2__ (geo_map_jac, geo_map_der2);
%! assert (uxfun (u, v), reshape (Jinv(1,1,:), 1, numel(u)), 1e-14)
%! assert (vyfun (u, v), reshape (Jinv(2,2,:), 1, numel(u)), 1e-14)
%! assert (uxxfun (u, v), reshape (geo_inv_der2(1,1,1,:), 1, numel(u)), 1e-14)
%! assert (vyyfun (u, v), reshape (geo_inv_der2(2,2,2,:), 1, numel(u)), 1e-14)


% Identical to isomapEmbed without the last part which sometimes causes exceptions

function [X, sigma2] = isomap2Embed(Y, dims)

% ISOMAP2EMBED Embed data set with Isomap.

% SHEFFIELDML

% Note: isomap code uses the transpose of a design matrix.
if any(any(isnan(Y)))
  error('Cannot initialise gplvm using isomap when missing data is present.')
else
  D = L2_distance(Y', Y', 1);
  options.dims = 1:dims;
  neighbours = 7;
   options.display = 0;

  [Xstruct, sigma2, E] = Isomap(D, 'k', neighbours, options);
  X = zeros(size(Y, 1), dims);
  if length(Xstruct.index) ~= size(Y, 1)
    % We don't really deal with this problem correctly here ...
    warning('Isomap graph is not fully connected');
  end
  X(Xstruct.index, :) = Xstruct.coords{dims}';
  % Rescale X so that variance is 1 and mean is zero.
  meanX = mean(X);
  X = X-ones(size(Y, 1), 1)*meanX;
  varX = var(X);
  X = X*diag(sqrt(1./varX));
end


function combo_test33 ( )

%*****************************************************************************80
%
%% COMBO_TEST33 tests RGF_G_TABLE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 July 2011
%
%  Author:
%
%    John Burkardt
%
  m = 6;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST33\n' );
  fprintf ( 1, '  RGF_G_TABLE tabulates generalized restricted\n' );
  fprintf ( 1, '  growth functions.\n' );
  fprintf ( 1, ' \n' );

  offset = 1;

  d = rgf_g_table ( m );

  for i = 0 : m
    for j = 0 : m - i
      fprintf ( 1, '%6d', d(i+offset,j+offset) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [min] = hr2min(hr)
% Convert time from hours to minutes.
% Chad A. Greene 2012
min = hr*60;


function [D, dist, aLongestString] = LCS(X,Y)
%%%Calculates the longest common substring between to strings.
%%%Code written by David Cumin
%%%email: d.cumin@auckland.ac.nz
%%%INPUT
%%%X, Y - both are strings e.g. 'test' or 'stingtocompare'
%%%OUTPUT
%%%D is the substring over the length of the shortest string
%%%dist is the length of the substring
%%%aLongestString is a sting of length dist (only one of potentially many)

%%%For example
%%% X = 'abcabc';
%%% Y = 'adcbac';
%%% [D dist str] = LCS(X,Y);
%%% results in:
%%% D = 0.6667
%%% dist = 4
%%% str = acbc
%%% this is seen for X: 'a-c-bc' and Y: 'a-cb-c'

%%%Make matrix
n =length(X);
m =length(Y);
L=zeros(n+1,m+1);
L(1,:)=0;
L(:,1)=0;
b = zeros(n+1,m+1);
b(:,1)=1;%%%Up
b(1,:)=2;%%%Left

for i = 2:n+1
    for j = 2:m+1
        if (X(i-1) == Y(j-1))
            L(i,j) = L(i-1,j-1) + 1;
            b(i,j) = 3;%%%Up and left
        else
            L(i,j) = L(i-1,j-1);
        end
        if(L(i-1,j) >= L(i,j))
            L(i,j) = L(i-1,j);
            b(i,j) = 1;%Up
        end
        if(L(i,j-1) >= L(i,j))
            L(i,j) = L(i,j-1);
            b(i,j) = 2;%Left
        end
    end
end
L(:,1) = [];
L(1,:) = [];
b(:,1) = [];
b(1,:) = [];
dist = L(n,m);

D = (dist / min(m,n));
if(dist == 0)
    aLongestString = '';
else
    %%%now backtrack to find the longest subsequence
    i = n;
    j = m;
    p = dist;
    aLongestString = {};
    while(i>0 && j>0)
        if(b(i,j) == 3)
            aLongestString{p} = X(i);
            p = p-1;
            i = i-1;
            j = j-1;
        elseif(b(i,j) == 1)
            i = i-1;
        elseif(b(i,j) == 2)
            j = j-1;
        end
    end

    if ischar(aLongestString{1})
        aLongestString = char(aLongestString)';
    else
        aLongestString = cell2mat(aLongestString);
    end
end

end


function unique_index = i4vec_unique_index ( n, a )

%*****************************************************************************80
%
%% I4VEC_UNIQUE_INDEX indexes the first occurrence of values in an I4VEC.
%
%  Discussion:
%
%    For element A(I) of the vector, FIRST_UNIQUE(I) is the uniqueness index
%    of A(I).  That is, if A_UNIQUE contains the unique elements of A,
%    gathered in order, then
%
%      A_UNIQUE ( UNIQUE_INDEX(I) ) = A(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of elements of A.
%
%    Input, integer A(N), the array.
%
%    Output, integer UNIQUE_INDEX(N), the unique index.
%
  unique_index(1:n) = -1;
  unique_num = 0;

  for i = 1 : n

    if ( unique_index(i) == -1 )

      unique_num = unique_num + 1;
      unique_index(i) = unique_num;

      for j = i + 1 : n
        if ( a(i) == a(j) )
          unique_index(j) = unique_num;
        end
      end

    end

  end

  return
end


function [Sl1, Sl2, Jac] = deformation_field_3d_analysis(Tr, pixel_resolution)
% calculates sliding amount jacobian and other parameters of transformation
% Tr should be given in physical units (mm)!

dT = cell(3,3);
sz = size(Tr);
[m1,m2,m3] = ndgrid(1:sz(1), 1:sz(2), 1:sz(3));
mm{1} = m1;
mm{2} = m2;
mm{3} = m3;
for i1 = 1 : 3
    for i2 = 1: 3
        tmp = Tr(:,:,:,i1)+mm{i1}*pixel_resolution(i1);
        dT{i1,i2} = DGradient(tmp, [], i2) / pixel_resolution(i2);
    end
end

% EV = zeros([3, sz(1), sz(2), sz(3)]);
Sl1 = zeros([sz(1), sz(2), sz(3)]);
Sl2 = zeros([sz(1), sz(2), sz(3)]);
Jac = zeros([sz(1), sz(2), sz(3)]);
for i3 = 1 : sz(3)
for i2 = 1 : sz(2)
for i1 = 1 : sz(1)

    mat = [dT{1,1}(i1,i2,i3), dT{1,2}(i1,i2,i3), dT{1,3}(i1,i2,i3);...
           dT{2,1}(i1,i2,i3), dT{2,2}(i1,i2,i3), dT{2,3}(i1,i2,i3);...
           dT{3,1}(i1,i2,i3), dT{3,2}(i1,i2,i3), dT{3,3}(i1,i2,i3); ];

    mat_sym = mat'*mat;

    v = eig(mat_sym);
    v = sort(v);
    v = sqrt(abs(v));
    Sl1(i1,i2,i3) = (v(3)-v(1))/2;
    Sl2(i1,i2,i3) = v(3)/(v(2) + v(1));
    Jac(i1,i2,i3) = det(mat);
end
end
end

end


function dimacs = computedimacs(b,c,A,xin,y,s,K);
% COMPUTEDIMACS
%
% min <C,X> s.t     AX = b, X > 0
% max b'y   s.t S-C+A'y =0, S > 0

% If no primal exist, fake till later
if isempty(xin)
    x = c*0;
else
    x = xin;
end

if isempty(s)
    s = c-A'*y;
end

xres = inf;
sres = inf;

% Not officially defined in DIMACS
if K.f>0
    sres = -min(norm(s(1:K.f),inf));
end

% Errors in linear cone
if K.l>0
    xres = min(x(1+K.f:K.f+K.l));
    sres = min(s(1+K.f:K.f+K.l));
end

% Errors in quadratic cone
if any(K.q)
    top = K.f+K.l;
    for i = 1:length(K.q)
        X = x(1+top:top+K.q(i));
        S = s(1+top:top+K.q(i));
        xres = min(xres,X(1)-norm(X(2:end)));
        sres = min(sres,S(1)-norm(S(2:end)));
        top = top + K.q(i);
    end
end

% Errors in semidefinite cone
if any(K.s)
    top = K.f+K.l+K.q+K.r;
    for i = 1:length(K.s)
        X = reshape(x(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        S = reshape(s(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        xres = min(xres,min(eig(full(X))));
        sres = min(sres,min(eig(full(S))));
        top = top + K.s(i)^2;
    end
end

err1 = norm(b-A*x)/(1 + norm(b,inf));
err2 = max(0,-xres)/(1 + norm(b,inf));
err3 = conenorm(s-(c-A'*y),K)/(1+norm(c,inf));
%err3 = norm(s-(c-A'*y))/(1+norm(c,inf)); % Used by some solvers
err4 = max(0,-sres)/(1+max(abs(c)));
err5 = (c'*x-b'*y)/(1+abs(c'*x)+abs(b'*y));
err6 = x'*(c-A'*y)/(1+abs(c'*x)+abs(b'*y));

% No primal was computed
if isempty(xin)
    err1 = nan;
    err2 = nan;
    err5 = nan;
    err6 = nan;
end
dimacs = [err1 err2 err3 err4 err5 err6];

function t = conenorm(s,K)

% Implementation of the norm described on
% http://plato.asu.edu/dimacs/node3.html

t = 0;

if K.f + K.l>0
    t = t + norm(s(1:K.f+K.l));
end

top = 1+K.f+K.l;
if any(K.q)
    for i = 1:length(K.q)
        t = t + norm(s(top:top+K.q(i)-1));
        top  = top + K.q(i);
    end
end

if any(K.s)
    for i = 1:length(K.s)
        S = reshape(s(top:top+K.s(i)^2-1),K.s(i),K.s(i));
        t = t + norm(S,'fro');
        top  = top + K.s(i)^2;
    end
end


function a = r8ri_to_r8ge ( nz, ija, sa, n )

%*****************************************************************************80
%
%% R8RI_TO_R8GE converts an R8RI matrix to R8GE form.
%
%  Discussion:
%
%    An R8RI matrix is in row indexed sparse storage form.
%
%    A R8GE matrix is in general storage.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Press, Brian Flannery, Saul Teukolsky, William Vetterling,
%    Numerical Recipes in FORTRAN: The Art of Scientific Computing,
%    Third Edition,
%    Cambridge University Press, 2007,
%    ISBN13: 978-0-521-88068-8,
%    LC: QA297.N866.
%
%  Parameters:
%
%    Input, integer NZ, the size required for the RI
%    or "row indexed" sparse storage.
%
%    Input, integer IJA(NZ), the index vector.
%
%    Input, real SA(NZ), the value vector.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix stored in GE
%    or "general" format.
%
  a = zeros ( n, n );

  for k = 1 : n
    i = k;
    j = k;
    a(i,j) = sa(k);
  end

  for i = 1 : n
    for k = ija(i) : ija(i+1) - 1
      j = ija(k);
      a(i,j) = sa(k);
    end
  end

  return
end


%   SCRIPT TO COMPUTE THE TORQUES AT EACH JOINT FOR DIFFERENT MOTION STATES OF
%   THE ARM.
%

function motor_selection

close all
global robot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETERS SECTION
%   Feel free to change the values of q, maximum_speeds and
%   maximum_accels.
%
%   This script tries to allow the student to test any mechanism
%   at the worst case. In this sense, q should be adjusted
%   as the pose where each joint would (statically) be needing a
%   higher torque.
%   The maximum_speeds and maximum_acceleration define a trapezoidal
%   speed profile. This trapezoidal speed is used by most machines
%   to command changes in speed in any of their joints.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

echo on
%LOAD ANY ROBOT YOU WOULD LIKE TO ANALYZE
%EXPERIMENTA CAMBIANDO EL ROBOT Y VIENDO LOS RESULTADOS!
robot = load_robot('ABB', 'IRB2600');
echo off

% robot pose: experiment by changing the pose while observing the different
%             torques at each joint
q=[0 pi/2 -pi/2 0 0 0]; %rad

%Velocidad maxima en cada articulacion en rad/s.
% robot.velmax = [deg2rad(175); %Axis 1, rad/s
%                 deg2rad(175); %Axis 2, rad/s
%                 deg2rad(175); %Axis 3, rad/s
%                 deg2rad(360); %Axis 4, rad/s
%                 deg2rad(360); %Axis 5, rad/s
%                 deg2rad(500)];%Axis 6, rad/s

maximum_speeds=[3.5 3.5 3.5 6.5 6.5 8.5];%rad/second

%maximum acceleration/deceleration for each joint

maximum_accels=maximum_speeds*2; %rad/second^2
%maximum_accels=[5 5 6 7 8 9];

% time of the trapezoidal profile that the joint moves at maximum speed
time_at_constant_speed=0.4; %seconds

%load robot parameters. Just uncomment this line

drawrobot3d(robot, q)

%START BY COMPUTING TORQUES AT G
%    robot.motors.G = [1 1 1 1 1 1 ]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  FIRST, COMPUTE TRAPEZOIDAL PROFILES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%compute acceleration plus deceleration times for every joint
time_acc = 2*maximum_speeds./maximum_accels+time_at_constant_speed;

%compute the total time for the slowest joint
total_time=max(time_acc);

% Trapezoidal speed profiles for each joint
[input_speeds, input_accels, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FINALLY, COMPUTE TORQUES FOR EACH MOTION STATE.
% Please note that we consider that the robot is placed at a fixed position and consider
% different motion situations when we change the acceleration and speed at
% each joint. For each motion state, the inverse dynamic model returns the
% torques at each joint that would bring the robot to that motion.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
compute_inverse_dynamics(q, input_speeds, input_accels, time);

  fprintf('\n\nOBSERVE THE PLOTS AND NOTE DOWN THE PEAK TORQUE, NOMINAL TORQUE AND MOTOR SPEEDS')
fprintf('\nNOW COMPUTE THE TORQUES FOR 5 DIFFERENT SELECTED MOTIONS STATES')
fprintf('\nPRESS ANY KEY TO CONTINUE...')

pause

% NOW COMPUTE THE WORST CASE CONSIDERING ONLY THE SPEED AND ACCELERATION AT
% 5 MOTIONS STATES
input_speeds = [zeros(6,1) maximum_speeds' maximum_speeds' maximum_speeds' zeros(6,1) ];
input_accels = [maximum_accels' maximum_accels' zeros(6,1) -maximum_accels' -maximum_accels' ];

compute_inverse_dynamics(q, input_speeds, input_accels, [1:5]);

%Computes a trapezoidal speed profile for every joint given maximum
%permitted accelerations and maximum joint speeds
function [input_speeds, input_accelerations, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time)

delta_time=0.01;

%build time vector: twice acceleration time plus time at constant speed
time = 0:delta_time:total_time;

input_speeds=[];
input_accelerations=[];

for j=1:length(maximum_speeds),
    vel_row=[];
    acc_row=[];
    for i=1:length(time),
        [vel acc] = compute_values(time(i), maximum_speeds(j), maximum_accels(j), total_time);
        vel_row = [vel_row vel];
        acc_row = [acc_row acc];
    end
    input_speeds = [input_speeds; vel_row];
    input_accelerations = [input_accelerations; acc_row];
end

%returns the values of velocity and speed corresponding to a given time
function [vel acc]=compute_values(time_i, vel_max, acc_max, total_time)

tacc = vel_max/acc_max;
tdec = total_time-tacc;

if time_i < tacc
    vel = time_i.*acc_max;
    acc = acc_max;
    return;
elseif (time_i >= tacc) & (time_i < tdec)
    vel = vel_max;
    acc = 0;
    return;
else % time_i> tdec
    vel = vel_max-(time_i-tdec)*acc_max;
    acc = -acc_max;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   COMPUTE THE INVERSE DYNAMICS FOR EACH MOTION STATE
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function compute_inverse_dynamics(q, input_speeds, input_accels, time)
global robot

%adjust_view(robot)
torques=[];
for j=1:length(time),
    fprintf('\nComputing time %d out of %d', j, length(time));
    % compute the torque to bring the robot instantaneously to this motion
    % state. change M=1  to add the effects of a 1kg mass load at the end effector
    M=20;
    %please note that the force due to the load acts on the z axis of
    tau=inversedynamic(robot, q, input_speeds(:,j), input_accels(:,j), [0  0 -9.81]', [ 0  0 -M 0 0 0 ]');
    torques=[torques tau];
end

%plot trapezoidal profiles
figure, hold, xlabel('time (s)'), ylabel('Input reference speeds (rad/s)')
plot(time, input_speeds(1,:), time, input_speeds(2,:), time, input_speeds(3,:),...
        time, input_speeds(4,:), time, input_speeds(5,:), time, input_speeds(6,:));
legend('Speed for joint 1 (qd1)','Speed for joint 2 (qd2)','Speed for joint 3 (qd3)',...
   'Speed for joint 4 (qd4)','Speed for joint 5 (qd5)','Speed for joint 6 (qd6)' )
%plot trapezoidal profiles, acceleration
figure, hold, xlabel('time (s)'), ylabel('Input reference acceleration (rad/s)')
plot(time, input_accels(1,:), time, input_accels(2,:), time, input_accels(3,:),...
        time, input_accels(4,:), time, input_accels(5,:), time, input_accels(6,:));
legend('Acceleration for joint 1 (qd1)','Acceleration for joint 2 (qd2)','Acceleration for joint 3 (qd3)',...
   'Acceleration for joint 4 (qd4)','Acceleration for joint 5 (qd5)','Acceleration for joint 6 (qd6)' )

% plot results. First, torques at each joint
figure, hold, xlabel('time (s)'), ylabel('Join Torques (N m)')
plot(time, torques(1,:), time, torques(2,:), time, torques(3,:),...
        time, torques(4,:), time, torques(5,:), time, torques(6,:));
legend('Torque for joint 1 ','Torque  for joint 2 ','Torque  for joint 3 ',...
   'Torque  for joint 4','Torque  for joint 5 ','Torque  for joint 6 ' )

%plot torques at each motor
figure, hold, xlabel('time (s)'), ylabel('Motor Torques (N m)')
plot(time, torques(1,:)/robot.motors.G(1), time, torques(2,:)/robot.motors.G(2), time, torques(3,:)/robot.motors.G(3),...
        time, torques(4,:)/robot.motors.G(4), time, torques(5,:)/robot.motors.G(5), time, torques(6,:)/robot.motors.G(6));
legend('Torque at motor 1 ','Torque at motor 2 ','Torque at motor 3 ',...
   'Torque at motor 4 ','Torque at motor 5 ','Torque at motor 6 ' )

%plot power needed by the motor at each time step, without considering the
%losses at the gears
figure, hold, xlabel('time (s)'), ylabel('Power needed by each motor (W)')
plot(time, torques(1,:).*input_speeds(1,:), time, torques(2,:).*input_speeds(2,:), time, torques(3,:).*input_speeds(3,:),...
        time, torques(4,:).*input_speeds(4,:), time, torques(5,:).*input_speeds(5,:), time, torques(6,:).*input_speeds(6,:));
legend('Power: motor 1','Power: motor 2','Power: motor 3',...
   'Power: motor 4','Power: motor 5','Power: motor 6' )

%plot motor speed in rpm for each motor
figure, hold, xlabel('time (s)'), ylabel('Speed in r.p.m of every motor (rev/min)')
plot(time, robot.motors.G(1)*input_speeds(1,:)*30/pi, time, robot.motors.G(2)*input_speeds(2,:)*30/pi, time, robot.motors.G(3)*input_speeds(3,:)*30/pi,...
        time, robot.motors.G(4)*input_speeds(4,:)*30/pi, time, robot.motors.G(5)*input_speeds(5,:)*30/pi, time, robot.motors.G(6)*input_speeds(6,:)*30/pi);
legend('Speed at motor 1 (qd1*G)','Speed at motor 2 (qd2*G)','Speed at motor 3 (qd3*G)',...
   'Speed at motor 4 (qd4*G)','Speed at motor 5 (qd5*G)','Speed at motor 6 (qd6*G)' )

%Now present results:
fprintf('\nMAIN RESULTS (referred to each motor): ')
fprintf('\n------------------------------------------------------------------------------------ ')
fprintf('\n                         Joint 1 - Joint 2 - Joint 3 - Joint 4  - Joint 5 - Joint 6: ')
fprintf('\nPeak Torque (N�m):        %.3f     %.3f     %.3f     %.3f      %.3f    %.3f ', max(abs(torques(1,:)/robot.motors.G(1))), max(abs(torques(2,:)/robot.motors.G(2))) , max(abs(torques(3,:)/robot.motors.G(3))) , max(abs(torques(4,:)/robot.motors.G(4))) , max(abs(torques(5,:)/robot.motors.G(5))) , max(abs(torques(6,:)/robot.motors.G(6))))
fprintf('\nNominal Torque (N�m):     %.3f     %.3f     %.3f     %.3f      %.3f    %.3f  ', abs(torques(1,round(length(torques)/2))/robot.motors.G(1)), abs(torques(2,round(length(torques)/2))/robot.motors.G(2)), abs(torques(3,round(length(torques)/2))/robot.motors.G(3))...
    , abs(torques(4,round(length(torques)/2))/robot.motors.G(4)), abs(torques(5,round(length(torques)/2))/robot.motors.G(5)), abs(torques(6,round(length(torques)/2))/robot.motors.G(6)))
fprintf('\nMax motor speed (r.p.m.):   %.1f     %.1f     %.1f     %.1f      %.1f    %.1f  ', max(abs(robot.motors.G(1)*input_speeds(1,:))*30/pi), max(abs(robot.motors.G(2)*input_speeds(2,:))*30/pi), max(abs(robot.motors.G(3)*input_speeds(3,:))*30/pi)...
    ,max(abs(robot.motors.G(4)*input_speeds(4,:))*30/pi), max(abs(robot.motors.G(5)*input_speeds(5,:))*30/pi), max(abs(robot.motors.G(6)*input_speeds(6,:))*30/pi))
fprintf('\n------------------------------------------------------------------------------------ ')


function [ undx, xdnu ] = r8col_tol_undex ( m, n, a, unique_num, tol )

%*****************************************************************************80
%
%% R8COL_TOL_UNDEX indexes tolerably unique entries of an R8COL.
%
%  Discussion:
%
%    An R8COL is an M x N array of R8 values, regarded as N columns
%    each of M R8 values.
%
%    The goal of this routine is to determine a vector UNDX,
%    which points to the unique elements of A, in sorted order,
%    and a vector XDNU, which identifies, for each entry of A, the index of
%    the unique sorted element of A.
%
%    This is all done with index vectors, so that the elements of
%    A are never moved.
%
%    The first step of the algorithm requires the indexed sorting
%    of A, which creates arrays INDX and XDNI.  (If all the entries
%    of A are unique, then these arrays are the same as UNDX and XDNU.)
%
%    We then use INDX to examine the entries of A in sorted order,
%    noting the unique entries, creating the entries of XDNU and
%    UNDX as we go.
%
%    Once this process has been completed, the object A could be
%    replaced by a compressed object XU, containing the unique entries
%    of A in sorted order, using the formula
%
%      XU(1:UNIQUE_NUM) = A(UNDX(1:UNIQUE_NUM)).
%
%    We could then, if we wished, reconstruct the entire vector A, or
%    any element of it, by index, as follows:
%
%      A(I) = XU(XDNU(I)).
%
%    We could then replace A by the combination of XU and XDNU.
%
%    Later, when we need the I-th entry of A, we can locate it as
%    the XDNU(I)-th entry of XU.
%
%    Here is an example of a vector A, the sort and inverse sort
%    index vectors, and the unique sort and inverse unique sort vectors
%    and the compressed unique sorted vector.
%
%      I    A   Indx  Xdni      XU   Undx  Xdnu
%    ----+-----+-----+-----+--------+-----+-----+
%      1 : 11.     1     1 :    11.     1     1
%      2 : 22.     3     5 :    22.     2     2
%      3 : 11.     6     2 :    33.     4     1
%      4 : 33.     9     8 :    55.     5     3
%      5 : 55.     2     9 :                  4
%      6 : 11.     7     3 :                  1
%      7 : 22.     8     6 :                  2
%      8 : 22.     4     7 :                  2
%      9 : 11.     5     4 :                  1
%
%    INDX(2) = 3 means that sorted item(2) is A(3).
%    XDNI(2) = 5 means that A(2) is sorted item(5).
%
%    UNDX(3) = 4 means that unique sorted item(3) is at A(4).
%    XDNU(8) = 2 means that A(8) is at unique sorted item(2).
%
%    XU(XDNU(I))) = A(I).
%    XU(I)        = A(UNDX(I)).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the dimension of the data values.
%
%    Input, integer N, the number of data values.
%
%    Input, real A(M,N), the data values.
%
%    Input, integer UNIQUE_NUM, the number of unique values in A.
%    This value is only required for languages in which the size of
%    UNDX must be known in advance.
%
%    Input, real TOL, a tolerance for equality.
%
%    Output, integer UNDX(UNIQUE_NUM), the UNDX vector.
%
%    Output, integer XDNU(N), the XDNU vector.
%
  undx = zeros ( unique_num, 1 );
  xdnu = zeros ( n, 1 );
%
%  Implicitly sort the array.
%
  indx = r8col_sort_heap_index_a ( m, n, a );
%
%  Consider entry I = 1.
%  It is unique, so set the number of unique items to K.
%  Set the K-th unique item to I.
%  Set the representative of item I to the K-th unique item.
%
  i = 1;
  k = 1;
  undx(k) = indx(i);
  xdnu(indx(i)) = k;
%
%  Consider entry I.
%
%  If it is unique, increase the unique count K, set the
%  K-th unique item to I, and set the representative of I to K.
%
%  If it is not unique, set the representative of item I to a
%  previously determined unique item that is close to it.
%
  for i = 2 : n

    unique = 1;

    for j = 1 : k
      diff = max ( abs ( a(1:m,indx(i)) - a(1:m,undx(j)) ) );
      if ( diff <= tol )
        unique = 0;
        xdnu(indx(i)) = j;
        break
      end
    end

    if ( unique )
      k = k + 1;
      undx(k) = indx(i);
      xdnu(indx(i)) = k;
    end

  end

  return
end


function [node,edge] = ...
        getiso2(xpos,ypos,zdat,ilev,filt)
%GETISO2 extract an iso-contour from a structured two-dimen-
%sional data-set.
%   [NODE,EDGE] = GETISO2(XPOS,YPOS,ZFUN,ZLEV) returns the
%   contour ZFUN(XPOS,YPOS) = ZLEV as a PSLG, by post-proce-
%   ssing the output of the CONTOUR function. The arguments
%   XPOS, YPOS and ZFUN must all be N-by-M arrays, and ZLEV
%   a scalar contouring value.
%
%   See also GETNAN2, FIXGEO2, BFSGEO2, REFINE2

%-----------------------------------------------------------
%   Darren Engwirda : 2018 --
%   Email           : de2363@columbia.edu
%   Last updated    : 25/03/2018
%-----------------------------------------------------------

    if (nargin < +5), filt = +0. ; end

%---------------------------------------------- basic checks
    if ( ~isnumeric(xpos) || ...
         ~isnumeric(ypos) || ...
         ~isnumeric(zdat) || ...
         ~isnumeric(ilev) || ...
         ~isnumeric(filt) )
        error('getiso2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(xpos) ~= +2 || ...
        ndims(ypos) ~= +2 || ...
        ndims(zdat) ~= +2 )
        error('getiso2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    if (isvector(xpos))
        xnum = length(xpos);
    else
        xnum = size(xpos,2);
    end

    if (isvector(ypos))
        ynum = length(ypos);
    else
        ynum = size(ypos,1);
    end

    if (xnum ~= size(zdat,2) || ...
        ynum ~= size(zdat,1) || ...
        numel(ilev) ~= +1 || ...
        numel(filt) ~= +1 )
        error('getiso2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%------------------------------------ compute the isocontour
    cmat = contourc( ...
        xpos,ypos,zdat,[ilev,ilev]) ;

%------------------------------------ "walk" contour segment
    node = [] ; edge = [] ; ipos = +1 ;

    while (ipos < size(cmat,2))

        numc = cmat(2,ipos);
        ppts =[cmat(1,ipos+1:ipos+numc)', ...
               cmat(2,ipos+1:ipos+numc)'
              ] ;

        pmin = min(ppts,[],1);
        pmax = max(ppts,[],1);

        pdel = pmax - pmin ;

        if (min(pdel)>=filt)

            if all(ppts(1,:) == ppts(end,:))

    %-------------------------------- closed - back to start
            enew = ...
           [(1:numc-1)',(2:numc-0)'; numc,1] ;

            else

    %-------------------------------- open - dangling endpts
            enew = ...
           [(1:numc-1)',(2:numc-0)'] ;

            end

            enew = ...
            enew + size(node,1);

            node = [node; ppts];
            edge = [edge; enew];

        end

        ipos = ipos + numc + 1 ;

    end

end


function z = cpf_tangent(V, lam, Ybus, Sbusb, Sbust, pv, pq, ...
                            zprv, Vprv, lamprv, parameterization, direction)
%CPF_TANGENT  Computes normalized tangent predictor for continuation power flow
%   Z = CPF_TANGENT(V, LAM, YBUS, SBUSB, SBUST, PV, PQ, ...
%                                 ZPRV, VPRV, LAMPRV, PARAMETERIZATION, DIRECTION)
%
%   Computes a normalized tangent predictor for the continuation power flow.
%
%   Inputs:
%       V : complex bus voltage vector at current solution
%       LAM : scalar lambda value at current solution
%       YBUS : complex bus admittance matrix
%       SBUSB : handle of function returning nb x 1 vector of complex
%               base case injections in p.u. and derivatives w.r.t. |V|
%       SBUST : handle of function returning nb x 1 vector of complex
%               target case injections in p.u. and derivatives w.r.t. |V|
%       PV : vector of indices of PV buses
%       PQ : vector of indices of PQ buses
%       ZPRV : normalized tangent prediction vector from previous step
%       VPRV : complex bus voltage vector at previous solution
%       LAMPRV : scalar lambda value at previous solution
%       PARAMETERIZATION : value of cpf.parameterization option.
%       DIRECTION: continuation direction (+1 for postive lambda
%                  increase, -1 otherwise)
%
%   Outputs:
%       Z : the normalized tangent prediction vector

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Shrirang Abhyankar, Argonne National Laboratory
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% sizes
nb = length(V);
npv = length(pv);
npq = length(pq);

Vm = abs(V);

%% compute Jacobian for the power flow equations
[dSbus_dVa, dSbus_dVm] = dSbus_dV(Ybus, V);
[dummy, neg_dSdb_dVm] = Sbusb(Vm);
[dummy, neg_dSdt_dVm] = Sbust(Vm);
dSbus_dVm = dSbus_dVm - neg_dSdb_dVm - lam * (neg_dSdt_dVm - neg_dSdb_dVm);

j11 = real(dSbus_dVa([pv; pq], [pv; pq]));
j12 = real(dSbus_dVm([pv; pq], pq));
j21 = imag(dSbus_dVa(pq, [pv; pq]));
j22 = imag(dSbus_dVm(pq, pq));

J = [   j11 j12;
        j21 j22;    ];

Sxf = Sbust(Vm) - Sbusb(Vm);    %% "transfer" at current voltage level
dF_dlam = -[real(Sxf([pv; pq])); imag(Sxf(pq))];
[dP_dV, dP_dlam] = cpf_p_jac(parameterization, zprv, V, lam, Vprv, lamprv, pv, pq);

%% linear operator for computing the tangent predictor
J = [   J   dF_dlam;
      dP_dV dP_dlam ];

% J = [ J, dF_dlam;
%       z([pv; pq; nb+pq; 2*nb+1])'];

%% compute normalized tangent predictor
z = zeros(size(zprv));
s = zeros(npv+2*npq+1, 1);
s(end,1) = sign(direction);
z([pv; pq; nb+pq; 2*nb+1]) = J\s;   %% tangent vector
z = z/norm(z);                      %% normalize tangent predictor


% largely drawn from run_readlsl of BCILAB
function lslin(handles)
% find stream
result = lsl_resolve_bypred(handles.lsllib, ['name=''' handles.streamName '''']);

% open inlet
inlet = lsl_inlet(result{1});
% info = inlet.info();

% create online stream data structure in base workspace (using appropriate meta-data)
onl_newstream(parseStreamName(handles.streamName), 'srate', result{1}.nominal_srate(), ...
    'chanlocs', {handles.chanlocs.labels}, 'buffer_len', 10);

% state variables for recursive least squares jitter correction
P = 1e10*eye(2);        % precision matrix (inverse covariance matrix of predictors)
w = [0 0]';             % linear regression coefficients [offset,slope]
lam = 2^(-1/(128 * 30)); % forget factor in RLS calculation
n = 0;                  % number of samples observed so far
numeric_offset = [];    % time-stamp offset to keep numerics healthy; will be initialized with first measured time stamp

% start reading
onl_read_background(parseStreamName(handles.streamName), @read_data, 20);

    % reads from inlet
    function results = read_data()
        [chunk, stamps] = inlet.pull_chunk();
        data_clock = inlet.time_correction([], 'median', 30);
        stamps = stamps + data_clock;
        stamps = update_regression(stamps);
        chunk = double(chunk);

        % this is the source of grief
        % taking only the last timestamp allows REST to run but the
        % timestamps are garbage
        % giving all the timestamps soft errors in onl_append. onl_append
        % seems to expect only one value so perhaps this idea of many
        % timestamps if not correct.
        try %#ok<TRYNC>
            stamps = stamps(end);
        end
        results = {chunk, stamps};
    end

    % perform RLS block update of regression coefficients
    % this is a regression from sample index onto timestamp of the sample
    function y = update_regression(y)
        if ~isempty(y)
            % sanitize numerics (all done relative to the first observed time stamp)
            if isempty(numeric_offset)
                numeric_offset = y(1); end
            y = y - numeric_offset;
            % define predictor matrix (bias, sample index)
            X = [ones(1,length(y)); n + (1:length(y))];
            n = n + length(y);
            % apply updates...
            for t=1:length(y)
                u = X(:,t);
                d = y(t);
                pi = u'*P;
                gam = lam + pi*u;
                k = pi'/gam;
                al = d - w'*u;
                w = w + k*al;
                Pp = k*pi;
                P = (1/lam)*(P-Pp);
            end
            % predict y
            y = w'*X + numeric_offset;
        end
    end

    % parse streamname
    function streamnames = parseStreamName(streamnames)
        if ~isvarname(streamnames)
            streamnames = streamnames(~ismember(streamnames,['-' ' ']));
        end
    end

end


function [u,eqn,info] = fracLapP2P2(node,elem,pde,option)
%% FRACLAPP2P2 solves fractional Laplacian equation using P2-P2 element
%
% [u,eqn,info] = fracLapP1P1(node,elem,pde,option) solves the fractional
% Laplacian equation
%
%  (-\Delta^^s u = f in \Omega with u = 0 on \partial \Omega.

%% Options
if ~exist('option','var'), option = []; end
if ~isfield(option,'plotflag'), option.plotflag = 0; end

%% Parameters
global s;
alpha = 1-2*s;
if s == 0.5
    gamma = 1;
else
    gamma = 5/(2*s)+0.01; % a different grading factor
end

%% Mesh and data structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1); NE = size(edge,1);
My = round(pde.L*sqrt(NT/2));
y = gradmap(0,pde.L,gamma,My);
Ny = length(y); % number of vertices in y-direction
NTy = Ny - 1;   % number of elements in y-direction
NTtotal = NT*NTy;
Nxdof = (N + NE); % dof in x  is number of vertices plus edges
Nydof = (Ny + NTy); % dof in y direction is number of vertices plus elements
Ndof = Nxdof*Nydof;

tic;
%% Stiffness matrix and mass matrix in the extended direction
% quantities in y direction
hy = diff(y);
a = zeros(NTy,5);
for i = 1:5
    a(:,i) = diff(y.^(alpha+i)/(alpha+i));
end
y1 = y(1:end-1);
y2 = y(2:end);
y12 = y1 + y2;
y1y2 = y1.*y2;
ym = y12/2;
% stiffness matrix in y direction
Ay = zeros(NTy,3,3);
Ay(:,1,1) = a(:,1)./hy.^2;
Ay(:,1,2) = -Ay(:,1,1);
Ay(:,2,1) = -Ay(:,1,1);
Ay(:,2,2) = Ay(:,1,1);
Ay(:,1,3) = 8*(a(:,2) - ym.*a(:,1))./hy.^3;
Ay(:,3,1) = Ay(:,1,3);
Ay(:,2,3) = -Ay(:,1,3);
Ay(:,3,2) = Ay(:,2,3);
Ay(:,3,3) = 64*(a(:,3) - 2*ym.*a(:,2) + ym.^2.*a(:,1))./hy.^4;
% mass matrix in y direction
My = zeros(NTy,3,3);
My(:,1,1) = (a(:,3) - 2*y1.*a(:,2) + y1.^2.*a(:,1))./hy.^2;
My(:,1,2) = (-a(:,3) + (y1+y2).*a(:,2) - y1.*y2.*a(:,1))./hy.^2;
My(:,2,1) = My(:,1,2);
My(:,2,2) = (a(:,3) - 2*y2.*a(:,2) + y2.^2.*a(:,1))./hy.^2;
My(:,1,3) = 4*(a(:,4) - (y12+y2).*a(:,3) + (y2.*y12 + y1y2).*a(:,2) ...
             - y2.*y1y2.*a(:,1))./hy.^3;
My(:,3,1) = My(:,1,3);
My(:,2,3) = 4*(-a(:,4) + (y12+y1).*a(:,3) - (y1.*y12 + y1y2).*a(:,2) ...
             + y1.*y1y2.*a(:,1))./hy.^3;
My(:,3,2) = My(:,2,3);
My(:,3,3) = 16*(a(:,5) - 2*y12.*a(:,4) + (y12.^2 + 2*y1y2).*a(:,3)...
               -2*y12.*y1.*y2.*a(:,2) + y1y2.^2.*a(:,1))./hy.^4;

%% Stiffness matrix and mass matrix in the original direction
[Dlambda,area] = gradbasis(node,elem);
% Compute a piecewise constant diffusion coefficient
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                                 % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)       % d is a function
    [lambda,weight] = quadpts(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
        K = K + weight(p)*pde.d(pxy);
   end
end
if ~isempty(pde.d) % build the coefficients into the scaled area
    areaK = K.*area;
else
    areaK = area;
end
At = zeros(NT,6,6);
Mt = zeros(NT,6,6);
% stiffness matrix
[lambda, w] = quadpts(2);
nQuad = size(lambda,1);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    for i = 1:6
        for j = 1:6
            At(:,i,j) = At(:,i,j) + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*areaK;
        end
    end
end
% mass matrix
[lambda, w] = quadpts(4);
nQuad = size(lambda,1);
phi(:,6) = 4*lambda(:,1).*lambda(:,2);
phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
phi(:,4) = 4*lambda(:,2).*lambda(:,3);
phi(:,5) = 4*lambda(:,3).*lambda(:,1);
for i = 1:6
    for j = 1:6
        for p = 1:nQuad
            Mt(:,i,j) = Mt(:,i,j) + w(p)*phi(p,i).*phi(p,j);
        end
        Mt(:,i,j) = Mt(:,i,j).*areaK;
    end
end
clear phi Dphip lambda Dlambda

%% Assemble stiffness matrix
dofMap = repmat(1:Nxdof,Nydof,1) + repmat((0:Nydof-1)'*Nxdof,1,Nxdof);
ii = zeros(18*18*NTtotal,1);
jj = zeros(18*18*NTtotal,1);
sA = zeros(18*18*NTtotal,1);
index = 0;
for m = 1:3
    for n = 1:3
        for i = 1:6
            for j = 1:6
                if m < 3
                    ii(index+1:index+NTtotal) = dofMap(m:Ny+m-2,elem2dof(:,i));
                else % m = 3: middle points of elements in y direction
                    ii(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem2dof(:,i));
                end
                if n < 3
                    jj(index+1:index+NTtotal) = dofMap(n:Ny+n-2,elem2dof(:,j));
                else % n = 3: middle points of elements in y direction
                    jj(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem2dof(:,j));
                end
                sA(index+1:index+NTtotal) = kron(At(:,i,j),My(:,m,n)) + ...
                                            kron(Mt(:,i,j),Ay(:,m,n));
                index = index + NTtotal;
            end
        end
    end
end
A = sparse(ii,jj,sA,Ndof,Ndof);
clear ii jj sA At Mt Ay My

%% Assemble the right hand side
b = zeros(Ndof,1);
u = zeros(Ndof,1);

%% Set up boundary conditions
% bdDof is found in dofP2
lateralbd = dofMap(:,bdDof);
topbd = dofMap(Ny,:);

% Modify the matrix to include the Dirichlet boundary condition
bdidx = zeros(Ndof,1);
bdidx(lateralbd) = 1;
bdidx(topbd) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*A*T + Tbd;

% Compute boundary integral over the original domain
if ~isfield(option,'fquadorder')
    option.fquadorder = 6;
end
[lambdaf,weightf] = quadpts(option.fquadorder);
phif(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
phif(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
phif(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
phif(:,4) = 4*lambda(:,2).*lambda(:,3);
phif(:,5) = 4*lambda(:,3).*lambda(:,1);
phif(:,6) = 4*lambda(:,1).*lambda(:,2);
nQuadf = size(lambdaf,1);
ft = zeros(NT,6);
for p = 1:nQuadf
    % quadrature points in the x-y coordinate
    pxy = lambdaf(p,1)*node(elem(:,1),:) ...
        + lambdaf(p,2)*node(elem(:,2),:) ...
        + lambdaf(p,3)*node(elem(:,3),:);
    fp = pde.f(pxy);
    for i = 1:6
        ft(:,i) = ft(:,i) + weightf(p)*phif(p,i)*fp;
    end
end
ft = ds*ft.*repmat(area,1,6);
b = b + accumarray(elem2dof(:),ft(:),[Ndof,1]);
% Neumann edfts are considered as open set. So the corner points should be
% set as Dirichlet boundary condition!
b(lateralbd) = 0;
clear ft

%% Record assembling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
freeNode = find(bdidx==0);
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for larft size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeNode) = A(freeNode,freeNode)\b(freeNode);
        residual = norm(b - A*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        option.x0 = u;
        option.solver = 'VCYCLE';
        option.freeDof = freeDof;
        [u,info] = mgfracLapP2P2(A,b,elem,option);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(A(freeNode,freeNode),b(freeNode),option);
end

%% Compute error using boundary integral
if isfield(pde,'exactu')
    err = zeros(NT,1);
    [lambda,weight] = quadpts(7);
    phi(:,6) = 4*lambda(:,2).*lambda(:,1);
    phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
    phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
    phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,1).*lambda(:,3);
    nQuad = size(lambda,1);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
             + lambda(p,2)*node(elem(:,2),:) ...
             + lambda(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        uhp = u(elem2dof(:,1)).*phi(p,1) + ...
              u(elem2dof(:,2)).*phi(p,2) + ...
              u(elem2dof(:,3)).*phi(p,3) + ...
              u(elem2dof(:,4)).*phi(p,4) + ...
              u(elem2dof(:,5)).*phi(p,5) + ...
              u(elem2dof(:,6)).*phi(p,6);
        up = pde.exactu(pxy);
        err = err + weight(p)*fp.*(up - uhp);
    end
else
    err = 0;
end
err = ds*sum(err.*area);
err = sqrt(err);
info.errH1 = err;

%% Output information
eqn = struct('A',A,'b',b,'freeNode',freeNode,'edge',edge);
info.assembleTime = assembleTime;


function vAberr=aberCorr(vOrig,obsVel,sunDist)
%%ABERCORR Rotate vectors for stellar aberration, approximately including
%          the effects of the gravitational potential of the Sun if the
%          distances between the observers and the Sun are provided.
%
%INPUTS: vOrig A 3XN set of N vectors pointing from the observers to the
%              objects being observed without corruption due to
%              aberration. The units of the vector do not matter.
%              Normally, one would expect vOrig to be composed of unit
%              vectors as aberration affects the pointing direction of the
%              vectors.
%       obsVel The 3XN matrix of the velocity of each observer with
%              respect to the stellar coordinate system origin in meters
%              per second. For example, if using the barycentric celestial
%              reference system (BCRS), they are the set of velocity
%              vectors of the observers with respect to the barycenter.
%      sunDist An optional 3XN vector of the scalar distances in meters
%              from the Sun to the observer. If this parameter is
%              included, an approximate correction due to the
%              gravitational potential of the Sun is included.
%
%OUTPUTS: vAberr The 3XN set of N vectors rotated to deal with aberration
%                effects.
%
%If all parameters are provided, the function is essentially a wrapper for
%the function iauAb in the International Astronomical Union's Standards of
%Fundamental Astronomy library with an adjustment so that the vector in
%question need not have unit magnitude. If the distance to the Sun is
%omitted, then the standard special relativistic aberration correction
%without any gravitational effects is applied. The standard special
%relativistic aberration correction is described in Chapter 7 of [1]
%
%Note that if the vectors provided are meant to be apparent distances,
%rather than just unit vectors representing directions, the transformation
%does not adjust the magnitudes of the vectors to account for special
%relativistic contraction of space due to the motion of the observer with
%respect to the origin. The magnitudes of the output vectors equal the
%magnitudes of the input vectors.
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%vAberr=aberCorr(vOrig,obsVel,sunDist);
%or
%vAberr=aberCorr(vOrig,obsVel);
%
%REFERENCES:
%[1] S. E. Urban and K. P.Seidelmann, Eds.,Explanatory Supplement to the
%    Astronomical Almanac, 3rd ed. Mill Valley, CA: University Science
%    Books, 2013.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function fhat = convSO3(fhat1,fhat2)
%

% old sizes
s1 = size(fhat1);
s2 = size(fhat2);

% get bandwidth
L = min(dim2deg(s1(1)),dim2deg(s2(1)));

% new size
l=length(s2)-length(s1);
s = max([s1(2:end),ones(1,l);s2(2:end),ones(1,-l)]);

% compute Fourier coefficients of the convolution
fhat = zeros([deg2dim(L+1),s]);
if prod(s) == 1 %simple SO3Fun
  for l = 0:L
    ind = deg2dim(l)+1:deg2dim(l+1);
    fhat(ind) = reshape(fhat2(ind),2*l+1,2*l+1) * ...
      reshape(fhat1(ind),2*l+1,2*l+1) ./ sqrt(2*l+1);
  end
else % vector valued SO3Fun
  for l = 0:L
    ind = deg2dim(l)+1:deg2dim(l+1);
    fhat_l = pagemtimes( full(reshape(fhat2(ind,:),[2*l+1,2*l+1,s2(2:end)])) , ...
      full(reshape(fhat1(ind,:),[2*l+1,2*l+1,s1(2:end)])) ) ./ sqrt(2*l+1);
    fhat(ind,:) = reshape(fhat_l,[],prod(s));
  end
end

end


function [ imgOut ] = hyperOrthorectify( imgIn, altitude, hpbw )
%HYPERORTHORECTIFY Orthorectifies areal observed data.
%   Orthorectifies areal observed data using nearest neighbor interpolation.
%
% Inputs
%   imgIn       Input image (m x n) or (m x n x p)
%   altitude    Sensor altitude (meters)
%   hpbw        Half power beam width (radians).
% Outputs
%   imgOut      Orthorectified image.

% Input parameters
if (ndims(imgIn) == 2)
    [h, w] = size(imgIn);
    p = 1;
elseif (ndims(imgIn) == 3)
    [h, w, p] = size(imgIn);
end

radPerPix = hpbw/w;
x = tan(hpbw/2)*altitude;  % m
gsd = altitude*radPerPix;  % m
n = x/gsd;

imgOut = zeros(h, floor(n)*2, p);
for k=1:p
    for j=1:h
        for i=-floor(n):1:floor(n)-1
            boresiteDistance = gsd*i;
            theta = atan(boresiteDistance/altitude);
            imagePix = round(theta / radPerPix);
            imgOut(j, floor(n)+i+1, k) = imgIn(j, (w/2)+imagePix+1, k);
        end
    end
end


function pass = test_biharm(pref)
% Check the CHEBFUN2 BIHARM command.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e6*pref.cheb2Prefs.chebfun2eps;

% Function to be used:
ff = @(x,y) x.^2.*y.^2;

% Bihamrmonic operator applied to ff:
f = chebfun2(ff);
fB = biharm(f);

% Exact solution:
gg = @(x,y) 8;
g = chebfun2(gg);

% Compare:
pass = ( norm(fB - g) < tol );

end


function [sys,x0,str,ts]=NL_PID_fhan_c(t,x,u,flag,r0,h0,r1,h1,h,c,r2,h2)
switch flag,
    case 0,
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2,
        sys=mdlUpdates(x,u,r0,r1,h0,h1,h);
    case 3,
        sys=mdlOutputs(x,c,r2,h2);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9},
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=5;
    sizes.NumOutputs=2;
    sizes.NumInputs=2;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdates(x,u,r0,r1,h0,h1,h)
    fh0=fhan(x(1)-u(1),x(2),r0,h0);
    x(1)=x(1)+h*x(2);
    x(2)=x(2)+h*fh0;
    fh1=fhan(x(3)-u(2),x(4),r1,h1);
    x(3)=x(3)+h*x(4);
    x(4)=x(4)+h*fh1;
    x(5)=x(5)+h*(x(1)-x(3));
    sys=x;
function sys=mdlOutputs(x,c,r2,h2)
    sys(1)=-fhan(x(1)-x(3),c*(x(2)-x(4)),r2,h2);
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;
function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end


function [Btuph] = GW2Btuph(GW)
% Convert power from gigawatts to British thermal units per hour.
% Chad A. Greene 2012
Btuph = GW*3412141633.1279;


%% 2's complement of binary no;

function twos_comp = twos_complement_of_binary(bin)
c=0;
temp=bin;
twos_comp=0;
while(temp>0 && rem(temp,10)==0)
    twos_comp=twos_comp*10 + rem(temp,10);
    temp=fix(temp/10);
    c=c+1;
end
if(temp>0)
    twos_comp=twos_comp*10 + rem(temp,10);
    temp=fix(temp/10);
    c=c+1;
end
while(temp>0)
    if(rem(temp,10)==1)
        twos_comp=twos_comp*10 + 0;
    else
        twos_comp=twos_comp*10 + 1;
    end
    temp=fix(temp/10);
    c=c+1;
end

temp=twos_comp;
twos_comp=0;
while(c>0)      %reversing the order;
    twos_comp=twos_comp*10 + rem(temp,10);
    temp=fix(temp/10);
    c=c-1;
end
end