function [eta, Heta, inner_it, stop_tCG] ...
                 = tCG(problem, x, grad, eta, Delta, options, storedb, key)
% tCG - Truncated (Steihaug-Toint) Conjugate-Gradient method
% minimize <eta,grad> + .5*<eta,Hess(eta)>
% subject to <eta,eta>_[inverse precon] <= Delta^2
%
% See also: trustregions

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
%
% Change log:
%
%   NB Feb. 12, 2013:
%       We do not project r back to the tangent space anymore: it was not
%       necessary, and as of Manopt 1.0.1, the proj operator does not
%       coincide with this notion anymore.
%
%   NB April 3, 2013:
%       tCG now also returns Heta, the Hessian at x along eta. Additional
%       esthetic modifications.
%
%   NB Dec. 2, 2013:
%       If options.useRand is activated, we now make sure the preconditio-
%       ner is not used, as was originally intended in GenRTR. In time, we
%       may want to investigate whether useRand can be modifed to work well
%       with preconditioning too.
%
%   NB Jan. 9, 2014:
%       Now checking explicitly for model decrease at each iteration. The
%       first iteration is a Cauchy point, which necessarily realizes a
%       decrease of the model cost. If a model increase is witnessed
%       (which is theoretically impossible if a linear operator is used for
%       the Hessian approximation), then we return the previous eta. This
%       ensures we always achieve at least the Cauchy decrease, which
%       should be sufficient for convergence.
%
%   NB Feb. 17, 2015:
%       The previous update was in effect verifying that the current eta
%       performed at least as well as the first eta (the Cauchy step) with
%       respect to the model cost. While this is an acceptable strategy,
%       the documentation (and the original intent) was to ensure a
%       monotonic decrease of the model cost at each new eta. This is now
%       the case, with the added line: "model_value = new_model_value;".
%
%   NB April 3, 2015:
%       Works with the new StoreDB class system.

% All terms involving the trust-region radius will use an inner product
% w.r.t. the preconditioner; this is because the iterates grow in
% length w.r.t. the preconditioner, guaranteeing that we will not
% re-enter the trust-region.
%
% The following recurrences for Prec-based norms and inner
% products come from [CGT2000], pg. 205, first edition.
% Below, P is the preconditioner.
%
% <eta_k,P*delta_k> =
%          beta_k-1 * ( <eta_k-1,P*delta_k-1> + alpha_k-1 |delta_k-1|^2_P )
% |delta_k|^2_P = <r_k,z_k> + beta_k-1^2 |delta_k-1|^2_P
%
% therefore, we need to keep track of
% 1)   |delta_k|^2_P
% 2)   <eta_k,P*delta_k> = <eta_k,delta_k>_P
% 3)   |eta_k  |^2_P
%
% initial values are given by:
%    |delta_0|_P = <r,z>
%    |eta_0|_P   = 0
%    <eta_0,delta_0>_P = 0
% because we take eta_0 = 0 (if useRand = false).
%
% [CGT2000] Conn, Gould and Toint: Trust-region methods, 2000.

inner = problem.M.inner;
lincomb = problem.M.lincomb;

theta = options.theta;
kappa = options.kappa;

if ~options.useRand % and therefore, eta == 0
    Heta = problem.M.zerovec(x);
    r = grad;
    e_Pe = 0;
else % and therefore, no preconditioner
    % eta (presumably) ~= 0 was provided by the caller.
    Heta = getHessian(problem, x, eta, storedb, key);
    r = lincomb(x, 1, grad, 1, Heta);
    e_Pe = inner(x, eta, eta);
end
r_r = inner(x, r, r);
norm_r = sqrt(r_r);
norm_r0 = norm_r;

% Precondition the residual.
if ~options.useRand
    z = getPrecon(problem, x, r, storedb, key);
else
    z = r;
end

% Compute z'*r.
z_r = inner(x, z, r);
d_Pd = z_r;

% Initial search direction.
delta  = lincomb(x, -1, z);
if ~options.useRand % and therefore, eta == 0
    e_Pd = 0;
else % and therefore, no preconditioner
    e_Pd = inner(x, eta, delta);
end

% If the Hessian or a linear Hessian approximation is in use, it is
% theoretically guaranteed that the model value decreases strictly
% with each iteration of tCG. Hence, there is no need to monitor the model
% value. But, when a nonlinear Hessian approximation is used (such as the
% built-in finite-difference approximation for example), the model may
% increase. It is then important to terminate the tCG iterations and return
% the previous (the best-so-far) iterate. The variable below will hold the
% model value.
model_fun = @(eta, Heta) inner(x, eta, grad) + .5*inner(x, eta, Heta);
if ~options.useRand
    model_value = 0;
else
    model_value = model_fun(eta, Heta);
end

% Pre-assume termination because j == end.
stop_tCG = 5;

% Begin inner/tCG loop.
j = 0;
for j = 1 : options.maxinner

    % This call is the computationally expensive step.
    Hdelta = getHessian(problem, x, delta, storedb, key);

    % Compute curvature (often called kappa).
    d_Hd = inner(x, delta, Hdelta);

    % Note that if d_Hd == 0, we will exit at the next "if" anyway.
    alpha = z_r/d_Hd;
    % <neweta,neweta>_P =
    % <eta,eta>_P + 2*alpha*<eta,delta>_P + alpha*alpha*<delta,delta>_P
    e_Pe_new = e_Pe + 2.0*alpha*e_Pd + alpha*alpha*d_Pd;

    if options.debug > 2,
        fprintf('DBG:   (r,r)  : %e\n', r_r);
        fprintf('DBG:   (d,Hd) : %e\n', d_Hd);
        fprintf('DBG:   alpha  : %e\n', alpha);
    end

    % Check against negative curvature and trust-region radius violation.
    % If either condition triggers, we bail out.
    if d_Hd <= 0 || e_Pe_new >= Delta^2,
        % want
        %  ee = <eta,eta>_prec,x
        %  ed = <eta,delta>_prec,x
        %  dd = <delta,delta>_prec,x
        tau = (-e_Pd + sqrt(e_Pd*e_Pd + d_Pd*(Delta^2-e_Pe))) / d_Pd;
        if options.debug > 2,
            fprintf('DBG:     tau  : %e\n', tau);
        end
        eta  = lincomb(x, 1,  eta, tau,  delta);

        % If only a nonlinear Hessian approximation is available, this is
        % only approximately correct, but saves an additional Hessian call.
        Heta = lincomb(x, 1, Heta, tau, Hdelta);

        % Technically, we may want to verify that this new eta is indeed
        % better than the previous eta before returning it (this is always
        % the case if the Hessian approximation is linear, but I am unsure
        % whether it is the case or not for nonlinear approximations.)
        % At any rate, the impact should be limited, so in the interest of
        % code conciseness (if we can still hope for that), we omit this.

        if d_Hd <= 0,
            stop_tCG = 1;     % negative curvature
        else
            stop_tCG = 2;     % exceeded trust region
        end
        break;
    end

    % No negative curvature and eta_prop inside TR: accept it.
    e_Pe = e_Pe_new;
    new_eta  = lincomb(x, 1,  eta, alpha,  delta);

    % If only a nonlinear Hessian approximation is available, this is
    % only approximately correct, but saves an additional Hessian call.
    new_Heta = lincomb(x, 1, Heta, alpha, Hdelta);

    % Verify that the model cost decreased in going from eta to new_eta. If
    % it did not (which can only occur if the Hessian approximation is
    % nonlinear or because of numerical errors), then we return the
    % previous eta (which necessarily is the best reached so far, according
    % to the model cost). Otherwise, we accept the new eta and go on.
    new_model_value = model_fun(new_eta, new_Heta);
    if new_model_value >= model_value
        stop_tCG = 6;
        break;
    end

    eta = new_eta;
    Heta = new_Heta;
    model_value = new_model_value; %% added Feb. 17, 2015

    % Update the residual.
    r = lincomb(x, 1, r, alpha, Hdelta);

    % Compute new norm of r.
    r_r = inner(x, r, r);
    norm_r = sqrt(r_r);

    % Check kappa/theta stopping criterion.
    % Note that it is somewhat arbitrary whether to check this stopping
    % criterion on the r's (the gradients) or on the z's (the
    % preconditioned gradients). [CGT2000], page 206, mentions both as
    % acceptable criteria.
    if j >= options.mininner && norm_r <= norm_r0*min(norm_r0^theta, kappa)
        % Residual is small enough to quit
        if kappa < norm_r0^theta,
            stop_tCG = 3;  % linear convergence
        else
            stop_tCG = 4;  % superlinear convergence
        end
        break;
    end

    % Precondition the residual.
    if ~options.useRand
        z = getPrecon(problem, x, r, storedb, key);
    else
        z = r;
    end

    % Save the old z'*r.
    zold_rold = z_r;
    % Compute new z'*r.
    z_r = inner(x, z, r);

    % Compute new search direction.
    beta = z_r/zold_rold;
    delta = lincomb(x, -1, z, beta, delta);

    % Update new P-norms and P-dots [CGT2000, eq. 7.5.6 & 7.5.7].
    e_Pd = beta*(e_Pd + alpha*d_Pd);
    d_Pd = z_r + beta*beta*d_Pd;

end  % of tCG loop
inner_it = j;

end


function sR=icassoExp(sR)
%function sR=icassoExp(sR)
%
%PURPOSE
%
%To prepare Icasso result structure for exploratory analysis, i.e.,
%to compute (dis)similarity matrix, clustering, and projection.
%
%EXAMPLES OF BASIC USAGE
%
%First we produce an Icasso result structure...
%
%   load megdata
%   sR=icassoEst('randinit',megdata,15,'lastEig',20);
%
%The following command performs the default Icasso clustering
%procedure:
%
%   sR=icassoExp(sR);
%
%The next step would be to return results launch the visualizations
%of the results: See icassoShow, icassoViz, and icassoResult.
%
%You can customize the Icasso procedure by using this script
%as a model but changing the optional input parameters. See also
%icassoCluster and icassoProjection.
%
%DETAILS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%icassoExp performs two functions icassoCluster and icassoProjection:
%
%Step 1
%
%Cluster the estimates
%
%Details:
% 1. Compute similarities (S) between the estimates
% 2. Store the similarity matrix S in field sR.cluster.similarity.
% 3. Record the similarity function into sR.cluster.simfcn, in
%    this case, 'abscorr'. Other possibility is 'power'. You can
%    also specify an explicit similarity matrix
% 4. Compute dissimilarities D from S using function
%    sim2dis (simply D=1-S). If you want something else, you can give another
%    function name
% 5. Compute hierarchical clustering according to the readily computed
%    dissimilarities using average-linkage strategy. Other
%    possibilities: 'CL' (comptele link) 'SL' (single-link)
% 6. Store the clustering into field sR.cluster.partition and the
%    strategy in sR.cluster.strategy
% 7. Compute a clustering solution validity index, R-index up to L
%    clusters (according to the partition and dissimilarities D)
%    ('rdim' sets  L equal to the (reduced) data dimension)
% 9. Store the index into field sR.cluster.index.R
%
%   sR=icassoCluster(sR,'strategy','AL','simfcn','abscorr','s2d','sim2dis','L','rdim');
%
%Step 2
%
%Compute the coordinates for correlation graph using Curvilinear
%Component Analysis. Other possibilities: 'mmds' (principal
%coordinates), 'sammon' (Sammon's projection). See icassoProjection
%for details.
%
%Note that the dissimilarities are slightly differently scaled than in clustering:
%
%Details:
%1. Compute similarity-to-dissimilarity transformation that is
%   found to be good for the proximity preserving projection:
%   D=sqrt(1-sR.cluster.similarity) (i.e., call function sqrtsim2dis)
%2. compute CCA on this dissimilarity matrix using 75 epochs and default
%   parameters (see icassoProjection).
%
%  sR=icassoProjection(sR,'cca','s2d','sqrtsim2dis','epochs',75);

%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.

% ver 1.21 040305

sR=icassoCluster(sR,'strategy','AL','simfcn','abscorr','s2d','sim2dis','L','rdim');
sR=icassoProjection(sR,'cca','s2d','sqrtsim2dis','epochs',75);


% 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 = RegCoeff_M(S, M, Ns, g, df, B, Nb, Nr)
% Calculates the regression coefficiients

v = g(:,end);   % start for backward induction

f = zeros(Nb,Nr-1);

% backward induction and regression from t_{Nr-1} up to t_1
for i = Nr-1:-1:1
        index = find(g(:,i+1) > 0); % all ITM paths
        s = S(index,i+1);           % values of S at given time points
        m = M(index,i+1);           % values of M at given time points
        v = v * df(i+1);            % option value at t_i

        Acell = B(s,m);             % evaluate basis function in cell array B
        A = cell2mat(Acell{:,:});   % convert to matrix

        f(:,i) = (A'*A)\(A'*v(index));  % determine coefficients
        c = A*f(:,i);                   % continuation value

        exercise = g(index,i+1) >= c;    % early exercise
        v(index(exercise)) = g(index(exercise),i+1);
end

y = f;

end


function [seg, t] = retroProjPlkEndPnts(Rob,Sen,Lmk,Obs)

% RETROPROJPLKENDPNTS  Retro project Plucker endpoints.
%   [SEG,T] = RETROPROJPLKENDPNTS(Rob,Sen,Lmk,Obs) retroprojects the
%   segment endpoints in Bos.meas.y onto the Plucker line Lmk.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

global Map

% Plucker line
l = Map.x(Lmk.state.r);

% rays in sensor frame
r1s = pix2PluckerRay(Sen.par.k,Obs.meas.y(1:2));
r2s = pix2PluckerRay(Sen.par.k,Obs.meas.y(3:4));

% rays in world frame
r1 = fromFramePlucker(Rob.frame,fromFramePlucker(Sen.frame,r1s));
r2 = fromFramePlucker(Rob.frame,fromFramePlucker(Sen.frame,r2s));

% endpoints and abscissas
[e1,t1] = intersectPlucker(l,r1);
[e2,t2] = intersectPlucker(l,r2);

% build segment and abscissas vector
if t2 > t1
    seg = [e1;e2];
    t   = [t1;t2];
else
    seg = [e2;e1];
    t   = [t2;t1];
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


%% Colour Segmentation - VIBGYOR Colour Segmentation
%% This function can be used for VIBGYOR Colour segmentation from the RGB
%% Color images.
%% Function C = VIBGYORsegmentation(img)
%% input    img = Color image (The input image should be a color image)
%%  Example: C = VIBGYORsegmentation(img);
%%      Posted date : 14 - 07 - 2008
%%
%% Developed By : K.Kannan & Jeny Rajan
%%                  Medical Imaging Research Group (MIRG), NeST, Trivandrum.
%%
function C = VIBGYORsegmentation(img)
%% Electro-Magnetic wavelengths ranges in Nano Meter
% red 760-720
% orange 620-590
% yellow 590-545
% green 525-490
% blue 490-450
% indigo 450-420
% violet 420-380
%%
[row col plane] = size(img);
img = double(img);
C = zeros(row,col,plane);
if plane ~= 3
    disp('Input should be a color image');
    return;
end
GL = 255;
%% Color Choice
clc;
disp('   ');
disp('           r / R - for Red Color');
disp('           o / O - for Orange Color');
disp('           y / Y - for Yellow Color');
disp('           g / G - for Green Color');
disp('           b / B - for Blue Color');
disp('           i / I - for Indigo Color');
disp('           v / V - for Violet Color');
color = input('\nYour Color Choice = ','s');
%%
switch color
    case {'R','r'}
        f1 = (GL * 1);f2 = (GL * 0.4);f3 = (GL * 0.5);
        f4 = (GL * 0);f5 = (GL * 0.5);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,0);
    case {'O','o'}
        f1 = (GL * 1);f2 = (GL * 0.6);f3 = (GL * 0.68);
        f4 = (GL * 0.3);f5 = (GL * 0.3);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,0);
    case {'Y','y'}
        f1 = (GL * 1);f2 = (GL * 0.78);f3 = (GL * 1);
        f4 = (GL * 0.72);f5 = (GL * 0.52);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,1);
    case {'G','g'}
        f1 = (GL * 0.68);f2 = (GL * 0);f3 = (GL * 1);
        f4 = (GL * 0.4);f5 = (GL * 0.68);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,2,0);
    case {'B','b'}
        f1 = (GL * 0.5);f2 = (GL * 0);f3 = (GL * 0.68);
        f4 = (GL * 0);f5 = (GL * 1);f6 = (GL * 0.4);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,3,0);
    case {'I','i'}
        f1 = (GL * 0.68);f2 = (GL * 0);f3 = (GL * 1);
        f4 = (GL * 0.6);f5 = (GL * 1);f6 = (GL * 0.6);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,1);
    case {'V','v'}
        f1 = (GL * 1);f2 = (GL * 0.6);f3 = (GL * 0.68);
        f4 = (GL * 0);f5 = (GL * 1);f6 = (GL * 0.6);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,3,1);
    otherwise
        disp('unknown method');
end
%% Display
C = uint8(C);
figure, subplot(1,2,1),imshow(uint8(img),[]);title('Original Image');
subplot(1,2,2),imshow(uint8(C),[]);title('Color Segmented Image');

%% Function for Color Filter
function C = cfilter(img,f1,f2,f3,f4,f5,f6,m,flg)
[row col plane] = size(img);
C = zeros(row,col,plane);
for i = 1:row
    for j = 1:col
        if flg == 0
            if (img(i,j,1) <= f1 && img(i,j,1) >= f2 && ...
                img(i,j,2) <= f3 && img(i,j,2) >= f4 && ...
                img(i,j,3) <= f5 && img(i,j,3) >= f6 ...
                && img(i,j,m) == max([img(i,j,1) img(i,j,2) img(i,j,3)]))
                C(i,j,1:3) = img(i,j,1:3);
            else
                C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
            end
        else
            if (img(i,j,1) <= f1 && img(i,j,1) >= f2 && ...
                img(i,j,2) <= f3 && img(i,j,2) >= f4 && ...
                img(i,j,3) <= f5 && img(i,j,3) >= f6)
                C(i,j,1:3) = img(i,j,1:3);
            else
                C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
            end
        end
    end
end
%%


function [lb,ub,cand_rows] = findulb(F_struc,K)
%FINDULB Internal function to extract upper and lower variable bounds

% special code for the interval data case, to avoid overhead in the default
% function findulb

n = size(F_struc,2)-1;
lb = -inf*ones(n,1);
ub = inf*ones(n,1);
cand_rows = [];

if (K.f ~=0)
    A = -F_struc(1:K.f,2:end);
    b = F_struc(1:K.f,1);
    n = size(F_struc,2)-1;
    cand_rows = find(sum(A~=0,2)==1);
    for i = 1:length(cand_rows)
        j = find(A(cand_rows(i),:));
        ub(j)=min(ub(j),b(cand_rows(i))/A(cand_rows(i),j));
        lb(j)=max(lb(j),b(cand_rows(i))/A(cand_rows(i),j));
    end
end

if (K.l ~=0)
    A = -F_struc(K.f+1:K.f+K.l,2:end);
    b = F_struc(K.f+1:K.f+K.l,1);
    n = size(F_struc,2)-1;
    cand_rows = find(sum(A~=0,2)==1);
    for i = 1:length(cand_rows)
        j = find(A(cand_rows(i),:));
        if A(cand_rows(i),j)>0
            ub(j)=min(ub(j),sup(b(cand_rows(i))/A(cand_rows(i),j)));
        else
            lb(j)=max(lb(j),inf_(b(cand_rows(i))/A(cand_rows(i),j)));
        end
    end
end


% SIMSAMPLE Sample from SIM kernel

% KERN

colordef white
randn('seed', 1e8)
rand('seed', 1e8)

numSamp = 100;
t = linspace(0, 5, numSamp)';
kern = kernCreate(t, {'multi', 'rbf', 'sim', 'sim', 'sim'});
for i = 1:length(kern.comp)
  kern.comp{i}.inverseWidth = 1/(0.75*0.75);
end
kern.comp{2}.decay = 5;
kern.comp{2}.variance = 25;
kern.comp{3}.decay = 1;
kern.comp{3}.variance = 1;
kern.comp{4}.decay = 0.5;
kern.comp{4}.variance = 0.25;

params = kernExtractParam(kern);
kern = kernExpandParam(kern, params);

K = kernCompute(kern, t);

imagesc(K, [-1.1 1.1]);
handle = [];
fontSize = 24;
set(gca, 'fontname', 'times')
set(gca, 'fontsize', fontSize)
handle = [handle; text('Interpreter', 'latex', 'string', '$$f(t)$$', 'position', ...
            [50 410], 'fontsize', fontSize, 'horizontalalignment', 'center')];
handle = [handle; text('Interpreter', 'latex', 'string', '$$x_1(t)$$', 'position', ...
            [150 410], 'fontsize', fontSize, 'horizontalalignment', 'center')];
handle = [handle; text('Interpreter', 'latex', 'string', '$$x_2(t)$$', 'position', ...
            [250 410], 'fontsize', fontSize, 'horizontalalignment', 'center')];
handle = [handle; text('Interpreter', 'latex', 'string', '$$x_3(t)$$', 'position', ...
            [350 410], 'fontsize', fontSize, 'horizontalalignment', 'center')];
set(handle, 'horizontalalignment', 'right')
handle = [handle; text('Interpreter', 'latex', 'string', '$$f(t)$$', 'position', ...
            [-10 50], 'fontsize', fontSize, 'horizontalalignment', 'center')];
handle = [handle; text('Interpreter', 'latex', 'string', '$$x_1(t)$$', 'position', ...
            [-10 150], 'fontsize', fontSize, 'horizontalalignment', 'center')];
handle = [handle; text('Interpreter', 'latex', 'string', '$$x_2(t)$$', 'position', ...
            [-10 250], 'fontsize', fontSize, 'horizontalalignment', 'center')];
handle = [handle; text('Interpreter', 'latex', 'string', '$$x_3(t)$$', 'position', ...
            [-10 350], 'fontsize', fontSize, 'horizontalalignment', 'center')];
axis off
colorbar
if exist('printDiagram') && printDiagram
  printPlot('simTestKernelImage', '../tex/diagrams', '../html');
end

numSamp2 = 150;
y = gsamp(zeros(size(K), 1), 0.5*(real(K) + real(K')), numSamp2)';
t = t - 0;
counter = 0;
for i = 1:numSamp2
  x = reshape(y(:, i), numSamp, length(kern.comp));
  if any(x(:, 1)<0)
    continue
  end
  figure
  counter = counter + 1;
  a = plot(t, x(:, 1), 'k-');
  hold on
  a = [a plot(t, x(:, 2), 'r-')];
  a = [a plot(t, x(:, 3), 'g-')];
  a = [a plot(t, x(:, 4), 'b-')];
  set(a, 'linewidth', 2);
  set(gca, 'fontname', 'arial');
  set(gca, 'fontsize', 14);
  if exist('printDiagram') && printDiagram
    printPlot(['simSample'  num2str(counter)], '../tex/diagrams', '../html');
  end
end


function [varargout] = spm_diff(varargin)
% matrix high-order numerical differentiation
% FORMAT [dfdx] = spm_diff(f,x,...,n)
% FORMAT [dfdx] = spm_diff(f,x,...,n,V)
% FORMAT [dfdx] = spm_diff(f,x,...,n,'q')
%
% f      - [inline] function f(x{1},...)
% x      - input argument[s]
% n      - arguments to differentiate w.r.t.
%
% V      - cell array of matrices that allow for differentiation w.r.t.
% to a linear transformation of the parameters: i.e., returns
%
% df/dy{i};    x = V{i}y{i};    V = dx(i)/dy(i)
%
% q      - (char) flag to preclude default concatenation of dfdx
%
% dfdx          - df/dx{i}                     ; n =  i
% dfdx{p}...{q} - df/dx{i}dx{j}(q)...dx{k}(p)  ; n = [i j ... k]
%
%
% This routine has the same functionality as spm_ddiff, however it
% uses one sample point to approximate gradients with numerical (finite)
% differences:
%
% dfdx  = (f(x + dx)- f(x))/dx
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_diff.m 7143 2017-07-29 18:50:38Z karl $

% step size for numerical derivatives
%--------------------------------------------------------------------------
global GLOBAL_DX
if ~isempty(GLOBAL_DX)
    dx = GLOBAL_DX;
else
    dx = exp(-8);
end

% create inline object
%--------------------------------------------------------------------------
f     = spm_funcheck(varargin{1});

% parse input arguments
%--------------------------------------------------------------------------
if iscell(varargin{end})
    x = varargin(2:(end - 2));
    n = varargin{end - 1};
    V = varargin{end};
    q = 1;
elseif isnumeric(varargin{end})
    x = varargin(2:(end - 1));
    n = varargin{end};
    V = cell(1,length(x));
    q = 1;
elseif ischar(varargin{end})
    x = varargin(2:(end - 2));
    n = varargin{end - 1};
    V = cell(1,length(x));
    q = 0;
else
    error('improper call')
end

% check transform matrices V = dxdy
%--------------------------------------------------------------------------
for i = 1:length(x)
    try
        V{i};
    catch
        V{i} = [];
    end
    if isempty(V{i}) && any(n == i);
        V{i} = speye(spm_length(x{i}));
    end
end

% initialise
%--------------------------------------------------------------------------
m     = n(end);
xm    = spm_vec(x{m});
J     = cell(1,size(V{m},2));

% proceed to derivatives
%==========================================================================
if length(n) == 1

    % dfdx
    %----------------------------------------------------------------------
    f0    = f(x{:});
    for i = 1:length(J)
        xi    = x;
        xi{m} = spm_unvec(xm + V{m}(:,i)*dx,x{m});
        J{i}  = spm_dfdx(f(xi{:}),f0,dx);
    end

    % return numeric array for first-order derivatives
    %======================================================================

    % vectorise f
    %----------------------------------------------------------------------
    f  = spm_vec(f0);

    % if there are no arguments to differentiate w.r.t. ...
    %----------------------------------------------------------------------
    if isempty(xm)
        J = sparse(length(f),0);

    % or there are no arguments to differentiate
    %----------------------------------------------------------------------
    elseif isempty(f)
        J = sparse(0,length(xm));
    end

    % differentiation of a scalar or vector
    %----------------------------------------------------------------------
    if isnumeric(f0) && iscell(J) && q
        J = spm_dfdx_cat(J);
    end

    % assign output argument and return
    %----------------------------------------------------------------------
    varargout{1} = J;
    varargout{2} = f0;

else

    % dfdxdxdx....
    %----------------------------------------------------------------------
    f0        = cell(1,length(n));
    [f0{:}]   = spm_diff(f,x{:},n(1:end - 1),V);
    p         = true;

    for i = 1:length(J)
        xi    = x;
        xmi   = xm + V{m}(:,i)*dx;
        xi{m} = spm_unvec(xmi,x{m});
        fi    = spm_diff(f,xi{:},n(1:end - 1),V);
        J{i}  = spm_dfdx(fi,f0{1},dx);
        p     = p & isnumeric(J{i});
    end

    % or differentiation of a scalar or vector
    %----------------------------------------------------------------------
    if p && q
        J = spm_dfdx_cat(J);
    end
    varargout = [{J} f0];
end

function dfdx = spm_dfdx(f,f0,dx)
% cell subtraction
%__________________________________________________________________________
if iscell(f)
    dfdx  = f;
    for i = 1:length(f(:))
        dfdx{i} = spm_dfdx(f{i},f0{i},dx);
    end
elseif isstruct(f)
    dfdx  = (spm_vec(f) - spm_vec(f0))/dx;
else
    dfdx  = (f - f0)/dx;
end

return

function J = spm_dfdx_cat(J)
% concatenate into a matrix
%--------------------------------------------------------------------------
if isvector(J{1})
    if size(J{1},2) == 1
        J = spm_cat(J);
    else
        J = spm_cat(J')';
    end
end


%gampdf(2,3,4)
p = 0.018954;
q = wishpdf(2,3,1/4);
if abs(q - p) > 1e-6
	error('wishpdf failed');
end

x = [2 1; 1 3];
a = 5;
p = 0.0038062;
q = wishpdf(x,a);
if abs(q - p) > 1e-6
	error('wishpdf failed');
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [Sc,dS,d2S] = curvatureST(uc,omega,m,varargin)
%
% Matrix-free spatio-temporal curvature regularization energy for vc
% where vc is cell-centered
%
% Sv) = 0.5 * \int_{\omega}\int_0^1
%               alpha(1)*v(x,t)'*A*v(x,t)+ alpha(2)*v(x,t)'*B*v(x,t) dx dt,
%
% where A is the curvature operator and B the first-order time derivative
% operator.
%
% Input:
%
%   vc          instationary velocity field (cell-centered)
%   omega       spatial domain
%   m           number of discretization points in space
%   varargin    optional parameters (see below)
%
% Optional Input:
%   tspan       time span (default: [0 1])
%   nt          number of time points for velocity
%
%
% Output:
%
%   Sc          current value  (0.5 * hd * uc'* A *uc)
%   dS          derivative     (hd * uc'*A )
%   d2S         Hessian        A
%  if ~matrixFree,  d2S is sparse matrix; else, d2S is struct; endif
%
% see also curvature.m
% ==================================================================================

function [Sc,dS,d2S] = mfCurvatureST(vc,omega,m,varargin)
if nargin == 0
    help(mfilename);
    return;
end

if strcmp(vc,'para')
    Sc = 'cell-centered';       % grid
    dS = 1;                     % matrixFree
    d2S = @spectralPrecondPCG;  % solver
    return;
end

persistent A omegaOld mOld alphaOld ntOld tspanOld

if ~exist('mOld','var'),     mOld = [];     end;
if ~exist('omegaOld','var'), omegaOld = []; end;
if ~exist('alphaOld','var'), alphaOld = []; end;

matrixFree  = 0;
alpha       = [1 1e-3];
tspan       = [0 1];
nt          = [];
for k=1:2:length(varargin) % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

dim = numel(omega)/2;

if isempty(nt) % roughly estimate nt
    nt = round(numel(vc)/(prod(m)*dim))-1;
end

d2S.regularizer = regularizer;
d2S.alpha  = alpha;
d2S.B      = @(omega,m)     getCurvatureMatrixST( omega,tspan,m,nt,alpha);
d2S.d2S    = @(u,omega,m)   curvatureOperatorST(u,omega,tspan,m,nt,alpha);
d2S.diag   = @(omega,m)     getCurvatureDiag(omega,tspan,m,nt,alpha); %getDiag(omega,tspan,m,nt,alpha);
d2S.solver = @spectralPrecondPCG;
d2S.res    = vc;
dS         = d2S.d2S(vc,omega,m)';
Sc         = .5*dS*vc;

function C = getCurvatureMatrixST(omega,tspan,m,nt,alpha)

h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h);
% compute time-stepsize
dt = abs(tspan(2)-tspan(1))/nt;

% build gradient matrix for one transformation
C =  getCurvatureMatrix(omega,m);
a = sqrt(alpha(1).*hd.*dt*[1/2;ones(nt-1,1);1/2]);
% apply spatial regularization to all transformations and sum up in
% time
C =  kron(sdiag(a(:)),C);

% get time regularization matrix
if alpha(2)>0
    b = sqrt(alpha(2)*hd.*dt);
    Bt = b * getTimeDiffusionMatrix(nt,dt,prod(m),length(omega)/2);
    C  = [C;Bt];
end

function D = sdiag(v)
	D = diag(sparse(v(:)));

% matrix free implementation of spatio-temporal curvature operator
function Ay = curvatureOperatorST(uc,omega,tspan,m,nt,alpha)
dim = numel(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h(1:dim));
dt  = abs(tspan(1)-tspan(2))/nt;
w   = dt*[1/2;ones(nt-1,1);1/2];
n   = prod(m);

uc   = reshape(uc,dim*n,[]);
ntp1 = size(uc,2);
Ay = 0*uc;

switch dim
    case 2
        D2x =  D2(1,omega,m);
        D2y =  D2(2,omega,m);

        % space: curvature
        for k=1:ntp1
            uct = reshape(uc(:,k),[m dim]);
            t1  = D2x*uct(:,:,1) + uct(:,:,1)*D2y;
            t1  = D2x*t1 + t1*D2y;
            t2  = D2x*uct(:,:,2) + uct(:,:,2)*D2y;
            t2  = D2x*t2 + t2*D2y;

            Ay(:,k) = (w(k)*hd*alpha(1))*[t1(:);t2(:)];
        end
        Ay = Ay(:);
    case 3
        % space: curvature
        d2  = @(i) D2(i,omega,m); % this is a shortcut to the discrete second derivative

        % the following line is a shortcut for
        %  - permuting the 3d-array using the permutation J
        %  - reshape it to a 2D-array of size q-by-prod(m)/q, where q=m(J(1))
        %  - multiply by A (which is q-by-q)
        %  - undo the reshape, i.e. make the result to m(J(1))-by-m(J(2))-by-m(J(3))
        %  - undo the permutation
        operate = @(A,z,J) ipermute(reshape(A*reshape(permute(z,J),m(J(1)),[]),m(J)),J);
        for k=1:ntp1 % loop over all time steps
            uct = reshape(uc(:,k),[m,dim]);
            for ell=1:3 % run over all components y^ell of Y=(Y^1,Y^2,Y^3)
                % compute
                % (I_3\otimes I_2\otimes d2(1) + I_3\otimes d2(2)\otimes I_1 ...
                % + d2(3)\otimes I_2\otimes I_1) y^ell
                %                 utt = 0;
                for rep=1:2 % go twice to have the bi-Laplacian
                    for j=1:3
                        z = operate(d2(j),uct(:,:,:,ell),[j,setdiff(1:3,j)]);
                        Ay((ell-1)*n+(1:n),k) = Ay((ell-1)*n+(1:n),k) + reshape(z,[],1);
                    end;
                    if rep==1 % overwrite uct and reset Ay. Compare to t1,t2 in dim=2 implementation
                        uct(:,:,:,ell) = reshape(Ay((ell-1)*n+(1:n),k),m);
                        Ay((ell-1)*n+(1:n),k) = 0;
                    end
                end;
            end
            Ay(:,k) = w(k)*hd*alpha(1)*Ay(:,k);
        end
        Ay = Ay(:);
    otherwise
        error('%s - dimension %d not supported.',mfilename,dim);
end
% time: diffusion
if alpha(2)>0
    d3  = @(Y) (Y(:,2:end)-Y(:,1:end-1))/dt;
    d3T = @(Y) ([-Y(:,1),Y(:,1:end-1)-Y(:,2:end),Y(:,end)])/dt;
    At = d3T(d3(uc));
    Ay = Ay(:) + (alpha(2)* hd*dt)*At(:);
end

function D = getCurvatureDiag(omega,tspan,m,nt,alpha)
% just for testing - not very efficient
dim = numel(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h(1:dim));
dt  = abs(tspan(1)-tspan(2))/nt;

C = getCurvatureMatrix(omega,m);
D = full(diag(C'*C'));

D = hd*dt*alpha(1)*repmat(D(:),1,1);
if alpha(2)>0
    errory('nyi')
end

function D = D2(i,omega,m)
h = (omega(2:2:end)-omega(1:2:end))./m;
D = spdiags(ones(m(i),1)*[1,-2,1],-1:1,m(i),m(i))/h(i)^2;
D([1,end]) = -D([2,end-1]);

function A = getTimeDiffusionMatrix(nt,dt,n,dim)
Dt=spdiags(ones(nt+1,1)*[-1/dt 1/dt],0:1,nt,nt+1);
A = kron(Dt, speye(n*dim)); % 2nd deriveative over time for two components


function varargout = semilogy(varargin)
%SEMILOGY   Semi-log scale plot of a CHEBFUN.
%   SEMILOGY(...) is the same as PLOT(...), except a logarithmic (base 10) scale
%   is used for the Y-axis.
%
% See also PLOT, SEMILOGX, LOGLOG.

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

% Standard CHEBFUN/PLOT():
[h{1:3}] = plot(varargin{:});

% Find the CHEBFUNS:
isCheb = cellfun(@(v) isa(v, 'chebfun'), varargin);
% Choose a TOL based on vscale:
tol = max(cellfun(@(f) max(eps*vscale(f)), varargin(isCheb)));
% Loop over the different plot components:
for j = 1:numel(h)
    % Get the y data:
    yData = get(h{j}, 'yData');
    % Ensure it's a cell:
    if ( ~iscell(yData) )
        yData = {yData};
    end
    % Loop over each cell:
    for k = 1:numel(yData)
        if ( min(yData{k}(:)) > -tol )
            % If it's only a little negative, then take the absolute value:
            yData{k} = abs(yData{k});
        end
    end
    for k = 1:numel(h{j})
        % Put it back in h
        set(h{j}(k), 'yData', yData{k});
    end
end

% Set the YScale to be logarithmic:
set(gca, 'YScale', 'log');

% Output handle if requested:
if ( nargout > 0 )
    varargout = {h};
end

end


% psf_mismatch_example2.m
%
% 2D example showing the effect of PSF mismatch on ML-EM algorithm
%
% Copyright 2001-8-24, Jeff Fessler, The University of Michigan

if ~has_aspire, return, end

%
% generate data
%
if ~isvar('yi'), printm 'setup psf_mismatch_example2'
	f.dir = test_dir;
	f.dsc0 = [f.dir 't0.dsc'];
	f.dsc1 = [f.dir 't1.dsc'];
	f.dsc2 = [f.dir 't2.dsc'];
	f.wtf0 = strrep(f.dsc0, 'dsc', 'wtf');
	f.wtf1 = strrep(f.dsc1, 'dsc', 'wtf');
	f.wtf2 = strrep(f.dsc2, 'dsc', 'wtf');
	if 1
		Fwhm0 = 5;
		os_run(['wt -chat 0 dsc 12 fwhm_detector 5 >! ' f.dsc0])
		os_run(['echo y | wt -chat 0 gen ' f.dsc0])
		Fwhm1 = 7;
		os_run(['wt -chat 0 dsc 12 fwhm_detector 7 >! ' f.dsc1])
		os_run(['echo y | wt -chat 0 gen ' f.dsc1])
		Fwhm2 = 2;
		os_run(['wt -chat 0 dsc 12 fwhm_detector 2 >! ' f.dsc2])
		os_run(['echo y | wt -chat 0 gen ' f.dsc2])
	end

%	G0 = Gtomo2_wtmex('f.wtf0'); % cannot use with multiple .wtf's

	[t nx ny nb na] = wtf_read(f.wtf0);
	ig = image_geom('nx', nx, 'ny', ny, 'dx', 1);
	ig.mask = reshape(sum(t) ~= 0, nx, ny);

	G0 = Gsparse(f.wtf0, 'mask', ig.mask);
	G1 = Gsparse(f.wtf1, 'mask', ig.mask);
	G2 = Gsparse(f.wtf2, 'mask', ig.mask);

	sg = sino_geom('par', 'nb', nb, 'na', na, 'dr', 1);
	xtrue = ellipse_im(ig, [6.5 -0.5 5 5 0 100], 'oversample', 4);

	ri = 1;
	yi = sg.shape(G0 * xtrue(ig.mask)) + ri;
	im(yi, 'yi'), cbar
prompt
end

if ~isvar('Gb2'), printm 'Gbs'
	f.nblock = 6;
	Gb0 = Gblock(G0, f.nblock);
	Gb1 = Gblock(G1, f.nblock);
	Gb2 = Gblock(G2, f.nblock);
prompt
end

% uniform initial image
xinit = ones(sum(ig.mask(:)),1);

% FBP
if 0 && ~isvar('xfbp'), printm 'fbp'
	xfbp = em_fbp(sg, ig, yi, 1, ri);
	im(xfbp), cbar
prompt
end

if ~isvar('x2') && 0
	x0 = eml_em(xinit, G0, yi(:), 1, ri, 'isave', 'all', 'niter', f.niter);
	x1 = eml_em(xinit, G1, yi(:), 1, ri, 'isave', 'all', 'niter', f.niter);
	x2 = eml_em(xinit, G2, yi(:), 1, ri, 'isave', 'all', 'niter', f.niter);
	x0 = ig.embed(x0);
	x1 = ig.embed(x1);
	x2 = ig.embed(x2);
end

%
% OS-EM iterations
%
if ~isvar('xo2'), printm 'run os-em'
	f.niter = 40;
	rri = ri * ones(nb,na);
	osem = @(G) eml_osem(xinit, G, ...
		yi, [], rri, 'niter', f.niter, 'isave', 'all');
	xo0 = osem(Gb0);
	xo1 = osem(Gb1);
	xo2 = osem(Gb2);
	xo0 = ig.embed(xo0);
	xo1 = ig.embed(xo1);
	xo2 = ig.embed(xo2);
	im(xo2)
prompt
end

if ~isvar('fw2'), printm 'find widths'
	ix = 39+[-9:9];
	iy = 33+[-9:9];

	xx0 = xo0(ix,iy,2:end);
	xx1 = xo1(ix,iy,2:end);
	xx2 = xo2(ix,iy,2:end);
	im(xx0)
	clear fw0 fw1 fw2
	for ii=1:f.niter
		fw0(ii) = fwhm2(xx0(:,:,ii));
		fw1(ii) = fwhm2(xx1(:,:,ii));
		fw2(ii) = fwhm2(xx2(:,:,ii));
	end

	xx = xtrue(ix,iy);
	fx = fwhm2(xx);
end

if 1 && im
	im clf
	ii = 1:f.niter;
	plot(ii(1:4:end), fw0(1:4:end), 'yo', ...
		ii(1:4:end), fw1(1:4:end), 'cx', ...
		ii(1:4:end), fw2(1:4:end), 'g+', ...
		ii, fx + 0*ii, 'b-', ...
		ii, fw0, 'y-', ...
		ii, fw1, 'c-', ...
		ii, fw2, 'g-')
	axisy(3,11)
	legend(	sprintf('True system PSF, FWHM=%g', Fwhm0), ...
		sprintf('Model PSF too big, FWHM=%g', Fwhm1), ...
		sprintf('Model PSF too small, FWHM=%g', Fwhm2), ...
		'ideal')
	xlabel 'Iteration of OS-EM algorithm'
	ylabel 'Width of reconstructed circle'
end


function pass = test_vectorizeOp()
%TEST_VECTORIZEOP    Test that the CHEBOP vectorization method works as expected

%% Setup
% Domain and some of CHEBFUNs
d = [1, 3];
x = chebfun(@(x) x, d);
f = exp(sin(4*x));
g = cos(x.^2).^2;
h = 1./(5+x.^2);
u = sin(5*x.^2);
v = exp(x/2);
% Some parameters
b = 4*pi;
c = -5*exp(1);

%% Simple case, already vectorized
fun = @(u) diff(u, 2) + b.*u.^3;
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect (FUN above is already vectorized, so they're identical)
exactFun = fun;
% Check that we got the correct string and the correct function
pass(1,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(1,2) = norm(exactFun(u) - vecFun(u)) == 0;

%% Simple case, already vectorized, with both x and u
fun = @(x,u) diff(u, 2) + b.*u.^3;
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect (FUN above is already vectorized, so they're identical)
exactFun = fun;
% Check that we got the correct string and the correct function
pass(2,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(2,2) = norm(exactFun(x,u) - vecFun(x,u)) == 0;

%% Vectorization needed, scalar case
fun = @(u) diff(u, 2) + b*u^3;
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect
exactFun = @(u) diff(u, 2) + b.*u.^3;
% Check that we got the correct string and the correct function
pass(3,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(3,2) = norm(exactFun(u) - vecFun(u)) == 0;

%% Vectorization needed, scalar case with both x and u
fun = @(x,u) diff(u, 2) + b*u^3;
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect
exactFun = @(x,u) diff(u, 2) + b.*u.^3;
% Check that we got the correct string and the correct function
pass(4,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(4,2) = norm(exactFun(x,u) - vecFun(x,u)) == 0;

%% Vectorization needed, scalar case with a CHEBFUN
fun = @(u) diff(u, 2) + f*u^3;
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect
exactFun = @(u) diff(u, 2) + f.*u.^3;
% Check that we got the correct string and the correct function
pass(5,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(5,2) = norm(exactFun(u) - vecFun(u)) == 0;

%% Vectorization needed, scalar case with a CHEBFUN, both x and u
fun = @(x,u) diff(u, 2) + f*u^3;
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect
exactFun = @(x,u) diff(u, 2) + f.*u.^3;
% Check that we got the correct string and the correct function
pass(6,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(6,2) = norm(exactFun(x,u) - vecFun(x,u)) == 0;

%% System case, already vectorized
fun = @(x,u,v) [diff(u, 2) + b.*u.^3+v.*u./h; diff(v,2) + v.^2./c + g.*u./v];
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect (FUN above is already vectorized, so they're identical)
exactFun = fun;
% Check that we got the correct string and the correct function
pass(7,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(7,2) = norm(exactFun(x,u,v) - vecFun(x,u,v)) == 0;

%% System case, partially vectorized
fun = @(x,u,v) [diff(u, 2) + b*u.^3+v.*u/h; diff(v,2) + v.^2./c + g*u./v];
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect
exactFun = @(x,u,v) [diff(u, 2) + b.*u.^3+v.*u./h; ...
    diff(v,2) + v.^2./c + g.*u./v];
% Check that we got the correct string
pass(8,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(8,2) = norm(exactFun(x,u,v) - vecFun(x,u,v)) == 0;

%% System case, no vectorization
fun = @(x,u,v) [diff(u, 2) + b*u^3+v*u/h; diff(v,2) + v^2/c + g*u/v];
% Automatically vectorized
vecFun = chebop.vectorizeOp(fun);
% The answer we expect
exactFun = @(x,u,v) [diff(u, 2) + b.*u.^3+v.*u./h; ...
    diff(v,2) + v.^2./c + g.*u./v];
% Check that we got the correct string and the correct function
pass(9,1) = strcmp(func2str(exactFun), func2str(vecFun));
pass(9,2) = norm(exactFun(x,u,v) - vecFun(x,u,v)) == 0;

%% For a function handle, will get the same results
s = @sin;
sVec = chebop.vectorizeOp(s);
% Check that we got the correct string and the correct function
pass(10,1) = strcmp(func2str(s), func2str(sVec));
pass(10,2) = norm(s(u)-sVec(u)) == 0;
end


function value = r8vec_amin ( n, a )

%*****************************************************************************80
%
%% R8VEC_AMIN returns the minimum absolute value in an R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array.
%
%    Output, real VALUE, the value of the entry
%    of smallest magnitude.
%
  value = min ( abs ( a(1:n) ) );

  return
end


function Geo = Polygons_intersection_Posttreatment(S,Geo,Display_result,...
                                                                  Accuracy)

% This function splits the original polygons at intersection. Each time
% polygons intersect, a new one is created containing only the
% intersection of these polygons. Polygons are stored in structure with
% additional data, such the area and the polygons involded in the possible
% intersection.
%
% The result is a structure containg polygons, with for each the list of
% the polygon involved in the intersection.
%
% The union of the resulting polygons is equal to the union of the original
% polygons
%
%
% Input :  - S   : Structure containing initial polygon geometry
%                     S(i).P(j).x      : Vector
%                     S(i).P(j).y      : Vector
%                     S(i).P(j).hole   : Binary value (1 = hole, 0= fill)
%
%          - Geo : Structure containing
%                     Geo(m).index gives the indexes of original polygon
%                                  (S) involved in polygon m.
%                     Geo(m).P     contains the geometrical description of
%                                  polygon m.
%                     Geo(m).area  contains the area of polygon m.
%
%          - Display_result : Binary number used to display or not the
%                             result
%
% Output:  Geo structure containing splited polygons and their associated
%           data( indexes of the polygons involved in this polygon, area of
%           the polygon).
%           The output has exactly the same structure as the Geo given in
%           input.

% Guillaume JACQUENOT
% guillaume at jacquenot at gmail dot com
% 2007_10_08
% 2009_06_16

if nargin==0
    Polygons_intersection_posttreatment_data;
    Display_result = 1;
    Accuracy = 1e-3;
elseif nargin==2
    Display_result = 1;
    Accuracy = 1e-3;
end

S   = Polygons_intersection_Compute_area(S);
Geo = Polygons_intersection_Compute_area(Geo);

Geo   = [S,Geo];
Geo   = Polygons_intersection_Polygon_cleanup(Geo,Accuracy);
N_Geo =  numel(Geo);

% Reshape elements
for i=1:N_Geo
    for j=1:numel(Geo(i).P)
        Geo(i).P(j).x = reshape(Geo(i).P(j).x,1,numel(Geo(i).P(j).x));
        Geo(i).P(j).y = reshape(Geo(i).P(j).y,1,numel(Geo(i).P(j).y));
    end
end

% Performs geometrical computation.
% One uses the polygon containing the higher number of polygon indexes, and
% one substracts this polygon to polygons containing less indexes.
%
% It is a top down approach.
for i = numel(Geo(N_Geo).index):-1:1
    N_Geo = numel(Geo);
    N_index = [];
    for j = 1:N_Geo
         N_index(j) = numel(Geo(j).index);
    end
    Ref = (1:N_Geo).*(N_index == i);   Ref(Ref==0)=[];
    Mod = (1:N_Geo).*(N_index <= i-1); Mod(Mod==0)=[];
    To_delete = [];
    for j = 1:numel(Ref)
        for k = 1:numel(Mod)
            if ~isempty(Geo(Mod(k)).P) && ~isempty(Geo(Ref(j)).P)
                P = PolygonClip(Geo(Mod(k)).P,Geo(Ref(j)).P,0);
                P = Polygons_intersection_Polygon_cleanup(P,Accuracy);
                if ~isempty(P)
                    Geo(Mod(k)).P = P;
                else
                    To_delete = [To_delete,Mod(k)];
                end
            else
                Geo(Mod(k)).P = [];
            end
        end
    end
    Geo(To_delete)=[];
end

% % Each element P containing more than one polygon is split into several
% % elements P
% N_Geo = numel(Geo);
% index = 1;
% for i=1:N_Geo
%     if numel(Geo(i).P)>=2
%         for j=2:numel(Geo(i).P)
%             Geo(N_Geo+index).index = Geo(i).index;
%             Geo(N_Geo+index).P     = Geo(i).P(j);
%             index = index+1;
%         end
%         Geo(i).P(2:numel(Geo(i).P))=[];
%     end
% end

% % Cleaning up the results of the different operations
for i=1:numel(Geo)
    for j=1:numel(Geo(i).P)
        Geo(i).P(j).x   = reshape(Geo(i).P(j).x,1,numel(Geo(i).P(j).x));
        Geo(i).P(j).y   = reshape(Geo(i).P(j).y,1,numel(Geo(i).P(j).y));
    end
end

[Geo Geo_area] = Polygons_intersection_Compute_area(Geo);
N_Geo = numel(Geo);
To_delete = [];
for i=1:N_Geo
    for j=1:numel(Geo(i).P)
        if Geo_area(i).A(j)<1e-9,To_delete = [To_delete,[i;j]];end
    end
end
if ~isempty(To_delete)
    for i=size(To_delete,2):-1:1
        Geo(To_delete(1,i)).P(To_delete(2,i)) = [];
        Geo_area(To_delete(1,i)).A(To_delete(2,i)) = [];
    end
end

% Clean up geometry.
N_Geo = numel(Geo);
for i = 1:N_Geo
    for j=1:numel(Geo(i).P)
        % Delete colinear vectors of the geometry, which are useless.
        if Geo(i).P(j).x(1)==Geo(i).P(j).x(end) && ...
                Geo(i).P(j).y(1)==Geo(i).P(j).y(end)
            Angle = NaN(1,numel(Geo(i).P(j).x)-1);
            X1 =  Geo(i).P(j).x(1:end-1);
            X2 = [Geo(i).P(j).x(2:end-1) Geo(i).P(j).x(1)];
            X3 = [Geo(i).P(j).x(3:end-1) Geo(i).P(j).x(1:2)];

            Y1 =  Geo(i).P(j).y(1:end-1);
            Y2 = [Geo(i).P(j).y(2:end-1)  Geo(i).P(j).y(1)];
            Y3 = [Geo(i).P(j).y(3:end-1)  Geo(i).P(j).y(1:2)];
        else
            Angle = NaN(1,numel(Geo(i).P(j).x));
            X1 =  Geo(i).P(j).x(1:end);
            X2 = [Geo(i).P(j).x(2:end) Geo(i).P(j).x(1)];
            X3 = [Geo(i).P(j).x(3:end) Geo(i).P(j).x(1:2)];

            Y1 =  Geo(i).P(j).y(1:end);
            Y2 = [Geo(i).P(j).y(2:end)  Geo(i).P(j).y(1)];
            Y3 = [Geo(i).P(j).y(3:end)  Geo(i).P(j).y(1:2)];
        end

        P1 = (X3-X2).*(X1-X2) + (Y3-Y2).*(Y1-Y2);
        P2 = (X3-X2).*(Y1-Y2) - (Y3-Y2).*(X1-X2);

        CL1 = logical((P1==0.0).*(P2==0.0));
        Angle(CL1) = 0;
        Angle(~CL1) = atan2(P2(~CL1),P1(~CL1));
        CL2 = Angle<0.0;
        Angle(CL2) = Angle(CL2)+2*pi;

        % If any angle between P1 P2 & P3 is null or equals 2pi or pi, one
        % deletes P2.

        % A threshold value is used to detect colinear vectors
        % The smaller the value is , the better it is.
        threshold = 1e-3;
        To_delete = (abs([Angle(end) Angle(1:end-1)])<=threshold) |...
            ((abs([Angle(end) Angle(1:end-1)])<=pi+threshold) & ...
             (abs([Angle(end) Angle(1:end-1)])>=pi-threshold)) | ...
             (abs([Angle(end) Angle(1:end-1)])>=2*pi-threshold);
        Geo(i).P(j).x(To_delete)=[];
        Geo(i).P(j).y(To_delete)=[];

        % One closes the polygon
        Geo(i).P(j).x = [Geo(i).P(j).x Geo(i).P(j).x(1)];
        Geo(i).P(j).y = [Geo(i).P(j).y Geo(i).P(j).y(1)];
    end
end

if Display_result
    % Display figures
    N_polygons = numel(S);
    colour(1:N_polygons,1:3) = rand(N_polygons,3);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    figure
    subplot(1,2,2)
    grid on
    hold on
    box on
    axis equal
    Leg = cell(N_polygons,1);
    h = zeros(1,N_polygons);
    for i=1:N_polygons
        for j=numel(S(i).P):-1:1
        h(i) = plot(S(i).P(j).x,S(i).P(j).y,'color',colour(i,:),'LineWidth',3);
%         text(S(n).x(1)+0.2,S(n).y(1)+0.2,num2str(n),...
%              'color',colour(n,:),'FontSize',14);
            if S(i).P(j).hole==1
                set(h(i),'LineStyle','--');
            end
        end
        Leg{i} = ['Shape ' int2str(i)];
    end
    legend(h,Leg),clear Leg
    for i = 1:N_Geo
        for j=numel(Geo(i).P):-1:1
            if Geo(i).P(j).hole==1
                temp = [1 1 1];
            else
                temp  = rand(1,3);
            end
            patch([Geo(i).P(j).x,Geo(i).P(j).x(1)],...
                  [Geo(i).P(j).y,Geo(i).P(j).y(1)],...
                  temp,'FaceAlpha',0.3)
            geom = center_gravity( Geo(i).P(j).x, Geo(i).P(j).y );
            Str='';
            for g=1:numel(Geo(i).index)
                Str = strcat(Str,int2str(Geo(i).index(g)),',');
            end
            Str = Str(1:end-1);
            if inpolygon(geom(1),geom(2),...
                         [Geo(i).P(j).x,Geo(i).P(j).x(1)],...
                         [Geo(i).P(j).y,Geo(i).P(j).y(1)])
                X1 = geom(1);
                Y1 = geom(2);
            else
                temp1 = axis;
                X1 = min(Geo(i).P(j).x) + (temp1(1)+temp1(2))/100;
                Y1 = min(Geo(i).P(j).y) + (temp1(3)+temp1(4))/100;
            end
            text(X1,Y1,Str,...%'BackgroundColor',[1 1 1],...
                'Color',temp,...
                'FontSize',14,...
                'VerticalAlignment','middle',...
                'HorizontalAlignment','center');
        end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(1,2,1)
    hold on
    grid on
    box on
    axis equal
    for i=1:N_polygons
        for j=1:numel(S(i).P)
        h = plot(S(i).P(j).x,S(i).P(j).y,'color',colour(i,:),'LineWidth',3);
%         text(S(n).x(1)+0.2,S(n).y(1)+0.2,num2str(n),...
%              'color',colour(n,:),'FontSize',14);
            if S(i).P(j).hole==1
                set(h,'LineStyle','--');
            end
        end
    end
    subplot(1,2,1)
    axis(axis+0.05*[-1 1 -1 1])
    subplot(1,2,2)
    axis(axis+0.05*[-1 1 -1 1])
end

function Cen = center_gravity( x, y )
% This function computes the center gravity of a polygon whose coordinates
% are given with x and y

% This function is extracted from Polygeom function.
% Credit goes to
% H.J. Sommer III - 02.05.14 - tested under MATLAB v5.2
%
% code available at:
%    http://www.me.psu.edu/sommer/me562/polygeom.m
% derivation of equations available at:
%    http://www.me.psu.edu/sommer/me562/polygeom.doc

% number of vertices
[ x ] = shiftdim( x );
[ y ] = shiftdim( y );
[ n ] = size( x,1 );

% temporarily shift data to mean of vertices for improved accuracy
xm = mean(x);
ym = mean(y);
x = x - xm*ones(n,1);
y = y - ym*ones(n,1);

% delta x and delta y
dx = x( [ 2:n 1 ] ) - x;
dy = y( [ 2:n 1 ] ) - y;

% summations for CW boundary integrals
A = sum( y.*dx - x.*dy )/2;
Axc = sum( 6*x.*y.*dx -3*x.*x.*dy +3*y.*dx.*dx +dx.*dx.*dy )/12;
Ayc = sum( 3*y.*y.*dx -6*x.*y.*dy -3*x.*dy.*dy -dx.*dy.*dy )/12;

% check for CCW versus CW boundary
if A < 0,
  A = -A;
  Axc = -Axc;
  Ayc = -Ayc;
end

% centroidal moments
xc = Axc / A;
yc = Ayc / A;

% replace mean of vertices
Cen = [xc + xm yc + ym];


d = 500;
n = 20;
C = 100;
k = 10;
q = 50;
q2 = 50;

ODLSI = C*k*(k*d+d*n + q*k*n) + C*q*k*d^3
EDLSI = C*k*(k*d+d*n + q*k*n) + C*d^3 + C*q*d*k*(q*k + d)
OFDDL = C^2*d*k*(n+C*k+C*n)+C*k^2*q*(d + C^2*n)
EFDDL = C^2*k*((q+1)*k*(d+C*n) + 2*d*n)

OCOPAR = C^3*k^2*(2*d + C*k + q*n) + C*q*k*d^3
ECOPAR = C^3*k^2*(2*d + C*k + q*n) + C*d^3 + C*q*d*k*(q*k + d)

LRSDL = C^2*k*((q+1)*k*(d + C*n) + 2*d*n) + C^2*d*k*n + (q + q2)*d*k^2

%%
ODLSID = C*q*k*d^3
EDLSID = C*d^3 + C*q*d*k*(q*k + k)
OFDDLX = C^2*k*(d*n + q*C*k*n + C*d*k)
EFDDLX = C^2*k*(d*n + q*C *n*k + d*k)
OFDDLD = C*d*k*(q*k + C^2*n)
EFDDLD = C*d*k*(C*n + C*q*k) + C^3*k^2 *n


classdef EigModes < handle

    properties (Access = public)

    end

    properties (Access = private)
        mesh
        dim
        V
        v1
        v2
        D
        eigModesPlotter
        lambda
        freeNodes
        mode1Disp
        mode2Disp
    end

    properties (Access = private)
        designVariable
        stiffnessMatComputer
        bendingMatComputer
    end

    methods (Access = public)

        function obj = EigModes(cParams)
            obj.init(cParams)
            obj.createEigModesPlotter();
        end

        function plot(obj,A,iter)
            p = obj.eigModesPlotter;
            [m1,m2] = obj.computeBucklingModes(obj.v1,obj.v2);
            obj.mode1Disp = m1;
            obj.mode2Disp = m2;
            p.plot(A,m1,m2,iter,obj.D)
        end

        function fx = provideFunction(obj,eigNum)
            obj.computeEigenModesAndValues();
            obj.lambda = obj.computeLambda();
            gamma = obj.designVariable.getFirstEigenMode();
            fx = gamma-obj.lambda(eigNum);
        end

       function grad = provideDerivative(obj,eigNum)
            obj.reorderModes(obj.lambda,obj.V,obj.D);
            Belem =  obj.bendingMatComputer.elementalBendingMatrix;
            x = obj.designVariable.getColumnArea();
            nElem = obj.mesh.nelem;
            eigV1 = obj.D(1,1);
            eigV2 = obj.D(2,2);
            difEigs = abs(eigV2-eigV1);
            if difEigs > 1
                dfdx = obj.computeSimpleEig(Belem,x);
            else
                dfdx = obj.computeDoubleEig(Belem,x);
            end
            dfdx(1,nElem+1) = 1;
            dfdx(2,nElem+1) = 1;
            grad = dfdx(eigNum,:);
        end

    end

    methods (Access = private)

        function dfdx = computeSimpleEig(obj,Belem,x)
            d = obj.dim;
            free = obj.freeNodes;
            ndofe = d.ndofPerElement;
            ndofn = d.ndimField;
            W = zeros(d.ndof,2);
            W(free,1) = obj.v1;
            W(free,2) = obj.v2;
            nElem = obj.mesh.nelem;

            for i = 1:nElem
                index = ndofn*(i-1)+1: ndofn*(i-1)+ndofe;
                dx = 2*x(i,1);
                dfdx(1,i) = -dx*(W(index,1)'*Belem(:,:,i)*W(index,1));
                dfdx(2,i) = -dx*(W(index,2)'*Belem(:,:,i)*W(index,2));

            end
%             q = Quadrature.set(obj.mesh.type);
%             q.computeQuadrature('LINEAR');
%             l = sum(obj.mesh.computeDvolume(q));
%             dfdx(1,:) = 1./1.*dfdx(1,:);
%             dfdx(2,:) = 1./1.*dfdx(2,:);
        end

        function dfdx = computeDoubleEig(obj,Belem,x)
            d    = obj.dim;
            free = obj.freeNodes;
            ndofe = d.ndofPerElement;
            ndofn = d.ndimField;
            nElem = obj.mesh.nelem;
            W1    = zeros(d.ndof,1);
            W2    = zeros(d.ndof,1);
            dW1   = zeros(nElem,1);
            dW2   = zeros(nElem,1);
            dW1W2 = zeros(nElem,1);
            W1(free,1) = obj.v1;
            W2(free,1) = obj.v2;
            for i=1:nElem
                index = ndofn*(i-1)+1: ndofn*(i-1)+ndofe;
                dx = 2*x(i,1);
                dW1(i,1)= dx*(W1(index,1)'*Belem(:,:,i)*W1(index,1));
                dW2(i,1)= dx*(W2(index,1)'*Belem(:,:,i)*W2(index,1));
                dW1W2(i,1)= (2*x(i,1))*(W1(index,1)'*Belem(:,:,i)*W2(index,1));
                A = [dW1(i,1) dW1W2(i,1); dW1W2(i,1) dW2(i,1)];
                [U,R] = eigs(A,2,'SM');
                S = sort(diag(R));
                dfdx(1,i) = -S(1);
                dfdx(2,i) = -S(2);
            end
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh                 = cParams.mesh;
            obj.dim                  = cParams.dim;
            obj.designVariable       = cParams.designVariable;
            obj.stiffnessMatComputer = cParams.stiffnessMatComputer;
            obj.bendingMatComputer   = cParams.bendingMatComputer;
        end

        function computeEigenModesAndValues(obj)
            obj.stiffnessMatComputer.compute();
            obj.bendingMatComputer.compute();
            [Kfree,free]  = obj.stiffnessMatComputer.provideFreeStiffnessMatrix();
            obj.freeNodes = free;
            Bfree  = obj.bendingMatComputer.provideFreeBendingMatrix();
            obj.computeEigenFunctionAndValues(Bfree,Kfree);
        end

        function createEigModesPlotter(obj)
            s.mesh = obj.mesh;
            p = EigModesPlotter(s);
            obj.eigModesPlotter = p;
        end

        function l = computeLambda(obj)
            l = sort(diag(obj.D));
        end

        function computeEigenFunctionAndValues(obj,B,K)
            [v,d] = eigs(B,K,2,'SM');
            obj.V  = v;
            obj.D  = d;
        end

        function reorderModes(obj,lambda,V,D)
            if lambda(1)==D(1,1)
                V1=V(:,1);
                V2=V(:,2);
            else
                V1=V(:,2);
                V2=V(:,1);
            end
            obj.v1 = V1;
            obj.v2 = V2;
        end

        function [m1disp,m2disp] = computeBucklingModes(obj,v1,v2)
            N = obj.mesh.nelem;
            Mode1=zeros(2*(N+1),1);
            Mode2=zeros(2*(N+1),1);
            for i=3:2*N
                Mode1(i)=v1(i-2);
                Mode2(i)=v2(i-2);
            end
            m1 = Mode1;
            m2 = Mode2;
            m1disp = m1(1:2:end);
            m2disp = m2(1:2:end);
        end

    end

end


function [Estimation_X] = notREKF_propagate(Estimation_X, OdometryFromThis2Next, Sigma_ODO )

% retrieve v and w
v = OdometryFromThis2Next(1:3);
w = OdometryFromThis2Next(4:6);

NumberOfLandmarks = size(Estimation_X.landmarks, 2);
Jrw = jaco_r(-w);
ExpMinusM = so3_exp(-w);

temp = repmat({Estimation_X.orientation}, NumberOfLandmarks+2,1 );
A = blkdiag(temp{:});
 A(4:6,1:3)=skew(Estimation_X.position)*Estimation_X.orientation;

if NumberOfLandmarks>0
   for i=1:NumberOfLandmarks
    A(6+3*i-2:6+3*i,1:3)=skew(Estimation_X.landmarks(1:3,i))*Estimation_X.orientation;
   end
end

B1=[-Jrw  zeros(3,3); -skew(v)*Jrw -eye(3)];
B=[B1; sparse(3*NumberOfLandmarks,6)];
B=sparse(B);

adA=A*B;

odoCov=diag([w.^2;v.^2])*Sigma_ODO^2;

% final update the covariance
Estimation_X.cov = Estimation_X.cov+ adA*odoCov*adA';

% update position and orientation
Estimation_X.position = Estimation_X.position+Estimation_X.orientation*v;
Estimation_X.orientation = Estimation_X.orientation*so3_exp(w);

end


function s=v_sprintcpx(z,f)
%V_SPRINTCPX  format a complex number for printing S=(Z,F)
%
% Usage: fprintf('%s',v_sprintcpx(z));
%
%  Inputs: z   a complex number to print
%          f   optional formatting string as in fprintf e.g. '0.2f' [default: 'g']
%              may also include 'i' or 'j' [default] to control sqrt(-1) symbol.
%
% Outputs: s   formatted output string

%      Copyright (C) Mike Brookes 2015
%      Version: $Id: v_sprintcpx.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<2 || ~numel(f)
    f='g';
end
if any(f=='i')
    ij='i';
else
    ij='j';
end
f((f=='i')|(f=='j'))=[]; % remove i and j specifiers
if ~numel(f)
    f='g';
end
if any(f=='+')
    pl='';
else
    pl='+';
end
f=['%' f];
a=real(z);
b=imag(z);
jx=[1 3 2 4 3 4 1 3 2];
ix=jx(3*sign(a)+sign(b)+5);
switch(ix)
    case 1
        s=sprintf([f f ij],a,b);
    case 2
        s=sprintf([f pl f ij],a,b);
    case 3
        s=sprintf(f,a);
    case 4
        s=sprintf([f ij],b);
end


function [filteredPrice,filteredTime,actualTime] = realized_price_filter(price,time,timeType,samplingType,samplingInterval)
% Price filtering for computing realized variances and other
% high-frequency price based returns
%
% USAGE:
%   [FILTEREDPRICE,FILTEREDTIME,ACTUALTIME] =  realized_price_filter(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL)
%
% INPUTS:
%   PRICE             - m by 1 column vector of prices
%   TIME              - m by 1 column vector of times corresponding to PRICE.
%                         Must be sorted and increasing.
%   TIMETYPE          - String describing the way times are measured
%                        'wall'    24-hour clock of the form HHMMSS, e.g. 101543 or 153217
%                        'seconds' Time measures in seconds past midnight.
%                          TIME must satisfy 0<=TIME<86400
%                        'unit'  Unit normalized date format, e.g. .1, .234, .9
%                          Unit normalized times are more general than the
%                          other types and can be applied to data from more
%                          than one calendar day
%   SAMPLINGTYPE      - String describing the type of sampling to use when
%                         filtering PRICE
%                         'CalendarTime' - Sample in calendar time using
%                           observations separated by SAMPLINGINTERVAL
%                           seconds. If TIMETYPE is 'unit',
%                           SAMPLINGINTERVAL must be between 0 and 1 and
%                           represents the fraction of the sample to skip
%                           when sampling.
%                         'CalendarUniform' - Sample in calendar time using
%                           SAMPLINGINTERVAL observations spread uniformly
%                           between TIME(1) and TIME(m)
%                         'BusinessTime' - Sample in business (tick) time
%                           using observation separated by SAMPLINGINTERVAL
%                           ticks
%                         'BusinessUniform' - Sample in business (tick)
%                           time using observations uniformly spaced in
%                           business time.
%                         'Fixed' - Sample at specific points in time. When
%                           using fixed, SAMPLINGINTERVAL must be a n by 1 vector
%                           of times with the same TIMETYPE as TIME (i.e.
%                           seconds if TIME is in seconds)
%   SAMPLINGINTERVAL   - Scalar integer or n by 1 vector whose meaning depends on the
%                          selected SAMPLINGTYPE
%
% OUTPUTS:
%   FILTEREDPRICE      - n by 1 vector of filtered prices.  n depends on
%                          SAMPLINGTYPE as well as the data
%   FILTEREDTIME       - n by 1 vector of sampling times corresponding to
%                          the chosen sampling scheme
%   ACTUALTIME         - n by 1 vector. ACTUALTIME(i) provides the actual
%                          observation time that was used when sampling at FILTEREDTIME(i)
%
%
% COMMENTS:
%   This is a helper function for REALIZED_KERNEL.  Last price
%   interpolation is always used.  If SAMPLINGTYPE is 'Fixed' then any
%   requested samples before the first observation will be back filled
%   with PRICE(1). An indicator for back filled values can be constructed
%   using ACTUALTIME>FILTEREDTIME
%
% EXAMPLES:
%   % 5-minute prices
%   FILTEREDPRICE =  realized_price_filter(PRICE,TIME,'wall','CalendarTime',300)
%
%   % 79 prices spread uniformly in calendar time
%   FILTEREDPRICE =  realized_price_filter(PRICE,TIME,'wall','CalendarUniform',79)
%
%   % 20-tick returns
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'wall','CalendarUniform',79)
%
%   % 391 prices spread uniformly in business time
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'wall','CalendarUniform',79)
%
%   % 5-minute prices coresponding to 9:30:00, 9:35:00, ...
%   sampleTimes =  seconds2wall(wall2seconds(93000):300:wall2seconds(160000))';
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'wall','Fixed',sampleTimes)
%
%   % .05-calendar time sampling when using unit times
%   TIME = wall2unit(TIME,min(TIME),max(TIME));
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'unit','CalendarTime',.05)
%
%
%  See also REALIZED_KERNEL, REALIZED_KERNEL_CORE, REALIZED_KERNEL_WEIGHTS
%  REALIZED_KERNEL_SELECT_LAG_LENGTH, REALIZED_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=5
    error('Five inputs required.')
end
if size(price,2)>size(price,1)
    price=price';
end
if size(price,2)>1
    error('PRICE must be a m by 1 vector.')
end
if size(time,2)>size(time,1)
    time=time';
end
if any(diff(time)<0)
    error('TIME must be sorted and increasing')
elseif any(diff(time)==0)
    warning('oxfordRealized:realizedPriceFilter','TIME contains multiple entries with the same value. This creates an ambiguity and FILTEREDPRICE will contain the last price if TIME does not only unique elements.')
end
if size(time,2)>1 || length(time)~=length(price)
    error('TIME must be a m by 1 vector.')
end
timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','unit'})
    error('TIMETYPE must be one of ''wall'', ''seconds'' or ''unit''.');
end
% Inserted to protect against inputing integer times
time = double(time);

samplingType=lower(samplingType);
if ~ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform','fixed'})
    error('SAMPLINGTYPE must be one of ''CalendarTime'', ''CalendarUniform'', ''BusinessTime'', ''BusinessUniform'' or ''Fixed''.');
end

m=size(price,1);
t0Original=time(1);
tTOriginal=time(m);
if ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform'})
    % Must be a scalar integer, unless using unit times
    if (~isscalar(samplingInterval) || floor(samplingInterval)~=samplingInterval || samplingInterval<1) && ~strcmp(timeType,'unit')
        error('SAMPLINGINTERVAL must be a positive integer for the SAMPLINGTYPE selected.')
    end
else
    if size(samplingInterval,2)>size(samplingInterval,1)
        samplingInterval=samplingInterval';
    end
    if ~(any(samplingInterval>=t0Original) && any(samplingInterval<=tTOriginal))
        error('At least one sampling interval must be between min(TIME) and max(TIME) when using ''Fixed'' as SAMPLINGTYPE.')
    end
    if any(diff(samplingInterval)<=0)
        error('When using ''Fixed'' as SAMPLINGTYPE the vector of sampling times in SAMPLINGINTERVAL must be sorted and strictly increasing.')
    end
end

if strcmp(timeType,'unit') && strcmp(samplingType,'calendartime')
    % samplingInterval must be between 0 and 1
    if samplingInterval>1
        error('When TIMETYPE is ''unit'' and SAMPLINGTYPE is ''CalendarTime'', SAMPLINGINTERVAL must also be in ''unit'' terms, and so must be between 0 and 1')
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Get the size of the price
m=size(price,1);

% Convert times
if strcmp(samplingType,'fixed')
    time0 = min(samplingInterval);
    time1 = max(samplingInterval);
else
    time0 = min(time);
    time1 = max(time);
end
if strcmp(timeType,'wall')
    time = wall2unit(time,time0,time1);
elseif strcmp(timeType,'seconds')
    time = seconds2unit(time,time0,time1);
end
% If fixed convert these times as well
if strcmp(samplingType,'fixed')
    if strcmp(timeType,'wall')
        samplingInterval = wall2unit(samplingInterval,time0,time1);
    elseif strcmp(timeType,'seconds')
        samplingInterval = seconds2unit(samplingInterval,time0,time1);
    end
end

% Get the first and last time
t0=time(1);
tT=time(m);
% Switch based on the cases
switch samplingType
    case 'calendartime'
        % If calendar time
        % Convert sampling interval to be a fraction of a period
        % Unit strides
        if strcmp(timeType,'wall')
            timeBase = wall2seconds([time0 time1]);
            samplingInterval = samplingInterval/diff(timeBase);
        elseif strcmp(timeType,'seconds')
            samplingInterval = samplingInterval/(time1-time0);
        end
        filteredTime=(t0:samplingInterval:tT)';
        % Append the final observation
        if ~ismember(tT,filteredTime)
            filteredTime=[filteredTime; tT];
        end
        % Compute the filtered prices
        [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime);
    case 'calendaruniform'
        % Sampling interval contains the number of samples
        % Uniform spacing
        filteredTime=linspace(t0,tT,samplingInterval)';
        % Compute the filtered prices
        [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime);
    case 'businesstime'
        indices=(1:samplingInterval:m)';
        if ~ismember(m,indices)
            indices=[indices;m];
        end
        filteredPrice=price(indices);
        filteredTime=time(indices);
        actualTime = filteredTime;
    case 'businessuniform'
        % Sampling interval contains the number of samples
        indices=floor(linspace(1,m,samplingInterval));
        filteredPrice=price(indices);
        filteredTime=time(indices);
        actualTime = filteredTime;
    case 'fixed'
        filteredTime = samplingInterval;
        % Compute the filtered prices
        [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime);
    otherwise
        error('Unrecogniced SAMPLINGTYPE.')
end

% Finally convert filteredTime back to the original time format
if strcmp(timeType,'wall')
    filteredTime = unit2wall(filteredTime,time0,time1);
    actualTime = unit2wall(actualTime,time0,time1);
elseif strcmp(timeType,'seconds')
    filteredTime = unit2seconds(filteredTime,time0,time1);
    actualTime = unit2seconds(actualTime,time0,time1);
end

function [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime)

% Get the size of the inputs
m=size(price,1);
n=size(filteredTime,1);
% This uses a 2-index algorithm so that it is fast for any realistic size.
timeIndex=1;
filteredTimeIndex=1;
% Pl hold the index values.  Initializing to 1 makes the back fill easy
pl=ones(n,1);
while timeIndex<=m && filteredTimeIndex<=n
    if time(timeIndex)<=filteredTime(filteredTimeIndex)
        pl(filteredTimeIndex)=timeIndex;
        timeIndex=timeIndex+1;
    elseif filteredTimeIndex<n
        % Increment filteredTimeIndex until filteredTime>=time(timeIndex)
        while filteredTimeIndex<n && filteredTime(filteredTimeIndex)<time(timeIndex)
            filteredTimeIndex = filteredTimeIndex + 1;
            % Last price interploation for these since there is no new
            % information
            pl(filteredTimeIndex) = pl(filteredTimeIndex-1);
        end
        % Only assign if you aren't on the last one
        if filteredTimeIndex<n
            pl(filteredTimeIndex)=timeIndex;
        end
    elseif time(timeIndex)>filteredTime(n)
        % No more to assign, so break
        break
    end
end

% Clean up any trailing ones
starter = find(diff(pl)<0) + 1;
if ~isempty(starter)
    pl(starter:length(pl)) = pl(starter-1);
end
% Use pl the index the filteredPrice and filterTimeActual
filteredPrice = price(pl);
actualTime = time(pl);


function [varargout]=quiverVec(varargin)

% function [h]=quiverVec(P,V,vecSize,colorSpec,edgeColorOpt,quiverStyleOpt,alphaLevel)
% ------------------------------------------------------------------------
% Plots the vectors V with origins P as patch data arrows. The vectors size
% can be specified as vecSize allong with the colors in colorSpec. Other
% options include the edge color, quiver style, and transparency level.
%
%
% Kevin Mattheus Moerman
%
% Change log:
% 2020/07/14 Added single origin support
% ------------------------------------------------------------------------

%% Parse input

switch nargin
    case 2
        P=varargin{1};
        V=varargin{2};
        vecSize=[];
        colorSpec=[];
        edgeColorOpt='none';
        quiverStyleOpt=1;
        alphaLevel=1;
    case 3
        P=varargin{1};
        V=varargin{2};
        vecSize=varargin{3};
        colorSpec=[];
        edgeColorOpt='none';
        quiverStyleOpt=1;
        alphaLevel=1;
    case 4
        P=varargin{1};
        V=varargin{2};
        vecSize=varargin{3};
        colorSpec=varargin{4};
        edgeColorOpt='none';
        quiverStyleOpt=1;
        alphaLevel=1;
    case 5
        P=varargin{1};
        V=varargin{2};
        vecSize=varargin{3};
        colorSpec=varargin{4};
        edgeColorOpt=varargin{5};
        quiverStyleOpt=1;
        alphaLevel=1;
    case 6
        P=varargin{1};
        V=varargin{2};
        vecSize=varargin{3};
        colorSpec=varargin{4};
        edgeColorOpt=varargin{5};
        quiverStyleOpt=varargin{6};
        alphaLevel=1;
    case 7
        P=varargin{1};
        V=varargin{2};
        vecSize=varargin{3};
        colorSpec=varargin{4};
        edgeColorOpt=varargin{5};
        quiverStyleOpt=varargin{6};
        alphaLevel=varargin{7};
end

if size(P,1)==1 %Expand to match V if single origin is given
    P=P(ones(size(V,1),1),:);
end

%%
if ~isempty(P) || ~isempty(V)

    if isempty(edgeColorOpt)
        edgeColorOpt='none';
    end

    if isempty(quiverStyleOpt)
        quiverStyleOpt=1;
    end

    switch quiverStyleOpt
        case 1 %Depart from origin
            %Keep as is
        case 2 %Arrive at origin
            P=P-V;
        case 3 %Pass through origin
            P=P-(V/2);
        case 4 %Two-sided
            P=[P;P];
            V=[V;-V];
            if ~ischar(colorSpec) && size(colorSpec,1)>1
                colorSpec=[colorSpec;colorSpec];
            end
    end

    if size(P,2)==2
        P(:,3)=0;
    end

    if size(V,2)==2
        V(:,3)=0;
    end

    if numel(vecSize)==1
        vecSize=vecSize*ones(1,2);
    end

    if ischar(colorSpec)
        [F,P,~]=quiver3Dpatch(P(:,1),P(:,2),P(:,3),V(:,1),V(:,2),V(:,3),[],vecSize);
        C=colorSpec;
    else
        if size(colorSpec,1)==1 %If only 1 color is provided
            colorSpec=colorSpec(ones(size(P,1),1),:); %copy for all vectors
        end

        [F,P,C]=quiver3Dpatch(P(:,1),P(:,2),P(:,3),V(:,1),V(:,2),V(:,3),colorSpec,vecSize);
    end

    h=gpatch(F,P,C,edgeColorOpt,alphaLevel);
else
    h=[];
end

if nargout>0
    varargout{1}=h;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function p = power(a,b)
	if isscalar(a)
		a = a*ones(size(b),'uint64');
	elseif isscalar(b)
		b = a*ones(size(b),'uint64');
	end
	if ~isequal(size(a),size(b))
		error('Size mismatch');
	end

	p = ones(size(a),'uint64');
	for k = 1:length(a)
		if b(k)>63
			error('Power>63');
		end
		if a(k)==0 && b(k)==0
			error('0^0');
		end
		for pow = 1:double(b(k))
			p(k) = p(k)*a(k);
		end
	end
end


function fdallfig()
    % Plot the correlation integral versus the distance calculated at each grid point.
    % Francesco Pacchiani 1/2000
    %
    %
    % turned into function by Celso G Reyes 2017

    ZG=ZmapGlobal.Data; % used by get_zmap_globals
    HCIfig=findobj('Type','Figure','-and','Name','Correlation Integral');

    if ~isempty(HCIfig)
        fig = 'addfig';
    else
        fig = 'orifig';
    end

    D = coef(1,1);
    col = 0.6:0.45:3.3;
    colin = jet(64);
    Db = D-0.6;
    Dc = round((Db*64)/2.7);

    switch(fig);

        case 'orifig'

            HCIfig = figure_w_normalized_uicontrolunits('Numbertitle','off','Name','Correlation Integral', 'Visible','on');
            Haxes = gca;

            if D < 0.6 | 3.3 < D

                str5 = 'The fractal dimension calculated is not comprised in the interval 0.6-3.3. The scaling range might be the problem. ';
                msg3 = msgbox(str5, 'Input Error');
                waitforbuttonpress;
                close(msg3);
                close(HCIfig);
            end

            Hline = loglog(r, corint,'color', [colin(Dc,1) colin(Dc,2) colin(Dc,3)]);
            set(Hline,'Linewidth',1);
            xlabel('Interevent Distance R [km]', 'fontsize',12);
            ylabel('Correlation Integral C(R)', 'fontsize',12);
            title('Correlation Integral of All Subsets', 'fontsize',14);
            set(Haxes, 'fontsize', 11);
            set(gca,'NextPlot','add');

        case 'addfig'

            figure(HCIfig);
            set(gcf,'visible','on');
            axes(Haxes);
            set(gca,'NextPlot','add');
            if Dc < 1; Dc = 1; end
            if Dc > 63; Dc = 63; end
            Hline = loglog(r, corint,'color', [colin(Dc,1) colin(Dc,2) colin(Dc,3)]);
            axis([0.001 100 0.000001 5]);
            set(Haxes, 'fontsize', 11);

    end %switch

end


%  generate a random mxn matrix with entries in [-1,1]
%
%  syntax  >> A = Srand(m,n)


function [Y, X] = minandmax2( f )
%MINANDMAX2   Find global minimum and maximum of a SEPARABLEAPPROX.
%   Y = minandmax2(F) returns the minimum and maximum value of a SEPARABLEAPPROX over
%   its domain. Y is a vector of length 2 such that Y(1) = min(f(x,y)) and Y(2)
%   = max(f(x,y)).
%
%   [Y, X] = minandmax2(F) also returns the position of the minimum and
%   maximum. For example,
%
%       F(X(1,1),X(1,2)) = Y(1)     and      F(X(2,1),X(2,2)) = Y(2)
%
% See also MAX2, MIN2, NORM.

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

% check for empty SEPARABLEAPPROX.
if ( isempty( f ) )
    Y = [];
    X = [];
    return
end

% This is a default implementation for separableApprox children that do not
% have know how to properly implement this method.

% Maximum possible sample matrix size:
maxsize = 4e3;

% Is the function the zero function?
if ( iszero( f )  )
    dom = f.domain;
    X = [ (dom(2) + dom(1))/2 (dom(4) + dom(3))/2 ];
    X = [ X ; X ];
    Y = [0 ; 0];
    return;
end

% Extract low rank representation:
frows = f.rows;
fcols = f.cols;
piv = f.pivotValues;
dom = f.domain;

% Convert rows and columns to chebfuns.
frows = simplify(chebfun(frows, dom(1:2), maxsize));
fcols = simplify(chebfun(fcols, dom(3:4), maxsize));

% Share out scaling:
sgn = sign( piv ).';
sq = 1 ./ sqrt( abs( piv ) );
frows = frows * diag( sq.'.*sgn );
fcols = fcols * diag( sq );

if ( length(f) == 1 ) % Rank-1 is easy:
    % We can find it from taking maximum and minimum in x and y direction.

    % Find minandmax of rows and columns:
    [yr, xr] = minandmax( frows );
    [yc, xc] = minandmax( fcols );
    % All possible combinations:
    vv = [yr(1)*yc(1), yr(1)*yc(2), yr(2)*yc(1), yr(2)*yc(2)];

    [Y(2), indmx] = max( vv );
    [Y(1), indmn] = min( vv );

    % Work out the location of the maximum.
    X = zeros(2);
    X(1,1) = xr(2);
    X(1,2) = xc(2);

    if ( indmn <= 2 )
        X(1,1) = xr(1);
    end
    if ( mod(indmn,2) == 1 )
        X(1,2) = xc(1);
    end
    X(2,1) = xr(2);
    X(2,2) = xc(2);

    if ( indmx <= 2 )
        X(2,1) = xr(1);
    end

    if ( mod(indmx,2) == 1 ),
        X(2,2) = xc(1);
    end

elseif ( length(f) <= maxsize )

    % We seek a fast initial guess. So we first truncate the SEPARABLEAPPROX.
    ypts = chebpts(length(fcols), fcols.domain);
    xpts = chebpts(length(frows), frows.domain);
    cvals = feval(fcols, ypts);
    rvals = feval(frows, xpts);

    A = cvals*rvals.';
    % Maximum entry in discretisation.
    [ignored, ind] = min( A(:) ); %#ok<ASGLU>
    [row, col] = ind2sub(size(A), ind);
    X(1,1) = xpts( col );
    X(1,2) = ypts( row );
    Y(1) = feval( f, X(1,1), X(1,2) );
    % Minimum entry in discretisation.
    [ignored, ind] = max(A(:)); %#ok<ASGLU>
    [row, col] = ind2sub(size(A), ind);
    X(2,1) = xpts(col);
    X(2,2) = ypts(row);
    Y(2) = feval(f, X(2,1), X(2,2));

    % Get more digits with optimisation algorithms.
    lb = [ dom(1) ; dom(3) ];
    ub = [ dom(2) ; dom(4) ];

    try

        % If the optimization toolbox is available then use it to get a better maximum.

        warnstate = warning;
        warning('off'); %#ok<WNOFF> % Disable verbose warnings from fmincon.
        options = optimset('Display', 'none', 'TolFun', eps, 'TolX', eps, ...
            'algorithm', 'active-set');
        [mn, Y(1)] = fmincon(@(x,y) feval(f, x(1), x(2)), X(1, :), ...
            [], [], [], [], lb, ub, [], options);
        [mx, Y(2)] = fmincon(@(x) -feval(f, x(1), x(2)), X(2,:), ...
            [], [], [], [], lb, ub, [], options);
        Y(2) = -Y(2);
        X(1,:) = mn;
        X(2,:) = mx;
        warning(warnstate);

    catch

        try
            % Try converting to an unconstrained problem and using built-in solver.

            % Maps from [-1, 1] to [dom(1:2)] and [dom(3:4)], respectively.
            map1 = bndfun.createMap(dom(1:2));
            map2 = bndfun.createMap(dom(3:4));
            % Unconstrained initial guesses:
            Z(:,1) = asin(map1.Inv(X(:,1)));
            Z(:,2) = asin(map2.Inv(X(:,2)));
            % Maps from R to [dom(1), dom(2)] and [dom(3), dom(4)], respectively.
            map1 = @(x) map1.For(sin(x));
            map2 = @(x) map2.For(sin(x));
            % Set options:
            options = optimset('Display', 'off', 'TolFun', eps, 'TolX', eps);
            warnstate = warning;
            warning('off'); %#ok<WNOFF> % Disable verbose warnings from fminsearch.
            f_mapped = @(x) feval(f, map1(x(1)), map2(x(2)));
            [mn, Y(1)] = fminsearch(@(x) f_mapped(x), Z(1, :), options);
            [mx, Y(2)] = fminsearch(@(x) -f_mapped(x), Z(2, :), options);
            Y(2) = -Y(2);
            X(1:2,1) = map1([mn(1) ; mx(1)]);
            X(1:2,2) = map2([mn(2) ; mx(2)]);
            warning(warnstate);

        catch

            % Nothing is going to work so initial guesses will have to do.

        end

    end

elseif ( length(f) >= maxsize )

    error('CHEBFUN:SEPARABLEAPPROX:minandmax2:length', 'Rank is too large.');

end

end

%%%
% Use the approach below when bivariate rootfinding is fully implemented.
%%%
% % Use bivariate rootfinding to find all the local extrema:
% F = gradient( f );
% r = roots( F );
% if ( ~isempty( r ) )
%     [inMax, idInMax] = max( feval(f, r(:,1), r(:,2) ) );
%     [inMin, idInMin] = min( feval(f, r(:,2), r(:,2) ) );
% else
%     inMax = inf;   % max and min must occur on boundary.
%     inMin = inf;
% end
%
% % Search along boundary:
% dom = f.domain;
% left = feval(f, dom(1), ':');
% right = feval(f, dom(2), ':');
% down = feval(f, ':', dom(3));
% up = feval(f, ':', dom(4));
% [Yleft, Xleft] = minandmax( left );
% [Yright, Xright] = minandmax( right );
% [Yup, Xup] = minandmax( up );
% [Ydown, Xdown] = minandmax( down );
%
% % Store Min/Max location for later:
% BcMinLocations = [ Xleft(1,:) ; Xright(1,:) ; Xup(1,:) ; Xdown(1,:) ];
% BcMaxLocations = [ Xleft(2,:) ; Xright(2,:) ; Xup(2,:) ; Xdown(2,:) ];
%
% [ BcMax, idBcMax ] = max( [ Yleft(2), Yright(2), Yup(2), Ydown(2) ].' );
% [ BcMin, idBcMin ] = min( [ Yleft(1), Yright(1), Yup(1), Ydown(1) ].' );
%
% % What is the global min and max?:
% [Ymax, inOrOutMax] = max( [inMax, BcMax].' );
% [Ymin, inOrOutMin] = min( [inMin, BcMin].' );
% Y = [Ymin, Ymax];
% X = zeros(2);
%
% % Unravel to find locations:
% if ( inOrOutMin == 1 )
%     X(1,:) = [ r(idInMin,1), r(idInMin,2) ];
% else
%     X(1,:) = BcMinLocations(idBcMin, :);
% end
% if ( inOrOutMax == 1 )
%     X(2,:) = [ r(idInMax,1), r(idInMax,2) ];
% else
%     X(2,:) = BcMaxLocations(idBcMax, :);
% end
%
% end