function C = train_HLDA(XTr, YTr, nSegments, varargin)
% TRAIN_HLDA - Hierarchical linear discriminant analysis
%
%Synopsis:
%   C = train_HLDA(XTr, YTr, nSegments, varargin)
%
%Arguments:
%   XTR: DOUBLE [TxNxM] - Data matrix, with T temporal features, N
%                           N spatial features, and M
%                           training points/examples.
%   YTR: INT [CxM]      - Class membership labels of points in X_TR. C by M
%                           matrix of training labels, with C representing
%                           the number of classes and M the number of
%                           training examples/points.
%                           YTR(i,j)==1 if the point j belongs to class i.
%   nSegments: INT      - the number of non-overlapping segments the interval
%                           should be separated in
%   OPT: PROPLIST       - Structure or property/value list of optional
%                           properties. Options are also passed to clsutil_shrinkage.
%     'Regression'  - BOOL (default 0): If true, the top level classifier is
%                           a logistic regression classifier.
%     'nChannels'   - INT (default 0): Used when using 'crossvalidation' in order to
%                           reconstruct the 2D feature matrix to its original 3D
%                           [T x Ch x N] shape.
%Returns:
%   C: STRUCT           - Structure containing the trained classifiers for
%                           each segment and the final top level
%                           classifier. The structure C includes the fields:
%       'seg': STRUCT []      - contains a trained LDA classifier for each segment
%       'final': STRUCT       - the final top-level LDA classifier
%       'nChannels': INT      - (optional) the number of channels in the training data
%Description:
%   train_HLDA trains a hierarchical LDA classifier given training data,
%   labels and a number of segments. Either LDA (default) or logistic regression
%   is used as a top-level classifier.
%
%   References:Gerson, A.D., Parra, L.C., Sajda, P.: Cortically coupled
%   computer vision for rapid image search. IEEE Transactions on Neural
%   Systems and Rehabilitation Engineering 14, 174–179 (2006).

%
%
%Examples:
%   train_HLDA(XTr, YTr, nSegments)
%   train_HLDA(XTr, YTr, nSegments, 'Regression', 1, 'nChannels', 64)
%
%See also:
%   APPLY_HLDA

props= {'Regression'      0                             'BOOL'
        'nChannels'       0                             'INT'
       };

opt= opt_proplistToStruct(varargin{:});
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

% validate argument types
misc_checkType(XTr, 'DOUBLE');

if opt.nChannels ~= 0
  misc_checkType(XTr, 'DOUBLE[- -]');
end

misc_checkType(YTr, 'DOUBLE[2 -]');
misc_checkType(nSegments, 'INT');

dims = size(XTr);

% make data matrix 3D if it has been tranformed to 2D for crossvalidation
if (length(size(XTr)) == 2) && opt.nChannels > 0
  XTr = reshape(XTr, [], opt.nChannels, dims(2));
end

dims = size(XTr);

%boundary indices between segments
seg_idx = round(linspace(0, dims(1), nSegments+1));

seg_scores = zeros(nSegments, dims(end));

for i = 1:nSegments
    if length(size(XTr)) > 2
      seg = XTr(seg_idx(i) + 1 : seg_idx(i + 1), :, :); % i-th segment of X
      seg = reshape(seg, [], dims(end));
    else
      seg = XTr(seg_idx(i) + 1 : seg_idx(i + 1), :); % i-th segment of X
    end
    %train LDA classifier for this segment
    seg_LDA = train_RLDAshrink(seg, YTr, 'Scaling', 1);
    C.seg(i) = seg_LDA;
    seg_scores(i,:) = apply_separatingHyperplane(seg_LDA, seg); %get classifier scores for segment
end

if opt.Regression
    %logistic regression as top-level classifier
    C.final.B = mnrfit(seg_scores', YTr');
else
    %LDA as top-level classifier
    C.final = train_RLDAshrink(seg_scores, YTr, 'Scaling', 1);
end

% add number of channels to classifier so they can be used in the apply function
if opt.nChannels > 0
  C.nChannels = opt.nChannels;
end


function [w, infos] = hb(problem, in_options)
% Heavy Ball algorithm.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Oct. 28, 2020

    % set dimensions and samples
    d = problem.dim;
    n = problem.samples;

    % set local options
    local_options = [];
    local_options.step_alg = 'backtracking';
    local_options.beta = 0.01;

    % merge options
    options = mergeOptions(get_default_options(d), local_options);
    options = mergeOptions(options, in_options);

    % initialise
    iter = 0;
    grad_calc_count = 0;
    w = options.w_init;
    w_old = w;
    prev_step = options.step_init;

    % for stepsize
    if strcmp(options.step_alg, 'fix') || strcmp(options.step_alg, 'no_change')
        if isprop(problem, 'L')
            if problem.L > 0
                if isprop(problem, 'mu')
                    if problem.mu > 0
                        % This casse is L-smooth and mu-strongly convex.
                        cn = problem.L/problem.mu;
                        options.step_init = 4/(sqrt(problem.L)+sqrt(problem.mu))^2;
                        options.beta = ( (sqrt(cn)-1)/(sqrt(cn)+1) )^2;
                    else
                        % This casse is L-smooth
                        options.step_init = 1/problem.L;
                    end
                else
                    % This casse is L-smooth
                    options.step_init = 1/problem.L;
                end
            else
                options.step_alg = 'backtracking';
            end
        end
    end

    % initialize by BB step-size
    if strcmp(options.step_init_alg, 'bb_init')
        options.step_init = bb_init(problem, w);
    end

    % store first infos
    clear infos;
    [infos, f_val, optgap, grad, gnorm] = store_infos(problem, w, options, [], iter, grad_calc_count, 0);
    grad_old = grad;

    % display info
    if options.verbose
        fprintf('HB: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while (optgap > options.tol_optgap) && (gnorm > options.tol_gnorm) && (iter < options.max_epoch)

        options.iter = iter;
        [step, ~] = options.linesearchfun(options.step_alg, problem, w, w_old, grad, grad_old, prev_step, options);

        prev_step = step;
        w_w_old_diff = w - w_old;
        w_old = w;

        % update w
        w = w - step * grad + options.beta * w_w_old_diff;

        % proximal operator
        if ismethod(problem, 'prox')
            w = problem.prox(w, step);
        end

        % store gradient
        grad_old = grad;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % count gradient evaluations
        grad_calc_count = grad_calc_count + n;

        % update iter
        iter = iter + 1;

        % store infos
        [infos, f_val, optgap, grad, gnorm] = store_infos(problem, w, options, infos, iter, grad_calc_count, elapsed_time);

        % display infos
        if options.verbose
            fprintf('HB: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
        end
    end

    if gnorm < options.tol_gnorm
        fprintf('Gradient norm tolerance reached: tol_gnorm = %g\n', options.tol_gnorm);
    elseif optgap < options.tol_optgap
        fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', options.tol_optgap);
    elseif iter == options.max_epoch
        fprintf('Max iter reached: max_epoch = %g\n', options.max_epoch);
    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_PA_10_6DOF(robot, T)
%   Solves the inverse kinematic problem for the MITSUBISHI PA-10 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_PA_10_6DOF returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('MISUBISHI', 'pa-10');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic( robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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/>.
function q = inversekinematic_pa_10_6DOF(robot, T)

%initialize, eight possible solutions
q=zeros(6,8);

%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%L1=d(1);
%L2=a(2);
%L3=d(4);
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Calculamos la posici�n de la mu�eca. W es el tercer vector en el extremo
%del brazo
W = T(1:3,3);
%Pm: posici�n de la mu�eca
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
q(3,:) = normalize(q(3,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist_pa_10(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist_pa_10(robot, q(:,i), T, -1); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(p1(2), p1(1));
gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2)));
if ~isreal(gamma)
    disp('WARNING:inversekinematic_pa_10: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end
%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = beta + gamma-pi/2; %elbow up
q2(2) = beta - gamma-pi/2; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
%theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
%alpha = eval(robot.DH.alpha);

%See geometry
L2=a(2);
L3=d(4);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = acos((L2^2 + L3^2 - r^2)/(2*L2*L3));

if ~isreal(eta)
   disp('WARNING:inversekinematic_pa_10: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end
%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = -pi + eta;
q3(2) = -eta + pi;

% Solve the special case of this spherical wrist
% For wrists that whose reference systems have been placed as in the
% ABB IRB 140--> use solve_spherical_wrist2
% For wrists with the same orientation as in the KUKA KR30_jet
%--> use solve_spherical_wrist
function q = solve_spherical_wrist_pa_10(robot, q, T, wrist)

% T is the noa matrix defining the position/orientation of the end
% effector's reference system
vx6=T(1:3,1);
vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

% Obtain the position and orientation of the system 3
% using the already computed joints q1, q2 and q3
T01=dh(robot, q, 1);
T12=dh(robot, q, 2);
T23=dh(robot, q, 3);
T03=T01*T12*T23;

vx3=T03(1:3,1);
vy3=T03(1:3,2);
vz3=T03(1:3,3);

% find z4 normal to the plane formed by z3 and a
vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

% in case of degenerate solution,
% when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
if norm(vz4) <= 0.00000001
    if wrist == 1 %wrist up
        q(4)=0;
    else
        q(4)=-pi; %wrist down
    end
else
    %this is the normal and most frequent solution
    cosq4=wrist*dot(vy3,vz4);
    sinq4=wrist*dot(-vx3,vz4);
    q(4)=atan2(sinq4, cosq4);
end
%propagate the value of q(4) to compute the system 4
T34=dh(robot, q, 4);
T04=T03*T34;
vx4=T04(1:3,1);
vy4=T04(1:3,2);

% solve for q5
cosq5=dot(-vy4,vz5);
sinq5=dot(vx4,vz5);
q(5)=atan2(sinq5, cosq5);

%propagate now q(5) to compute T05
T45=dh(robot, q, 5);
T05=T04*T45;
vx5=T05(1:3,1);
vy5=T05(1:3,2);

% solve for q6
cosq6=dot(vx6,vx5);
sinq6=dot(vx6,vy5);
q(6)=atan2(sinq6, cosq6);


function varargout = gallerysphere(name)
%CHEB.GALLERYSPHERE   Spherefun example functions.
%   F = CHEB.GALLERYSPHERE(NAME) returns a spherefun corresponding to NAME.
%   See the listing below for available names.
%
%   For example,  plot(cheb.gallerysphere('football')) plots the classic
%   icosahedral pattern of the Addias Telestar football (or soccer ball for
%   Americans).  For details of how each function is constructed try
%   'type +cheb/gallerysphere' or 'edit cheb.gallerysphere'.
%
%   [F,FA] = CHEB.GALLERYSPHERE(NAME) also returns the anonymous function
%   FA used to define the function. Some gallery functions are generated by
%   operations beyond the usual Spherefun constructor, so FA in those cases
%   is equal to F.
%
%   CHEB.GALLERYSPHERE with no input argument returns a function chosen at
%   random from the gallery.
%
%   CHEB.GALLERYSPHERE with no output argument creates a plot of the selected
%   function.
%
%   Available names:
%
%   deathstar   A function resembling the surface of the Death Star.
%   gaussian    Gaussian function on the sphere centered at Gauss's birth place.
%   geomag      Radial component of the International Geomagnetic Reference
%               field from the IGRF-12 model for 2015.
%   football    Icosahedral pattern found on a traditional soccer ball.
%   jet         A zonal jet stream over the mid-latitudes of the northern hemisphere.
%   moire       Moire pattern from waves generated at two point sources.
%   neamtu      Function created by Mike Neamtu for testing various spline
%               interpolation methods on the sphere (see Alfeld, Neamtu,
%               Schumaker, J. Comput. Appl. Math. 1996).
%   peaks       Peaks like function on the sphere taken from the geopeaks
%               function in the MATLAB mapping toolbox.
%   randn       Random linear combination of all real spherical harmonics
%               of exact degree 40.  The coefficients are generated from a
%               i.i.d. Gaussian (normal) distribution with std=1.
%   reprodkern  Reproducing kernel for spherical harmonics of degree 10
%               centered at (x,y,z) = (-1/sqrt(3),-1/sqrt(3),1/sqrt(3)).
%   soccerball  Same as football, but for the American users.
%   stripes     Alternating striped pattern.
%   vortices    Two antipodal vortices taken from  Nair, Cote, and
%               Staniforth (1999).
%   :)          A function to make you happy
%
%   Gallery functions are subject to change in future releases of Chebfun.
%
% See also CHEB.GALLERY, CHEB.GALLERYTRIG, CHEB.GALLERY2, CHEB.GALLERY3, CHEB.GALLERYDISK.

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

% If the user did not supply an input, return a function chosen at random
% from the gallery.
if ( nargin == 0 )
    names = {'football','jet','soccerball','deathstar', 'vortices'...
        'gaussian','reprodkern','geomag','peaks','neamtu','randn',...
        'moire','stripes',':)'};
    name = names{randi(length(names))};
end

type=1;                  % Default plotting type
addEarthPlot = 0;        % Flag on whether or not to plot the earth (1=yes)
viewAngle = [-37.5 30];  % Default viewing angle
clrmap = parula(64);     % Default colormap
clraxis = [];            % Color axis for the colormap; empty means use default

% The main switch statement.
switch lower(name)
    % The classic football (or soccerball) pattern
    case {'football','soccerball'}
        f = spherefun.sphharm(6,0) + sqrt(14/11)*spherefun.sphharm(6,5);
        fa = f;
        cntrlvl = -[0.25 0.25];
        type = 3;

    % A function resembling the surface of the Death Star
    case 'deathstar'
        fa = @(x,y,z) -(exp(-30*((y+sqrt(3)/2).^2 + x.^2 + (z-1/2).^2)) + exp(-100*z.^2));
        f = spherefun(fa);
        type = 1;
        clrmap = flipud(jet);
        viewAngle = [-35 8];

    % Two diametrically oposed vortices taken from
    % R. D. Nair, J. Cote, A. Staniforth, Cascade interpolation for
    % semi-Lagrangian advection over the sphere, Quart. J. Roy. Meteor.
    % Soc. 125 (1999) 1445-1468
    case 'vortices'
        rho = @(th) 3*(sin(th)); w = @(th) (3*sqrt(2)/2*sech(rho(th)).^2.*tanh(rho(th)))./(rho(th)+eps);
        fa = @(lam,th) -tanh(rho(th)/5.*sin(lam-20*w(th)));
        f = spherefun(fa);
        cntrlvl = [0 0];
        type = 3;
        clrmap = jet;

    % A simple Gaussian
    case 'gaussian'
        % Coordinates of Braunschweig, where Gauss was born;
        coords = [10.516667 52.266667]/180*pi;
        [xc,yc,zc] = sph2cart(coords(1),coords(2),1);
        fa = @(x,y,z,xc,yc,zc) exp(-20*((x-xc).^2 + (y-yc).^2 + (z-zc).^2));
        f = spherefun(@(x,y,z) fa(x,y,z,xc,yc,zc));
        addEarthPlot = 1;
        viewAngle = [80 15];

    % The reproducing kernel for all spherical harmonics of degree 20
    case 'reprodkern'
        [lam0,th0] = cart2sph(-1/sqrt(3),-1/sqrt(3),1/sqrt(3));
        f = spherefun(@(lam,th) sphRPK(lam,th,lam0,pi/2-th0,20));
        fa = f;
        viewAngle = [-50 5];

    % The IGRF-12 geomagnetic field
    case 'geomag'
        % Spherical harmonic coefficients for the IGRF-12 2015 Geomagnetic
        % Model; see http://www.ngdc.noaa.gov/IAGA/vmod/igrf.html
        c = [-29442.0, -1501.0, 4797.1, -2445.1, 3012.9, -2845.6, 1676.7,...
            -641.9, 1350.7, -2352.3, -115.3, 1225.6, 244.9, 582.0, -538.4,...
            907.6, 813.7, 283.3, 120.4, -188.7, -334.9, 180.9, 70.4, -329.5,...
            -232.6, 360.1, 47.3, 192.4, 197.0, -140.9, -119.3, -157.5, 16.0,...
            4.1, 100.2, 70.0, 67.7, -20.8, 72.7, 33.2, -129.9, 58.9, -28.9,...
            -66.7, 13.2, 7.3, -70.9, 62.6, 81.6, -76.1, -54.1, -6.8, -19.5,...
            51.8, 5.7, 15.0, 24.4, 9.4, 3.4, -2.8, -27.4, 6.8, -2.2, 24.2, 8.8,...
            10.1, -16.9, -18.3, -3.2, 13.3, -20.6, -14.6, 13.4, 16.2, 11.7,...
            5.7, -15.9, -9.1, -2.0, 2.1, 5.4, 8.8, -21.6, 3.1, 10.8, -3.3, 11.8,...
            0.7, -6.8, -13.3, -6.9, -0.1, 7.8, 8.7, 1.0, -9.1, -4.0, -10.5, 8.4,...
            -1.9, -6.3, 3.2, 0.1, -0.4, 0.5, 4.6, -0.5, 4.4, 1.8, -7.9, -0.7,...
            -0.6, 2.1, -4.2, 2.4, -2.8, -1.8, -1.2, -3.6, -8.7, 3.1, -1.5, -0.1,...
            -2.3, 2.0, 2.0, -0.7, -0.8, -1.1, 0.6, 0.8, -0.7, -0.2, 0.2, -2.2,...
            1.7, -1.4, -0.2, -2.5, 0.4, -2.0, 3.5, -2.4, -1.9, -0.2, -1.1, 0.4,...
            0.4, 1.2, 1.9, -0.8, -2.2, 0.9, 0.3, 0.1, 0.7, 0.5, -0.1, -0.3, 0.3,...
            -0.4, 0.2, 0.2, -0.9, -0.9, -0.1, 0.0, 0.7, 0.0, -0.9, -0.9, 0.4,...
            0.4, 0.5, 1.6, -0.5, -0.5, 1.0, -1.2, -0.2, -0.1, 0.8, 0.4, -0.1,...
            -0.1, 0.3, 0.4, 0.1, 0.5, 0.5, -0.3, -0.4, -0.4, -0.3, -0.8];
        % Compute the vertical component of the magnetic field
        f = 0*spherefun.sphharm(0,0);
        k = 1;
        for l=1:13
            f = f + -((l+1))*sqrt(4*pi/(2*l+1))*c(k)*spherefun.sphharm(l,0);
            k = k + 1;
            for m=1:l
                f = f - (-1)^m*((l+1))*sqrt(4*pi/(2*l+1))*...
                        (c(k)*spherefun.sphharm(l,m) + ...
                        c(k+1)*spherefun.sphharm(l,-m));
                k = k + 2;
            end
        end
        fa = f;
        type = 3;
        addEarthPlot = 1;
        cntrlvl = -60000:5000:60000;

    % A "peaks-like" function for the sphere
    case 'peaks'
        fa = @(x,y,z) 8*(1-x).^2.*exp(-4*(x - 0.059).^2 - 2*(y + 0.337).^2 - 2*(z + 0.940).^2) - ...
            30*(z/10 - x.^3 - y.^5) .* exp(-3*(x - 0.250).^2 - 2*(y - 0.433).^2 - 3*(z - 0.866).^2) + ...
            (20*y - 8*z.^3) .* exp(-2*(x + 0.696).^2 - 3*(y + 0.123).^2 - 2*(z - 0.707).^2) + ...
            (7*y - 10*x + 10*z.^3) .* exp(-3*(x - 0.296).^2 - 3*(y + 0.814).^2 - 3*(z + 0.5).^2);
        f = spherefun(fa);

    % A function developed by Mike Neamtu of Vanderbilt University to test
    % approximation schemes on the sphere.
    case 'neamtu'
        fa = @(x,y,z) 1 + x.^8 + exp(2*y.^3) + exp(2*z.^2) + 10*x.*y.*z;
        f = spherefun(fa);

    % A Guassian random function on the sphere suggested by Dmitry Belyaev
    % at Oxford.
    case 'randn'
        % Compute a random spherefun
        f = randnfunsphere(0.079,'monochromatic');
        fa = f;
        type = 4;
        clrmap = gray(2);
        % Set the color axis so that the transition from white to black is
        % exactly at the value zero.
        minmax = minandmax2est(f);
        clraxis = [-1 1]*max(abs(minmax));

    % An interesting Moire pattern generated by two sources on the sphere
    case 'moire'
        % Centers of the beacons
        boise = [-116.237651 43.613739]*pi/180;
        oxford = [-1.257778 51.751944]*pi/180;
        % ithaca = [-76.5 42.443333]*pi/180;
        % stellenbosh = [18.86 -33.92]*pi/180;
        [xb,yb,zb] = sph2cart(boise(1),boise(2),1);
        [xo,yo,zo] = sph2cart(oxford(1),oxford(2),1);
        % Pick the number of oscillations and make each of the "waves"
        % vanish at the anti-podal points from their centers.
        omega = besselroots(0,30); omega = omega(end)/2;
        % Use a combination of the J0 bessel functions centered at Boise
        % and Oxford to generate the Moire pattern.
        fa = @(x,y,z,omega) 2 + besselj(0,omega*sqrt((x-xb).^2+(y-yb).^2+(z-zb).^2)) + ...
            2 + besselj(0,omega*sqrt((x-xo).^2+(y-yo).^2+(z-zo).^2));
        f = spherefun(@(x,y,z) fa(x,y,z,omega));
        type = 1;
        addEarthPlot = 1;
        viewAngle = [32 8];

    % A zipper-like stripe pattern for the sphere
    case 'stripes'
        fa = @(x,y,z) (1 + cos(10*pi*x)).*exp(-exp(-20*z)) + (1 - cos(10*pi*x)).*exp(-exp(20*z));
        f = spherefun(fa);
        type = 3;
        cntrlvl = [1 1];
        viewAngle = [-36 8];
        clrmap = hot;

    % A zonal jet stream in the mid-latitudes of the northern hemisphere.
    case 'jet'
        fa = spherefun(@(lam,th) sin(th-(pi/4+0.01*cos(12*lam))).*exp(-300*(1-cos(th-(pi/4+0.01*cos(12*lam))))));
        f = spherefun(fa);
        type = 1;
        clrmap = jet;
        viewAngle = [32 8];
        addEarthPlot = 1;

    % A smiley face emoticon
    case ':)'
        fa = @(x,y,z) exp(-20*(4*(x+cos(pi/6)*sin(pi/4)).^2 + 4*(y+sin(pi/6)*sin(pi/4)).^2 + (z-cos(pi/4)).^2)) + ...
                      exp(-20*(4*(x+cos(-pi/6)*sin(pi/4)).^2 + 4*(y+sin(-pi/6)*sin(pi/4)).^2 + (z-cos(pi/4)).^2)) + ...
                   9*exp(-100*((x + 1).^2 - x.*z + (z - 1/2).^2 - 1/10).^2);
        f = spherefun(fa);
        viewAngle = [-75 20];

    otherwise
        error('CHEB:GALLERYSPHERE:unknown:unknownFunction', ...
            'Unknown function.')
end

% Only return something if there is an output argument.
if ( nargout > 0 )
    varargout = {f, fa};
    return;
end

ptitle = [name ', rank = ' num2str(length(f))];
% Determine the type of plot to make
if type==1
    % Plot the function with a grid
    surf(f,'grid','k-')
    axis off, title(ptitle)
elseif type==2
    contour(f)
    axis off, title(ptitle)
elseif type==3
    plot(f), hold on
    contour(f,cntrlvl,'k-'), hold off
    axis off, title(ptitle)
else
    plot(f)
    axis off, title(ptitle)
end

view(viewAngle);
colormap(clrmap);

% Set the color axis if required.
if ( ~isempty(clraxis) )
    caxis(clraxis);
end

if ( addEarthPlot )
    hold on, spherefun.plotEarth('w-'), hold off
end

end

function f = sphRPK(lam,th,lam0,th0,deg)
%SPHRPK  Reproducing kernel for the spherical harmonics of a given degree.
%   F = SPHRPK(LAM,TH,LAM0,TH0,DEG) is the reproducing kernel centered at
%   (LAM0,TH0) for the space of spherical harmonics of degree DEG.

t = cos(lam).*sin(th).*cos(lam0).*sin(th0) + sin(lam).*sin(th).*sin(lam0).*sin(th0) + cos(th).*cos(th0);
pl = legpoly(0:deg,[-1,1]);
[m,n] = size(t);
t = t(:);
c = ones(length(t),1)*((2*(0:deg)+1)/4/pi);
f = reshape(sum(c.*pl(t),2),m,n);

end


function res = difonzo(Y,x,z,ta,sc,type,f)
% PURPOSE: Multivariate temporal disaggregation with transversal constraint
% ----------------------------------------------------------------------------------
% SYNTAX: res = difonzo(Y,x,z,ta,sc,type,f);
% ----------------------------------------------------------------------------------
% OUTPUT: res: a structure
%         res.meth  = 'Multivariate Di Fonzo';
%         res.N     = Number of low frequency data
%         res.n     = Number of high frequency data
%         res.pred  = Number of extrapolations
%         res.ta    = Type of disaggregation
%         res.sc     = Frequency conversion
%         res.type  = Model for high frequency innovations
%         res.beta  = Model parameters
%         res.y     = High frequency estimate
%         res.d_y   = High frequency estimate: std. deviation
%         res.z     = High frequency constraint
%         res.et    = Elapsed time
% ----------------------------------------------------------------------------------
% INPUT: Y: NxM  ---> M series of low frequency data with N observations
%        x: nxm  ---> m series of high frequency data with n observations, m>=M see (*)
%        z: nzx1 ---> high frequency transversal constraint with nz obs.
%        ta: type of disaggregation
%            ta=1 ---> sum (flow)
%            ta=2 ---> average (index)
%            ta=3 ---> last element (stock) ---> interpolation
%            ta=4 ---> first element (stock) ---> interpolation
%        sc: number of high frequency data points for each low frequency data points
%            sc= 4 ---> annual to quarterly
%            sc=12 ---> annual to monthly
%            sc= 3 ---> quarterly to monthly
%        type: model for the high frequency innvations
%            type=0 ---> multivariate white noise
%            type=1 ---> multivariate random walk
% (*) Optional:
%        f: 1xM ---> Set the number of high frequency indicators linked to
%                    each low frequency variable. If f is explicitly included,
%                    the high frequency indicators should be placed in
%                    consecutive columns
% ----------------------------------------------------------------------------------
% NOTE: Extrapolation is automatically performed when n>sN.
%       If n=nz>sN restricted extrapolation is applied.
%       Finally, if n>nz>sN extrapolation is perfomed in constrained
%       form in the first nz-sN observatons and in free form in
%       the last n-nz observations.
% ----------------------------------------------------------------------------------
% LIBRARY: aggreg, dif, vec, desvec
% ----------------------------------------------------------------------------------
% SEE ALSO: denton, rossi, mtd_print, mtd_plot
% ----------------------------------------------------------------------------------
% REFERENCE: Di Fonzo, T.(1990)"The estimation of M disaggregate time
% series when contemporaneous and temporal aggregates are known", Review
% of Economics and Statistics, vol. 72, n. 1, p. 178-182.

% written by:
%  Enrique M. Quilis
%  Macroeconomic Research Department
%  Ministry of Economy and Competitiveness
%  <enrique.quilis@mineco.es>

% Version 3.0 [August 2006]

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

t0 = clock;

%--------------------------------------------------------
%       Preliminary checking

[N,M] = size(Y);
[n,m] = size(x);
[nz,mz] = size(z);

if ((M > m) | (n < sc*N) | (mz ~= 1) | (nz > n) | (nz < sc*N))
   error (' *** INCORRECT DIMENSIONS *** ');
else
   % Number of extrapolations
   h1 = n - nz;
   h2 = n - sc*N;
   clear nz mz;
end

%--------------------------------------------------------
%       Checking of "ta"

if (ta < 1) | (ta > 4)
    error (' *** INCORRECT TA OPTION *** ');
end

%--------------------------------------------------------
%       Checking of "sc"

if (sc ~= 3) & (sc ~= 4) & (sc ~= 12)
    error (' *** INCORRECT FREQUENCY CONVERSION (sc) *** ');
end

%--------------------------------------------------------
%       Checking of "type"

if (type < 0) | (type > 1)
    error (' *** INCORRECT TYPE OPTION *** ');
end

%--------------------------------------------------------
%       Checking (and definition) of vector f

if (nargin == 6)
    f = ones(1,M);
end

if ( (f < 1) | (sum(f) ~= m) | (length(f) ~= M))
   error (' *** IMPROPER ASSIGNMENT OF INDICATORS IN f VECTOR *** ');
end

%--------------------------------------------------------
%  **** CONSTRAINT MATRICES ***
%--------------------------------------------------------
% Required:
%              H1 ---> transversal
%              H2 ---> longitudinal
%
%---------------------------------------------------------------
%       Generate H1: (n-h1) x nM

H1 = kron(ones(1,M),[eye(n-h1) zeros(n-h1,h1)]);

%---------------------------------------------------------------
%       Generate H2: NM x nM.
%
% Generation of aggregation matrix C

C = aggreg(ta,N,sc);
C = [C zeros(N,h2)];

H2 = kron(eye(M),C);

%---------------------------------------------------------------
%       Generate H: (n-h1+NM) x nM.
%
%       H = [ H1
%             H2 ]

H = [H1
   H2];

%--------------------------------------------------------
%  **** PREPARING DATA MATRICES ***
%--------------------------------------------------------
% Required:
%               x_diag
%               Y_big,  Y_e
%               X_diag, X_e

%--------------------------------------------------------
%       Generate x_diag: nM x M+m
%
% It is a diagonal matrix formed by the high frequency
% indicators, including a vector of ones for the intercept
%
%       x_diag = [ x1 0  0  ... 0
%                  0  x2 0  ... 0
%                  0  0  x3 ... 0
%                  ..............
%                  0  0  0  ... xM ]
%
% It is made by means of a recursion.

ac(1)=f(1);                             % Initialization of the recursion
x_diag = [ones(n,1) x(:,1:ac(1))];

j=2;
while (j <= M)
   xaux = [ones(n,1) x(:,ac(j-1)+1:ac(j-1)+f(j))];
   [a2,b2] = size(xaux);
   [a1,b1] = size(x_diag);
   x_diag = [ x_diag         zeros(a1,b2)
              zeros(a2,b1)   xaux ];
   ac(j) = ac(j-1) + f(j);
   j = j + 1;
end
clear xaux;

%--------------------------------------------------------
%       Generate X_diag: NM x M+m
%
% Low frequency analog of x_diag. It is the result of
% applying the temporal aggregator H2 to x_diag.

X_diag = H2 * x_diag;

%--------------------------------------------------------
%       Generate X_e: (n-h1+NM) x M+m
%
%
% It is the result of applying the complete aggregator H
% (temporal as well as transversal).
% Lower part of X_e is X_diag.

X_e = H * x_diag;

%--------------------------------------------------------
%       Generate Y_big: NM x 1
%
% It is column vector containing all the observations on the
% low frequency series according to: Y_big = [Y1 Y2 ... YM]'
% Formally: Y_big = vec(Y)

Y_big = vec(Y);

%--------------------------------------------------------
%       Generate Y_e: (n-h1+NM) x 1
%
% It is column vector containing the transversal constraint
% and all the observations on the low frequency series
% according to: Y_e = [ z Y1 Y2 ... YM]' = [z Y_big]'

Y_e = [ z
      Y_big];

%--------------------------------------------------------
%  **** PRELIMINARY ESTIMATION OF SIGMA ***
%--------------------------------------------------------
% The method of di Fonzo requires the previous estimation of VCV
% matrix SIGMA for the (implied) low frequency model. This
% preliminary estimation is performed by means of estimating, equation
% by equation, the model. Formally, this is equivalent to estimate an
% unrelated SURE model. Computationally, this is also the applied procedure.

BETA = (X_diag' * X_diag) \ (X_diag' * Y_big); % OLS estimator
U_big = Y_big - X_diag * BETA;                 % Residuals in vec format

% Residuals (columnwise) U: NxM

U = desvec(U_big,M);

% Preliminary estimation of SIGMA

SIGMA = cov(U,1);

%--------------------------------------------------------
%  **** APPLYING DI FONZO PROCEDURE ***
%--------------------------------------------------------

%--------------------------------------------------------
%       High frequency VCV matrix v: nM x nM

switch type
case 0 % White noise
   v = kron(SIGMA,eye(n));
case 1 % Random walk, with U(0)=0
   D = dif(1,n);
   DDi = inv(D'*D);
   v = kron(SIGMA,DDi);
end;

%--------------------------------------------------------
%       Low frequency VCV matrix V: (n-h1+NM) x (n-h1+NM)
%       and its generalized inverse

V = H * v * H';
Vi = pinv(V);      % Moore-Penrose generalized inverse

%--------------------------------------------------------
%       Generation of distribution filter L: nM x (n-h1+NM)

L = v * H' * Vi;

%--------------------------------------------------------
%	     GLS estimation of beta in a SURE context

beta = (X_e' * Vi * X_e) \ (X_e' * Vi * Y_e);

U_e = Y_e - X_e * beta;

%--------------------------------------------------------
%       Estimation of high frequency series

y_big = x_diag * beta + L * U_e;

% Series y columnwise y: nxM

y = desvec(y_big,M);

%--------------------------------------------------------
%       VCV matrix of estimations y: nM x nM

sigma_y = (eye(n*M) - L*H)*v + ...
   (x_diag - L*X_e)*inv(X_e'*Vi*X_e)*(x_diag - L*X_e)';

% Vector format of std. dev.

d_y_big = sqrt(diag(sigma_y));

% Std. dev. series in column format dt_y: n x M

d_y = desvec(d_y_big,M);

% -----------------------------------------------------------------------
% Loading the structure
% -----------------------------------------------------------------------
% Basic parameters

res.meth = 'Multivariate Di Fonzo';
res.N = N;
res.n = n;
res.pred = h2;
res.ta= ta;
res.sc = sc;
res.type = type;

% -----------------------------------------------------------------------
% Parameters

res.beta=beta;

% -----------------------------------------------------------------------
% Series

res.y   = y;
res.d_y = d_y;
res.z   = z;

% -----------------------------------------------------------------------
% Elapsed time

res.et        = etime(clock,t0);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = psabr_4_2(a, b, r, n, f, k, t,m, mu, nu, l, u)
% sabr prices using an admissible region kl <= k <= ku where sabr is used
% for 0 <= k <= kl    we use a put pricing function
%           f(x) = x^mu exp(a + bx + cx^2)
% for ku < k < +infty we use a call pricing function
%           f(x) = x^(-nu) exp(a + bx^(-1) + cx(-2)
% a big range (small kl and big ku) guarantee that the prices are in line
% with the observed calls and puts
%
% this is the preferred version of the density for computations
%

eps = 1e-004;                              % .1 bp

s = @(x) psabr(a, b, r, n, f, x, t);
index = find(s(k)>0,1,'first');

if (isempty(index) || index ==1)
   kl = l *f;
else
    kl = max(l * f,k(index)+(f-k(index))/m);% lower strike level
end
s1 = s(kl-eps);                          % for calc of derivatives
s2 = s(kl);                              % for calc of derivatives
s3 = s(kl+eps);                          % for calc of derivatives

V1 = log(s2);                            % log density

U2 = (s3-s1)/(2*eps);                    % derivative of density
V2 = U2/s2;                              % derivative of log density

U3 = (s3-2*s2+s1)/eps^2;                 % second derivative of density
V3 = U3/s2 - V2^2;                       % second derivative of log density

% fix mu and solve
%       V1 = mu log kl + a + b kl + c kl^2
%       V2 = mu / kl + b + 2c kl
%       V3 = - mu / kl^2 + 2c

%mu = 1.5;                               % controls the left tails
cl = .5*(V3+mu/kl^2);
bl = V2-mu/kl - 2*cl*kl;
al = V1 - mu * log(kl) - bl*kl - cl*kl^2;

% upper strike level (the bigger the strike level the better fit the call
% prices to Hagan formula!)
ku = u * f;
s = @(x) psabr(a, b, r, n, f, x, t);        % sabr price (1) for calls
s1 = s(ku-eps);                             % for calc of derivatives
s2 = s(ku);                                 % for calc of derivatives
s3 = s(ku+eps);                             % for calc of derivatives

V1 = log(s2);                               % log call price

U2 = (s3-s1)/(2*eps);                       % derivative of  density
V2 = U2/s2;                                 % derivative of log density

U3 = (s3-2*s2+s1)/eps^2;                    % second derivative of density
V3 = U3/s2 - (U2/s2)^2;                     % second derivative of density

% fix nu and solve
%       V1 = -nu log ku + a + b/ku + c/ku^2
%       V2 = -nu / ku - b / ku^2 - 2c/ku^3
%       V3 = nu / ku^2 + 2 b / ku^3 - 6 c / ku^4

cu = (-1.5*nu / ku + .5*V3 * ku - V2)*ku^3/5;
bu = -ku^2*(V2 + nu/ku +2*cu/ku^3);
au = V1 + nu * log(ku) - bu / ku - cu / ku^2;

    yl = real(k(k<kl).^mu .* exp(al + bl .* k(k<kl) + cl * k(k<kl).^2));
    ym = real(psabr(a,b,r,n,f,k((kl<=k)&(k<=ku)),t));
    yu = real(k(k>ku).^(-nu) .* exp(au+bu./k(k>ku)+cu./k(k>ku).^2));

y = [yl ym yu];                            % output
end


% Get vertex coordinates for the patch type 'P'
% [C, N, S, W, E] = HealpixGetPatchVertexCoordsP(n, i, j, INFO)
%
% Parameters
% n : grid resolution
% i : ring index
% j : intra-ring index
% INFO : intermediate information (output of HealpixSelectPatchClass())
% C : intra-patch coordinates
% N : coordinates for north vertex
% S : coordinates for south vertex
% W : coordinates for west vertex
% E : coordinates for east vertex

function [C, N, S, W, E] = HealpixGetPatchVertexCoordsP(n, i, j, INFO)

% gradient of the border
grad = INFO.polar_part;

decimal_i = INFO.decimal_i_n;
int_i = INFO.int_i_n;

int_j = fix(j - grad * decimal_i);
decimal_j = j - int_j;
if int_j == 0
    int_j = 4 * int_i;
end

if decimal_j > (grad + 1) * decimal_i
    north_i = int_i - 1;
    south_i = int_i + 1;
    north_int_j = mod(int_j - grad - 1, 4 * north_i) + 1;
    south_int_j = mod(int_j + (grad + 1) - 1, 4 * south_i) + 1;
    east_int_j = mod(int_j + 1 - 1, 4 * int_i) + 1;

    N = [north_i, north_int_j];
    S = [south_i, south_int_j];
    W = [int_i  , int_j];
    E = [int_i  , east_int_j];

    offset_j = (grad + 0.5) * decimal_i;
    C = [decimal_i, decimal_j - offset_j - 0.5];
else
    curnt_i = int_i + 1;
    south_i = int_i + 2;
    if south_i > n
        south_i = n;
    end
    west_int_j = mod(int_j + grad - 1, 4 * curnt_i) + 1;
    east_int_j = mod(int_j + grad + 1 - 1, 4 * curnt_i) + 1;
    south_int_j = mod(west_int_j + (grad + 1) - 1, 4 * south_i) + 1;

    N = [int_i    , int_j];
    S = [int_i + 2, south_int_j];
    W = [int_i + 1, west_int_j];
    E = [int_i + 1, east_int_j];

    offset_j = (grad + 0.5) * (1 - decimal_i);
    C = [decimal_i - 1, decimal_j + offset_j - (grad + 0.5)];
end

if INFO.is_south_pole
    TMP = N;
    N = S;
    S = TMP;
    N(1) = 4 * n - N(1);
    S(1) = 4 * n - S(1);
    W(1) = 4 * n - W(1);
    E(1) = 4 * n - E(1);
    C(1) = -C(1);
end


function [w,run] = train_mis(x,w,lambda)
% Modified iterative scaling
% x is premultiplied by y

% Written by Thomas P Minka

if nargin < 3
  lambda = 0;
end
if lambda > 0
  error('must have lambda = 0')
end
[d,n] = size(x);
flops(0);
step = 1/max(sum(abs(x),1));

i1 = (x > 0);
x1 = abs(x).*i1;
x2 = abs(x).*(1-i1);
flops(flops + 3*d*n);
if nargout > 1
  run.w = [];
  run.flops = [];
  run.e = [];
end
for iter = 1:10000
  old_w = w;
  % s1 = 1-sigma
  s1 = 1./(1+exp(w'*x));
  delta = (x1*s1')./(x2*s1');
  w = w + step*0.5*log(delta);
  if iter == 1
    % same for every iteration
    % use spmul because x1,x2 have structural zeros
    fl = flops_mul(w',x)+n*(flops_exp+2) + ...
	flops_spmul(x1,s1')+flops_spmul(x2,s1')+d + ...
	d*(flops_exp+2);
  end
  flops(flops + fl);

  if nargout > 1 & rem(iter,100) == 1
    run.w(:,end+1) = w;
    run.flops(end+1) = flops;
    run.e(end+1) = logProb(x,w) -0.5*lambda*w'*w;
  end
  if rem(iter,1000) == 0
    fprintf('MIS iter %d\n', iter)
  end

  if max(abs(w - old_w)) < 1e-6
    break
  end
end
if iter == 10000
  warning('not enough iters')
end
if nargout > 1
  figure(2)
  plot(run.e)
end


function [ml] = likelihood(hyp, inf, mean, cov, lik, input, target)
% Calculates the negative log marginal likelihood.
%
%% Syntax
%  ml = likelihood(hyp, inf, mean, cov, lik, input, target)
%
%% Description
% Function for calculating the negative log marginal likelihood of given
% hyperparameters, covariance function and data. If more sets of
% hyperparameters are given it outputs a vector of calculated negative log
% marginal likelihoods.
%
% Based on the work of C.E.Rasmussen.
%
% Input:
% * hyp    ... the hyperparameter struct(s), row vector of structs
% * inf    ... the function specifying the inference method
% * mean   ... the prior mean function
% * cov    ... the prior covariance function
% * lik    ... the likelihood function
% * input  ... the input part of the training data,  NxD matrix
% * target ... the output part of the training data (ie. target), Nx1 vector
%
% Output:
% * ml     ... the negative log marginal likelihood(s), vector
%
% See Also:
% gpx, minimize, covFunction, gp_initial
%
% Examples:
% gp_initial.m
%%

if nargin < 7 % input validation
  error('Too few parameters are given.'); %
end

ml=zeros(size(hyp)); % allocate

for i = 1 : size(hyp,2)
  try

    mlt = gp(hyp(i), inf, mean, cov, lik, input, target);
  catch
    mlt = +Inf;
  end

  % stability
  if (~isfinite(mlt) || isnan(mlt) || ~isreal(mlt))
    mlt = +Inf;
  end
  ml(i) = mlt;
end


function [U,S,output] = lmlra_nls(T,U0,S0,options)
%LMLRA_NLS LMLRA by nonlinear least squares.
%   [U,S,output] = lmlra_nls(T,U0,S0) computes the factor matrices U{1},
%   ..., U{N} and core tensor S belonging to a low multilinear rank
%   approximation of the N-th order tensor T by minimizing
%   0.5*frob(T-lmlragen(U,S))^2. Each term U{r} is a cell array of N factor
%   matrices U{r}{n}, followed by a core tensor U{r}{N+1}. The algorithm is
%   initialized with the factor matrices U0{n} and core tensor S0. The
%   structure output returns additional information:
%
%      output.Name  - The name of the selected algorithm.
%      output.<...> - The output of the selected algorithm.
%
%   lmlra_nls(T,U0,S0,options) may be used to set the following options:
%
%      options.Algorithm =   - The desired optimization method.
%      [@nls_gncgs| ...
%       {@nls_gndl}|@nls_lm]
%      options.M =           - The preconditioner to use when
%      [{'block-Jacobi'}|...   options.LargeScale is true.
%       false]
%      options.<...>         - Parameters passed to the selected method,
%                              e.g., options.TolFun, options.TolX and
%                              options.PlaneSearchOptions. See also help
%                              [options.Algorithm].
%
%   See also lmlra_minf.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.
%   [2] L. Sorber, M. Van Barel, L. De Lathauwer, "Unconstrained
%       optimization of real functions in complex variables," SIAM J. Opt.,
%       Vol. 22, No. 3, 2012, pp. 879-898.

% Forward problem to btd_nls as a one-term BTD.
if nargin < 4, options = struct; end
[U,output] = btd_nls(T,{[U0(:).',S0]},options);
S = U{1}{end};
U = U{1}(1:end-1);


function result = clara(x,kclus,vtype,stdize,metric,nsamp,sampsize)

%CLARA is the 'Clustering Large Applications' clustering algorithm.
% It returns a list representing a clustering of the data
% into kclus clusters following the clara algorithm which is
% designed for large data sets.
%
%The algorithm is fully described in:
%   Kaufman, L. and Rousseeuw, P.J. (1990),
%   "Finding groups in data: An introduction to cluster analysis",
%   Wiley-Interscience: New York (Series in Applied Probability and
%   Statistics), ISBN 0-471-87876-6.
%
% Required input arguments:
%       x : Data matrix (rows = observations, columns = variables)
%   kclus : The number of desired clusters
%   vtype : Variable type vector (length equals number of variables)
%           Possible values are 1  Asymmetric binary variable (0/1)
%                               2  Nominal variable (includes symmetric binary)
%                               3  Ordinal variable
%                               4  Interval variable
%
% Optional input arguments:
%     stdize : standardise the variables given by the x-matrix
%              Possible values are 0 : no standardisation (default)
%                                  1 : standardisation by the mean
%                                  2 : standardisation by the median
%     metric : Metric to be used
%              Possible values are 'eucli' Euclidian (all interval variables, default)
%                                  'manha' Manhattan
%                                  'mixed' Mixed (not all interval variables, default)
%      nsamp : Number of samples to be drawn from the data set
%   sampsize : Number of observations in each sample (should be higher
%              than the number of clusters and lower than the number of
%              observations)
%
% I/O:
%   result=clara(x,kclus,vtype,'eucli',5,40+2*kclus)
%
% Example (subtracted from the referenced book)
%   load obj200.mat
%   result=clara(obj200,3,[4 4]);
%
% The output of CLARA is a structure containing:
%   result.dysobs     : dissimilarities for each observation with the medoids
%   result.metric     : metric used
%   result.number     : number of observations
%   result.idmed      : Id of medoid observations
%   result.ncluv      : A vector with length equal to the number of observations,
%                       giving for each observation the number of the cluster to
%                       which it belongs
%   result.obj        : Objective function for the best subsample
%   result.clusinf    : Matrix, each row gives numerical information for
%                       one cluster. These are the cardinality of the cluster
%                       (number of observations), the maximal and average
%                       dissimilarity between the observations in the cluster
%                       and the cluster's medoid, the diameter of the cluster
%                       (maximal dissimilarity between two observations of the
%                       cluster), and the separation of the cluster (minimal
%                       dissimilarity between an observation of the cluster
%                       and an observation of another cluster).
%   result.sylinf     : Matrix based on the best subsample, with for each
%                       observation i of this subsample the cluster to
%                       which i belongs, as well as the neighbor cluster of i
%                       (the cluster, not containing i, for which the average
%                       dissimilarity between its observations and i is minimal),
%                       and the silhouette width of i.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Guy Brys (May 2006)

%Checking and filling out the inputs
if (nargin<3)
    error('Three input arguments required')
elseif (nargin<4)
    stdize = 0;
    if (sum(vtype)~=4*size(x,2))
        metric = 'mixed';
    else
        metric = 'eucli';
    end
    nsamp=5;
    sampsize=40+2*kclus;
elseif (nargin<5)
    if (sum(vtype)~=4*size(x,2))
        metric = 'mixed';
    else
        metric = 'eucli';
    end
    nsamp=5;
    sampsize=40+2*kclus;
elseif (nargin<6)
    nsamp=5;
    sampsize=40+2*kclus;
elseif (nargin<7)
    sampsize=40+2*kclus;
end

%Standardization
if (stdize==1)
    x = ((x - repmat(mean(x),size(x,1),1))./(repmat(std(x),size(x,1),1)));
elseif (stdize==2)
    x = ((x - repmat(median(x),size(x,1),1))./(repmat(mad(x),size(x,1),1)));
end

%Actual calculations
obj = Inf;
for i=1:nsamp
    sampindex = randperm(size(x,1));
    restemp = pam(x(sampindex(1:sampsize),:),kclus,vtype,metric);
    if (restemp.obj(1)<obj)
        obj = restemp.obj(1);
        idmed = sampindex(restemp.idmed);
    end
end

%Calculating some extra dissimilarities for output
for i=1:size(x,1)
    for j=1:kclus
        distemp = daisy(x([i idmed(j)],:),vtype,metric);
        disv(i,j) = distemp.disv(1);
    end
    [zz,clu(i)] = min(disv(i,:));
end
mindisv=[];
for j=1:kclus
    clusinf(j,1) = length(find(clu==j));
    clusinf(j,2) = max(disv(find(clu==j),j));
    clusinf(j,3) = mean(disv(find(clu==j),j));
end
for i=1:kclus
    for j=1:kclus
        distemp = daisy(x([idmed(i) idmed(j)],:),vtype,metric);
        mindisv = [mindisv distemp.disv(1)];
    end
end
clusinf(:,4) = clusinf(:,2)/min(mindisv(mindisv~=0));

%Putting things together
result = struct('dysobs',disv,'metric',metric,'number',size(x,1),...
    'idmed',idmed,'ncluv',clu,'obj',obj,'clusinf',clusinf,...
    'sylinf',restemp.sylinf,'x',x);


function varargout = clipPoints3d(points, shape, varargin)
%CLIPPOINTS3D Clip a set of points by a box or other 3d shapes.
%
%   CLIP = clipPoints3d(POINTS, BOX);
%   Returns the set of points which are located inside of the box BOX.
%
%   [CLIP, IND] = clipPoints3d(POINTS, BOX);
%   Also returns the indices of clipped points.
%
%   ... = clipPoints3d(..., 'shape', 'sphere') Specify the shape.
%   Default is 'box'. But it is also possible to use 'sphere' or 'plane'.
%
%   ... = clipPoints3d(..., 'inside', false) returns the set of
%   points outside the shape instead of inside.
%
%   See also
%   points3d, boxes3d, spheres
%

% ------
% Author: David Legland, oqilipo
% E-mail: david.legland@inra.fr
% Created: 2008-10-13, using Matlab 7.4.0.287 (R2007a)
% Copyright 2008-2022 INRA - BIA Nantes - MIAJ Jouy-en-Josas

parser = inputParser;
validStrings = {'box', 'sphere', 'plane'};
addParameter(parser, 'shape', 'box', @(x) any(validatestring(x, validStrings)));
addParameter(parser, 'inside', true, @islogical);
parse(parser,varargin{:});

switch parser.Results.shape
    case 'box'
        LI = clipPointsByBox(points, shape);
    case 'plane'
        LI = clipPointsByPlane(points, shape);
    case 'sphere'
        LI = clipPointsBySphere(points, shape);
end

if parser.Results.inside
    % keep points inside the shape
    ind = find(LI);
else
    % keep points outside the shape
    ind = find(~LI);
end
points = points(ind, :);

% process output arguments
varargout{1} = points;
if nargout == 2
    varargout{2} = ind;
end

    function LI = clipPointsByBox(points, box)
        % get bounding box limits
        xmin = box(1);
        xmax = box(2);
        ymin = box(3);
        ymax = box(4);
        zmin = box(5);
        zmax = box(6);

        % compute indices of points inside visible area
        xOk = points(:,1) >= xmin & points(:,1) <= xmax;
        yOk = points(:,2) >= ymin & points(:,2) <= ymax;
        zOk = points(:,3) >= zmin & points(:,3) <= zmax;

        LI = xOk & yOk & zOk;
    end

    function LI = clipPointsByPlane(points, plane)
        % points inside and on the surface of the sphere
        LI = isBelowPlane(points, plane);
    end

    function LI = clipPointsBySphere(points, sphere)
        % points inside and on the surface of the sphere
        LI = distancePoints3d(points, sphere(1:3)) <= sphere(4);
    end

end


function a = r8bb_set ( n1, n2, ml, mu, a, i, j, value )

%*****************************************************************************80
%
%% R8BB_SET sets an entry of a R8BB matrix.
%
%  Discussion:
%
%    The R8BB storage format is for a border banded matrix.  Such a
%    matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (2*ML+MU+1)*N1 entries of A, using standard LINPACK
%    general band format.  The reason for the factor of 2 in front of
%    ML is to allocate space that may be required if pivoting occurs.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+ML+MU+1)+(J-1)*(2*ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= n1.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than N1-1.
%
%    Input, real A((2*ML+MU+1)*N1+2*N1*N2+N2*N2), the R8BB matrix.
%
%    Input, integer I, J, the row and column of the entry to be set.
%
%    Input, real VALUE, the value to be assigned to the (I,J) entry.
%
%    Output, real A((2*ML+MU+1)*N1+2*N1*N2+N2*N2), the updated R8BB matrix.
%
  if ( i <= 0 | n1+n2 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8BB_SET - Fatal error!\n' );
    fprintf ( 1, 'R8BB_SET - Illegal value of row index I = %d\n', i );
    error ( 'R8BB_SET - Fatal error!' );
  end

  if ( j <= 0 | n1+n2 < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8BB_SET - Fatal error!\n' );
    fprintf ( 1, 'R8BB_SET - Illegal value of column index J = %d\n', j );
    error ( 'R8BB_SET - Fatal error!' );
  end
%
%  The A1 block of the matrix.
%
%  Check for out of band problems.
%
%  Normally, we would check the condition MU < (J-I), but the storage
%  format requires extra entries be set aside in case of pivoting, which
%  means that the condition becomes MU+ML < (J-I).
%
  if ( i <= n1 & j <= n1 )
    if ( mu+ml < (j-i) | ml < (i-j) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8BB_SET - Warning!\n' );
      fprintf ( 1, 'R8BB_SET - Unable to set entry A(%d,%d).\n', i, j );
      error ( 'R8BB_SET - Warning!' );
    else
      ij = (i-j+ml+mu+1)+(j-1)*(2*ml+mu+1);
    end
%
%  The A2 block of the matrix.
%
  elseif ( i <= n1 & n1 < j )
    ij = (2*ml+mu+1)*n1+(j-n1-1)*n1+i;
%
%  The A3 and A4 blocks of the matrix.
%
  elseif ( n1 < i )
    ij = (2*ml+mu+1)*n1+n2*n1+(j-1)*n2+(i-n1);
  end

  a(ij) = value;

  return
end


function coord_w = minc_voxel2world(coord_v,mat,opt);
% Convert coordinates in the voxel space into coordinates in the world
% space.
%
% SYNTAX:
% COORD_W = MINC_VOXEL2WORLD(COORD_V,MAT,OPT)
%
% _________________________________________________________________________
% INPUTS:
%
% COORD_V
%       (matrix N*3) each row is a vector of 3D coordinates in voxel space.
%
% MAT
%       (matrix 4*4) an affine transformation from voxel to world
%       coordinates. See the help of NIAK_READ_VOL for more infos. It is
%       generally the HDR.INFO.MAT field of the header of a volume file.
%
% OPT
%       (structure, optional) with the following fields :
%
%       FLAG_ZERO
%           (boolean, default false) if FLAG_ZERO is true, voxel
%           coordinates start from 1 (default behaviour in matlab),
%           otherwise they start from 0 (default behaviour in C/C++ or
%           MINC).
%
% _________________________________________________________________________
% OUTPUTS:
%
% COORD_W
%       (matrix N*3) each row is a vector of 3D coordinates in world space.
%
% _________________________________________________________________________
% SEE ALSO:
% MINC_READ, MINC_WRITE, MINC_VOXEL2WORLD, MINC_WORLD2VOXEL
%
% _________________________________________________________________________
% COMMENTS:
%
% Copyright (c) Pierre Bellec, Centre de recherche de l'institut de
% gériatrie de Montréal, Département d'informatique et de recherche
% opérationnelle, Université de Montréal, 2013.
% See licensing information in the code.
% Keywords : affine transformation, coordinates

% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.
if nargin < 3
    flag_zero = false;
else
    if isfield(opt,'flag_zero')
        flag_zero = opt.flag_zero;
    else
        flag_zero = false;
    end
end
if flag_zero
    coord_w = [coord_v ones([size(coord_v,1) 1])]*(mat');
else
    coord_w = [coord_v-1 ones([size(coord_v,1) 1])]*(mat');
end
coord_w = coord_w(:,1:3);


function [U,Sigma,V,numiter,out]  = SVT(n,Omega,b,tau,delta,maxiter,tol,EPS)
% [U,Sigma,V,numiter,output]  = SVT(n,Omega,b,tau,delta,maxiter,tol,EPS)
%
% Finds the minimum of   tau ||X||_* + .5 || X ||_F^2
%
% subject to P_Omega(X) = P_Omega(M)
%
% using linear Bregman iterations
%
% Usage:  [U,S,V,numiter]  = SVT(n,Omega,b,delta,maxiter,tol)
%
% Inputs:
%
%   n - size of the matrix X assumed n(1) by n(2). If n is a single integer, it
% is understood that n(1) = n(2).
%
%   Omega - set of observed entries.  Should be linearly indexed.
%
%   b - data vector of the form M(Omega)
%
%   tau - parameter defining the objective functional
%
%   delta - step size.  Choose delta less than 2 to be safe but
%       conservative; choose delta closer to n(1)*n(2)/length(Omega)
%       to be riskier (i.e. algorithm may diverge)
%
%   maxiter - maximum number of iterations
%
%   tol - stopping criteria (default: 1e-4)
%
%   EPS - noise constraint.  This relaxes the constraints, so that they
%       are now of the form | X(i,j) - M(i,j) | <= EPS,
%       for all indices (i,j) in omega.  Default: 0
%
% Outputs: matrix X stored in SVD format X = U*diag(S)*V'
%
%   U - n1xr left singular vectors
%
%   S - rx1 singular values
%
%   V - n2xr right singular vectors
%
%   numiter - number of iterations to achieve convergence
%
%   output - a structure with data from each iteration.  Includes:
%       output.nuclearNorm  - nuclear norm of current iterate
%       output.rank         - rank of current iterate
%       output.time         - time taken for one iteraration
%       output.residual     - the relative residual, norm(x-b)/norm(b)
% Description:
% Reference:
%
%    Cai, Candes and Shen
%    A singular value thresholding algorithm for matrix completion
%    Submitted for publication, October 2008.
%
%    See also more general code as part of the TFOCS package,
%    available at tfocs.stanford.edu as of November 2010.
%
% Written by: Emmanuel Candes
% Email: emmanuel@acm.caltech.edu
% Created: October 2008
% Efficient mex-file and PROPACK version: Stephen Becker, Nov 2008
% Modified: Stephen Becker, March 2009
% Modified: Stephen Becker, May 2009  works with complex numbers
% Modified: Farshad Harirchi and Stephen Becker, April 2011, fixing a bug.

global VERBOSE
if isempty(VERBOSE)
    % -- feel free to change these 'verbosity' parameters
    % VERBOSE = false;
    VERBOSE = 1;    % a little bit of output
    % VERBOSE = 2;    % even more output
end

time1 = cputime;
if nargin < 8 || isempty(EPS)
    EPS = false;
end
if nargin < 7 || isempty(tol)
    tol = 1e-4;
end
if nargin < 6 || isempty(maxiter)
    maxiter = 500;
end

if length(n) == 1,
    n1 = n(1); n2 = n1;
elseif length(n) == 2,
    n1 = n(1); n2 = n(2);
end
if n1*n2 < 100*100, SMALLSCALE = true; else SMALLSCALE = false; end

m = length(Omega); [temp,indx] = sort(Omega);
% simpler: sort b also
incre = 5;
normb = norm(b);

[i, j] = ind2sub([n1,n2], Omega);
USE_SLOW_UPDATE     = false;
if EPS
    % with inequality constraints, should take delta = delta/sqrt(2) at
    % least, or delta = delta/2
    delta = delta/sqrt(2);
%     delta = delta/2; tau = 2*tau;
%     y1 = max(b-EPS,0); y2 = max(-b-EPS,0); % doesn't work well
    y1 = max(b,0); y2 = max(-b,0);
    Y = sparse(i,j,y1-y2,n1,n2,m);
    normProjM = normest(Y,1e-2);
    k0 = ceil(tau/(delta*normProjM));
    y1 = k0*delta*y1;
    y2 = k0*delta*y2;
    try
        updateSparse(Y,y1-y2,indx);
    catch
        l = lasterror;
        if strcmpi( l.identifier, 'MATLAB:UndefinedFunction')
            % mex file not installed, so do this instead:
            [indx_i,indx_j,s] = find(Y);
            Y = updateSparse_slow(Y,y1-y2,indx,indx_i,indx_j);
            USE_SLOW_UPDATE     = true;
        else
            % some other error (unexpected)
            rethrow(lasterror)
        end
    end
else
    Y = sparse(i,j,b,n1,n2,m);
    normProjM = normest(Y,1e-2);
    k0 = ceil(tau/(delta*normProjM));
    normb = norm(b);
    y = k0*delta*b; % kicking by k0 steps
    try
        updateSparse(Y,y,indx);
    catch
        l = lasterror;
        if strcmpi( l.identifier, 'MATLAB:UndefinedFunction')
            % mex file not installed, so do this instead:
            [indx_i,indx_j,s] = find(Y);
            Y = updateSparse_slow(Y,y,indx,indx_i,indx_j);
            USE_SLOW_UPDATE     = true;
        else
            % some other error (unexpected)
            rethrow(lasterror)
        end
    end

end
r = 0;

out.residual = zeros(maxiter,1);
out.rank= zeros(maxiter,1);
out.time = zeros(maxiter,1);
out.nuclearNorm = zeros(maxiter,1);

% What the best way to multiply a sparse matrix?
[forwardType, transposeType] = findBestMultiply(Y,.2);

if VERBOSE==1, fprintf('\nIteration:   '); end
for k = 1:maxiter,
    if VERBOSE==1, fprintf('\b\b\b\b%4d',k);  end
    s = r + 1;

    rInc = 4;  % make this larger for more accuracy
    %if tol < 1e-4  && relRes < 1e-1
    %rInc = rInc + max( round(log10( abs(1e-1/relRes) )), 5 );
    %end
    s = min( [r + rInc, n1, n2] );

    if SMALLSCALE
        [U,Sigma,V] = svd(full(Y),'econ');
    else
        % Make routines for multiplying by a sparse matrix
        Yt = Y';
        switch forwardType
            case 1, Yforward = @(x) Y*x;
            case 2, Yforward = @(x) Yt'*x;
            case 3, Yforward = @(x) smvp(Y,x);
        end
        switch transposeType
            case 1, Ytranspose = @(x) Yt*x;
            case 2, Ytranspose = @(x) Y'*x;
            case 3, Ytranspose = @(x) smvp(Yt,x);
        end
        OK = 0;
        while ~OK
            opts = [];
            if ~isreal(b), opts.eta = 1e-16; end
            [U,Sigma,V] = lansvd(Yforward,Ytranspose,n1,n2,s,'L',opts);
            %[U,Sigma,V] = lansvd(Y,s,'L');
            OK = (Sigma(s,s) <= tau) || ( s == min(n1,n2) );
            s = min(s + incre, min(n1,n2));
        end
    end

    sigma = diag(Sigma); r = sum(sigma > tau);
    U = U(:,1:r); V = V(:,1:r); sigma = sigma(1:r) - tau; Sigma = diag(sigma);

    x = XonOmega(U*diag(sigma),V,Omega);
    eTime = cputime - time1;
    if VERBOSE == 2
        fprintf('iteration %4d, rank is %2d, rel. residual is %.1e\n',k,r,norm(x-b)/normb);
    end
    relRes = norm(x-b)/normb;
    out.residual(k) = relRes;
    out.time(k) = eTime;
    out.rank(k) = r;
    out.nuclearNorm(k) = sum(sigma);

    time1 = cputime;

    if (relRes < tol)
        break
    end
    if EPS && norm(x-b,'inf') < 2*EPS
        break
    end
    if (norm(x-b)/normb > 1e5)
        disp('Divergence!');
        break
    end

    if EPS
        y1 = max( y1 + delta*( -(x-b) - EPS), 0 );
        y2 = max( y2 + delta*(  (x-b) - EPS), 0 );
        if USE_SLOW_UPDATE
            % mex file not installed, so do this instead:
            Y = updateSparse_slow(Y,y1-y2,indx,indx_i,indx_j);
        else
            updateSparse(Y,y1-y2,indx);
        end
    else
        y = y + delta*(b-x);
        if USE_SLOW_UPDATE
            % mex file not installed, so do this instead:
            Y = updateSparse_slow(Y,y,indx,indx_i,indx_j);
        else
            updateSparse(Y,y,indx);
        end
    end
end

if VERBOSE==1, fprintf('\n'); end
numiter = k;
out.residual = out.residual(1:k,:);
out.time = out.time(1:k,:);
out.rank= out.rank(1:k,:);
out.nuclearNorm= out.nuclearNorm(1:k,:);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Jellyfish Example Courtesy of Alexander P. Hoover, PhD
%
% Converted from IBAMR: 1/16/2018 by NAB.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Make_Jelly_Geometry()

close all;
clear all;
L = 8;                              % height of computational domain (m) for keeping desired resolution
Lh = 10;                            % actual height of computational domain (m) (MATCHES INPUT2D)
Lw = 3;                             % width of computational domain (m) (MATCHES INPUT2D)
N = 768;                            % number of Cartesian grid meshwidths at the finest level of the AMR grid
dx = L/N;                           % Cartesian mesh width (m)
ds = dx/2;

a=.5;                               % bell radius (semi-minor axis, horizontal axis, note width=2a)
b=.75;                              % bell semi-major axis
d=-0.25;
factor_a=.8;

F=1e5; %5e0

theta=zeros(1000,1);
theta_lim=asin(d/b);
theta_test=pi/2;

x_points=zeros(1000,1);
z_points=zeros(1000,1);
id_points=zeros(1000,1);
offset = 0;

kappa_spring = 1e7; %1e5               % spring constant (Newton)
kappa_beam = 2.5e5; %1e5    %5e3              % beam stiffness constant (Newton m^2)
%kappa_beam_flexible = kappa_beam/5;   % beam stiffness constant (Newton m^2)
kappa_target = kappa_spring;           % target point penalty spring constant (Newton)

c=0;
while(theta_test<(pi-theta_lim))
    c=c+1;
    theta(c)=theta_test;

    x_points(c)=a*cos(theta(c));
    z_points(c)=b*sin(theta(c));
    id_points(c)=c-1;

    theta_test=ds/((a*sin(theta(c)))^(2)+(b*cos(theta(c)))^(2))^(.5)+theta(c);

end

c_stiff=c;

npts=2*c-1;
npts_wing=floor(npts/2);
npts_musc=floor(npts_wing/4);

for j=(c+1):(npts)
    x_points(j)=-1*x_points(j-c+1);
    z_points(j)=z_points(j-c+1);
    id_points(j)=j-1;
end

mesh_name = 'jelly';
xShift = 1.5;
yShift = 2;

x_points=x_points(1:npts)+xShift;
z_points=z_points(1:npts)+yShift;
it_points=id_points(1:npts);

plot(x_points(:),z_points(:),'*'); hold on;
axis([0 8 0 8])

% Lag Pts to Mess up Flow At Edge
xBlock = ds:4*ds:Lw-ds;
yBlock = (Lh-5*ds)*ones(1,length(xBlock))+ds;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Print .vertex information
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

vertex_fid = fopen([mesh_name num2str(N) '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', npts + npts_musc*2 + length(xBlock));
    lag_ct = 0;

    %
    % bell
    %
    for j=1:npts
        fprintf(vertex_fid, '%1.16e %1.16e\n', x_points(j), z_points(j));
        lag_ct = lag_ct + 1;
    end

    %
    % muscles
    %
    for s = 1:npts_musc
        fprintf(vertex_fid, '%1.16e %1.16e\n', x_points(npts_wing+1-npts_musc+s), z_points(npts_wing+1-npts_musc+s));
        plot(x_points(npts_wing+1-npts_musc+s),z_points(npts_wing+1-npts_musc+s),'r*'); hold on;
        lag_ct = lag_ct + 1;
    end
    for s = 1:npts_musc
        fprintf(vertex_fid, '%1.16e %1.16e\n', x_points(npts-npts_musc+s), z_points(npts-npts_musc+s));
        plot(x_points(npts-npts_musc+s),z_points(npts-npts_musc+s),'r*'); hold on;
        lag_ct = lag_ct + 1;
    end

    for ii=1:length(xBlock)
        fprintf(vertex_fid, '%1.16e %1.16e\n', xBlock(ii), yBlock(ii));
    end

fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Print .spring information
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

spring_fid = fopen([mesh_name num2str(N) '.spring'], 'w');

    npts_spring_type1=npts-1;

    fprintf(spring_fid, '%d\n', npts-1 + npts_musc);

    fprintf('\nNumber of springs before muscles: %d \n\n',npts-1)

    factor = 1;%ds^2/ds;

    %
    % bell
    %
    for s = 1:c-1
        resting=sqrt((x_points(s)-x_points(s+1))^(2)+(z_points(s)-z_points(s+1))^(2));
        fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n', id_points(s)+1, id_points(s+1)+1, kappa_spring*ds/(ds^2)*factor, resting, 1);
    end
    for s = c+1:npts-1
        resting=sqrt((x_points(s)-x_points(s+1))^(2)+(z_points(s)-z_points(s+1))^(2));
        fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n', id_points(s)+1, id_points(s+1)+1, kappa_spring*ds/(ds^2)*factor, resting, 1);
    end
    resting=sqrt((x_points(1)-x_points(c+1))^(2)+(z_points(1)-z_points(c+1))^(2));
    fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n', id_points(1)+1, id_points(c+1)+1, kappa_spring*ds/(ds^2)*factor, resting, 1);

    %
    % muscles
    %
    for s = 1:npts_musc
        fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n',npts+s-1+1, npts+s+npts_musc-1+1, F, 0, 1);
    end

    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Print .nonInv_beam information
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

beam_fid = fopen([mesh_name num2str(N) '.nonInv_beam'], 'w');

    fprintf(beam_fid, '%d\n', npts-2);

    factor=1;% = (ds^4)/ds;

    for s = 2:c-1
        C1 = x_points(s-1)+x_points(s+1)-2*x_points(s);
        C2 = z_points(s-1)+z_points(s+1)-2*z_points(s);
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(s-1)+1, id_points(s)+1, id_points(s+1)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);
    end
    for s = c+2:npts-1
        C1 = x_points(s-1)+x_points(s+1)-2*x_points(s);
        C2 = z_points(s-1)+z_points(s+1)-2*z_points(s);
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(s-1)+1, id_points(s)+1, id_points(s+1)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);
    end

    C1 = x_points(c+2)+x_points(1)-2*x_points(c+1);
    C2 = z_points(c+2)+z_points(1)-2*z_points(c+1);
    fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(c+2)+1, id_points(c+1)+1, id_points(1)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);

    C1 = x_points(c+1)+x_points(2)-2*x_points(1);
    C2 = z_points(c+1)+z_points(2)-2*z_points(1);
    fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(c+1)+1, id_points(1)+1, id_points(2)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);

    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PRINT TARGET POINTS!!!
%
% print target points (flow blocker along edge)
k_Target = 2.5e6;
nBefore = lag_ct; % Counts pts in jellyfish for bookkeeping for .target file
struct_name = ['jelly' num2str(N)];
print_Lagrangian_Target_Pts(xBlock,k_Target,struct_name,nBefore)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called 'struct_name'.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,nBefore)

    N = length(xLag);
    Nstart = nBefore+1;
    Nend = nBefore+N;

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = Nstart:Nend
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);


function [s, err_mse, iter_time]=greed_omp_pinv(x,A,m,varargin)
% greed_omp_pinv: Orthogonal Matching Pursuit algorithm based on matlab
% pinv solution. Use not recommended, provided for reference only!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Usage
% [s, err_mse, iter_time]=greed_omp_pinv(x,P,m,'option_name','option_value')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input
%   Mandatory:
%               x   Observation vector to be decomposed
%               P   Either:
%                       1) An nxm matrix (n must be dimension of x)
%                       2) A function handle (type "help function_format"
%                          for more information)
%                          Also requires specification of P_trans option.
%                       3) An object handle (type "help object_format" for
%                          more information)
%               m   length of s
%
%   Possible additional options:
%   (specify as many as you want using 'option_name','option_value' pairs)
%   See below for explanation of options:
%__________________________________________________________________________
%   option_name    |     available option_values                | default
%--------------------------------------------------------------------------
%   stopCrit       | M, corr, mse, mse_change                   | M
%   stopTol        | number (see below)                         | n/4
%   P_trans        | function_handle (see below)                |
%   maxIter        | positive integer (see below)               | n
%   verbose        | true, false                                | false
%   start_val      | vector of length m                         | zeros
%
%   Available stopping criteria :
%               M           -   Extracts exactly M = stopTol elements.
%               corr        -   Stops when maximum correlation between
%                               residual and atoms is below stopTol value.
%               mse         -   Stops when mean squared error of residual
%                               is below stopTol value.
%               mse_change  -   Stops when the change in the mean squared
%                               error falls below stopTol value.
%
%   stopTol: Value for stopping criterion.
%
%   P_trans: If P is a function handle, then P_trans has to be specified and
%            must be a function handle.
%
%   maxIter: Maximum of allowed iterations.
%
%   verbose: Logical value to allow algorithm progress to be displayed.
%
%   start_val: Allows algorithms to start from partial solution.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Outputs
%    s              Solution vector
%    err_mse        Vector containing mse of approximation error for each
%                   iteration
%    iter_time      Vector containing times for each iteration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Description
%   greed_omp_pinv performs a greedy signal decomposition.
%   In each iteration a new element is selected depending on the inner
%   product between the current residual and columns in P.
%   The non-zero elements of s are approximated by orthogonally projecting
%   x onto the selected elements in each iteration.
%   This implementation uses matlab's pinv command to calculate the required
%   projection. This is very slow and only works for small matrices. It is
%   therefore only provided for reference.
%
% See Also
%   greed_qr, greed_omp_chol, greed_omp_cg, greed_omp_cgp,
%   greed_omp_linsolve, greed_gp, greed_nomp
%
% Copyright (c) 2007 Thomas Blumensath
%
% The University of Edinburgh
% Email: thomas.blumensath@ed.ac.uk
% Comments and bug reports welcome
%
% This file is part of sparsity Version 0.1
% Created: April 2007
%
% Part of this toolbox was developed with the support of EPSRC Grant
% D000246/1
%
% Please read COPYRIGHT.m for terms and conditions.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                    Default values and initialisation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n1 n2]=size(x);
if n2 == 1
    n=n1;
elseif n1 == 1
    x=x';
    n=n2;
else
   display('x must be a vector.');
   return
end

sigsize     = x'*x/n;
initial_given=0;
err_mse     = [];
iter_time   = [];
STOPCRIT    = 'M';
STOPTOL     = ceil(n/4);
MAXITER     = n;
verbose     = false;
s_initial   = zeros(m,1);
vectnfact   = ones(m,1);

if verbose
   display('Initialising...')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           Output variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

switch nargout
    case 3
        comp_err=true;
        comp_time=true;
    case 2
        comp_err=true;
        comp_time=false;
    case 1
        comp_err=false;
        comp_time=false;
    case 0
        error('Please assign output variable.')
    otherwise
        error('Too many output arguments specified')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Look through options
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Put option into nice format
Options={};
OS=nargin-3;
c=1;
for i=1:OS
    if isa(varargin{i},'cell')
        CellSize=length(varargin{i});
        ThisCell=varargin{i};
        for j=1:CellSize
            Options{c}=ThisCell{j};
            c=c+1;
        end
    else
        Options{c}=varargin{i};
        c=c+1;
    end
end
OS=length(Options);
if rem(OS,2)
   error('Something is wrong with argument name and argument value pairs.')
end

for i=1:2:OS
   switch Options{i}
        case {'stopCrit'}
            if (strmatch(Options{i+1},{'M'; 'corr'; 'mse'; 'mse_change'},'exact'));
                STOPCRIT    = Options{i+1};
            else error('stopCrit must be char string [M, corr, mse, mse_change]. Exiting.'); end
        case {'stopTol'}
            if isa(Options{i+1},'numeric') ; STOPTOL     = Options{i+1};
            else error('stopTol must be number. Exiting.'); end
        case {'P_trans'}
            if isa(Options{i+1},'function_handle'); Pt = Options{i+1};
            else error('P_trans must be function _handle. Exiting.'); end
        case {'maxIter'}
            if isa(Options{i+1},'numeric'); MAXITER     = Options{i+1};
            else error('maxIter must be a number. Exiting.'); end
        case {'verbose'}
            if isa(Options{i+1},'logical'); verbose     = Options{i+1};
            else error('verbose must be a logical. Exiting.'); end
        case {'vecNormFac'}
            if isa(Options{i+1},'numeric')& length(Options{i+1}) == m , vectnfact = Options{i+1};
            else error('verbose must be a logical. Exiting.'); end
        case {'start_val'}
            if isa(Options{i+1},'numeric') & length(Options{i+1}) == m ;
                s_initial     = Options{i+1};
                initial_given=1;
            else error('start_val must be a vector of length m. Exiting.'); end
        otherwise
            error('Unrecognised option. Exiting.')
   end
end

if strcmp(STOPCRIT,'M')
    maxM=STOPTOL;
else
    maxM=MAXITER;
end

if nargout >=2
    err_mse = zeros(maxM,1);
end
if nargout ==3
    iter_time = zeros(maxM,1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Make P and Pt functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if          isa(A,'float')      P =@(z) A*z;  Pt =@(z) A'*z;
elseif      isobject(A)         P =@(z) A*z;  Pt =@(z) A'*z;
elseif      isa(A,'function_handle')
    try
        if          isa(Pt,'function_handle'); P=A;
        else        error('If P is a function handle, Pt also needs to be a function handle. Exiting.'); end
    catch error('If P is a function handle, Pt needs to be specified. Exiting.'); end
else        error('P is of unsupported type. Use matrix, function_handle or object. Exiting.'); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Do we start from zero or not?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if initial_given ==1;
    IN          = find(s_initial);
    s=zeros(m,1);
    if isa(A,'function_handle') || isobject(A)
         Pmat=zeros(n,length(IN));
         for i=1:length(IN)
             mask=zeros(m,1);
             mask(IN(i))=1;
             Pmat(:,i)=P(mask);
         end
         s(IN)=pinv(Pmat)*x;
     else
         s(IN)=pinv(A(:,IN))*x;
     end
    Residual    = x-P(s);
    oldERR      = Residual'*Residual/n;

else
    IN          = [];
    Residual    = x;
    s           = s_initial;
    sigsize     = x'*x/n;
    oldERR      = sigsize;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 Random Check to see if dictionary is normalised
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%         mask=zeros(m,1);
%         mask(ceil(rand*m))=1;
%         nP=norm(P(mask));
%         if abs(1-nP)>1e-3;
%             display('Dictionary appears not to have unit norm columns.')
%         end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Main algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if verbose
   display('Main iterations...')
end
tic
t=0;
DR=Pt(Residual).*vectnfact;
done = 0;
iter=1;
while ~done
     DR(IN)=0;
     [v I]=max(abs(DR));
     IN=[IN I];
     if isa(A,'function_handle') || isobject(A)
         Pmat=zeros(n,length(IN));
         for i=1:length(IN)
             mask=zeros(m,1);
             mask(IN(i))=1;
             Pmat(:,i)=P(mask);
         end
         s(IN)=pinv(Pmat)*x;
     else
         s(IN)=pinv(A(:,IN))*x;
     end
         Residual=x-P(s);
         DR=Pt(Residual).*vectnfact;

    ERR=Residual'*Residual/n;
     if comp_err
         err_mse(iter)=ERR;
     end

     if comp_time
         iter_time(iter)=toc;
     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Are we done yet?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     if strcmp(STOPCRIT,'M')
         if iter >= STOPTOL
             done =1;
         elseif verbose && toc-t>10
            display(sprintf('Iteration %i. --- %i iterations to go',iter ,STOPTOL-iter))
            t=toc;
         end
    elseif strcmp(STOPCRIT,'mse')
         if comp_err
            if err_mse(iter)<STOPTOL;
                done = 1;
            elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse',iter ,err_mse(iter)))
                t=toc;
            end
         else
             if ERR<STOPTOL;
                done = 1;
             elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse',iter ,ERR))
                t=toc;
             end
         end
     elseif strcmp(STOPCRIT,'mse_change') && iter >=2
         if comp_err && iter >=2
              if ((err_mse(iter-1)-err_mse(iter))/sigsize <STOPTOL);
                done = 1;
             elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse change',iter ,(err_mse(iter-1)-err_mse(iter))/sigsize ))
                t=toc;
             end
         else
             if ((oldERR - ERR)/sigsize < STOPTOL);
                done = 1;
             elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i mse change',iter ,(oldERR - ERR)/sigsize))
                t=toc;
             end
         end
     elseif strcmp(STOPCRIT,'corr')
          if max(abs(DR)) < STOPTOL;
             done = 1;
          elseif verbose && toc-t>10
                display(sprintf('Iteration %i. --- %i corr',iter ,max(abs(DR))))
                t=toc;
          end
     end

    % Also stop if residual gets too small or maxIter reached
     if comp_err
         if err_mse(iter)<1e-16
             display('Stopping. Exact signal representation found!')
             done=1;
         end
     else

         if iter>1
             if ERR<1e-16
                 display('Stopping. Exact signal representation found!')
                 done=1;
             end
         end
     end

     if iter >= MAXITER
         display('Stopping. Maximum number of iterations reached!')
         done = 1;
     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                    If not done, take another round
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     if ~done
        iter=iter+1;
        oldERR=ERR;
     end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                  Only return as many elements as iterations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargout >=2
    err_mse = err_mse(1:iter);
end
if nargout ==3
    iter_time = iter_time(1:iter);
end

if verbose
   display('Done')
end

% Change history
%
% 8 of Februray: Algo does no longer stop if dictionary is not normaliesd.