function stats = calcStatistics(optObj,limit,smoothBounds)
%Calculate confidence and variance statistics of an OPTI curve fitting problem
%
%   Called By opti fitStats

%   Copyright (C) 2011-2014 Jonathan Currie (IPL)

% This function uses ideas from:
% - David M. Himmelblau. Process Analysis by Statistical Methods.
%   John Wiley & Sons, 1970
% - David I. Wilson. Advanced Control Using MATLAB, AUT University, 2014.
% - NonLinearModel.m from the Statistics Toolbox
% - confint.m from the Curve Fitting Toolbox
% - predint.m from the Curve Fitting Toolbox

% Value explanations
% http://www.ats.ucla.edu/stat/sas/output/reg.htm

if(nargin < 3 || isempty(smoothBounds)), smoothBounds = true; end
if(nargin < 2 || isempty(limit)), limit = 0.95; end

if(limit <= 0 || limit >= 1)
    error('Confidence limit must be 0 < limit < 1');
end

%Get Problem
prob = optObj.prob;
x = optObj.sol;
nparam = length(x);
ndata = length(prob.ydata); %check whether we need original data?
wlevel = optiWarnLevel(optObj.opts.warnings);
%Get Solution SSE
sse = optObj.obj;
dfe = ndata-nparam;
rmse = sqrt(sse/dfe);
if(wlevel>1)
    optiwarn('optistats:dfe','Your problem has #data <= #parameters, confidence limits and most statistics cannot be estimated');
end
%Build Return Structure
stats = struct('SSE',sse,'Rsquare',NaN,'AdjRsquare',NaN,'RMSE',NaN,'DFE',ndata-nparam,'ConfInt',NaN,'ConfBnds',struct('xdata',NaN,'bnds',NaN),'Cov',NaN,'BndIdx',NaN,...
               'Param',struct('StdError',NaN,'tStat',NaN,'pValues',NaN),'Model',struct('FStat',NaN,'pValue',NaN),'Conf',limit,'SolverStatus','');

nidx = strfind(prob.Name,'optifit:');
if(~isempty(nidx))
    stats.ModelStructure = prob.Name(9:end);
end

%Don't be overly enthusiastic
if(~isempty(strfind(optObj.info.Status,'Optimal')))
    stats.SolverStatus = 'Converged';
else
    stats.SolverStatus = optObj.info.Status;
end

%Ensure we have a NLS or DNLS
if(~any(strcmpi(prob.type,{'NLS','DNLS'})))
    error('This function can only be used on curve fitting (NLS,DNLS) problems');
end

%Get ydata and weights
ydata = prob.ydata;
weights = prob.weighting;

%Get Partial Derivative Matrix (data/parameters) [original xdata]
if(~isempty(prob.xdata))
    try
        X = prob.misc.fitGrad(x,prob.xdata);
        gmode = 2;
    catch
        X = prob.misc.fitGrad(x);
        gmode = 1;
    end
else
    try
        X = prob.misc.fitGrad(x);
    catch
        error('Unknown gradient field!');
    end
    gmode = 1;
end
%If we have weights, weight the Jacobian
Xnowts = X;
if(~isempty(weights))
    X = bsxfun(@times, weights, X);
end
%Check for active bounds, if so, remove cols of X
bidx = false(size(X,2),1);
if(~isempty(prob.lb) || ~isempty(prob.ub))
    if(any(~isinf(prob.lb)))
        bidx = bidx | abs(x-prob.lb) < sqrt(eps);
    end
    if(any(~isinf(prob.ub)))
        bidx = bidx | abs(x-prob.ub) < sqrt(eps);
    end
end
if(any(bidx))
    if(wlevel > 1)
        optiwarn('opti:conf','One or more bounds are active at the solution. The confidence interval and parameter statistics cannot be calculated for these variables.');
    end
    X = X(:,~bidx);
    %Correct dfe, rmse
    nparam      = nparam - sum(bidx);
    dfe         = ndata-nparam;
    rmse        = sqrt(sse/dfe);
    stats.DFE   = dfe;
    stats.RMSE  = rmse;
end

try
    %Solve inverse of X'*X, but only interested in diagonal
    [~,R] = qr(X,0);
    if(size(R,1)~=size(R,2))
        throw(MException('opti:conf','QR R not square'));
    end
    s = warning('off','MATLAB:singularMatrix');
    s1 = warning('off','MATLAB:nearlySingularMatrix');
    Rinv = R \ eye(length(R));
    warning(s1);
    warning(s);
catch
    error('The problem is poorly scaled and resulted in a (near) singular matrix, limits cannot be calculated');
end

%Solve Covariance Matrix
s = warning('off','MATLAB:singularMatrix');
s1 = warning('off','MATLAB:nearlySingularMatrix');
try
    L = chol(X'*X,'lower');
    stats.Cov = L'\(L\eye(length(L)))*rmse^2;
catch
    if(wlevel)
        optiwarn('opti:conf','The X''*X matrix has been found to be singular, and a general inverse will be used to (attempt to) solve the system covariance.\nResults are now suspicious at best...');
    end
    stats.Cov = inv(X'*X)*rmse^2; %#ok<MINV> %poor method - any better ideas?
end
warning(s1);
warning(s);
stats.BndIdx = bidx;

%Generate ypred for ANOVA stuff
if(nargin(prob.misc.fitFun)==2)
    sumypred = sum(prob.misc.fitFun(x,prob.xdata).^2);
else
    sumypred = sum(prob.misc.fitFun(x).^2);
end

%Solve Confidence Interval
v = sum(Rinv.^2,2) * (sse / dfe);
alpha = (1-limit)/2;
ConfInt = (-rmathlib('qt',alpha,dfe)* sqrt(v'))';
stats.ConfInt = NaN(length(bidx),1);
stats.ConfInt(~bidx) = ConfInt;

% Return X to the unweighted version
% This seems to be the accepted convention for bounds calculation
X = Xnowts;

%Solve Confidence Bounds for Plotting (note we must have system covariance for this to work!)
if(~isempty(stats.Cov))
    %If we have an ODE and simple time step (i.e. not a cell array or repeated measurements, try for a smooth curve)
    if(isfield(prob.misc,'xdata_orig') && ~isempty(prob.misc.xdata_orig) && ~iscell(prob.misc.xdata_orig))
        havRep = length(unique(prob.misc.xdata_orig)) ~= length(prob.misc.xdata_orig);
    else
        havRep = false;
    end
    % Try generate smooth bounds
    if (smoothBounds == true)
        if(~isempty(prob.ode) && ~iscell(prob.misc.xdata_orig) && length(unique(prob.xdata))==length(prob.xdata) && ~havRep)
            try
                %Lazy way to convert problem again
                xd = linspace(min(prob.xdata),max(prob.xdata),max(10*length(prob.xdata),1e2))';
                prob.ydata = ones(length(prob.ydata)/length(prob.xdata)*length(xd),1); prob.xdata = xd;
                prob = DNLS2NLS(prob,optObj.opts);
                Xb = prob.misc.fitGrad(x);
                ypred = prob.misc.fitFun(x,xd);
            catch %no luck
                xd = prob.xdata;
                Xb = X;
                if(nargin(prob.misc.fitFun)==2)
                    ypred = prob.misc.fitFun(x,xd);
                else
                    ypred = prob.misc.fitFun(x);
                end
            end
        else %algebraic system or complicated ode problem
            if(gmode==2)
                %try evaluate at many intermediate points for a smoother curve
                try
                    %Assume if xdata is a matrix, or not sorted, don't try smooth
                    if(size(prob.xdata,1) > 1 && size(prob.xdata,2) > 1 || (any(sort(prob.xdata) ~= prob.xdata) && any(sort(prob.xdata,'descend') ~= prob.xdata)))
                        error('skip');
                    end
                    xd = linspace(min(prob.xdata),max(prob.xdata),max(10*length(prob.xdata),1e2))';
                    Xb = prob.misc.fitGrad(x,xd);
                    ypred = prob.misc.fitFun(x,xd);
                catch
                    xd = prob.xdata;
                    Xb = X;
                    ypred = prob.misc.fitFun(x,prob.xdata);
                end
            else %no luck
                xd = prob.xdata;
                Xb = X;
                if(nargin(prob.misc.fitFun)==2)
                    %if function has two input arguments, try a numerical gradient
                    try
                        if(~isempty(strfind(char(prob.misc.fitFun),'odeEstim'))), error('no luck'); end %integrator most likely has set time points it is expecting
                        xd = linspace(min(prob.xdata),max(prob.xdata),max(10*length(prob.xdata),1e2))';
                        Xb = mklJac(@(x) prob.misc.fitFun(x,xd),x);
                        ypred = prob.misc.fitFun(x,xd);
                    catch
                        xd = prob.xdata;
                        Xb = X;
                        ypred = prob.misc.fitFun(x,xd);
                    end
                else
                    ypred = prob.misc.fitFun(x);
                end
            end
        end
    else
        xd = prob.xdata;
        Xb = X;
        if(nargin(prob.misc.fitFun)==2)
            ypred = prob.misc.fitFun(x,xd);
        else
            ypred = prob.misc.fitFun(x);
        end
    end

    %Ensure ypred is a column
    if(size(ypred,2) > 1), ypred = ypred'; end
    %Solve Prediction Bounds (Simultaneous Functional)
    crit = sqrt(length(x) * rmathlib('qf',limit, length(x), dfe));
    %If we have active bounds, best to calculate Jacobian at each point we want to plot, then drop columns, rather than use Covariance below
    if(any(bidx))
        try
            J = prob.misc.fitGrad(x,xd);
            E = J(:,~bidx)*Rinv;
            delta = crit * sqrt(sum(E.*E,2)) * sqrt(sse/dfe);
        catch
            delta = crit * sqrt(sum((Xb*stats.Cov) .* Xb,2));
        end
    else
        delta = crit * sqrt(sum((Xb*stats.Cov) .* Xb,2));
    end
    if(isempty(xd))
        stats.ConfBnds.xdata = (1:length(ypred))';
    else
        stats.ConfBnds.xdata = xd;
    end
    stats.ConfBnds.bnds = [ypred-delta ypred+delta];

    %Reshape based on number of curves
    if(~isempty(prob.odez0))
        if(~isempty(optObj.opts.dynamicOpts) && isfield(optObj.opts.dynamicOpts,'stateIndex') && ~isempty(optObj.opts.dynamicOpts.stateIndex))
            nc = length(optObj.opts.dynamicOpts.stateIndex);
        else
            nc = length(prob.odez0);
        end
        nd = size(ypred,1)/nc;
        if(floor(nd)==nd) %have to work out xdata associated with which point... anyone interested?
            cb = zeros(nd,nc*2);
            idx = 1;
            for i = 1:2:nc*2
                cb(:,i:i+1) = stats.ConfBnds.bnds(idx:idx+nd-1,:);
                idx = idx + nd;
            end
            stats.ConfBnds.bnds = cb;
        else
            stats.ConfBnds.bnds = NaN;
            stats.ConfBnds.xdata = NaN;
            if(wlevel)
                optiwarn('opti:confbnds','Cannot currently determine confidence prediction bounds for this type of problem, sorry!');
            end
        end
    end
    %If we had repeated points, try index out...
    if(havRep)
        %Check for repeated measurements
        try
            [~,ia] = unique(prob.misc.xdata_orig,'legacy');
        catch
            [~,ia] = unique(prob.misc.xdata_orig);
        end
        stats.ConfBnds.bnds = stats.ConfBnds.bnds(ia,:);
    end

    %Make sure in order for plotting
    [stats.ConfBnds.xdata,sbidx] = sort(stats.ConfBnds.xdata);
    stats.ConfBnds.bnds = stats.ConfBnds.bnds(sbidx,:);
end

%Solve Standard Statistics
[SST,stats.Rsquare,stats.AdjRsquare,stats.RMSE,SST0] = fitstats(ydata,weights,sse,dfe,ndata);
smodel = sumypred-sse;
stats.DF = [nparam dfe ndata]; %[model error uncorrected]
stats.SOS = [smodel sse sumypred];
stats.MS = [smodel/nparam sse/(ndata-nparam)];

%Parameter statistics require covariance too...
if(~isempty(stats.Cov))
    %Parameter Statistics
    SE = NaN(nparam,1); T = SE; P = SE;
    SE(~bidx) = sqrt(diag(stats.Cov));
    T(~bidx) = x(~bidx) ./ SE(~bidx);
    P(~bidx) = 2*rmathlib('pt',-abs(T(~bidx)),dfe);
    stats.Param.StdError = SE;
    stats.Param.tStat = T;
    stats.Param.pValues = P;
end

%Model F-Test & P Value
%Check for intercept (constant column in X)
Xmin = min(X,[],1); Xmax = max(X,[],1);
if(nparam > 1 && any(abs(Xmax-Xmin) <= sqrt(eps)))
    %Intercept Model
    DFR = nparam - 1;
    DFE = ndata - 1 - DFR;
    SSR = max(SST - sse,0);
    stats.Model.FStat = (SSR/DFR) / (sse / DFE);
    stats.Model.pValue = rmathlib('pf',1/stats.Model.FStat,DFE,DFR);
    stats.Model.Int = true;
else
    %Zero Model
    DFR = nparam;
    DFE = ndata - nparam;
    SSR = max(SST0 - sse,0);
    stats.Model.FStat = (SSR/DFR) / (sse / DFE);
	stats.Model.pValue = rmathlib('pf',1/stats.Model.FStat,DFE,DFR);
    stats.Model.Int = false;
end

function [sst,rsquare,adjrsquare,rmse,sst0] = fitstats(ydata,weights,sse,dfe,ndata)
%Calculate standard statistical measures

%Spread about mean
if(isempty(weights))
    ybar = mean(ydata);
    sst = sum((ydata - ybar).^2);
    sst0 = sum(ydata.^2);
else
    ybar = sum(ydata.*weights.^2)/sum(weights.^2);
    sst = sum(weights.^2.*(ydata - ybar).^2);
    sst0 = sum(weights.^2.*ydata.^2);
end
%R^2
if(sst~=0)
    rsquare = 1 - sse/sst;
    adjrsquare = 1 - (1-rsquare)*(ndata-1)/dfe; %1 - (sse/dfe)/(sst/ndata);
else
    rsquare = NaN;
    adjrsquare = NaN;
end
%RMSE
if(dfe > 0)
    rmse = sqrt(sse/dfe);
else
    rmse = NaN;
end


function seed = pm_seed(angvar,mask,pxs)
% Find a suitable (hopefully) seed point from which
% to start watershed-based unwrapping.
% FORMAT: seed = pm_seed(angvar,mask,pxs)
%
% Input:
% angvar  : Map of variance of (voxelwise) estimates
%           of phase angle.
% mask    : Tells us which part of angvar to consider.
% pxs     : Array of voxel sizes, used to ensure
%           isotropic smoothing.
%
% Output:
% seed    : Coordinates of suitable seed point.
%
% In order to find a seed point we first threshold the
% variance map at a quarter of the variance of a U(-pi,pi)
% distribution. This gives us a binary image with ones only
% for low variance regions. This is then smoothed with a
% very wide gaussian kernel (50mm). The maximum of
% the smoothed map is then pretty much a centre-of-mass
% of the "low-variance volume". It could however in
% principle be a relatively high variance voxel
% surrounded by low-variance voxels. Therefore we pick
% a percentage of the highest voxels in the smooth map
% (i.e. we pick a neighbourhood) and then pick the location
% of those that has the lowest variance in the original
% variance map.
%___________________________________________________________
% Jesper Andersson 1/10-03
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Jesper Andersson
% $Id: pm_seed.m 4842 2012-08-15 18:02:30Z guillaume $

if nargin < 3
   mask = ones(size(angvar));
end

%
% First let us create a volume where a high
% value indicates a "high density of relatively
% low variance voxels".
%
dim = size(angvar);
if length(dim) == 2 dim(3) = 1; end

M = eye(4)*diag([pxs(1) pxs(2) pxs(3) 1]);
M = M - [zeros(4,3) M*[mean(1:dim(1)) mean(1:dim(2)) mean(1:dim(3)) 0]'];

P = struct('dim',     [dim 64],...
           'pinfo',   [1 0]',...
           'mat',     M);
P.dat = double(angvar<(pi^2)/12);
svol = zeros(size(angvar));
spm_smooth(P.dat,svol,50);

%
% A high value in svol "probably" indicates a
% voxel with a low variance, surrounded by a
% lot of other voxels with low variance (i.e.
% a good place to start unwrapping from).
% However, it COULD also be a voxel with
% "not so low variance" surrounded by low
% variance voxels. To avoid that trap we pick
% the voxel with the lowest variance in the
% unsmoothed variance map, out of the 5% of
% voxels in the svol with the highest values.
% It's all very heuristic and ugly.
%

[N,X] = hist(svol(logical(mask(:))),100);
indx = find(cumsum(N)>0.95*length(find(mask)));
thres = X(indx(1)-1);

indx = find(svol(:)>thres);
[mv,mi] = min(angvar(indx));
seed = zeros(1,3);
[seed(1),seed(2),seed(3)] = ind2sub(size(angvar),indx(mi));

return


function gj_2d_ani(before,after,x,xy_ans,varargin)

params = inputParser;
params.CaseSensitive = false;
params.addParameter('record', false);
params.addParameter('record_filename','record');
params.parse(varargin{:});

%Extract values from the inputParser
h_record = params.Results.record;
record_filename = params.Results.record_filename;

if h_record
    v = VideoWriter([record_filename,'.mp4'],'MPEG-4');
    v.FrameRate = 60;
    v.Quality = 100;
    open(v);
end

n_steps = 200;

diff = after-before;
% figure;
for i_step = 1:n_steps
    tempA = before+diff/n_steps*i_step;

    y1 = (-tempA(1,1)*x+tempA(1,3))/(tempA(1,2)+eps);
    y2 = (-tempA(2,1)*x+tempA(2,3))/(tempA(2,2)+eps);

    plot(x,y1);
    hold on;
    plot(x,y2);
    plot(xy_ans(1,1), xy_ans(2,1),'r.','markersize',20)

    mArrow2(xy_ans(1,1), xy_ans(2,1), xy_ans(1,1)+tempA(1,1),xy_ans(2,1)+tempA(1,2),{'color',[0 0.4470 0.7410]});
    mArrow2(xy_ans(1,1), xy_ans(2,1), xy_ans(1,1)+tempA(2,1),xy_ans(2,1)+tempA(2,2),{'color',[0.85 0.325 0.098]});
    grid on;
    hold off;
    xlim([-5 5])
    ylim([-5 5])

    set(gcf,'color','w')

    if h_record
        F=getframe(gcf);
        writeVideo(v,F);
    end

    pause(0.01);

end

end


function padua_test05 ( )

%*****************************************************************************80
%
%% PADUA_TEST05 tests PADUA_WEIGHTS and PADUA_WEIGHTS_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 June 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PADUA_TEST05\n' );
  fprintf ( 1, '  PADUA_WEIGHTS computes the weights of a Padua rule.\n' );
  fprintf ( 1, '  PADUA_WEIGHTS_SET looks them up in a table.\n' );

  for l = 3 : 4
    n = padua_order ( l );
    w1 = padua_weights ( l );
    w2 = padua_weights_set ( l );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Level %d  Padua weights\n', l );
    fprintf ( 1, '\n' );
    diff = 0.0;
    for j = 1 : n
      diff = max ( diff, abs ( w1(j) - w2(j) ) );
      fprintf ( 1, '  %4d  %14.6g  %14.6g\n', j, w1(j), w2(j) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Maximum difference = %g\n', diff );
  end

  return
end


function graphToDot(adj, varargin)
% GRAPHTODOT Makes a GraphViz (AT&T) ile representing  an adjacency matrix
% function graphToDot(adj, ...)
% Optional arguments should be passed as name/value pairs [default]
%
% 'filename' - if omitted, writes to 'tmp.dot'
% 'arc_label' - arc_label{i,j} is a string attached to the i-j arc [""]
% 'node_label' - node_label{i} is a string attached to the node i ["i"]
% 'width'     - width in inches [10]
% 'height'    - height in inches [10]
% 'leftright' - 1 means layout left-to-right, 0 means top-to-bottom [0]
% 'directed'  - 1 means use directed arcs, 0 means undirected [1]
%
% For details on graphviz, See http://www.research.att.com/sw/tools/graphviz
%
% See also dot_to_graph and draw_dot
%
% First version written by Kevin Murphy 2002.
% Modified by Leon Peshkin, Jan 2004.

node_label = [];   arc_label = [];   % set default args
width = 10;        height = 10;
leftright = 0;     directed = 1;     filename = 'tmp.dot';

for i = 1:2:nargin-1                    % get optional args
    switch varargin{i}
        case 'filename', filename = varargin{i+1};
        case 'node_label', node_label = varargin{i+1};
        case 'arc_label', arc_label = varargin{i+1};
        case 'width', width = varargin{i+1};
        case 'height', height = varargin{i+1};
        case 'leftright', leftright = varargin{i+1};
        case 'directed', directed = varargin{i+1};
    end
end

fid = fopen(filename, 'w');
if directed
    fprintf(fid, 'digraph G {\n');
    arctxt = '->';
    if isempty(arc_label)
        labeltxt = '';
    else
        labeltxt = '[label="%s"]';
    end
else
    fprintf(fid, 'graph G {\n');
    arctxt = '--';
    if isempty(arc_label)
        labeltxt = '[dir=none]';
    else
        labeltext = '[label="%s",dir=none]';
    end
end
edgeformat = strcat(['%d ',arctxt,' %d ',labeltxt,';\n']);
fprintf(fid, 'center = 1;\n');
fprintf(fid, 'size=\"%d,%d\";\n', width, height);
if leftright
    fprintf(fid, 'rankdir=LR;\n');
end
Nnds = length(adj);
for node = 1:Nnds               %  process nodes
    if isempty(node_label)
        fprintf(fid, '%d;\n', node);
    else
        fprintf(fid, '%d [ label = "%s" ];\n', node,
node_label{node});
    end
end
for node1 = 1:Nnds   % process edges
    if directed
        arcs = find(adj(node1,:));         % children(adj, node);
    else
        arcs = find(adj(node1,node1+1:Nnds)); % remove duplicate arcs
    end
    for node2 = arcs
        fprintf(fid, edgeformat, node1, node2);
    end
end
fprintf(fid, '}');
fclose(fid);


function coeffsRet=shrinkMultiDimPoly2Fit(coeffs)
%%SHRINKMULTIDIMPOLY2FIT Given a hypermatrix of coefficients of a
%                  multivariate polynomial, many of which might be zero,
%                  return the smallest hypermatrix possible that can
%                  represent the polynomial. This function effectively gets
%                  rid of high-order terms that are all zero.
%
%INPUTS: coeffs A hypermatrix of the coefficients for the multivariate
%               polynomial. These are arranged such that
%               coeffs(a1,a2,a3...an) corresponds to the coefficient of an
%               x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1) term.  Thus, the
%               number of indices coeffs takes is equal to the
%               dimensionality of x (not counting singleton dimensions at
%               the end of coeffs). Note that this ordering is the reverse
%               that used in the 1D polyval function that is built into
%               Matlab. The number of elements for each index in coeffs is
%               the maximum order of that dimension +1.
%
%OUTPUTS: coeffsRet The same multivariate polynomial as represented by
%               coeffs, but shrunk as small as possible.
%
%As an example, consider a 2D polynomial:
%440-288*x2+60*x2^2-4*x2^3-110*x1+72*x1*x2-15*x1*x2^2+x1*x2^3
% coeffs=zeros(5,5);
% coeffs(0+1,0+1)=440;
% coeffs(0+1,1+1)=-288;
% coeffs(0+1,2+1)=60;
% coeffs(0+1,3+1)=-4;
% coeffs(1+1,0+1)=-110;
% coeffs(1+1,1+1)=72;
% coeffs(1+1,2+1)=-15;
% coeffs(1+1,3+1)=1;
% coeffsRet=shrinkMultiDimPoly2Fit(coeffs)
%One will find that coeffsRet is only 2X4 in size, versus the 5X5 for the
%original matrix. Both represent the same 2D polynomial.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

dimSizeList=size(coeffs);
numIdx=length(dimSizeList);

newDimSizeList=zeros(numIdx,1);

%Go through to see which if any of the coefficients including other terms
%is nonzero.
idxVec(1:numIdx)={':'};
for curIdx=1:numIdx
    %idxVec(1:(curIdx-1))={':'};
    %idxVec((curIdx+1):end)={':'};

    maxIdx=1;
    for i=1:dimSizeList(curIdx)
        idxVec{curIdx}=i;
        %This type of element selection is faster than loops in Matlab, but
        %actually copying the elements into els just to use the any
        %function is very inefficient.
        els=coeffs(idxVec{:});
        if(any(els(:))~=0)
            maxIdx=i;
        end

    end
    idxVec{curIdx}=':';
    newDimSizeList(curIdx)=maxIdx;
end

%Allocate space for the reduced-size array to return and make it the
%correct size.
totalNewEls=prod(newDimSizeList);
coeffsRet=zeros(totalNewEls,1);
coeffsRet=reshape(coeffsRet,newDimSizeList(:)');
%Now, we must copy all of the elements in coeffs that are being kept into
%the proper positions in coeffsRet.

%This time, we need to use a loop and perform the copy one element at a
%time. The copying is performed by going through the linear indices of the
%destination matrix, translating them into the indices for the source
%matrix, one at a time.
for curEl=1:totalNewEls
    coeffsRet(curEl)=coeffs(nDim2Index(dimSizeList,index2NDim(newDimSizeList,curEl)));
end

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 value = zsign2 ( z1, z2 )

%*****************************************************************************80
%
%% ZSIGN2 is a complex transfer-of-sign function.
%
%  Discussion:
%
%    The L2 norm is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, complex Z1, Z2, the arguments.
%
%    Output, complex VALUE,  a complex value, with the magnitude of
%    Z1, and the argument of Z2.
%
  if ( zabs2 ( z2 ) == 0.0 )
    value = 0.0;
  else
    value = zabs2 ( z1 ) * ( z2 / zabs2 ( z2 ) );
  end

  return
end


%% Copyright (C) 2016, 2019 Colin B. Macdonald
%% Copyright (C) 2016 Lagu
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym intersect (@var{A}, @var{B})
%% Return the common elements of two sets.
%%
%% Example:
%% @example
%% @group
%% A = finiteset(sym(1), 2, 3);
%% B = finiteset(sym(pi), 2);
%% intersect(A, B)
%%   @result{} ans = (sym) @{2@}
%% @end group
%% @end example
%%
%% The sets can also be intervals or a mixture of finite sets
%% and intervals:
%% @example
%% @group
%% C = interval(sym(2), 10);
%% intersect(A, C)
%%   @result{} ans = (sym) @{2, 3@}
%%
%% D = interval(0, sym(pi));
%% intersect(C, D)
%%   @result{} ans = (sym) [2, π]
%% @end group
%% @end example
%%
%% @seealso{@@sym/union, @@sym/setdiff, @@sym/setxor, @@sym/unique,
%%          @@sym/ismember, @@sym/finiteset, @@sym/interval}
%% @end defmethod

function r = intersect(a, b)

  if (nargin ~= 2)
    print_usage ();
  end

  cmd = {
         'a, b = _ins'
         'if isinstance(a, sp.Set) or isinstance(b, sp.Set):'
         '    return a & b,'
         ''
         'A = sp.FiniteSet(*(list(a) if isinstance(a, sp.MatrixBase) else [a]))'
         'B = sp.FiniteSet(*(list(b) if isinstance(b, sp.MatrixBase) else [b]))'
         'C = A & B'
         'return sp.Matrix([list(C)]),'
        };

    r = pycall_sympy__ (cmd, sym(a), sym(b));

end

%!test
%! A = sym([1 2 3]);
%! B = sym([1 2 4]);
%! C = intersect(A, B);
%! D = sym([1 2]);
%! assert (isequal (C, D))

%!test
%! % one nonsym
%! A = sym([1 2 3]);
%! B = [1 2 4];
%! C = intersect(A, B);
%! D = sym([1 2]);
%! assert (isequal (C, D))

%!test
%! % empty
%! A = sym([1 2 3]);
%! C = intersect(A, A);
%! assert (isequal (C, A))

%!test
%! % empty input
%! A = sym([1 2]);
%! C = intersect(A, []);
%! assert (isequal (C, sym([])))

%!test
%! % scalar
%! syms x
%! assert (isequal (intersect([x 1], x), x))
%! assert (isequal (intersect(x, x), x))

%!test
%! A = interval(sym(1), 3);
%! B = interval(sym(2), 5);
%! C = intersect(A, B);
%! assert( isequal( C, interval(sym(2), 3)))


function blend_test06 ( )

%*****************************************************************************80
%
%% BLEND_TEST06 checks out BLEND_IJK_1D1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2008
%
%  Author:
%
%    John Burkardt
%
  m1 = 4;
  m2 = 3;
  m3 = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLEND_TEST06\n' );
  fprintf ( 1, '  BLEND_IJK_1D1 interpolates data in a table,\n' );
  fprintf ( 1, '  from edge data.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The table is %d rows by %d columns by %d layers.\n', ...
    m1, m2, m3 );
%
%  Load data on the faces.
%
  for i = 1 : m1
    r = ( i - 1 ) / ( m1 - 1 );
    for j = 1 : m2
      s = ( j - 1 ) / ( m2 - 1 );
      for k = 1 : m3
        t = ( k - 1 ) / ( m3 - 1 );

        num_extreme = 0;
        if ( i == 1 | i == m1 )
          num_extreme = num_extreme + 1;
        end
        if ( j == 1 | j == m2 )
          num_extreme = num_extreme + 1;
        end
        if ( k == 1 | k == m3 )
          num_extreme = num_extreme + 1;
        end

        if ( 2 <= num_extreme )
          x(i,j,k) = quad_rst ( r, s, t, 1 );
        else
          x(i,j,k) = 0.0;
        end

      end
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data given to BLEND_IJK_1D1:\n' );
  fprintf ( 1, '\n' );

  for k = 1 : m3
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Layer K =  %d\n', k );
    fprintf ( 1, '\n' );
    for i = 1 : m1
      fprintf ( 1, '  %10f  %10f  %10f\n', x(i,1:m2,k) );
    end
  end

  x = blend_ijk_1d1 ( x, m1, m2, m3 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Values interpolated by BLEND_IJK_1D1:\n' );
  fprintf ( 1, '\n' );

  for k = 1 : m3
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Layer K =  %d\n', k );
    fprintf ( 1, '\n' );
    for i = 1 : m1
      fprintf ( 1, '  %10f  %10f  %10f\n', x(i,1:m2,k) );
    end
  end
%
%  Load all data.
%
  for i = 1 : m1
    r = ( i - 1 ) / ( m1 - 1 );
    for j = 1 : m2
      s = ( j - 1 ) / ( m2 - 1 );
      for k = 1 : m3
        t = ( k - 1 ) / ( m3 - 1 );
        x(i,j,k) = quad_rst ( r, s, t, 1 );
      end
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Exact data:\n' );
  fprintf ( 1, '\n' );

  for k = 1 : m3
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Layer K =  %d\n', k );
    fprintf ( 1, '\n' );
    for i = 1 : m1
      fprintf ( 1, '  %10f  %10f  %10f\n', x(i,1:m2,k) );
    end
  end

  return
end


function [h, array] = display_network(A, opt_normalize, opt_graycolor, cols, opt_colmajor)
% This function visualizes filters in matrix A. Each column of A is a
% filter. We will reshape each column into a square image and visualizes
% on each cell of the visualization panel.
% All other parameters are optional, usually you do not need to worry
% about it.
% opt_normalize: whether we need to normalize the filter so that all of
% them can have similar contrast. Default value is true.
% opt_graycolor: whether we use gray as the heat map. Default is true.
% cols: how many columns are there in the display. Default value is the
% squareroot of the number of columns in A.
% opt_colmajor: you can switch convention to row major for A. In that
% case, each row of A is a filter. Default value is false.
oldwarning = warning;
warning off all

figure();

if ~exist('opt_normalize', 'var') || isempty(opt_normalize)
    opt_normalize= true;
end

if ~exist('opt_graycolor', 'var') || isempty(opt_graycolor)
    opt_graycolor= true;
end

if ~exist('opt_colmajor', 'var') || isempty(opt_colmajor)
    opt_colmajor = false;
end

% rescale
A = A - mean(A(:));

if opt_graycolor, colormap(gray); end

% compute rows, cols
[L M]=size(A);
sz=sqrt(L);
buf=1;
if ~exist('cols', 'var')
    if floor(sqrt(M))^2 ~= M
        n=ceil(sqrt(M));
        while mod(M, n)~=0 && n<1.2*sqrt(M), n=n+1; end
        m=ceil(M/n);
    else
        n=sqrt(M);
        m=n;
    end
else
    n = cols;
    m = ceil(M/n);
end

array=-ones(buf+m*(sz+buf),buf+n*(sz+buf));

if ~opt_graycolor
    array = 0.1.* array;
end

if ~opt_colmajor
    k=1;
    for i=1:m
        for j=1:n
            if k>M,
                continue;
            end
            clim=max(abs(A(:,k)));
            if opt_normalize
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz)/clim;
            else
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz)/max(abs(A(:)));
            end
            k=k+1;
        end
    end
else
    k=1;
    for j=1:n
        for i=1:m
            if k>M,
                continue;
            end
            clim=max(abs(A(:,k)));
            if opt_normalize
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz)/clim;
            else
                array(buf+(i-1)*(sz+buf)+(1:sz),buf+(j-1)*(sz+buf)+(1:sz))=reshape(A(:,k),sz,sz);
            end
            k=k+1;
        end
    end
end

if opt_graycolor
    % h=imagesc(array,'EraseMode','none',[-1 1]);
    h=imagesc(array, [-1 1]);
else
    % h=imagesc(array,'EraseMode','none',[-1 1]);
    h=imagesc(array, [-1 1]);
end
axis image off
drawnow;

% warning on all
warning(oldwarning);


function [p,q] = bandwidth(A)
%BANDWIDTH    Upper and lower bandwidth of matrix A
%
%   A   = 0  for  i-j > p  or j-i > q
%    ij
%
%    [p,q] = bandwidth(A)
%

% written  10/16/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  [p,q] = bandwidth(A.x);


% SCRIPT TEST FOR THE KINEMATIC PROBLEM FOR THE SAWYER ROBOT
% LOAD THE SAWYER ROBOT. SIMILAR RESULTS CAN BE ACHIEVED WITH OTHER
% REDUNDANT ROBOTS

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
close all
fprintf('\nSimple test: try to reach T with the Sawyer robot')
robot = load_robot('practicals', 'SAWYER');
adjust_view(robot)

T1 = [1   0  0  0.5;
     0   1  0  0.5;
     0   0  1  0.5;
     0   0  0   1];

T2 =[0.0000    0.0000    1.0000    1.0148;
   -0.0000   -1.0000    0.0000    0.1603;
    1.0000   -0.0000   -0.0000    0.3170;
         0         0         0    1.0000];

T3 =[0.3164    0.0369    0.9479    0.9796;
   -0.2628   -0.9567    0.1250    0.2655;
    0.9115   -0.2886   -0.2931    0.1686;
         0         0         0    1.0000];

T = T1;

% try this initial seed for the inverse kinematicdifferent seeds
% do allow to obtain different solutions
q = [0.2 -0.2 -0.4 -0.4 -0.2 -0.2 -0.2]';
% q=[0.0 0.0 0.0 0.0 0.0 0.0 0.0]';

drawrobot3d(robot, q)
qinv = inversekinematic(robot, T, q)
T_reach = directkinematic(robot, qinv)

'diff T-Treach'
T-T_reach


load hall1-200;
matlabpool open;
X = normalize(XO);
[P Q] = RPMF(X, 2, 1, 1, 1e-2);
show(X, P * Q, abs(X - P * Q), [144 176]);
matlabpool close;
%%
L = P * Q;
%S = abs(X - P * Q);
S = X - P * Q;
isize = [144 176];
for i=1:min([300,size(X,2)])
    subplot(1,3,1);imshow(reshape(X(:,i),isize),[]), title('X(Sample)');
    subplot(1,3,2);imshow(reshape(L(:,i),isize),[]), title('L(Low-rank)');
    subplot(1,3,3);imshow(reshape(S(:,i),isize),[]), title('S(Sparse)');
    pause(0.01);
end


function F = coeffs2diskfun(X)
%COEFFS2DISKFUN   Convert a matrix of Chebyshev-Fourier coefficients to a
%                 diskfun.
%
%   F = coeffs2diskfun( X ) returns a diskfun object F that has a
%   Chebyshev-Fourier matrix of coefficients X.  This is useful for
%   computing quantities on the disk with the function F.
%
% See also DISKFUN/COEFFS2

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% To get the correct doubled-up grid, we can only accept
% certain coeff matrix sizes:

% Get size:
[m, n] = size( X );

% If n is odd, then make it even.
if ( mod(n, 2) == 1 )
    X = [ zeros(m, 1) X ];
end

%If m is even, we need it to be odd:
if (mod(m, 2) == 0)
    m=m+1;
    X = chebtech2.alias(X, m);
end

% Convert to values on the grid.
vals = trigtech.coeffs2vals(chebtech2.coeffs2vals(X).').';

% Assume that the function is real-valued.
vals = real( vals );

% Restrict to the region of interest.
vals = vals(floor(m/2)+1:m, :);

% Finally, make a diskfun object out of the values.
F = diskfun( vals );

end


function [FV] = mesh_shrink(FV,origin,dist),

% mesh_shrink - implode vertices of mesh by specific distance
%
% FV = mesh_shrink(FV,origin,dist)
%
% FV is a struct with fields:
%
% FV.vertices   - Nx3 matrix of Cartesian vertex coordindates (X,Y,Z)
% FV.faces      - Mx3 matrix of triangulation of FV.vertices
%
% origin        - 1x3 row vector, usually (0,0,0)
%
% dist          - how far to implode the mesh toward the origin;
%                 this distance is relative to current distance from
%                 the origin, not the total distance from the origin.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:57 $

% Licence:  GNU GPL, no implied or express warranties
% History:  10/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    xo = origin(1); yo = origin(2); zo = origin(3);

    Nvert = size(FV.vertices,1);

    fprintf('...mesh implosion...'); tic;

    for v = 1:Nvert,

        x = FV.vertices(v,1);
        y = FV.vertices(v,2);
        z = FV.vertices(v,3);

        % Find direction cosines for line from centre to vertex
        d = sqrt( (x-xo)^2 + (y-yo)^2 + (z-zo)^2 );

        l = (x-xo)/d; % cos alpha
        m = (y-yo)/d; % cos beta
        n = (z-zo)/d; % cos gamma

        % now decrease d by dist
        d = d - dist;

        % locate vertex at this new distance
        x = (l * d) + xo;
        y = (m * d) + yo;
        z = (n * d) + zo;

        FV.vertices(v,:) = [ x y z ];
    end

    t = toc; fprintf('...done (%5.2f sec)\n',t);

return


function N = delaunay_neighbors(q,P)
  % DELAUNAY_NEIGHBORS Find the Delaunay neighbors of q in a list of points P.
  %
  % N = delaunay_neighbors(q,P)
  %
  % Experimentally for uniformly randomly distributed points the number of
  % iterations is O(|P|) and specifically seems to do 1.355*|P| "iterations".
  % There's a sort on the angles so at best this implementation is O(|P|log|P|),
  % but it's very simple... so maybe the constants and overheads are much
  % smaller than doing a full Delaunay triangulation.
  %
  % Inputs:
  %   q  2D position of query point
  %   P  #P by 2 list of nearby points
  % Outputs:
  %   N  #N list of indices into P of neighboring points
  %

  % Example:
  %   q = [0 0];
  %   P = rand(60,2)*2-1;
  %   N = delaunay_neighbors(q,P);
  %   Nm = find(adjacency_matrix(delaunay([P;q]))*sparse(size(P,1)+1,1,1,size(P,1)+1,1));
  %   assert(isempty(setxor(N,Nm)));

  % 2----1
  % |θ₂ / \
  % |  /   \
  % | /     \
  % |/θq   θ₀\
  % q---------0
  %% Angle at p0-q-p1
  %thq = th(p1)-th(p0);
  %% Law of sines: θq/|01| = θ₀/|q1|
  %% θ₀ = |q1|*θq/|01|
  %th0 = l(p1)*thq/sqrt(sum((P(p0,:)-P(p1,:)).^2,2));
  % Vector from p0 to p1

  % Inputs:
  %   P  #P by 2 list of nearby points
  %   l  #P by 2 list of distances to origin: l = normrow(P)
  %   p0  first 2d point
  %   p1  second 2d point
  %   p2  third 2d point
  % Outputs:
  %   flag  whether edge from origin to p1 forms a Delaunay edge between points
  %     p1 and p2.
  %
  isdel = @(P,l,p0,p1,p2) ...
    acos((-P(p0,:)/l(p0))*(((P(p1,:)-P(p0,:))/sqrt(sum((P(p1,:)-P(p0,:)).^2,2)))')) + ...
    acos((-P(p2,:)/l(p2))*(((P(p1,:)-P(p2,:))/sqrt(sum((P(p1,:)-P(p2,:)).^2,2)))')) <= pi;

  %% Or perhaps: http://stackoverflow.com/a/8523979
  %function f = isdel(P,l,p0,p1,p2)
  %  f = acos((-P(p0,:)/l(p0))*(((P(p1,:)-P(p0,:))/sqrt(sum((P(p1,:)-P(p0,:)).^2,2)))')) + ...
  %  acos((-P(p2,:)/l(p2))*(((P(p1,:)-P(p2,:))/sqrt(sum((P(p1,:)-P(p2,:)).^2,2)))')) <= pi;
  %  % Shewchuk's incircle test via determinant
  %  h(1,:) = [P(p0,:) 1];
  %  h(2,:) = [P(p1,:) 1];
  %  h(3,:) = [P(p2,:) 1];
  %  d = det([ ...
  %    q(1) q(2) q(1)^2+q(2)^2 1 ; ...
  %    h(1,1) h(1,2) h(1,1)^2+h(1,2)^2 h(1,3) ; ...
  %    h(2,1) h(2,2) h(2,1)^2+h(2,2)^2 h(2,3) ; ...
  %    h(3,1) h(3,2) h(3,1)^2+h(3,2)^2 h(3,3) ; ...
  %    ]);
  %  if f ~= (d<0)
  %    fprintf('%d %d\n',f,d<0);
  %    pause
  %  end
  %  %if any(l([p0 p1 p2]) > 1e3)
  %  %%if all((l([p0 p1 p2])>1e3)==[0;1;0]) && f
  %  %  fprintf('%d %d %d --> %d\n',l([p0 p1 p2]) > 1e5,f);

  %  %  clf;
  %  %  hold on;
  %  %  tsurf([1 2 3;1 3 4],[0 0;P([p0 p1 p2],:)],'FaceColor','none','LineWidth',2,'EdgeColor','b');
  %  %  tsurf([1 2 4;1 4 2],[0 0;P([p0 p1 p2],:)],'FaceColor','none','LineWidth',1,'EdgeColor','r');
  %  %  scatter([0;P([p0 p1 p2],1)],[0;P([p0 p1 p2],2)],'.','SizeData',500);
  %  %  text([0;P([p0 p1 p2],1)],[0;P([p0 p1 p2],2)],['q';num2str((0:2)')],'FontSize',20);
  %  %  hold off;
  %  %  axis equal;
  %  %  axis([-3 3 -3 3]);
  %  %  drawnow;

  %  %  pause
  %  %end
  %end

  % Subtract q from P and q
  P = bsxfun(@minus,P,q);
  q = [0 0];
  % Compute distances to origin
  l = sqrt(sum(P.^2,2));

  % Handle case where q is on the convex hull.
  % TODO: Shouldn't _always_ have to added bounding points.
  max_l = max(l);
  % This is HUGE number is a hack. Should handle boundary "protectors" explicitly then.
  % Something like angle between:
  %   finite-∞-finite --> 0
  %   ∞-finite-∞ --> pi
  %   finite-finite-∞ --> ?
  % Maybe this is a case for homogenous coordinates?
  s = 1e10*max_l;
  extra = [size(P,1)+(1:4)];
  P = [P;s*[1 1;1 -1;-1 -1;-1 1]];
  l = [l;repmat(s*sqrt(2),4,1)];
  assert(size(P,1)==size(l,1));

  % Number of input points
  n = size(P,1);
  % Compute angle with x-axis
  th = atan2(P(:,2),P(:,1));
  [th,I] = sort(th);
  [~,closest] = min(l);

  % Reorder so that closest comes first
  I = I([find(I==closest):end 1:find(I==closest)-1]);
  % Assumption: closest point must be Delaunay neighbor
  N = [I(1)];
  % Consider next two points
  p1i = 2;
  p2i = 3;
  p1 = I(p1i);
  p2 = I(p2i);
  p0 = N(end);
  k = 0;
  while true
    k = k+1;
    % is the edge q-p1 between p0 and p2 is delaunay
    if isdel(P,l,p0,p1,p2)
      % push p1 onto "keepers stack" N
      N = [N;p1];
    else
      % edge q-p1 between p0 and p2 is **not** delaunay
      % first try to work backward until finding delaunay
      while numel(N)>1 && ~isdel(P,l,N(end-1),N(end),p2)
        % MOVE BACKWARD
        k = k+1;
        p0 = N(end-1);
        p1 = N(end);
        N = N(1:end-1);
      end
    end
    if p1i == n
      break;
    end
    % MOVE FORWARD
    p1i = mod(p1i,n)+1;
    p2i = mod(p2i,n)+1;
    p1 = I(p1i);
    p2 = I(p2i);
    p0 = N(end);
  end
  N = setdiff(N,size(P,1)-(0:3));
  P = P(1:end-4,:);
end


function [sys,x0,str,ts]=Plant1_fhan(t,x,u,flag)

switch flag,
    case 0,
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 1,
        sys=mdlDerivatives(t,x,u);
    case 3,
        sys=mdlOutputs(x);
    case {2,4,9},
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes
    sizes=simsizes;
    sizes.NumContStates=2;
    sizes.NumDiscStates=0;
    sizes.NumOutputs=1;
    sizes.NumInputs=1;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0];
    str=[];
    ts=[0 0];
function sys=mdlDerivatives(t,x,u)
    sys(1)=x(2);
    sys(2)=cos(0.6*t)*x(1)+cos(0.7*t)*x(2)+u;
function sys=mdlOutputs(x)
    sys=x(1);


function [X, info] = IRhtv(A, b, varargin)
% IRhtv Least squares solver with heuristic total variation penalization
%
% options  = IRhtv('defaults')
% [X,info] = IRhtv(A,b)
% [X,info] = IRhtv(A,b,K)
% [X,info] = IRhtv(A,b,options)
% [X,info] = IRhtv(A,b,K,options)
%
% This penalized restarted iteration method incorporates a heuristic TV
% penalization term (it does not produce a strict TV solution).
%
% It is assumed that x represents an n-times-n image such that N = n^2.
%
% IRhtv is a simplified driver for IRrestart, which uses an inner-outer
% iteration scheme. Semi-convergent or hybrid iterative solvers are used
% in the inner iterations, using one of the iterative methods in IRtools
% (e.g., IRhybrid_gmres). In the case of IRhtv, the TV penalization
% is updated at each outer iteration.
%
% The regularization parameter and number of inner iterations influence
% the behavior and convergence of the outer iterations.
%
% With 'defaults' as input returns the default options.  Otherwise outputs
% the iterates specified in K, using max(K) as MaxIter, and using all other
% default options.  With options as input: uses the user-specified options
% and all the other default options.
%
% Inputs:
%  A : either (a) a full or sparse matrix
%             (b) a matrix object that performs the matrix*vector operation
%             (c) user-defined function handle
%  b : right-hand side vector
%  K : (optional) integer vector that specifies which (total) iterates are
%      returned in X; the maximum number of iterations is assumed to be max(K)
%      [ positive integer | vector of positive components ]
%  options : structure with the following fields (optional)
%      x0          - initial guess for the iterations; default = zero vector
%                    [ array | {'none'} ]
%      MaxIterIn   - maximum number of inner iterations
%      MaxIterOut  - maximum number of outer iterations
%      x_true      - true solution; allows us to returns error norms with
%                    respect to x_true at each iteration
%                    [ array | {'none'} ]
%      RegParam    - a value or a method to find the regularization used in
%                    the inner iterations
%                    [non-negative scalar | {'gcv'} | 'discrep' ]
%                    This also determines which stopping rule is used for
%                    the inner iterations.
%                    If 'gcv' is chosen, the inner iteration is stopped when
%                      the GCV function minimum stabilizes or increases
%                      within a certain window of iterations (see 'stopGCV',
%                      'FlatTol' and 'MinTol').
%                     If 'discrep' is chosen, and NoiseLevel is rovided,
%                       then the discrepancy principle is used as stopping
%                       criterion (see 'NoiseLevel' and 'eta').
%      stopGCV      - stopping criterion for the inner iterations when
%                     GCV is used
%                     [ 'GCVvalues' | {'resflat'} ]
%      FlatTol      - tolerance for detecting flatness (stabilization)
%                     in the GCV function as a stopping criterion for the
%                     inner iterations
%                     [ {10^-6} | non-negative scalar ]
%      MinTol       - window of iterations - if the GCV minimum continues
%                     to increase over this window, then the inner
%                     iterations are stopped:
%                     [ {3} | positive integer ]
%      RegMatrix    - priorconditioner for the inner iterations
%                     [ {'identity'} | square nonsingular matrix |
%                     function handle ]
%      NoiseLevel   - norm of noise in rhs divided by norm of rhs
%                     (must be assigned in RegParam is 'discrep')
%                     [ {none} | nonnegative scalar ]
%      eta          - safety factor for the discrepancy principle
%                     [ {1.01} | scalar greater than (and close to) 1 ]
%      RegParam0    - first regularization parameter, used only on the
%                     very first iteration (needed if RegParam is 'discrep')
%                     [ {1} | positive scalar ]
%      stopOut      - stopping criterion for the outer iterations;
%                     [ {'xstab'} | 'Lxstab' | 'regPstab' ]
%      inSolver     - solver to be employed during the inner iterations
%                     [ {'gmres'} | 'lsqr' ]
%      adaptConstr  - approximate constraint or regularization to be
%                     incorporated
%                     [ {'tv'} | 'tvnn' ]
%      nonnegativity - may be used to also impose nonnegativity
%                      (similarly to 'tvnn')
%                      [ 'on' | {'off'} ]
%      IterBar       - shows the progress of the outer iterations
%                      [ {'on'} | 'off' ]
%      NoStopIn      - specifies whether the inner iterations should
%                      proceed after a stopping criterion has been satisfied
%                      [ 'on' | {'off'}]
%      NoStopOut     - specifies whether the outer iterations should
%                      proceed after a stopping criterion is satisfied
%                      [ 'on' | {'off'} ]
%      verbosity     - switch on or off the "verbosity" of the function
%                      [ {'on'} | 'off' ]
% Note: the options structure can be created using the function IRset.
%
% Outputs:
%   X : computed solutions, stored column-wise (at the iterations listed in K)
%   info: structure with the following fields:
%      its          - number of the last computed iteration
%      saved_iterations - iteration numbers of iterates stored in X
%      StopFlag_in  - string that describes the inner stopping condition:
%                       * Stopping criterion of the inner iterations is
%                         never satisfied
%                       * Stopping criterion is satisfied at least once
%                         during the inner iterations
%      StopFlag_out - string that describes the outer stopping condition;
%                     depending on the inputs it can be one of the following:
%                       * Outer stopping criterion is never satisfied
%                       * Diagonal weighting matrix is numerically zero
%                       * Solution stabilizes
%                       * Transformed solution stabilizes
%                       * Regularization parameter stabilizes
%      Rnrm     - relative residual norms at each iteration
%      Xnrm     - solution norms at each iteration
%      Enrm     - relative error norms (requires x_true) at each iteration
%      StopReg  - struct containing information about the solution that
%                 satisfies the stopping criterion.  Fields:
%                   It   : iteration where the stopping criterion is satisfied
%                   X    : solution satisfying the stopping criterion
%                   Enrm : the corresponding relative error (requires x_true)
%      BestReg  - struct containing information about the solution that
%                 minimizes Enrm (requires x_true). Fields:
%                   It   : iteration where the minimum is attained
%                   X    : best solution
%                   Enrm : best relative error
%      Xout     - approximate solutions at the end of each inner cycle,
%                 stored column-wise
%      itsInOut - 3-column matrix whose the columns store
%                   1. outer iteration count
%                   2. inner iteration count (i.e., for each cycle)
%                   3. total iteration count
%
% See also: IRell1, IRirn, IRrestart, IRget, IRset

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD License. A separate license file should be provided as part
% of the package.

% Set default values for options.
defaultopt = struct('x0', 'none', 'MaxIterIn', 30 , 'MaxIterOut', 20 , ...
    'RegParam', 'gcv', 'stopGCV', 'resflat', ...
    'resflatTol', 0.05, 'GCVflatTol', 10^-6, 'GCVminTol', 3,...
    'x_true', 'none', 'IterBar', 'on', 'NoStop', 'off', 'NoStopIn', 'off',...
    'NoStopOut', 'off', 'stopOut', 'xstab', 'stabOut', 1e-6, ...
    'thr0', 1e-10, 'NoiseLevel', 'none', 'eta', 1.01, 'RegParam0', 1,...
    'inSolver', 'gmres', 'adaptConstr', 'tv', 'nonnegativity', 'off', ...
    'verbosity', 'off');

% If input is 'defaults,' return the default options in X.
if nargin==1 && nargout <= 1 && isequal(A,'defaults')
    X = defaultopt;
    return;
end

% Check for acceptable number of optional input arguments.
switch length(varargin)
    case 0
        K = []; options = [];
    case 1
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = [];
        else
            K = []; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = varargin{2};
        else
            K = varargin{2}; options = varargin{1};
        end
    otherwise
        error('Too many input parameters')
end

n = length(b(:));
nosquare = 0;
test_sq = ones(n,1);
try
    test_sq = A_times_vec(A, test_sq);
    if (length(test_sq)~=n)
        nosquare = 1;
    end
catch
    nosquare = 1;
end

if isfield(options, 'inSolver') && ~isempty(options.inSolver)
    inSolver   = IRget(options, 'inSolver', [], 'fast');
    if strcmp(inSolver, 'gmres')
        if nosquare
            warning(['The matrix A is rectangular, and a solver like hybrid gmres cannot handle it. ',...
                'The solver is changed to hybrid lsqr.'])
            options.inSolver = 'lsqr';
        end
    end
end

if isempty(options)
    options = defaultopt;
end

options = IRset(defaultopt, options);
inSolver   = IRget(options, 'inSolver', [], 'fast');

if nosquare && strcmp(inSolver, 'gmres')
    options.inSolver = 'lsqr';
end

nn = IRget(options, 'nonnegativity',  [], 'fast');

if strcmp(nn, 'on')
    options.adaptConstr = 'tvnn';
end

% Call IRrestart with the specified options.
options = rmfield(options, 'nonnegativity');
[X, info] = IRrestart(A, b, K, options);


% [PYR, INDICES, STEERMTX, HARMONICS] = buildSpyr(IM, HEIGHT, FILTFILE, EDGES)
%
% Construct a steerable pyramid on matrix IM.  Convolutions are
% done with spatial filters.
%
% HEIGHT (optional) specifies the number of pyramid levels to build. Default
% is maxPyrHt(size(IM),size(FILT)).
% You can also specify 'auto' to use this value.
%
% FILTFILE (optional) should be a string referring to an m-file that
% returns the rfilters.  (examples: 'sp0Filters', 'sp1Filters',
% 'sp3Filters','sp5Filters'.  default = 'sp1Filters'). EDGES specifies
% edge-handling, and defaults to 'reflect1' (see corrDn).
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.  This is compatible with the MatLab Wavelet toolbox.
% See the function STEER for a description of STEERMTX and HARMONICS.

% Eero Simoncelli, 6/96.
% See http://www.cis.upenn.edu/~eero/steerpyr.html for more
% information about the Steerable Pyramid image decomposition.

function [pyr,pind,steermtx,harmonics] = buildSpyr(im, ht, filtfile, edges)

%-----------------------------------------------------------------
%% DEFAULTS:

if (exist('filtfile') ~= 1)
  filtfile = 'sp1Filters';
end

if (exist('edges') ~= 1)
  edges= 'reflect1';
end

if (isstr(filtfile) & (exist(filtfile) == 2))
   [lo0filt,hi0filt,lofilt,bfilts,steermtx,harmonics] = eval(filtfile);
else
  fprintf(1,'\nUse buildSFpyr for pyramids with arbitrary numbers of orientation bands.\n');
  error('FILTFILE argument must be the name of an M-file containing SPYR filters.');
end

max_ht = maxPyrHt(size(im), size(lofilt,1));
if ( (exist('ht') ~= 1) | (ht == 'auto') )
  ht = max_ht;
else
  if (ht > max_ht)
    error(sprintf('Cannot build pyramid higher than %d levels.',max_ht));
  end
end

%-----------------------------------------------------------------

hi0 = corrDn(im, hi0filt, edges);
lo0 = corrDn(im, lo0filt, edges);

[pyr,pind] = buildSpyrLevs(lo0, ht, lofilt, bfilts, edges);

pyr = [hi0(:) ; pyr];
pind = [size(hi0); pind];


function nu = transport( X, Y, xi )
%TRANSPORT Calculates vector transport
%   NU = TRANSPORT( X, Y, XI ) calculates the vector transport from
%
%         T_X --> X_Y
%
%   where X and Y are given in Tucker format (ttensor) and applies it
%   to the tangent vector XI. XI is decomposed into
%     XI = Y_tilde x U1 x U2 x U3
%          + S x U1_tilde x U2 x U3
%          + S x U1 x U2_tilde x U3
%          + S x U1 x U2 x U3_tilde
%
%   where only Y_tilde, U1_tilde, U2_tilde and U3_tilde are stored
%   as a struct in factorized form.
%
%   See also calcProjection, addFactorized
%

%   GeomCG Tensor Completion. Copyright 2013 by
%   Michael Steinlechner
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

    V1 = Y.U{1}'*X.U{1};
    V2 = Y.U{2}'*X.U{2};
    V3 = Y.U{3}'*X.U{3};

    V1_tilde = Y.U{1}'*xi.U1_tilde;
    V2_tilde = Y.U{2}'*xi.U2_tilde;
    V3_tilde = Y.U{3}'*xi.U3_tilde;

    M1 = ttm( xi.Y_tilde, {V1, V2, V3} );
    M2 = ttm( X.core, {V1_tilde, V2, V3 } );
    M3 = ttm( X.core, {V1, V2_tilde, V3 } );
    M4 = ttm( X.core, {V1, V2, V3_tilde } );

    %first part:
    nu.Y_tilde = M1 + M2 + M3 + M4;

    %second part;

    Y1 = ttm(xi.Y_tilde, {X.U{1}, V2, V3} );
    Y2 = ttm(xi.Y_tilde, {V1, X.U{2}, V3} );
    Y3 = ttm(xi.Y_tilde, {V1, V2, X.U{3}} );

    G1_1 = ttm( X.core, {xi.U1_tilde,   V2,         V3} );
    G1_2 = ttm( X.core, {X.U{1},        V2_tilde,   V3} );
    G1_3 = ttm( X.core, {X.U{1},        V2,         V3_tilde} );

    G2_1 = ttm( X.core, {V1_tilde,  X.U{2},         V3} );
    G2_2 = ttm( X.core, {V1,        xi.U2_tilde,    V3} );
    G2_3 = ttm( X.core, {V1,        X.U{2},         V3_tilde} );

    G3_1 = ttm( X.core, {V1_tilde,  V2,         X.U{3}} );
    G3_2 = ttm( X.core, {V1,        V2_tilde,   X.U{3}} );
    G3_3 = ttm( X.core, {V1,        V2,         xi.U3_tilde} );

    S1_inv = pinv( double( tenmat( Y.core, 1 ) ));
    S2_inv = pinv( double( tenmat( Y.core, 2 ) ));
    S3_inv = pinv( double( tenmat( Y.core, 3 ) ));

    U1_tilde = double( tenmat(Y1 + G1_1 + G1_2 + G1_3, 1) ) * S1_inv;
    U2_tilde = double( tenmat(Y2 + G2_1 + G2_2 + G2_3, 2) ) * S2_inv;
    U3_tilde = double( tenmat(Y3 + G3_1 + G3_2 + G3_3, 3) ) * S3_inv;

    nu.U1_tilde = U1_tilde - Y.U{1} * ( Y.U{1}' * U1_tilde );
    nu.U2_tilde = U2_tilde - Y.U{2} * ( Y.U{2}' * U2_tilde );
    nu.U3_tilde = U3_tilde - Y.U{3} * ( Y.U{3}' * U3_tilde );

end


% EX_STOKES_DRIVEN_CAVITY_TH: solve the Stokes problem in the driven cavity with generalized Taylor-Hood elements.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined from the aspect ratio using the NURBS toolbox
aspect_ratio=[1.5 0.3];
nrb_section = nrb4surf([0 0], [1 0], [0 aspect_ratio(1)], [1 aspect_ratio(1)]);
problem_data.geo_name = nrbextrude (nrb_section, [0 0 aspect_ratio(2)]);

% Type of boundary conditions for each side of the domain
problem_data.drchlt_sides = 1:6;
problem_data.nmnn_sides = [];

% Physical parameters
problem_data.viscosity = @(x, y, z) ones (size (x));

% Force and boundary terms
problem_data.f  = @(x, y, z) zeros ([3, size(x)]);
problem_data.h  = @test_stokes_3d_symdrivcav_h_drchlt;

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.element_name = 'sg';        % Element type for discretization
method_data.degree       = [2  2  2];  % Degree of the splines (pressure space)
method_data.regularity   = [1  1  1];  % Regularity of the splines (pressure space)
method_data.nsub         = [2  2  2];  % Number of subdivisions
method_data.nquad        = [4  4  4];  % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = ...
                       solve_stokes (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW
output_file = 'Driven_cavity_3d_SG_Deg2_Reg1_Sub2';

fprintf ('The result is saved in the files %s \n and %s \n \n', ...
           [output_file '_vel'], [output_file '_press']);
vtk_pts = {linspace(0, 1, 15), linspace(0, 1, 15), linspace(0, 1, 15)};
sp_to_vtk (press, space_p, geometry, vtk_pts, [output_file '_press'], 'press')
sp_to_vtk (vel,   space_v, geometry, vtk_pts, [output_file '_vel'  ], {'velocity', 'divergence'}, {'value', 'divergence'})

%!test
%! aspect_ratio=[1.5 0.3];
%! nrb_section = nrb4surf([0 0], [1 0], [0 aspect_ratio(1)], [1 aspect_ratio(1)]);
%! problem_data.geo_name = nrbextrude (nrb_section, [0 0 aspect_ratio(2)]);
%! problem_data.drchlt_sides = 1:6;
%! problem_data.nmnn_sides = [];
%! problem_data.viscosity = @(x, y, z) ones (size (x));
%! problem_data.f  = @(x, y, z) zeros ([3, size(x)]);
%! problem_data.h  = @test_stokes_3d_symdrivcav_h_drchlt;
%! method_data.element_name = 'sg';        % Element type for discretization
%! method_data.degree       = [2  2  2];  % Degree of the splines (pressure space)
%! method_data.regularity   = [1  1  1];  % Regularity of the splines (pressure space)
%! method_data.nsub         = [2  2  2];  % Number of subdivisions
%! method_data.nquad        = [4  4  4];  % Points for the Gaussian quadrature rule
%! [geometry, msh, space_v, vel, space_p, press] = ...
%!                        solve_stokes (problem_data, method_data);
%! assert (msh.nel, 64)
%! assert (space_p.ndof, 64)
%! assert (space_v.ndof, 1029)


function [grid1, grid2, freq1, freq2, biv, cond2on1, cond1on2, cexp1, cexp2] = bivkernrest(data1,data2,res1,res2,observed)
%[grid1, grid2, freq1, freq2, biv, cond2on1, cond1on2, cexp1, cexp2] = bivkern(data1,data2,res1,res2)
%data1,2 = data for which a kernel density needs to be derived
%res1,2 = step size
%observed = vector of dummies that identifies which elements of data1,2 are
%observations (1) rather than restrictions (0)
%
%restrictions are added by extending the data vectors with
%data1(i) = y for data2(i) = x, observed(i) = 0
%for example, data1(17) = 0, data2(17) = 0, observed(17) = 0
%
%Richard Tol, 13 September 2013

tic

ampl = 0.1; %bandwidth of the restriction
range = 4;  %data are evaluated between minimum minus range*standard deviation and maximum plus range*standard deviation

data1std = std(data1(observed==1));
data1min = res1*round((min(data1(observed==1)) - range*data1std)/res1); %minimum at observed minus range*standard deviation
data1min = max(0,data1min);
data1max = res1*((max(data1(observed==1)) + range*data1std)/res1); %maximum at observed plus range*standard deviation
nopnt = length(data1);
nocon = nopnt - sum(observed);
noobs = nopnt - nocon;

data2std = std(data2(observed==1));
data2min = res2*round((min(data2(observed==1)) - range*data2std)/res2); %minimum at observed minus range*standard deviation
%data2min = max(0,data2min);
data2max = res2*round((max(data2(observed==1)) + range*data2std)/res2); %maximum at observed plus range*standard deviation

%make grid
nogrid1 = round((data1max-data1min)/res1);
grid1 = zeros(nogrid1,1);
grid1(1) = data1min;
for i=2:nogrid1
    grid1(i) = grid1(i-1) + res1;
end

nogrid2 = round((data2max-data2min)/res2);
grid2 = zeros(nogrid2,1);
grid2(1) = data2min;
for i=2:nogrid2
    grid2(i) = grid2(i-1) + res2;
end

%bandwith
sigma = cov(data1(observed==1), data2(observed==1))*1.06^2*noobs^-0.4;
h = inv(sigma);
h1 = 1.06 * noobs^-0.2 * std(data1); %for univariate
h2 = 1.06 * noobs^-0.2 * std(data2); %for univariate

%normal kernel
biv = zeros(nogrid1,nogrid2);
vbiv = biv;
vfreq1 = ones(nogrid1,noobs);
vfreq2 = ones(nogrid2,noobs);
x= zeros(2,1);
for i=1:nopnt,
    %disp(i);
    vfreq1(:,i) = exp(-0.5*((grid1-data1(i))/(h1*observed(i)+ampl*h1*(1-observed(i)))).^2)/(h1*observed(i)+ampl*h1*(1-observed(i)))/sqrt(2*pi);
    vfreq2(:,i) = exp(-0.5*((grid2-data2(i))/(h2*observed(i)+ampl*h2*(1-observed(i)))).^2)/(h2*observed(i)+ampl*h2*(1-observed(i)))/sqrt(2*pi);
    dev1 = grid1-data1(i);
    dev2 = grid2-data2(i);
    for k=1:nogrid1,
        for l=1:nogrid2,
            x1 = dev1(k);
            x2 = dev2(l);
            vbiv(k,l) = exp(-0.5*((h(1,1)*observed(i)+h(1,1)/ampl*(1-observed(i)))*x1^2 + (h(2,2)*observed(i)+h(2,2)/ampl*(1-observed(i)))*x2^2 + (h(1,2)*observed(i)+h(1,2)/ampl*(1-observed(i)))*x1*x2));
        end
    end
    biv = biv + vbiv/sum(sum(vbiv));
end
biv = biv/sum(sum(biv));
freq1 = sum(vfreq1,2);
freq1 = freq1/sum(freq1);
freq2 = sum(vfreq2,2);
freq2 = freq2/sum(freq2);

cond2on1=biv./repmat(sum(biv,2),1,nogrid2);
cond1on2=biv./repmat(sum(biv,1),nogrid1,1);
cond1on2=cond1on2';

cexp1 = sum(cond1on2.*repmat(grid1,1,nogrid2)',2);
cexp2 = sum(cond2on1.*repmat(grid2',nogrid1,1),2);

toc

end


function result = isintersect_3dof(SHAPE, LINE)
% isintersect(SHAPE, LINE)
% this function check whether we intersect the shape or not
% SHAPE could be polygon or line
% LINE is always line
%

vert_num = size(SHAPE(:,1),1);
result = 0;

m = (LINE(2, 2) - LINE(1, 2)) /  (LINE(2, 1) - LINE(1, 1));
b =  LINE(1, 2) - m * LINE(1, 1);
radius = 0.1;

for k = 1:vert_num-1
    % y = m * x + b

    m_obs = (SHAPE(k, 2) - SHAPE(k+1, 2)) / (SHAPE(k, 1) - SHAPE(k+1, 1));
    b_obs = SHAPE(k, 2) - m_obs * SHAPE(k, 1);

    % consider this lines ???

    if (m_obs + radius > m) && (m_obs - radius < m) && (b_obs + radius > b) && (b_obs - radius < b)
        result = 1;
        return;
    end

    % ???

    x_intersection = (b - b_obs)/(m_obs - m);

    x_max = max (SHAPE(k, 1),SHAPE(k+1, 1));
    x_min = min (SHAPE(k, 1), SHAPE(k+1, 1));

    y_max = max (SHAPE(k, 2), SHAPE(k+1, 2));
    y_min = min (SHAPE(k, 2), SHAPE(k+1, 2));

    x_edge_max = max(LINE(1, 1), LINE(2, 1));
    x_edge_min = min(LINE(1, 1), LINE(2, 1));

    y_edge_max = max(LINE(1, 2), LINE(2, 2));
    y_edge_min = min(LINE(1, 2), LINE(2, 2));

    y_intersection = m_obs * x_intersection + b_obs;

    if ((x_intersection >= (x_min-radius) && x_intersection <= (x_max + radius)) ...
            && (x_intersection >= (x_edge_min - radius) && x_intersection <= (x_edge_max+radius)) ...
            && y_intersection<= y_edge_max + radius && y_intersection >= y_edge_min - radius ...
            && y_intersection<= y_max+radius && y_intersection >= y_min - radius)
        result = 1;
        return;
    end
end


function  [ p_xy, p_type ] = state_initialize ( nb, ni, np, r );

%*****************************************************************************80
%
%% STATE_INITIALIZE initializes the state.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NB, the number of boundary points.
%
%    Input, integer NI, the number of interior points.
%
%    Input, integer NP, the total number of points.
%
%    Input, real R, the radius of the circle.
%
%    Output, real P_XY(2,NP), the coordinates of the points.
%
%    Output, integter P_TYPE(1,NP), the type of each point.
%    1, the point is constrained to the boundary.
%    2, the point is constrained to the interior.
%
  if ( 0 )
    p_xy = disk_sample_uniform ( np, r );
  else
    p_xy = disk_sample_nonuniform ( np, r );
  end

  p_type = zeros ( 1, np );
  p_type(1,1:nb)    = 1;
  p_type(1,nb+1:np) = 2;

  p_norm(1,1:np) = sqrt ( sum ( p_xy.^2, 1 ) );

  p_xy(1,1:nb) = r * p_xy(1,1:nb) ./ p_norm(1:nb);
  p_xy(2,1:nb) = r * p_xy(2,1:nb) ./ p_norm(1:nb);

  return
end


function mono_between_random_test ( )

%*****************************************************************************80
%
%% MONO_BETWEEN_RANDOM_TEST tests MONO_BETWEEN_RANDOM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONO_BETWEEN_RANDOM_TEST\n' );
  fprintf ( 1, '  MONO_BETWEEN_RANDOM selects at random a monomial\n' );
  fprintf ( 1, '  in M dimensions of total degree between N1 and N2.\n' );

  m = 3;
  n1 = 2;
  n2 = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Let M =  %d\n', m );
  fprintf ( 1, '      N1 = %d\n', n1 );
  fprintf ( 1, '      N2 = %d\n', n2 );
  fprintf ( 1, '\n' );

  seed = 123456789;
  test_num = 5;

  for test = 1 : test_num

    [ x, rank, seed ] = mono_between_random ( m, n1, n2, seed );

    fprintf ( 1, '  %3d:', rank );
    for j = 1 : m
      fprintf ( 1, '  %1d', x(j) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


function seed = latin_edge_test01 ( seed )

%*****************************************************************************80
%
%% LATIN_EDGE_TEST01 tests LATIN_EDGE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEED, an initial seed for the random number generator.
%
%    Output, integer SEED, the updated random number seed.
%
  dim_num = 2;
  point_num = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LATIN_EDGE_TEST01\n' );
  fprintf ( 1, '  LATIN_EDGE chooses a Latin cell arrangement,\n' );
  fprintf ( 1, '  which includes the edge points.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', dim_num );
  fprintf ( 1, '  Number of points =  %d\n', point_num );
  fprintf ( 1, '  Using seed = %d\n', seed );

  [ x, seed ] = latin_edge ( dim_num, point_num, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Latin Edge Square points:\n' );
  fprintf ( 1, '\n' );

  for j = 1: point_num
    for i = 1: dim_num
      fprintf ( 1, '%10f  ', x(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% Discrete Cepstral Envelope (DCE)
%
% Input
%  af            : Array of structures with matrices containing sinusoidal
%                  parameters (as the output of sin_analysis.m).
%                  Each matrix is made of:
%                   The 1st line for the frequency of each sinusoid [Hz]
%                   The 2nd line for their amplitude (linear scale)
%                  The DC has to be included (in the first column)
%  fs            : [Hz] Signal's sampling frequency
%  order         : Cepstral order
%  [extrap_dcny] : If true (default), alleviate stability problems of the DCE
%                     by replacing the DCE value and extrapolating sinusoidal
%                     components up to Nyquist.
%                  If false, use the sinusoidal components as they are.
%  [scale]       : empty  : Frequency linear scale (default)
%                 'mel'  : see frq2mel (for MFCC computation)
%                 'bark' : see frq2bark
%                 'erb'  : see frq2erb
%  [Bw]          : [Hz] Standard-deviation of the Gaussian used as weighting
%                       function (for emphasizing the importance of the low
%                       frequencies in the solution).
%                       Bw As to be big enough to have the weights still
%                       significant up to Nyquist.
%  [lr]          : Regularization parameter (as in [2]) (def. 0)
%  [dftlen]      : DFT's length, if the 4th output argument is requested.
%
% Output
%  cc             : Cepstral coefficients
%  E              : The amplitude cepstral envelope
%
% References
%  [1] T. Galas and X. Rodet, "Generalized discrete cepstral analysis for
%      deconvolution of source-filter system with discrete spectra,"
%      in IEEE Applications of Signal Processing to Audio and Acoustics (ASSP)
%      Workshop, pp. 71-72, 1991.
%  [2] M. Campedel-Oudot, O. Cappe and E. Moulines, "Estimation of the Spectral
%      Envelope of Voiced Sounds Using a Penalized Likelihood Approach", IEEE
%      Transactions on Speech and Audio Processing, vol. 9, no. 5, pp. 469-481,
%      2001.
%
% Copyright (c) 2011 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the COVAREP project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function [cc E] = env_dce_sfa(af, fs, order, extrap_dcny, scale, Bw, lr, dftlen)

    % Input parameters
    if nargin<4; extrap_dcny=true; end
    if nargin<5; scale = []; end
    if nargin<6 || isempty(Bw); Bw = 2000*(fs/16000); end % To make it as
                                           % similar as possible to the DCE-MFA
    if nargin<7 || isempty(lr); lr = 0.035; end % [2]
    if nargin<8; dftlen=4096; end

    if ~isempty(scale)
        Bw = 100*fs;
        lr = 0.035; % To make it as similar as possible to [2]
        eval(['fnscale=@frq2' scale ';']);
        af.f = 0.5*fs*fnscale(af(n).f)/fnscale(fs/2);
    end

    % If asked, extrapolate components at DC and up to Nyquist
    if extrap_dcny; af = env_extrap_sins_dcny(af, fs); end

    af.f = af.sins(1,:);
    af.a = log(af.sins(2,:));

    % Prepare the weighting functions
    fk = af.f(:);
    Nk = length(fk);
    wk = exp(-fk.^2 / (2 * Bw * Bw))';
    wk = wk./sum(wk);
    Wk = diag(wk);

    % Compute the DCE envelope
    fk = af.f(:);
    ak = af.a(:);
    Nk = length(fk);
    Bk = [ones(Nk,1) cos(2*pi*fk/fs*(1:order))];

    BWB = (Bk'*Wk*Bk);
    rt =  (Bk'*Wk*(ak));

    if lr>0
        cc = (BWB+lr*diag(ones(size(BWB,1),1)))\rt;
    else
        cc = BWB\rt;
    end

    % If asked, compute the envelope
    if nargout>1
        if isempty(scale)
            E = fft(cc, dftlen);
            E = exp(E(1:end/2+1));
        else
            if strcmp(scale,'bark')
                E = barkcc2spec(cc, fs, dftlen);
                E = E(1:end/2+1);
            elseif strcmp(scale,'mel')
                E = exp(fft(cc, dftlen));
                E = E(1:dftlen/2+1);
                E = cc2hspec(cc, fs, dftlen);
                E = fwcep2hspec(cc, fs, dftlen);
            end
        end
    end

    % Plot final solution
    if 0
        hold off;

        global S Erefg selfr;
        if ~isempty(S);
            Sbins = fs*(0:dftlen/2)/dftlen;
            plot(Sbins, mag2db(abs(S)), ':k');
            hold on;
        else; hold off; end
        if ~isempty(Erefg);
            Erefgbins = 0.5*fs*(0:length(Erefg)-1)/length(Erefg);
            plot(Erefgbins, mag2db(abs(Erefg)), 'k');
            hold on;
        end

        plot(af.f, mag2db(exp(af.a)),'xr');
        hold on;
        F = fs*(0:dftlen/2)/dftlen;
%          plot(F, mag2db(abs(L)), 'g');
        hold on;
        plot(F, mag2db(abs(E)), 'b', 'LineWidth', 2);
        keyboard
    end

return


function offset = find_offset(sig, sig_fs, ref_signal, ref_signal_fs)
    % Find the lag between two signals using cross-correlation
    sig_us = resample(sig, ref_signal_fs, sig_fs);
    sig_us = normalization(sig_us);
    [c, lags] = xcorr(normalization(ref_signal), sig_us);
    [~, ind] = max(c);
    offset = lags(ind) + 1; % add 1 for Matlab indexing
end


function [y,deriv] = exp_mv2df(w)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
% y = exp(w), vectorized as MATLAB usually does.

if nargin==0
    test_this();
    return;
end

if isempty(w)
    y = @(w)exp_mv2df(w);
    return;
end

if isa(w,'function_handle')
    outer = exp_mv2df([]);
    y = compose_mv(outer,w,[]);
    return;
end

w = w(:);
y = exp(w);
deriv = @(dy) deriv_this(dy,y);

function [g,hess,linear] = deriv_this(dy,y)

linear  = false;
g = dy.*y;
hess = @(d) hess_this(d,dy,y);

function [h,Jv] = hess_this(d,dy,y)

h = dy.*y.*d;
if nargout>1
    Jv = d.*y;
end

function test_this()
f = exp_mv2df([]);
test_MV2DF(f,randn(3,1));


function Y = ttm(X,V,varargin)
%TTM Tensor times matrix.
%
%   Y = TTM(X,A,N) computes the n-mode product of tensor X with a
%   matrix A; i.e., X x_N A.  The integer N specifies the dimension
%   (or mode) of X along which A should be multiplied.  If size(A) =
%   [J,I], then X must have size(X,N) = I.  The result will be the
%   same order and size as X except that size(Y,N) = J.
%
%   Y = TTM(X,{A,B,C,...}) computes the n-mode product of tensor X
%   with a sequence of matrices in the cell array.  The n-mode
%   products are computed sequentially along all dimensions (or modes)
%   of X. The cell array contains ndims(X) matrices.
%
%   Y = TTM(X,{A,B,C,...},DIMS) computes the sequence tensor-matrix
%   products along the dimensions specified by DIMS.
%
%   Y = TTM(...,'t') performs the same computations as above except
%   the matrices are transposed.
%
%   Examples
%   X = tensor(rand(5,3,4,2));
%   A = rand(4,5); B = rand(4,3); C = rand(3,4); D = rand(3,2);
%   Y = ttm(X, A, 1)         %<-- computes X times A in mode-1
%   Y = ttm(X, {A,B,C,D}, 1) %<-- same as above
%   Y = ttm(X, A', 1, 't')   %<-- same as above
%   Y = ttm(X, {A,B,C,D}, [1 2 3 4]) %<-- 4-way multiply
%   Y = ttm(X, {D,C,B,A}, [4 3 2 1]) %<-- same as above
%   Y = ttm(X, {A,B,C,D})            %<-- same as above
%   Y = ttm(X, {A',B',C',D'}, 't')   %<-- same as above
%   Y = ttm(X, {C,D}, [3 4])     %<-- X times C in mode-3 & D in mode-4
%   Y = ttm(X, {A,B,C,D}, [3 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, [1 2 4])   %<-- 3-way multiply
%   Y = ttm(X, {A,B,C,D}, [1 2 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, -3)        %<-- same as above
%   Y = ttm(X, {A,B,C,D}, -3)      %<-- same as above
%
%   See also TENSOR, TENSOR/TTT, TENSOR/TTV.
%
%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

%% Check the number of arguments
if (nargin < 2)
    error('TTM requires at least two arguments.');
end

%% Create 'n' and 'tflag' arguments from varargin
n = 1:ndims(X);
tflag = '';
if numel(varargin) == 1
    if ischar(varargin{1})
        tflag = varargin{1};
    else
        n = varargin{1};
    end
elseif numel(varargin) == 2
    n = varargin{1};
    tflag = varargin{2};
end

%% Handle cell array
if iscell(V)
    % Copy n into dims
    dims = n;
    % Check that the dimensions are valid
    [dims,vidx] = tt_dimscheck(dims,ndims(X),numel(V));
    % Calculate individual products
    Y = ttm(X, V{vidx(1)}, dims(1), tflag);
    for i = 2 : numel(dims)
        Y = ttm(Y, V{vidx(i)}, dims(i), tflag);
    end
    % All done
    return;
end

%% Check the second argument
if ndims(V) ~= 2
    error('tensor/ttm: 2nd argument must be a matrix.');
end

%% Check n
if (numel(n) ~= 1 || (n < 0) || n > ndims(X))
    error('Dimension N must be between 1 and NDIMS(X).');
end

%% COMPUTE THE PRODUCT

N = ndims(X);
sz = size(X);
order = [n,1:n-1,n+1:N];
newdata = double(permute(X,order));
newdata = reshape(newdata,sz(n),prod(sz([1:n-1,n+1:N])));
if tflag == 't'
    newdata = V'*newdata;
    p = size(V,2);
else
    newdata = V*newdata;
    p = size(V,1);
end
newsz = [p,sz(1:n-1),sz(n+1:N)];
Y = tensor(newdata,newsz);
Y = ipermute(Y,order);

return;

end


function mean = uniform_01_mean ( )

%*****************************************************************************80
%
%% UNIFORM_01_MEAN returns the mean of the Uniform 01 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real MEAN, the mean of the discrete uniform PDF.
%
  mean = 0.5;

  return
end


function Y = ttm(X,V,varargin)
%TTM Tensor times matrix.
%
%   Y = TTM(X,A,N) computes the n-mode product of tensor X with a
%   matrix A; i.e., X x_N A.  The integer N specifies the dimension
%   (or mode) of X along which A should be multiplied.  If size(A) =
%   [J,I], then X must have size(X,N) = I.  The result will be the
%   same order and size as X except that size(Y,N) = J.
%
%   Y = TTM(X,{A,B,C,...}) computes the n-mode product of tensor X
%   with a sequence of matrices in the cell array.  The n-mode
%   products are computed sequentially along all dimensions (or modes)
%   of X. The cell array contains ndims(X) matrices.
%
%   Y = TTM(X,{A,B,C,...},DIMS) computes the sequence tensor-matrix
%   products along the dimensions specified by DIMS.
%
%   Y = TTM(...,'t') performs the same computations as above except
%   the matrices are transposed.
%
%   Examples
%   X = tensor(rand(5,3,4,2));
%   A = rand(4,5); B = rand(4,3); C = rand(3,4); D = rand(3,2);
%   Y = ttm(X, A, 1)         %<-- computes X times A in mode-1
%   Y = ttm(X, {A,B,C,D}, 1) %<-- same as above
%   Y = ttm(X, A', 1, 't')   %<-- same as above
%   Y = ttm(X, {A,B,C,D}, [1 2 3 4]) %<-- 4-way multiply
%   Y = ttm(X, {D,C,B,A}, [4 3 2 1]) %<-- same as above
%   Y = ttm(X, {A,B,C,D})            %<-- same as above
%   Y = ttm(X, {A',B',C',D'}, 't')   %<-- same as above
%   Y = ttm(X, {C,D}, [3 4])     %<-- X times C in mode-3 & D in mode-4
%   Y = ttm(X, {A,B,C,D}, [3 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, [1 2 4])   %<-- 3-way multiply
%   Y = ttm(X, {A,B,C,D}, [1 2 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, -3)        %<-- same as above
%   Y = ttm(X, {A,B,C,D}, -3)      %<-- same as above
%
%   See also TENSOR, TENSOR/TTT, TENSOR/TTV.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) 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

%% Check the number of arguments
if (nargin < 2)
    error('TTM requires at least two arguments.');
end

%% Create 'n' and 'tflag' arguments from varargin
n = 1:ndims(X);
tflag = '';
if numel(varargin) == 1
    if ischar(varargin{1})
        tflag = varargin{1};
    else
        n = varargin{1};
    end
elseif numel(varargin) == 2
    n = varargin{1};
    tflag = varargin{2};
end

%% Handle cell array
if iscell(V)
    % Copy n into dims
    dims = n;
    % Check that the dimensions are valid
    [dims,vidx] = tt_dimscheck(dims,ndims(X),numel(V));
    % Calculate individual products
    Y = ttm(X, V{vidx(1)}, dims(1), tflag);
    for i = 2 : numel(dims)
        Y = ttm(Y, V{vidx(i)}, dims(i), tflag);
    end
    % All done
    return;
end

%% Check the second argument
if ndims(V) ~= 2
    error('tensor/ttm: 2nd argument must be a matrix.');
end

%% Check n
if (numel(n) ~= 1 || (n < 0) || n > ndims(X))
    error('Dimension N must be between 1 and NDIMS(X).');
end

%% COMPUTE THE PRODUCT

N = ndims(X);
sz = size(X);
order = [n,1:n-1,n+1:N];
newdata = double(permute(X,order));
newdata = reshape(newdata,sz(n),prod(sz([1:n-1,n+1:N])));
if tflag == 't'
    newdata = V'*newdata;
    p = size(V,2);
else
    newdata = V*newdata;
    p = size(V,1);
end
newsz = [p,sz(1:n-1),sz(n+1:N)];
Y = tensor(newdata,newsz);
Y = ipermute(Y,order);

return;

end


function [flowx,flowy] = specific_wind(x,y,nel)
%circular_wind   Reference problem 3.4 convective wind
%   [flowx,flowy] = specific_wind(x,y,nel);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          nel        number of elements
%
%   specifies circular wind /Morton pp.10/
%   IFISS function: DJS; 5 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      flowx =  2*y.*(1-x.*x);
      flowy = -2*x.*(1-y.*y);
      return


function [ i, j ] = r8mat_min_index ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_MIN_INDEX returns the location of the minimum entry of an R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the M by N matrix.
%
%    Output, integer I, J, the indices of the minimum entry of A.
%
  i = -1;
  j = -1;

  for jj = 1 : n
    for ii = 1 : m
      if ( ii == 1 && jj == 1 )
        i = ii;
        j = jj;
      elseif ( a(ii,jj) < a(i,j) )
        i = ii;
        j = jj;
      end
    end
  end

  return
end


function [YYact,XXact] = YXB_(YY,lags,constant_timetrend)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'YXB_' organizes the data in the form
% of Y = XB+E
% NO dummy observations
% constant and trends are at the end

% Filippo Ferroni, 6/1/2015
% Revised, 2/15/2017
% Revised, 3/21/2018
% Revised, 9/11/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<3
    constant = 1;
    timetrend = 0;
else
    constant  = constant_timetrend(1);
    timetrend = constant_timetrend(2);
end

nlags_   = lags;                 % number of lags   */
T0       = lags;                 % size of pre-sample */

nv      = size(YY,2);            %* number of variables */
nobs    = size(YY,1)-T0;         %* number of observations */

% Actual observations

YYact = YY(T0+1:T0+nobs,:);
XXact = zeros(nobs,nv*nlags_);
i = 1;

while (i <= nlags_)
    XXact(:,(i-1)*nv+1:i*nv) = YY(T0-(i-1):T0+nobs-i,:);
    i = i+1;
end

if constant
    % last column of XXact = constant
    XXact = [XXact ones(nobs,1)];
end

if timetrend
    % last column of XXact = constant
    XXact = [XXact (1:nobs)'];
end


function h=similaritygraph(coord,S,lim,lcolor)
%function h=similaritygraph(coord,[S],[lim],[lcolor])
%
%PURPOSE
%
%To visualize a weighted graph
%
%INPUT
%
%[An argument in brackets is optional. If it isn't  given or it's
% an empty matrix/string, the function will use a default value.]
%
% coord    (Mx2 matrix) coordinates
% [S]      (MxM matrix) similarity matrix: if this not given, no edges
%            are drawn, only vertices (black dots).
% [lim]     (vector) 1xK of monotonically increasing values between
%            0 and 1 sets limits for line colors: default [0.5 0.7 0.9]
% [lcolor] (Kx3 matrix) of RGB colors that define line colors
%            for each interval ]lim(1),lim(2)], ]lim(2),lim(3)],
%            ..., ]lim(end-1),lim(end)]  default: shades of red
%
%OUTPUT
%
% h         (struct) various graphic handles and other info
%             (see details)
%
%DETAILS
%
%Draws a weighted graph where the black points are vertices and
%the red shaded lines between them are edges. The level is
%determined by dividing values of S (the weights) into bins between the
% <= lim(1)                 not drawn
% ]lim(1),lim(2)]           colormap(1,:)    default: light red
%  ...                        ...               ...
% ]lim(end-1),lim(end)]     colormap(end,:)  default: bright red
% >lim(2)                   not drawn
%
%NOTE The function always adds to a plot (turns 'hold on' temporarily).
%
%Output is a structure of handles to graphic objects etc
%
% h.graph   (vector) graphic handles to all edges (lines)
% h.vertex  (vector) graphic handles to all vertices (markers)
% h.example (vector) contains one example (graphic handle) of each
%             different line type (gray level); needed for legend
% h.text    (cell array of strings) contains labels for lines in h.example
%             legend(h.example,h.text) sets a proper legend for the
%             graph line colors
%
%If there are no values in ith bin, the shade is left out, and
%h.graph(i)=[], h.example(i)=NaN, and h.text(i)='' will be set.
%
%USED IN
% icassoGraph

%COPYRIGHT NOTICE
%This function is a part of Icasso software library
%Copyright (C) 2003-2005 Johan Himberg
%
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or 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 this program; if not, write to the Free Software
%Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

isHold=ishold;
hold on;

if nargin<3|isempty(lim),
  lim=[.5 .75 .9 1];
end

if nargin<2,
  S=zeros(size(coord,1));
end

lim=lim(:)';

if any(diff(lim)<=0),
  error('Limits must be monotonically increasing');
end

h.edge=[];
h.vertex=[];
h.example=[];
h.text=[];

% Change this to alter vertex color and line attrib. and line color
% scale
MARKERSIZE=3;
MARKERCOLOR=[0 0 0];
LINEWIDTH=1;

Nclass=length(lim)-1;
N=size(S,1);

if all(S(:)<lim(1)),
  isLines=0;
else
  isLines=1;
end

if isLines,
  if nargin<4|isempty(lcolor)
    lcolor=redscale(Nclass+1);
    lcolor=redscale(Nclass+1,0.9);
    lcolor(1:end-1,:)=lcolor(2:end,:);
  elseif size(lcolor,1)~=Nclass,
    error('Wrong number of colors.');
  end

  for i=1:Nclass,
    S_=S;
    S_(S_<=lim(i) | S_>lim(i+1))=0;
    links(:,:,i)=S_;
    hold on;
    [dummy,dummy,hd]=som_grid(S_,[N 1],'coord',coord,'marker','none', ...
			      'linecolor',lcolor(i,:),'linewidth',i.*LINEWIDTH);
    if ~isempty(hd),
      h.example(i)=hd(1); h.edge{i}=hd;
      h.text{i}=[sprintf('%0.2f',lim(i)) '<s_{ij}\leq ' sprintf('%0.2f',lim(i+1))];
    else
      % No lines in this slot
      h.example(i)=NaN; h.edge{i}=[];
      h.text{i}='';
    end
  end

  [dummy,h.vertex]=som_grid(S_,[N 1],'coord',coord,'marker','o','line','none',...
			    'markercolor',MARKERCOLOR,'markersize',MARKERSIZE);
else

  [dummy,h.vertex]=som_grid('rect',[N 1],'coord',coord,'marker','o','line','none',...
			    'markercolor',MARKERCOLOR,'markersize', ...
			    MARKERSIZE);
  h.edge=[];
  h.example=h.vertex(1);
  lim=[];
  h.text{1}='all s_{ij} below threshold: no lines shown';
end

if length(MARKERSIZE)==N,
  set(h.node,'markerfacecolor','none');
end

if ~isHold,
  hold off;
end

if nargout==0,
  clear h;
end


function [ nb, ni, np, p_xy, p_type ] = bud ( pr_bud, pr_bud_angular, ...
  r, nb, ni, np, p_xy, p_type )

%*****************************************************************************80
%
%% BUD carries out the budding process.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PR_BUD, the probability that a boundary point will bud.
%
%    Input, real PR_BUD_ANGULAR, the probability that a boundary point bud
%    will be constrained to the boundary.
%
%    Input, real R, the radius of the circle.
%
%    Input, integer NB, the updated number of boundary points.
%
%    Input, integer NI, the updated number of interior points.
%
%    Input, integer NP, the total number of points.
%
%    Input, real P_XY(2,NP), the point coordinates.
%
%    Input, integer P_TYPE(NP),
%    1, the point is constrained to the circle.
%    2, the point is constrainted to the disk.
%
%    Output, integer NB, the updated number of boundary points.
%
%    Output, integer NI, the updated number of interior points.
%
%    Output, integer NP, the updated total number of points.
%
%    Output, real P_XY(2,NP), the point coordinates.
%
%    Output, integer P_TYPE(NP),
%    1, the point is constrained to the circle.
%    2, the point is constrainted to the disk.
%    -1, the point is a new point, constrained to the circle.
%    -2, the point is a new point, constrained to the disk.
%
  r1 = rand ( 1, np );
  r2 = rand ( 1, np );

  b = find ( p_type == 1 & r1 <= pr_bud & r2 <= pr_bud_angular );
  i = find ( p_type == 1 & r1 <= pr_bud & pr_bud_angular < r2 );

  nb_inc = length ( b );
  ni_inc = length ( i );

  b_xy = bud_angular ( nb_inc, p_xy(1:2,b) );
  b_type(1:nb_inc) = -1;

  i_xy = bud_radial ( ni_inc, p_xy(1:2,i) );
  i_type(1:ni_inc) = -2;

  p_xy = [ b_xy, p_xy, i_xy ];
  p_type = [ b_type, p_type, i_type ];

  nb = nb + nb_inc;
  ni = ni + ni_inc;
  np = nb + ni;

% fprintf ( 1, 'BUD: size(P_XY)=%d,%d\n', size(p_xy) );
% fprintf ( 1, '     NB = %d, NI = %d, NP = %d\n', nb, ni, np );

  return
end


function square_arbq_rule_test03 ( degree, n, header )

%*****************************************************************************80
%
%% SQUARE_ARBQ_RULE_TEST03 gets a rule and creates GNUPLOT input files.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.  0 <= DEGREE <= 50.
%
%    Input, integer N, the number of nodes to be used by the rule.
%
%    Input, string HEADER, an identifier for the filenames.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_ARBQ_RULE_TEST03\n' );
  fprintf ( 1, '  Get a quadrature rule for the symmetric square.\n' );
  fprintf ( 1, '  Set up GNUPLOT graphics input.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );
%
%  Retrieve a symmetric quadrature rule.
%
  [ x, w ] = square_arbq ( degree, n );
%
%  Create files for input to GNUPLOT.
%
  square_arbq_gnuplot ( n, x, header );

  return
end


%% Copyright (C) 2014, 2016 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod  @@sym divergence (@var{F})
%% @defmethodx @@sym divergence (@var{F}, @var{x})
%% Symbolic divergence of symbolic expression.
%%
%% Consider a vector expression @var{F}:
%% @example
%% @group
%% syms f_1(x,y,z) f_2(x,y,z) f_3(x,y,z)
%% F = [f_1; f_2; f_3]
%%   @result{} F = (sym 3×1 matrix)
%%       ⎡f₁(x, y, z)⎤
%%       ⎢           ⎥
%%       ⎢f₂(x, y, z)⎥
%%       ⎢           ⎥
%%       ⎣f₃(x, y, z)⎦
%% @end group
%% @end example
%% The divergence of @var{F} is the scalar expression:
%% @example
%% @group
%% divergence(F)
%%   @result{} (sym)
%%       ∂                 ∂                 ∂
%%       ──(f₁(x, y, z)) + ──(f₂(x, y, z)) + ──(f₃(x, y, z))
%%       ∂x                ∂y                ∂z
%% @end group
%% @end example
%%
%% Examples:
%% @example
%% @group
%% syms x y
%% F = [x^2/2  y^2/2];
%% divergence(F)
%%   @result{} (sym) x + y
%% @end group
%%
%% @group
%% syms z
%% F = [y x x*y];
%% divergence(F, [x; y; z])
%%   @result{} (sym) 0
%% @end group
%% @end example
%%
%% Note: assumes @var{x} is a Cartesian coordinate system.
%%
%% @seealso{@@sym/gradient, @@sym/curl, @@sym/laplacian, @@sym/jacobian,
%%          @@sym/hessian}
%% @end defmethod

function g = divergence(f, x)

  assert (isvector(f), 'divergence: defined for vectors')

  if (nargin == 1)
    x = symvar(f);
  elseif (nargin == 2)
    % np-op
  else
    print_usage ();
  end

  assert (length(f) == length(x), 'divergence: num vars must match vec length')

  idx1.type='()';
  if (iscell(x))
    idx2.type='{}';
  else
    idx2.type='()';
  end
  g = sym(0);
  for i = 1:length(f)
    idx1.subs={i};
    idx2.subs={i};
    g = g + diff (subsref(f,idx1), subsref(x,idx2));
  end

end

%!shared x,y,z
%! syms x y z

%!test
%! % 1D
%! f = x^2;
%! assert (isequal (divergence(f), diff(f,x)))
%! assert (isequal (divergence(f,{x}), diff(f,x)))
%! assert (isequal (divergence(f,[x]), diff(f,x)))
%! assert (isequal (divergence(f,x), diff(f,x)))

%!test
%! % const
%! f = [sym(1); 2; exp(sym(3))];
%! assert (isequal (divergence(f,{x,y,z}), 0))
%! f = [sym(1); 2; exp(sym('c'))];
%! assert (isequal (divergence(f,{x,y,z}), 0))

%!test
%! % double const
%! f = [1 2];
%! g = sym(0);
%! assert (isequal (divergence(f, [x y]), g))
%! % should fail, calls @double: divergence(f, {x y}), g))

%!test
%! % 1D fcn in 2d/3d
%! f = [x y z];
%! assert (isequal (divergence(f), 3))
%! assert (isequal (divergence(f, {x,y,z}), 3))
%! assert (isequal (divergence(f, [x,y,z]), 3))

%!test
%! % 2d fcn in 2d/3d
%! f = sin(exp(x)*y+sinh(z));
%! g2 = [diff(f,x); diff(f,y)];
%! l2 = diff(g2(1),x) + diff(g2(2),y);
%! g3 = [diff(f,x); diff(f,y); diff(f,z)];
%! l3 = diff(g3(1),x) + diff(g3(2),y) + diff(g3(3),z);
%! assert (isequal (divergence(g2, {x,y}), l2))
%! assert (isequal (divergence(g3, {x,y,z}), l3))

%!error divergence ([1 2], [sym('x')])
%!error divergence ([1 2], sym('x'), 42)


%              ==== VERY IMPORTANT ====
% This code needs the support of Tensor Toolbox developed by Tamara Kolda
%  which is available at:
%         http://www.sandia.gov/~tgkolda/TensorToolbox/index-2.5.html
%
clear;
clc;
I=[50,50,50];
R=[5,6,7];
N=numel(I);

% Generate data;
A=cell(N,1);
for n=1:N
    A{n}=rand(I(n),R(n));
end
Y=ttensor(tensor(rand(R)),A);
Y=tensor(Y);

opts=struct('NumOfComp',R,'nlssolver','hals','maxiter',100,'maxiniter',20,'tdalgFile','call_tucker_als_opts.mat');
tic;
[Ydec]=lraNTD_ANLS(Y,opts);
toc;
fprintf('Complete. Fit=%f\n',fitness(Y,Ydec));


function [trans, rot] = mrDoAlignVol(inpts, volpts, scaleFac, inpSize, ...
	sagSize, volume, numSlices, retwin, volwin, obwin);
%
%[trans, rot] = mrDoAlignVol(inpts, volpts, scaleFac, inpSize, ...
%	sagSize, volume, numSlices, retwin, volwin, obwin);
%
%	returns alignment matrix given inpts and volpts as corresponding points
%	rot rotates inpts into volpts coordinate frame.
%	scaleFac is a vector containing scalings of the x,y,and z axes
%		such that inpts*scalefac is at the same scale as volpts

global volslimin1 volslimax1;

nuinpts = inpts ./ (ones(length(inpts),1)*scaleFac(1,:));
nuvolpts = volpts ./ (ones(length(volpts),1)*scaleFac(2,:));
nuvolpts = nuvolpts - (mean(nuvolpts)'*ones(1,length(nuvolpts)))';
nuinpts = nuinpts - (mean(nuinpts)'*ones(1,length(nuinpts)))';

H = zeros(3,3);
for i = 1:length(nuvolpts)
	H = H + (nuinpts(i,:)')*(nuvolpts(i,:));
end
[U,S,V] = svd(H);

mirrorFixer = [ 1 0 0;0 1 0; 0 0 det(U*V);];
rot = [V*mirrorFixer*(U')];

if det(rot) == -1
	disp('Warning: rotation matrix has -1 determinant');
end

alinpts = (rot*(inpts'./(ones(length(inpts),1)*scaleFac(1,:))'))';
nuvolpts = volpts ./ (ones(length(volpts),1)*scaleFac(2,:));
trans = mean(nuvolpts) - mean(alinpts);

% Check that vol = rot*inp + trans;
%alinpts = alinpts+(trans'*ones(1,length(alinpts)))';
%figure(retwin);
%hold on
%plot(inpts(:,1),inpts(:,2),'r-');
%hold off
%figure(volwin);
%hold on
%plot(alinpts(:,1),alinpts(:,2),'r-');
%hold off

% Check it out by displaying anatomy rotated to inplanes first plane

img = mrCheckAlignVol(rot,trans,scaleFac,inpSize,inpts(size(inpts,1),3), ...
			volume,sagSize,numSlices,obwin);