function [ vX ] = ProjectL1BallDual( vY, ballRadius, vLowerBound, vUpperBound )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProjectL1Ball( vY, ballRadius, vLowerBound, vUpperBound )
%   Solving the Orthogonal Porjection Problem of the input vector onto the
%   L1 Ball with Box Constraints using Dual Function.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - ballRadius    -   Ball Radius.
%                       Sets the Radius of the L1 Ball. For Unit L1 Ball
%                       set to 1.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
%   - vLowerBound   -   Lower Bound Vector.
%                       Sets the lower bound values of the solution
%                       (Element wise).
%                       Structure: Vector.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vUpperBound   -   Upper Bound Vector.
%                       Sets the upper bound values of the solution
%                       (Element wise).
%                       Structure: Vector.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - vX            -   Output Vector.
%                       The projection of the Input Vector onto the L1
%                       Ball.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  https://math.stackexchange.com/a/2830242/33.
% Remarks:
%   1.  S
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     24/06/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

DEBUG_MODE = OFF;

paramLambda     = 0; %<! Initialization value

% Check feasibility of the problem
minSum = 0;
for ii = 1:size(vY, 1)
    if(sign(vLowerBound(ii)) == sign(vUpperBound(ii)))
        % Zero isn't within the boundary (Or both are zero)
        minSum = minSum + min(abs(vLowerBound(ii)), abs(vUpperBound(ii)));
    end
end

if(minSum > ballRadius)
    % The problem is infeasible
    vX = mean([vLowerBound, vUpperBound], 2);
    return;
end

% The dual objective function which should be maximized to find the optimal
% 'paramLambda'.
% The functios is negated as we want to maximize while using
% MATLAB's minimization function
hObjFun = @(paramLambda) -ObjectiveDualFunction(vY, ballRadius, vLowerBound, vUpperBound, paramLambda); %<! Objective function

if(DEBUG_MODE == ON)
    numSamples = 1000;

    vParamLambda    = linspace(0, 5, numSamples);
    vObjVal         = zeros([numSamples, 1]);

    for ii = 1:numSamples
        vObjVal(ii) = hObjFun(vParamLambda(ii));
    end

    figure();
    plot(vParamLambda, vObjVal);
end

paramLambda = fminsearch(hObjFun, paramLambda); %<! Objective Function isn't smooth hence can't use Newton Method
paramLambda = max(paramLambda, 0);

% Solution as derived for the updated forumaltion of the problem.
[~, vX] = ObjectiveDualFunction(vY, ballRadius, vLowerBound, vUpperBound, paramLambda);

end

function [ valObj, vX ] = ObjectiveDualFunction( vY, ballRadius, vL, vU, paramLambda )

vX      = zeros([size(vY, 1), 1]);

for ii = 1:size(vY, 1)

    if(sign(vL(ii)) == sign(vU(ii)))
        valX = ProjBoxFunction(vY(ii) - (paramLambda * sign(vL(ii))), vL(ii), vU(ii));
    elseif(sign(vY(ii)) == sign(vL(ii)))
        % Implictily vL(ii) <= 0, vU(ii) >= 0 and vY(ii) <= 0
        valX = ProjBoxFunction(vY(ii) + paramLambda, vL(ii), 0); %<! Making sure sign of valX is non positive
    elseif(sign(vY(ii)) == sign(vU(ii)))
        % Implictily vU(ii) >= 0, vL(ii) <= 0 and vY(ii) >= 0
        valX = ProjBoxFunction(vY(ii) - paramLambda, 0, vU(ii)); %<! Making sure sign of valX is non negative
    end

    vX(ii) = valX;

end

valObj = (0.5 * sum((vX - vY) .^ 2)) + (paramLambda * (sum(abs(vX)) - ballRadius));

end

function [ outVal ] = ProjBoxFunction( valIn, valL, valU )

outVal = min(max(valIn, valL), valU);

end


function [H_filt, h_filt] = arraySHTfiltersMeas_regLSHD(H_array, order_sht, grid_dirs_rad, w_grid, nFFT, amp_threshold)
%ARRAYSHTFILTERSMEAS_REGLSHD Generate SHT filters based on measured responses
%(regularized least-squares in the SHD)
%
%   Generate the filters to convert microphone signals from a spherical
%   microphone array to SH signals, based on a least-squares solution with
%   a constraint on noise amplification, using Tikhonov regularization. The
%   method formulates the LS problem in the spherical harmonic domain, by
%   first expanding the array response to SH coefficients, and then solving
%   the LS problem, similar to
%
%       Jin, C.T., Epain, N. and Parthy, A., 2014.
%       Design, optimization and evaluation of a dual-radius spherical microphone array.
%       IEEE/ACM Transactions on Audio, Speech, and Language Processing, 22(1), pp.193-204.
%
%   Inputs:
%       H_array:    nFFT x nMics x nMeasurement_dirs matrix of measured
%           responses, in the frequency domain with length nFFT
%       order_sht:  order of SH signals to generate
%       grid_dirs:  nMeasurement_dirs x 2 matrix of [azi elev] of the
%           measurement directions, in rads
%       w_grid:     nMeasurement_dirs x 1 vector of weights for
%           weighted-least square solution, based on the importance or area
%           around each measurement point (leave empty if not known, or
%           not important)
%       nFFT:       number of FFT points in the responses
%       amp_threshold:  max allowed amplification for filters, in dB
%
%   Outputs:
%       H_filt: nSH x nMics x (nFFT/2+1) returned filters in the frequency
%           domain (half spectrum up to Nyquist)
%       h_filt: nFFT x nMics x nSH impulse responses of the above filters
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% ARRAYSHTFILTERSMEAS_REGLSHD.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nGrid = size(H_array,3);
nMics = size(H_array,2);
if order_sht>sqrt(nMics)-1
    warning('Set order too high for the number of microphones, should be N<=sqrt(Q)-1')
    order_sht = floor( sqrt(nMics)-1 );
end
nBins = nFFT/2+1;
if isempty(w_grid)
    w_grid = ones(nGrid,1);
end

% SH matrix at grid directions
order_array = floor(sqrt(nGrid)/2-1);
aziElev2aziPolar = @(dirs) [dirs(:,1) pi/2-dirs(:,2)]; % function to convert from azimuth-inclination to azimuth-elevation
Y_grid = sqrt(4*pi) * getSH(order_array, aziElev2aziPolar(grid_dirs_rad), 'real')'; % SH matrix for grid directions

% compute inverse matrix
a_dB = amp_threshold;
alpha = 10^(a_dB/20);
beta = 1/(2*alpha);
W_grid = diag(w_grid);
H_filt = zeros((order_sht+1)^2, nMics, nBins);
% compute first the SHT of the array response
for kk=1:nBins
    tempH = squeeze(H_array(kk,:,:));
    H_nm(kk,:,:) = tempH * W_grid * Y_grid'* inv(Y_grid*W_grid*Y_grid');
end
% compute the inverse matrix in the SHD with regularization
for kk=1:nBins
    tempH_N = squeeze(H_nm(kk,:,:));
    tempH_N_trunc = tempH_N(:,1:(order_sht+1)^2);
    H_filt(:,:,kk) = tempH_N_trunc' * inv(tempH_N*tempH_N' + beta^2*eye(nMics));
end

if nargout>1
    % time domain filters
    h_filt = H_filt;
    h_filt(:,:,end) = abs(h_filt(:,:,end));
    h_filt = cat(3, h_filt, conj(h_filt(:,:,end-1:-1:2)));
    h_filt = real(ifft(h_filt, [], 3));
    h_filt = fftshift(h_filt, 3);
end


function welldrawdown
% Well drawdown - comparison for confined / half-confined / unconfined
% aquifers
%    using analytocal solutions
%
%   $Ekkehard Holzbecher  $Date: 2006/04/30 $
%--------------------------------------------------------------------------
K = 1.1e-5;            % hydraulic conductivity
H = 10;                % depth
T = K*H;               % transmissivity
Q = 1.e-4;             % pumping rate
r0 = 0.1;              % well radius
s0 = -1.;               % drawdown in well
c = 0.8e8;             % resistance of half-permeable layer
R = 35;                % maximum radius

r = linspace (r0,R,100);
hc = s0 + (Q/(2*pi*T))*log(r/r0);
hu = -H + s0 + sqrt(H*H + (Q/(pi*K))*log(r/r0));
hh = -(Q/(2*pi*T))*besselk(0,r/sqrt(T*c));

plot (r,hc,r,hh,r,hu);
legend ('confined','half-confined','unconfined')
xlabel ('distance [m]'); ylabel ('drawdown (neg) [m]');
title('Groundwater Drawdown due to Pumping')


function m=pesq2mos(p)
%PESQ2MOS convert PESQ speech quality scores to MOS m=(p)
%Inputs:    p  is a matrix of PESQ scores
%
%Outputs:   m  is a matrix, the same size as p, of MOS scores
%
% The PESQ measure is defined in [2]. The mapping function, defined in [3],
% converts raw PESQ scores (which lie in the range -0.5 to 4.5) onto the
% MOS-LQO (Mean Opinion Score - Listening Quality Objective [2]) scale in the
% range 1 to 5. The MOS scale is defined in [1] as
%           5=Excellent, 4=Good, 3=Fair, 2=Poor, 1=Bad.
%
% Refs: [1]	ITU-T. Methods for subjective determination of transmission quality.
%           Recommendation P.800, Aug. 1996.
%       [2]	ITU-T. Mean opinion score (MOS) terminology.
%           Recommendation P.800.1, July 2006.
%       [2]	ITU-T. Perceptual evaluation of speech quality (PESQ), an objective
%           method for end-to-end speech quality assessment of narrowband telephone
%           networks and speech codecs. Recommendation P.862, Feb. 2001.
%       [3]	ITU-T. Mapping function for transforming P.862 raw result scores to MOS-LQO.
%           Recommendation P.862.1, Nov. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: pesq2mos.m 3289 2013-08-01 13:56:02Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=0.999;
    b=4.999-a;
    c=-1.4945;
    d=4.6607;
end
if nargout>0
    m=a+b./(1+exp(c*p+d));
else
    if nargin<1 || isempty(p)
        pp=linspace(-0.5,4.5,100);
    else
        pp=p;
    end
    plot(pp,pesq2mos(pp));
    xlabel('PESQ (P.862)');
    ylabel('Mean Opimion Score (MOS)');
end


function [y, stopCondition, sigma, sigma0, t] ...
    = cons_smacof_pip(dx, y, isFree, bnd, w, con, smacof_opts, scip_opts)
% CONS_SMACOF_PIP  SMACOF algorithm with polynomial constraints (PIP file
% format).
%
% Scaling by MAjorizing a COnvex Function (SMACOF) is an iterative solution
% to the Multidimensional Scaling (MDS) problem (see de Leeuw and Mair [1]
% for an up to date review).
%
% The classic implementation of SMACOF uses an iterative algorithm that
% relies on the Guttman transform update. Our function here implements a
% different approach, where the Guttman transform update is replaced by
% solving a Quadratic Program (QP) (as proposed by Dwyer et al [2]) with
% polynomial constraints.
%
% In this implementation, we use the SCIP binary to solve the constrained
% QP. This binary can be downloaded for Linux, MacOS X and Windows from the
% Zuse Institute Berlin (ZIB) website
%
%   http://scip.zib.de/#download
%
% The objective function is
%
%   min_y 1/2 y' * H * y + f' * y
%
% subject to the constraints and bounds provided by the user.
%
% We use the PIP file format to formulate the constrained QP and pass it to
% SCIP.
%
%   http://polip.zib.de/pipformat.php
%
%
% [Y, STOPCONDITION, SIGMA, SIGMA0, T] = CONS_SMACOF_PIP(D, Y0, ISFREE, BND, [], CON)
%
%   D is an (N, N)-distance matrix, with distances between the points in an
%   N-point configuration. D can be full or sparse. D(i,j)=0 means that
%   vertices i and j are not directly connected.
%
%   Y0 is an initial guess of the solution, given as an (N, P)-matrix,
%   where P is the dimensionality of the output points. Currently, P must
%   be either 2 or 3.
%
%   BND is a cell array with the variable bounds in PIP format, and cannot
%   be empty (otherwise SCIP doesn't return a solution). E.g.
%
%      BND = {'Bounds', ' -1 <= x1 <= 4', ' -1 <= y1 <= 2.5', ...
%             ' -1 <= x2 <= 4', ' -1 <= y2 <= 2.5'};
%
%   CON is a cell array with the problem constraints in PIP format, and
%   cannot be empty (otherwise SCIP doesn't return a solution). E.g.
%
%      CON = {'Subject to', ...
%            ' c1: -0.5 x6 y7 +0.5 x3 y7 +0.5 x7 y6 -0.5 x3 y6 -0.5 x7 y3 +0.5 x6 y3 >= 0.1'};
%
%   Y is the best solution computed by the algorithm within all iterations.
%   Y is a point configuration with the same size as Y0. If the algorithm
%   cannot find any valid solution (e.g. because we set a too short time
%   limit, or because a valid solution doesn't exist, Y will be an array of
%   NaNs).
%
%   STOPCONDITION is a cell array with a string for each stop condition
%   that made the algorithm stop at the last iteration.
%
%   SIGMA is a vector with the stress value at each iteration. Weighted
%   stress is given as
%
%     SIGMA = \sum_{i<j} W_ij (D_ij - DY_ij)^2
%
%   where W is a weight matrix the same size as D. W_ij = 0 means that the
%   distance between points i and j does not affect the stress measure.
%
%   If the algorithm could not find any solution, SIGMA is a single scalar
%   with value = Inf. Note that even if the last SIGMA value is Inf, this
%   doesn't mean that the algorithm hasn't found a valid solution in a
%   previous iteration.
%
%   SIGMA0 is a scalar with the stress value of the initial guess Y0. Note
%   that we have no guarantee that Y0 is a valid solution, so be careful
%   when comparing SIGMA0 to SIGMA. In particular, SIGMA0 may be smaller
%   than any SIGMA, but may not be a valid solution according to the
%   constraints.
%
%   T is a vector with the time between the beginning of the algorithm and
%   each iteration. Units in seconds.
%
% Y = CONS_SMACOF_PIP(..., SMACOF_OPTS, SCIP_OPTS)
%
%   SMACOF_OPTS is a struct with parameters to tweak the SMACOF algorithm.
%
%     'MaxIter': (default = 0) Maximum number of majorization iterations we
%                allow the optimisation algorithm.
%
%     'Epsilon': (default = Inf) The algorithm will stop if
%                (SIGMA(I+1)-SIGMA(I))/SIGMA(I) < OPTS.Epsilon.
%
%     'Display': (default = 'off') Do not display any internal information.
%                'iter': display internal information at every iteration.
%
%     'TolFun':  (default = 1e-12) Termination tolerance of the stress
%                value.
%
%   SCIP_OPTS is a struct with parameters to tweak the SCIP algorithm.
%
%     'scipbin': (defaults = 'scip.linux.x86_64.gnu.opt.spx' (Linux),
%                            'scip.darwin.x86_64.gnu.opt.spx' (Mac),
%                            'scip.mingw.x86_64.intel.opt.spx.exe' (Win))
%                Name of the SCIP binary/executable. This binary should be
%                available in the system path. E.g. place it in
%                gerardus/programs. The binaries/executable can be
%                downloaded from http://scip.zib.de/#download. You may need
%                to rename them so that they fit the expected filenames.
%
%     'display_verblevel': (default 4) verbosity level of output (0: SCIP
%                quiet mode).
%
%     'display_freq': (default 100) frequency for displaying node
%                information lines.
%
%     'limits_absgap': (default 0.0) solving stops, if the absolute
%                gap = |primalbound - dualbound| is below the given value.
%
%     'limits_gap': (default 0.0) solving stops, if the relative
%                gap = |primal - dual|/MIN(|dual|,|primal|) is below the
%                given value.
%
%     'limits_time': (default 1e+20) maximal time in seconds to run.
%
%     'limits_solutions': (default -1) solving stops, if the given number
%                of solutions were found (-1: no limit).
%
%     'lp_threads': (default 0: automatic) number of threads used for
%                solving the LP.
%
%     'numerics_feastol': (default 1e-6) feasibility tolerance for
%                constraints in SCIP. If this value is changed from the
%                default, it needs to be passed to the function that
%                creates the constraints too, e.g.
%                tri_ccqp_smacof_nofold_sph_pip().
%
%
% [1] J de Leeuw, P Mair, "Multidimensional scaling using majorization:
% SMACOF in R", Journal of Statistical Software, 31(3), 2009.
%
% [2] T. Dwyer, Y. Koren, and K. Marriott, "Drawing directed graphs using
% quadratic programming," IEEE Transactions on Visualization and Computer
% Graphics, vol. 12, no. 4, pp. 536-548, 2006.
%
% See also: cmdscale, qcqp_smacof, tri_sphparam.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014, 2016 University of Oxford
% Version: 0.4.6
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

%% Input arguments

% check arguments
narginchk(6, 8);
nargoutchk(0, 5);

% start clock
tic

% number of points
N = size(dx, 1);

% dimensionality of the points
D = size(y, 2);
if ((D ~= 2) && (D ~= 3))
    error('Only implemented for 2D output')
end

% check inputs
if (N ~= size(dx, 2))
    error('D must be a square matrix')
end
if (N ~= size(y, 1))
    error('Y0 must have the same number of rows as D')
end

% defaults
if (nargin < 3 || isempty(isFree))
    isFree = true(N, 1);
end
Nfree = nnz(isFree);

if (isempty(w))
    % if the user doesn't provide a weight matrix, we simply assign 1 if
    % two vertices are connected, and 0 if not
    w = double(dx ~= 0);
    if (any(diag(w)) ~= 0)
        error('Assertion error: W matrix has diagonal elements that are non-zero')
    end
end
if (any(size(w) ~= [N N]))
    error('W must be a square matrix with the same size as D')
end

% if the user doesn't allow any vertices to be optimized, then we can exit
% the function
if (Nfree == 0)
    stopCondition = 'No free vertices to optimise';
    sigma = [];
    dy = dmatrix_con(dx, y);
    sigma0 = 0.5 * sum(sum(w .* (dx - dy).^2));
    t = [];
    return;
end

% SMACOF_OPTS defaults
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'MaxIter'))
    smacof_opts.MaxIter = 100;
end
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'Epsilon'))
    smacof_opts.Epsilon = 0;
end
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'Display'))
    smacof_opts.Display = 'off';
end
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'TolFun'))
    smacof_opts.TolFun = 1e-12;
end

% SCIP_OPTS
if (nargin < 8 || isempty(scip_opts) || ~isfield(scip_opts, 'scipbin'))

    % default name of the SCIP binary depending on the architecture
    if (isunix)
        SCIPBIN = 'scip.linux.x86_64.gnu.opt.spx';
    elseif (ismac)
        SCIPBIN = 'scip.darwin.x86_64.gnu.opt.spx';
    elseif (ispc)
        SCIPBIN = 'scip.mingw.x86_64.intel.opt.spx.exe';
    else
        error('Operating system not recognized: I do not know what is the default name for the SCIP binary')
    end
end

scip_opts_comm = {};
if (nargin >= 8 && ~isempty(scip_opts))

    % SCIP binary
    if (isfield(scip_opts, 'scipbin'))
        SCIPBIN = scip_opts.scipbin;
    end

    % display options
    QUIETFLAG = [];
    OUTPUTREDIR = []; % output redirecton, e.g. "> /dev/null"
    if isfield(scip_opts, 'display_verblevel')
        if (scip_opts.display_verblevel == 0)
            QUIETFLAG = ' -q ';

            % bug workaround: there's a bug in
            % scip-3.1.0.linux.x86_64.gnu.opt.spx, which causes the
            % solution to be written as an empty file when the quiet flag
            % is used. As a workaround, we disable the quiet flag in that
            % case, and instead send the output to /dev/null, as suggested
            % by Stefan Vigerske
            if strcmp(SCIPBIN, 'scip-3.1.0.linux.x86_64.gnu.opt.spx')
                QUIETFLAG = [];
                OUTPUTREDIR = ' > /dev/null';
            end

        end
        scip_opts_comm{end+1} = [' -c "set display verblevel ' num2str(scip_opts.display_verblevel) '"'];
    end

    % frequency for displaying node information lines [100]
    if (isfield(scip_opts, 'display_freq'))
        scip_opts_comm{end+1} = [' -c "set display freq ' num2str(scip_opts.display_freq) '"'];
    end

    % limits options

    % solving stops, if the absolute gap = |primalbound - dualbound| is below the given value [0.0]
    if (isfield(scip_opts, 'limits_absgap'))
        scip_opts_comm{end+1} = [' -c "set limits absgap ' num2str(scip_opts.limits_absgap) '"'];
    end

    % solving stops, if the relative gap = |primal - dual|/MIN(|dual|,|primal|) is below the given value [0.0]
    if (isfield(scip_opts, 'limits_gap'))
        scip_opts_comm{end+1} = [' -c "set limits gap ' num2str(scip_opts.limits_gap) '"'];
    end

    % maximal time in seconds to run [1e+20]
    if (isfield(scip_opts, 'limits_time'))
        scip_opts_comm{end+1} = [' -c "set limits time ' num2str(scip_opts.limits_time) '"'];
    end

    % solving stops, if the given number of solutions were found (-1: no limit) [-1]
    if (isfield(scip_opts, 'limits_solutions'))
        scip_opts_comm{end+1} = [' -c "set limits solutions ' num2str(scip_opts.limits_solutions) '"'];
    end

    % lp options

    % number of threads used for solving the LP (0: automatic)
    if (isfield(scip_opts, 'lp_threads'))
        scip_opts_comm{end+1} = [' -c "set lp advanced threads ' num2str(scip_opts.lp_threads) '"'];
    end

    % numerics options

    % feasibility tolerance for constraints in SCIP [1e-6]
    if (isfield(scip_opts, 'numerics_feastol'))
        scip_opts_comm{end+1} = [' -c "set numerics feastol ' num2str(scip_opts.numerics_feastol) '"'];
    end

end

%% Objective function: 1/2 nu' * H * nu + f' * nu

% pre-compute the weighted Laplacian matrix
V = -w;
V(1:N+1:end) = sum(w, 2);

% quadratic terms of the objective function

% upper triangular matrix terms (we assume symmetric matrix)
dx = tril(dx);

% remove pairs where both vertices are fixed, as those only contribute a
% constant to the objective function and can be ignored in the optimization
dx(~isFree, ~isFree) = 0;

Nterms = nnz(dx);
objfunq = cell(1, Nterms + Nfree);
count = 1;
for idx = find(dx)'

    % decode dx matrix index into [I, J] vertices (we swap I,J so that we
    % get I as smaller index, and J larger index
    [J, I] = ind2sub(size(dx), idx);

    % upper triangular terms

    % 2D outputs
    if (D == 2)

        % xi (free), xj (free)
        if (isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d x%d + %.16g y%d y%d', ...
                2*full(V(I, J)), I, J, ...
                2*full(V(I, J)), I, J);
        % xi (free), xj (fixed)
        elseif (isFree(I) && ~isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d', ...
                2*full(V(I, J)) * y(J, 1), I, ...
                2*full(V(I, J)) * y(J, 2), I);
        % xi (fixed), xj (free)
        elseif (~isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d', ...
                2*full(V(I, J)) * y(I, 1), J, ...
                2*full(V(I, J)) * y(I, 2), J);
        end
        count = count + 1;

    % 3D outputs
    elseif (D == 3)

        % xi (free), xj (free)
        if (isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d x%d + %.16g y%d y%d + %.16g z%d z%d', ...
                2*full(V(I, J)), I, J, ...
                2*full(V(I, J)), I, J, ...
                2*full(V(I, J)), I, J);
        % xi (free), xj (fixed)
        elseif (isFree(I) && ~isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d + %.16g z%d', ...
                2*full(V(I, J)) * y(J, 1), I, ...
                2*full(V(I, J)) * y(J, 2), I, ...
                2*full(V(I, J)) * y(J, 3), I);
        % xi (fixed), xj (free)
        elseif (~isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d + %.16g z%d', ...
                2*full(V(I, J)) * y(I, 1), J, ...
                2*full(V(I, J)) * y(I, 2), J, ...
                2*full(V(I, J)) * y(I, 3), J);
        end
        count = count + 1;

    else
        error('Assertion fail: Output dimension D is not 2 or 3')
    end
end

% main diagonal terms
for I = find(isFree)'

    % 2D outputs
    if (D == 2)

        % only free vertices contribute to the objective function
        objfunq{count} = sprintf(...
            '+%.16g x%d^2 + %.16g y%d^2', ...
            full(V(I, I)), I, ...
            full(V(I, I)), I);

    % 3D outputs
    elseif (D == 3)

        % only free vertices contribute to the objective function
        objfunq{count} = sprintf(...
            '+%.16g x%d^2 + %.16g y%d^2 + %.16g z%d^2', ...
            full(V(I, I)), I, ...
            full(V(I, I)), I, ...
            full(V(I, I)), I);

    else
        error('Assertion fail: Output dimension D is not 2 or 3')
    end
    count = count + 1;

end
objfunq{1} = [' obj: ' objfunq{1}];

% the linear term of the objective function (f) has to be computed at each
% iteration of the QPQC-SMACOF algorithm. Thus, it is not computed here

%% SMACOF algorithm

% file name and path to save PIP model, computed solution and initial guess
% (generate unique names so that it is possible to run several instances of
% this function in parallel)
[~, aux] = fileparts(tempname);
pipfilename = [tempdir 'model-' aux '.pip']; % PIP model
solfilename = [tempdir 'model-' aux '.sol']; % output solution
sol0filename = [tempdir 'model-' aux '.sol0']; % initial guess

% init stopCondition
stopCondition = [];

% Euclidean distances between vertices in the current solution
dy = dmatrix_con(dx, y);

% initial stress
sigma0 = 0.5 * sum(sum(w .* (dx - dy).^2));

% if dx, dy are sparse matrices, sigma0 will be a sparse scalar, and this
% gives an error with fprintf below
sigma0 = full(sigma0);

% vector of stress computed by the algorithm
sigma = zeros(1, smacof_opts.MaxIter);

% display algorithm's evolution
t = zeros(1, smacof_opts.MaxIter); % time past from 0th iteration
if (strcmp(smacof_opts.Display, 'iter'))
    fprintf('Iter\t\tSigma\t\t\tTime (sec)\n')
    fprintf('===================================================\n')
    fprintf('%d\t\t%.4e\t\t%.4e\n', 0, sigma0, 0.0)
end

% auxiliary intermediate result
mwdx = -w .* dx;

% initialize the storage of the best solution found by the algorithm. We
% start with Inf so that the initial guess will always be replaced by any
% valid solution. The reason is that we cannot be sure that the initial
% guess fulfills the constraints, but any valid solution from a later
% iteration does
sigmabest = Inf;
ybest = nan(size(y));

% majorization loop
for I = 1:smacof_opts.MaxIter

    % auxiliary matrix B: non-main-diagonal elements
    B = mwdx ./ dy;
    B(isnan(B)) = 0;

    % auxiliary matrix B: main diagonal elements
    B(1:N+1:end) = -sum(B, 2);

    % the linear term (f) of the quadratic objective function
    % 1/2 nu' * H * nu + f' * nu
    % has to be recomputed at every iteration
    f = -2 * B * y;

    % convert linear term to PIP format
    objfunl = cell(1, Nfree);
    J = find(isFree);
    for idx = 1:length(J)

        % 2D output
        if (D == 2)

            objfunl{idx} = sprintf(...
                '+%.16g x%d +%.16g y%d', ...
                f(J(idx), 1), J(idx), f(J(idx), 2), J(idx));

        % 3D output
        elseif (D == 3)

            objfunl{idx} = sprintf(...
                '+%.16g x%d +%.16g y%d +%.16g z%d', ...
                f(J(idx), 1), J(idx), f(J(idx), 2), J(idx), ...
                f(J(idx), 3), J(idx));

        else
            error('Assertion fail: Output dimension D is not 2 or 3')
        end

    end

    % create PIP file to describe problem
    fid = fopen(pipfilename, 'w');
    if (fid == -1)
        error(['Cannot open file ' pipfilename ' to save PIP model'])
    end
    fprintf(fid, '%s\n%s\n%s\n%s\n%s\n%s\n', ...
        'Minimize', objfunq{:}, objfunl{:}, bnd{:}, con{:}, 'End');
    if (fclose(fid) == -1)
        error(['Cannot close file ' pipfilename ' to save PIP model'])
    end

    % create file for the initial guess
    write_solution(sol0filename, y);

    % solve the quadratic problem
    system([...
        SCIPBIN...
        QUIETFLAG ...
        ' -c "read ' pipfilename '"'...
        strcat(scip_opts_comm{:}) ...
        ' -c "optimize"'...
        ' -c "write solution ' solfilename '"'...
        ' -c "quit"' ...
        OUTPUTREDIR]);

    % read solution
    [aux, status] = read_solution(solfilename, size(y));

    % delete temp files
    if (exist(pipfilename, 'file')), delete(pipfilename); end
    if (exist(solfilename, 'file')), delete(solfilename); end
    if (exist(sol0filename, 'file')), delete(sol0filename); end

    % if SCIP cannot find a valid solution (e.g. we set a too short time
    % limit or it doesn't exist), then we cannot update the current
    % solution. This also means that we need to stop the optimization,
    % because the problem matrices and vectors won't change, and basically
    % we would try to solve the same problem again, not reaching a solution
    % either. Note however that it's possible that we have been finding
    % valid solutions before, and it's only at a later iteration that we
    % get to this "not valid solution" situation. Thus, we cannot assume
    % that the stop condition below these lines means "error".
    if (isempty(aux))
        stopCondition{end+1} = ['SCIP: ' status];
        sigma(I) = NaN;
        t(I) = toc;
        break;
    end

    % we only have to update the positions of the free vertices. The values
    % for fixed vertices in aux are all 0, so they need to be ignored
    y(isFree, :) = aux(isFree, :);

    % check that no two vertices are overlapping. Duplicated vertices cause
    % errors in the SMACOF algorithm because they create dy=0 components,
    % that produce Inf values in B = mwdx ./ dy;. Ideally, we would like
    % to move one of them a bit so that they don't overlap, while
    % maintaining the positive orientation of the triangles they are
    % involved with. However, we don't have the triangulation in this
    % function, and leave that for future work. For the time being, we are
    % going to consider overlapping vertices as a failed solution, and exit
    if (size(unique(y, 'rows'), 1) ~= N)
        stopCondition{end+1} = 'Duplicated vertex/vertices';
        break
    end

    % recompute distances between vertices in the current solution
    dy = dmatrix_con(dx, y);

    % compute stress with the current solution
    sigma(I) = 0.5 * sum(sum(w .* (dx - dy).^2));

    % update best solution
    if (sigma(I) < sigmabest)
        sigmabest = sigma(I);
        ybest = y;
    end

    % display algorithm's evolution
    t(I) = toc;
    if (strcmp(smacof_opts.Display, 'iter'))
        fprintf('%d\t\t%.4e\t\t%.4e\n', I, sigma(I), t(I))
    end

    % check whether the stress is under the tolerance level requested by
    % the user
    if (sigma(I) < smacof_opts.TolFun)
        stopCondition{end+1} = 'TolFun';
    end

    % check whether the improvement in stress is below the user's request,
    % and it's positive (don't stop if the stress gets worse, because
    % stress can go up and down in the optimization)
    if (I > 1)
        if ((sigma(I-1)-sigma(I))/sigma(I-1) < smacof_opts.Epsilon ...
                && (sigma(I-1)-sigma(I))/sigma(I-1) >= 0)
            stopCondition{end+1} = 'Epsilon';
        end
    end

    % stop if any stop condition has been met
    if (~isempty(stopCondition))
        break;
    end

end

% return the best solution the algorithm has found in all iterations
y = ybest;

% check whether the "maximum number of iterations" stop condition has been
% met
if (I == smacof_opts.MaxIter)
    stopCondition{end+1} = 'MaxIter';
end

% prune stress and time vectors if convergence was reached before the
% maximum number of iterations
sigma(I+1:end) = [];
t(I+1:end) = [];

end

% read SCIP solution from a text file created by SCIP
%
% file: path and file name of the solution file.
%
% sz: size of the output matrix with the solution. This parameter makes the
%     code easier to write, and it also allows to detect missing variables
%     in the solution
%
% y: matrix with the solution as a point configuration (each row is a
%    point)
function [y, status] = read_solution(file, sz)

if ((sz(2) ~= 2) && (sz(2) ~= 3))
    error('We only know how to read solutions that are sets of 2D or 3D points')
end

fid = fopen(file, 'r');
if (fid == -1)
    error(['Cannot open file ' file ' to read solution'])
end

% read status of the solution
status = fgetl(fid);
if ((isnumeric(status) && status == -1) ...
        || ~strcmp(status(1:16), 'solution status:'))
    error(['Assertion fail: File with SCIP solution does not start with string ''solution status:''. File ' file])
end
status = status(18:end);

% unless we obtained a valid solution, we don't bother with the rest of the
% file (which should be empty anyway), we exit, and the main function
% should detect the empty solution y, and create a stopCondition with the
% status returned by SCIP
if (~strcmp(status, 'optimal solution found') ...
        && ~strcmp(status, 'solution limit reached'))
    fclose(fid);
    y = [];
    return;
end

% the second line in the file should be the value of the objective function
aux = fgetl(fid);
if (~strcmp(aux(1:16), 'objective value:'))
    error(['Assertion fail: Second line in file with SCIP solution does not start with string ''objective value:''. File ' file])
end

% read contents of the file. Example result:
%
% solution status: optimal solution found
% objective value:                     468.345678663118
% x1                                                 -4 	(obj:0)
% y1                                                  2 	(obj:0)
% x2                                  0.613516669331233 	(obj:0)
% y2                                                 -4 	(obj:0)
% x3                                   1.24777861008035 	(obj:0)
% y3                                  -3.43327058768579 	(obj:0)
% x4                                                 -4 	(obj:0)
% y4                                 -0.804343353251697 	(obj:0)
% x5                                                  2 	(obj:0)
% y5                                                 -4 	(obj:0)
% x6                                  -3.31069797707353 	(obj:0)
% y6                                   1.21192102496956 	(obj:0)
% x7                                     1.643412276563 	(obj:0)
% y7                                  -3.72674560989634 	(obj:0)
% quadobjvar                           468.345678663118 	(obj:1)
c = textscan(fid, '%s%f%s', 'Delimiter', ' ', 'MultipleDelimsAsOne', true);
fclose(fid);

if isempty(c{1})
    error('SCIP did not return any solution')
end

% if SCIP has found a solution, read it from the file into the output
% matrix. Note that if a variable = 0, SCIP will not put it in the output
% file
y = zeros(sz);
for I = 1:length(c{1})-1

    % variable name
    varname = c{1}{I};

    % if list of output variables has finished, we can exit the loop
    if (strcmp(varname, 'quadobjvar'))
        break;
    end

    % vertex index
    idx = str2double(varname(2:end));

    if (c{1}{I}(1) == 'x') % this is an x-coordinate
        y(idx, 1) = c{2}(I);
    elseif (c{1}{I}(1) == 'y') % this is a y-coordinate
        y(idx, 2) = c{2}(I);
    elseif (c{1}{I}(1) == 'z') % this is a z-coordinate
        y(idx, 3) = c{2}(I);
    end

end

end

% write SCIP initial solution to a text file that SCIP can understand
%
% y: matrix with the solution as a point configuration (each row is a
%    point), including the points that are not free points
%
% file: path and file name of the solution file.
function write_solution(file, y)

% size of full solution configuration
sz = size(y);

if ((sz(2) ~= 2) && (sz(2) ~= 3))
    error('We only know how to read solutions that are sets of 2D or 3D points')
end

fid = fopen(file, 'w');
if (fid == -1)
    error(['Cannot open file ' file ' to write solution'])
end

% write solution to file. Example result:
%
% x1                                                 -4 	(obj:0)
% y1                                                  2 	(obj:0)
% x2                                  0.613516669331233 	(obj:0)
% y2                                                 -4 	(obj:0)
% x3                                   1.24777861008035 	(obj:0)
% y3                                  -3.43327058768579 	(obj:0)
% x4                                                 -4 	(obj:0)
% y4                                 -0.804343353251697 	(obj:0)
% x5                                                  2 	(obj:0)
% y5                                                 -4 	(obj:0)
% x6                                  -3.31069797707353 	(obj:0)
% y6                                   1.21192102496956 	(obj:0)
% x7                                     1.643412276563 	(obj:0)
% y7                                  -3.72674560989634 	(obj:0)
for I = 1:sz(1)

    % write x-coordinate
    fprintf(fid, '%s%d\t%.16e\t%s\n', 'x', I, y(I, 1), '(obj:0)');

    % write y-coordinate
    fprintf(fid, '%s%d\t%.16e\t%s\n', 'y', I, y(I, 2), '(obj:0)');

    % write z-coordinate
    if (sz(2) == 3)
        fprintf(fid, '%s%d\t%.16e\t%s\n', 'z', I, y(I, 3), '(obj:0)');
    end

end

% close file
st = fclose(fid);
if (st == -1)
    error(['Cannot close file ' file ' after writing solution'])
end

end


function [ lines ] = findlines( imge,map,theta,rho,minLen,maxLineNum,maxPeakNum,resol,maxGap )
%FINDLINE Find line features in the image using Hough transform
%   LINES = FINDLINES(IMGE,MAP,THETA,RHO,MINLEN,MAXLINENUM,MAXPEAKNUM,RESOL,MAXGAP)
%	IMGE is the binary edge image;
%	MAP,THETA,RHO is the output of HOUGH;
%	MINLEN is the minimum length of lines that will be found;
%	No more MAXLINENUM lines will be returned;
%	MAXPEAKNUM is the maximum peaks	that will be detected on the map;
%	MAXGAP is the maximum gap length that will be merged;
%	RESOL is the resolution of the detected lines, the smaller, the lines
%	must be further away from each other. A suggest value is 100.
%	LINES is a struct array with members: theta,rho,point1([row,col]),point2.
%
%	Yan Ke @ THUEE, 20110123, xjed09@gmail.com

% imge = edge(img);
% [map theta rho] = hough1(imge,[]);
[pri pti] = findpeaks(map,minLen,maxPeakNum,resol);
peakNum = length(pri);
prho = rho(pri); % peak points
pthe = theta(pti)*pi/180;
rhoThres = abs(rho(2)-rho(1))/2;

fitPts = cell(1,peakNum);
fitPtsNum = zeros(1,peakNum);
for p = 1:peakNum
	 % store points which are on the lines
	fitPts{p} = zeros(2,ceil(map(pri(p),pti(p))*1.2));
end
[Y X] = find(imge);
X = X-1; % The origin is the bottom-left corner.
M = size(imge,1);
Y = M-Y;

% find the points that's on the selected lines
for p = 1:length(X)
	rho1 = X(p)*sin(pthe)+Y(p)*cos(pthe);
	isOnPeak = find(abs(rho1-prho)<=rhoThres);
	fitPtsNum(isOnPeak) = fitPtsNum(isOnPeak)+1;
	for q = isOnPeak
		fitPts{q}(:,fitPtsNum(q)) = [X(p);Y(p)];
	end
end

% track the points on each line, find the start points and the end points,
lines = [];
tempLine = struct;
for p = 1:peakNum
	t1 = [-cos(pthe(p)),sin(pthe(p))]; % the unit directional vector of the line
	dist1 = t1*fitPts{p}(:,1:fitPtsNum(p)); % dist along the line
	[ordDist ordIdx] = sort(dist1,'ascend');
	ordPts = fitPts{p}(:,ordIdx);

	% handle with the gaps
	dist2 = diff(ordDist); % dist with each other
	gapPos = [0 find(dist2>maxGap) length(ordDist)];
	lineLen = diff(gapPos);
	for q = find(lineLen>=minLen)
		tempLine.theta = pthe(p);
		tempLine.rho = prho(p);
		tempLine.point1 = [M-ordPts(2,gapPos(q)+1),ordPts(1,gapPos(q)+1)+1];
		tempLine.point2 = [M-ordPts(2,gapPos(q+1)),ordPts(1,gapPos(q+1))+1];
		lines = [lines,tempLine];
		if length(lines) == maxLineNum, break; end
	end
	if length(lines) == maxLineNum, break; end
end

end

function [rows cols] = findpeaks(map,minLen,maxPeakNum,resol)
%	Find the at most maxLinNum points that's no less than minLen in map, meanwhile not
%	8-adjacent to each other, in MAP. Rows and cols are returned.

if max(max(map)) < minLen, return; end
rows = zeros(1,maxPeakNum);
cols = rows;
sz = size(map);
sup = ceil(sz/resol);
for p = 1:maxPeakNum
	[V,I] = max(map(:));
	if V<minLen
		rows = rows(1:p-1);
		cols = cols(1:p-1);
		break;
	end
	[rows(p),cols(p)] = ind2sub(sz,I);

	% non-maximal suppression
	left = max(1,cols(p)-sup(2));
	lr = min(sup(2)*2+1,sz(2)-left);
	up = max(1,rows(p)-sup(1));
	ud = min(sup(1)*2+1,sz(1)-up);
	map(up:up+ud,left:left+lr) = 0;
end

% if max(max(map)) < minLen, return; end
% map(map<minLen) = 0;
% mapMax = ordfilt2(map,25,ones(5));
% mapPeak = (mapMax == map) & map; % non-maximal suppression
% pt = find(mapPeak);
% pv = map(pt);
% [pvo,idx] = sort(pv,'descend');
% pt = pt(idx);
% if length(pvo) > maxPeakNum, pt = pt(1:maxPeakNum); end
% [rows cols] = ind2sub(size(map),pt);

end


function value = index3_row ( i_min, i, i_max, j_min, j, j_max, ...
  k_min, k, k_max, index_min )

%*****************************************************************************80
%
%% INDEX3_ROW indexes a 3D array by rows.
%
%  Discussion:
%
%    When we say "by rows", we really just mean that entries of the array are
%    indexed starting at entry (I_MIN,J_MIN,K_MIN), and the increasing the LAST
%    index first, then the next-to-the-last, and so on.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I_MIN, I, I_MAX, for row indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer J_MIN, J, J_MAX, for column indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer K_MIN, K, K_MAX, for plane indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer INDEX_MIN, the index of (I_MIN,J_MIN,K_MIN).
%    Typically, this is 0 or 1.
%
%    Output, integer VALUE, the index of element (I,J,K).
%
  value = index_min ...
             + ( k - k_min ) ...
             + ( j - j_min ) * ( k_max + 1 - k_min ) ...
             + ( i - i_min ) * ( j_max + 1 - j_min ) * ( k_max + 1 - k_min );

  return
end


function poly2 = densifyPolygon(poly, N)
%DENSIFYPOLYGON Add several points on each edge of the polygon.
%
%   POLY2 = densifyPolygon(POLY, N)
%   POLY is a NV-by-2 array containing polygon coordinates. The function
%   iterates on polygon edges, divides it into N subedges (by inserting N-1
%   new vertices on each edges), and return the resulting polygon.
%   The new polygon POLY has therefore N*NV vertices.
%
%   Example
%     % Densifies a simple polygon
%     poly = [0 0 ; 10 0;5 10;15 15;5 20;-5 10];
%     poly2 = densifyPolygon(poly, 10);
%     figure; drawPolygon(poly); axis equal
%     hold on; drawPoint(poly2);
%
%   See also
%     drawPolygon, edgeToPolyline
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-11-25, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

% number of vertices, and of edges
Nv = size(poly, 1);

% number of vertices in new polygon
N2 = N * Nv;
poly2 = zeros(N2, 2);

% iterate on polygon edges
for i = 1:Nv
    % extract current edge
    v1 = poly(i, :);
    v2 = poly(mod(i, Nv) + 1, :);

    % convert current edge to polyline
    newVertices = edgeToPolyline([v1 v2], N);

    % indices of current polyline to resulting polygon
    i1 = (i-1)*N + 1;
    i2 = i * N;

    % fill up polygon
    poly2(i1:i2, :) = newVertices(1:end-1, :);
end


% another simple 1D convection example with periodic BC
% diffusion coefficients are defined but not used
% It compares the results of upwind with TVD and shows how diffusive the upwind
% See how diffusive the upwind scheme can be althou it is so bad
% everywhere. Play with flux limiters, time steps, initial condition and time steps
% and see the difference between schemes in action
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
% define a 1D domain and mesh
W = 1;
Nx = 500;
mesh1 = createMesh1D(Nx, W);
x = mesh1.cellcenters.x;
% define the boundaries
BC = createBC(mesh1); % all Neumann
BC.left.periodic=1;
BC.right.periodic =1;
% Initial values
phi_old = createCellVariable(mesh1, 0.0, BC);
phi_old.value(20:120) = 1;
phi_old.value(180:400)= sin(x(180:400)*10*pi());
% initial guess for phi
phi = phi_old;
% initial values for upwind scheme
phiuw = phi;
phiuw_old=phi;
% keep the initial values for visualization
phiinit=phi_old;
% velocity field
u = 0.3;
uf = createFaceVariable(mesh1, u);
% diffusion field
D = 1e-2;
Df = createFaceVariable(mesh1, D);
% transient term coefficient
alfa = createCellVariable(mesh1,1.0);
% upwind convection term
Mconvuw = convectionUpwindTerm1D(uf);
% define the BC term
[Mbc, RHSbc] = boundaryCondition(BC);
% choose a flux limiter
FL = fluxLimiter('Superbee');
% solver
dt = 0.0005; % time step
final_t = W/u;
t = 0;
while t<final_t
    t = t+dt;
    % inner loop for TVD scheme
    [Mt, RHSt] = transientTerm(phi_old, dt, alfa);
    for j = 1:5
        [Mconv, RHSconv] = convectionTvdTerm1D(uf, phi, FL);
        M = Mconv+Mt+Mbc;
        RHS = RHSt+RHSbc+RHSconv;
        phi = solvePDE(mesh1, M, RHS);
    end
    [Mtuw, RHStuw] = transientTerm(phiuw_old, dt, alfa);
    Muw = Mconvuw+Mtuw+Mbc;
    RHSuw = RHStuw+RHSbc;
    phiuw = solvePDE(mesh1, Muw, RHSuw);
    phiuw_old = phiuw;
    phi_old = phi;
    figure(1);plot(x, phiinit.value(2:Nx+1), x, phi.value(2:Nx+1), '-o', x, ...
        phiuw.value(2:Nx+1)); drawnow;

end


%%*******************************************************
%% maxcut: MAXCUT problem.
%%
%% (P)    min  Tr C*X
%%        s.t.  diag(X) = e;
%%
%% C = -(diag(B*e)-B)/4.
%%
%% (dual problem)   max  e'*y
%%                  s.t. diag(y) + S = C.
%%-------------------------------------------------------
%%
%% [X,y,objval] = maxcut(B);
%%
%% B: weighted adjacency matrix of a graph.
%%
%% DSDP5.0
%% Copyright (c) 2004 by
%% S. Benson, Y. Ye
%% Last modified: Jan 2004
%%*******************************************************

 function [X,y,objval] = maxcut(B);

   if ~isreal(B); error('only real B allowed'); end;

   n = length(B); e = ones(n,1);
   CC = -(spdiags(B*e,0,n,n)-B)/4.0;
   b = e;

   AC = cell(1,3);
   AC{1,1}='SDP';
   AC{1,2}=n;
   nn=n*(n+1)/2;

   AAC=sparse(nn,n);
   for k = 1:n; AAC(:,k) = dsparse(k,k,1,n,n); end;
   AAC=[AAC sparse(dvec(CC))];
   AC{1,3}=AAC;
   y0 = -1.1*abs(CC)*e;
   OPTIONS=doptions;
   OPTIONS.gaptol=0.0001;
   OPTIONS.r0=0;
   OPTIONS.print=1;
   OPTIONS.rho=5;
   OPTIONS.zbar=norm(B,1);
   [STAT,y,Xv] = dsdp(b,AC,OPTIONS,y0);

   [objval,dobj,err] = derror(STAT,y,Xv,b,AC);
   X=dmat(Xv{1});
return;
%%=======================================================


% icp_demo.m
% run
% >> icp_demo
% in MATLAB

% size of model/data
n_model=2000;
n_data=1300;

% model points
model=4*randn(3,n_model)-2*ones(3,n_model);
bol=(model(1,:).^2+model(2,:).^2)<2^2;
while any(not(bol))
    model(:,not(bol))=4*randn(3,sum(not(bol)))-2*ones(3,sum(not(bol)));
    bol=(model(1,:).^2+model(2,:).^2)<2^2;
end
model(:,not(bol))=4*randn(3,sum(not(bol)))-2*ones(3,sum(not(bol)));
model(3,:)=0.5*(model(1,:).^2-model(2,:).^2);
bol=(model(1,:).^2+model(2,:).^2)<0.9^2;
model(3,bol)=3.5-3.5*(1/0.9)*sqrt(model(1,bol).^2+model(2,bol).^2);

% data points
data=model(:,1:n_data);

% weights
weights=ones(1,n_data);weights=rand(1,n_data);

% Transform points in data (move away from start position).
deg=15;
v1=2*(rand-0.5)*deg*(pi/180);
v2=2*(rand-0.5)*deg*(pi/180);
v3=2*(rand-0.5)*deg*(pi/180);
TR0=[cos(v1) sin(v1) 0;-sin(v1) cos(v1) 0;0 0 1];
TR0=TR0*[cos(v2) 0 sin(v2);0 1 0;-sin(v2) 0 cos(v2)];
TR0=TR0*[1 0 0;0 cos(v3) sin(v3);0 -sin(v3) cos(v3)];
TT0=3.0*(rand(3,1)-0.5);
data=TR0*data+repmat(TT0,1,n_data);

% randvec
rndvec=uint32(randperm(n_data)-1);

% sizerand, number of point-matchings in each iteration.
sizernd=ceil(1.45*n_data);

% Number of iterations.
iter=40;

% Compile the c++ files (not nessesacily if it is already done).
try
    make
end

% Create the kd-tree, TreeRoot is the pointer to the kd-tree
[tmp, tmp, TreeRoot] = kdtree( model', []);

% Run the ICP algorithm.
[R,T]=icpCpp(model,data,weights,rndvec,sizernd,TreeRoot,iter);

% Free allocated memory for the kd-tree.
kdtree([],[],TreeRoot);

figure(1),plot3(model(1,:),model(2,:),model(3,:),'r.',data(1,:),data(2,:),data(3,:),'c.'),hold off;

% Transform the data points.
data=R*data+repmat(T,1,n_data);

figure(2),plot3(model(1,:),model(2,:),model(3,:),'r.',data(1,:),data(2,:),data(3,:),'b.'),hold off;

clear functions


function coeff = reg_als(basis_cr, y, r, n)
%REG_ALS Summary of this function goes here
%   d - dimension of the regression function
%   N - number of regression points
%   y - 1 x N array of values of the function to approximate
%   n - d x 1 array, stores number of basis functions in each dimension
%   r - d+1 x 1 array, stores TT-ranks of the tensor of coefficients of the
%       regression function
%   basis_cr - array that stores values of basis functions at regression
%   points
%   fixed - d x 1 cell of aggregated fixed dimensions of the regression
%   function at k-th step of the ALS algorithm
%   coeff - TT-tensor of coefficient of the regression function

N = size(y, 2);
d = numel(n);
coeff = tt_rand(n, d, r);
coeff_ps = coeff.ps;
coeff_cr = coeff.core;
basis_ps = cumsum([1 ; n*N]);

% assemble fixed part of the TT-function
fixed_ps = cumsum([1 ; N ; r(2:d)*N ; N]);
fixed_cr = zeros(fixed_ps(d+2) - fixed_ps(1), 1);
fixed_cr(fixed_ps(d+1): fixed_ps(d+2)-1) = ones(N, r(d+1));
for dim = d: -1: 2
    c = reshape(coeff_cr(coeff_ps(dim): coeff_ps(dim+1)-1), [r(dim) n(dim) r(dim+1)]);
    c = permute(c, [1 3 2]);
    b = reshape(basis_cr(basis_ps(dim): basis_ps(dim+1)-1), [n(dim) N]);
    t1 = ten_conv(c, 3, b); % r(dim) x r(dim+1) x N
    t2 = reshape(fixed_cr(fixed_ps(dim+1): fixed_ps(dim+2)-1), [r(dim+1), N]);
    t1 = reshape(t1, [r(dim), r(dim+1)*N]);
    t2 = kron(ones(r(dim),1), reshape(t2, [1, r(dim+1)*N]));
    t3 = t1.*t2;
    t3 = reshape(t3, [r(dim) r(dim+1) N]);
	t3 = sum(t3, 2);
    fixed_cr(fixed_ps(dim): fixed_ps(dim+1)-1) = reshape(t3, [r(dim)*N, 1]);
end
fixed_cr(fixed_ps(1): fixed_ps(2)-1) = ones(N, r(1));

% initial sum of squares
VAR = var(y);

n_swp = 0;
R2 = 0;
swp_tp = 'f';
dim = 1;
while R2 < 0.9999 && n_swp < 10
    n1 = r(dim);
    n2 = n(dim);
    n3 = r(dim+1);

    u1 = reshape(fixed_cr(fixed_ps(dim): fixed_ps(dim+1)-1), [N n1]);
    u2 = reshape(basis_cr(basis_ps(dim): basis_ps(dim+1)-1), [n2 N]);
    u3 = reshape(fixed_cr(fixed_ps(dim+1): fixed_ps(dim+2)-1), [n3 N]);

    t1 = kron(kron(ones(n3,1), ones(n2,1)), u1');
    t2 = kron(kron(ones(n3,1), u2), ones(n1,1));
    t3 = kron(kron(u3, ones(n2,1)), ones(n1,1));
    t = t1.*t2.*t3;

    A = t*t';
    b = t*y';
    c = A\b;
    coeff_cr(coeff_ps(dim):coeff_ps(dim+1)-1) = c;

    if (swp_tp == 'f' && dim < d) || (swp_tp == 'b' && dim == 1)
        c = permute(reshape(c, [n1 n2 n3]), [1 3 2]);
        t1 = ten_conv(c, 3, u2);
        t1 = reshape(t1, [n1*n3, N]);
        u1 = kron(ones(1,n3), u1);
        t2 = u1.*t1';
        t2 = reshape(t2, [N n1 n3]);
        t2 = sum(t2, 2);
        t2 = reshape(t2, [N, n3]);
        fixed_cr(fixed_ps(dim+1): fixed_ps(dim+2)-1) = reshape(t2, [N*n3, 1]);

        %R^2
        if swp_tp == 'b'
            MSE = sum((sum(t2'.*u3,1) - y).^2)/N;
            R2 = 1 - MSE/VAR;
        end
    elseif (swp_tp == 'b' && dim > 1) || (swp_tp == 'f' && dim == d)
        c = permute(reshape(c, [n1 n2 n3]), [1 3 2]);
        t1 = ten_conv(c, 3, u2);
        t1 = reshape(t1, [n1, n3*N]);
        u3 = kron(ones(n1,1), reshape(u3, [1, n3*N]));
        t2 = t1.*u3;
        t2 = reshape(t2, [n1 n3 N]);
        t2 = sum(t2, 2);
        t2 = reshape(t2, [n1 N]);
        fixed_cr(fixed_ps(dim): fixed_ps(dim+1)-1) = reshape(t2, [n1*N, 1]);

        %R^2
        if swp_tp == 'f'
            MSE = sum((sum(u1'.*t2,1) - y).^2)/N;
            R2 = 1 - MSE/VAR;
        end
    end

    if swp_tp == 'f'
        if dim < d
            dim = dim + 1;
        elseif dim == d
            swp_tp = 'b';
            dim = dim - 1;
            n_swp = n_swp + 1;
            fprintf('=tt_reg_als= Sweep %d, R^2: %.2f%%, MSE: %1.2e\n', n_swp, 100*R2, MSE);
        end
    elseif swp_tp == 'b'
        if dim > 1
            dim = dim - 1;
        elseif dim == 1
            swp_tp = 'f';
            dim = dim + 1;
            n_swp = n_swp + 1;
            fprintf('=tt_reg_als= Sweep %d, R^2: %.2f%%, MSE: %1.2e\n', n_swp, 100*R2, MSE);
        end
    end
end

coeff.core = coeff_cr;

end


%% Total Variation Denoising
% Test for Rudin-Osher-Fatemi denoising (ROF) using FB-like method.

addpath('../');
addpath('../toolbox/');

%%
% Load image.

n = 256;
y = load_image('lena',n*2);
y = rescale(crop(y,n));
y = y + randn(n)*.06;

%%
% Display it.

clf;
imageplot(clamp(y));

%%
% We aim at minimising:

%%
% |min_x 1/2*norm(y-x,'fro')^2 + lambda*norm(K(x),1)|

%%
% Regularization parameter.

lambda = .2;

%%
% where |K| is a vectorial gradient and |norm(u,1)| is a vectorial L1
% norme.

K = @(x)grad(x);
KS = @(x)-div(x);

%%
% It can be put as the minimization of |F(K*x) + G(x)|

Amplitude = @(u)sqrt(sum(u.^2,3));
F = @(u)lambda*sum(sum(Amplitude(u)));
G = @(x)1/2*norm(y-x,'fro')^2;

%%
% The proximity operator of |F| is the vectorial soft thresholding.

Normalize = @(u)u./repmat( max(Amplitude(u),1e-10), [1 1 2] );
ProxF = @(u,tau)repmat( perform_soft_thresholding(Amplitude(u),lambda*tau), [1 1 2]).*Normalize(u);
ProxFS = compute_dual_prox(ProxF);

%%
% The proximity operator of G.

ProxG = @(x,tau)(x+tau*y)/(1+tau);

%%
% Function to record progression of the functional.

options.report = @(x)G(x) + F(K(x));

%%
% Run the ADMM algorihtm.

options.niter = 300;
[xAdmm,EAdmm] = perform_admm(y, K,  KS, ProxFS, ProxG, options);

%%
% Display image.

clf;
imageplot(xAdmm);

%%
% Since the functional to mimize is stricly convex, we can use a FB scheme
% on the dual problem.

GradGS = @(x)x+y;
L = 8;
options.method = 'fista';
[xFista,EFista] = perform_fb_strongly(y, K, KS, GradGS, ProxFS, L, options);

%%
% Compare the energy decays.

clf;
plot([EAdmm(:) EFista(:)]);
axis tight;
legend('ADMM', 'FISTA');
axis([1 length(EAdmm) EFista(end)*.9 2000]);


%% Classic Monte Carlo simualtion
% Vincent Leclercq, The MathWorks, 2007, vincent.leclercq@mathworks.fr
%

clear all;
close all;

NbTrials = 10000;

%RunMode = 'LogNomrality';

RunMode = 'OptionPricing';
%% Load Data (retrieved originally from Thomson Datastream)

load Equities.mat
PastDate = today()-1 * 365;

%% Retrieve the dates in a numeric format

Dates   = cellfun(@(x)(datenum(x,'yyyy-mm-ddTHH:MM:SS')),Equities.DATE);
AssetPrices = cellfun(@(x) (str2double(x)),Equities.P );

%% Plot the Series

plot(Dates,AssetPrices);set(gcf,'WindowStyle','Docked');
legend(Equities.DISPNAME);
datetick('x','mmmyy');
xlim([PastDate, today()]);
grid on;

%% Compute the returns
% We can compute the returns for one or many stocks at the same time using
% matrix computation and matlab easy syntax

Suez_Returns = tick2ret(AssetPrices);
DailyVol = std(Suez_Returns);
AnnualVol = DailyVol * sqrt(252);

SpotPrice = AssetPrices(end);

InterestRate = 0.0375;

%% Portfolio simulation (Monte Carlo) using the Financial toolbox function
% For help, one can use the doc portsim function
% We call the portfolio simulation using Financial toolbox to
% simulate 10000 scenarios. Of course, correlation are preserved
% We assume an horizon of 6 * 22 trading days, ie 6 month maturity

%% Using Annual statistics

SimulatedRetsAnnual = portsim(InterestRate,AnnualVol^2, 12, 1/12, NbTrials,'Expected'); % NbStep * TimeStep = 1 (in years !!!)

%% Using Daily statistics
NumberOfSimulationSteps = 12;
SimulatedRetsDaily = portsim(InterestRate./252, DailyVol^2, 12, 252./12, NbTrials,'Expected');% NbStep * TimeStep = 252 (in days!!!)

%% Generate the Prices and plot them

SimulatedPricesAnnual = ret2tick(squeeze(SimulatedRetsAnnual) ,SpotPrice);
SimulatedPricesDaily  = ret2tick(squeeze(SimulatedRetsDaily)  ,SpotPrice );

figure;hist(SimulatedPricesAnnual(end,:),40);title('Prices, annual timestep used');set(gcf,'WindowStyle','Docked');
figure;hist(SimulatedPricesDaily(end,:),40);title('Prices, daily timestep used');set(gcf,'WindowStyle','Docked');

%% Check For LogNormality of the Price series

ExpectedVariance = (SpotPrice^2)  * (exp(AnnualVol^2) - 1)* exp(2*InterestRate);

disp(['Mean Price (Annual Parameters) -> ', num2str(mean(SimulatedPricesAnnual(end,:))) ' , Theoric value (Hull) :' num2str(SpotPrice * exp(InterestRate))]);
disp(['Mean Price (Daily Parameters) -> ', num2str(mean(SimulatedPricesDaily(end,:))) ' , Theoric value (Hull) :' num2str(SpotPrice * exp(InterestRate))]);

disp(['Expected Variance (Annual Parameters) -> ', num2str(var(SimulatedPricesAnnual(end,:))) ' , Theoric value (Hull) :' num2str(ExpectedVariance)]);
disp(['Expected Variance (Daily Parameters) -> ', num2str(var(SimulatedPricesDaily(end,:))) ' , Theoric value (Hull) :' num2str(ExpectedVariance)]);

%% Parameter sweep
% Now that we have done this, we can ccompute the same thing for different
% Exercise prices
if strcmp(RunMode,'OptionPricing')
    k = 1;
    NumberOfSteps = 400;
    ExercisePrices= linspace(0.8 * SpotPrice,1.2 * SpotPrice,NumberOfSteps);

    VanillaPriceAnnual    = zeros(NumberOfSteps,1);
    VanillaPriceDaily    = zeros(NumberOfSteps,1);

    ProbabilityITMAnnual  = zeros(NumberOfSteps,1);
    ProbabilityITMDaily  = zeros(NumberOfSteps,1);
    CIAnnual    =     zeros(NumberOfSteps,2);
    CIDaily     =     zeros(NumberOfSteps,2);
    BLSPrices       = zeros(NumberOfSteps,1);

    %%
    TimeInYear = 1;
    for i = 1 : NumberOfSteps
        [BLSPrices(i),dummy]   = blsprice(SpotPrice, ExercisePrices(i), InterestRate, TimeInYear, AnnualVol, 0);
        [VanillaPriceAnnual(i), ProbabilityITMAnnual(i) ,CIAnnual(i,:)] = GetOptionPrice(SimulatedPricesAnnual,ExercisePrices(i),TimeInYear,InterestRate,'Vanilla');
        [VanillaPriceDaily(i), ProbabilityITMDaily(i) ,  CIDaily(i,:)] = GetOptionPrice(SimulatedPricesDaily,ExercisePrices(i),TimeInYear,InterestRate,'Vanilla');

    end;

%%

    h =    figure;
    [AX,H1,H2] = plotyy(ExercisePrices,[VanillaPriceAnnual BLSPrices CIAnnual] , ExercisePrices,ProbabilityITMAnnual);

    xlabel('Exercise Price');
    title('Option prices for a Vanilla option using a 1 year - Annual volatility');
    Axes_YLabels = get(AX,'Ylabel');
    set(Axes_YLabels{1},'String','Option Price') ;

    set(H1(1),'LineStyle','-');
    set(H1(1),'Color','r');
    set(H1(1),'LineWidth',2);

    set(H1(2),'LineStyle','-');
    set(H1(2),'Color','b');
    set(H1(2),'LineWidth',2);

    set(H1(3),'LineStyle','--');
    set(H1(3),'Color','r');
    set(H1(3),'LineWidth',1);

    set(H1(4),'LineStyle',':');
    set(H1(4),'Color','r');
    set(H1(4),'LineWidth',1);

    set(H2,'LineStyle','-');
    set(H2,'Color','g');
    set(H2,'LineWidth',2);

    set(get(AX(2),'Ylabel'),'String','Probability of being In the Money');
    legend(H1,{['Option Price (Monte Carlo)'], ['Option Price (Black Scholes)'], ['99% Confidence interval (Lower)'],['99% Confidence interval (Upper)']},'Location','NorthEast');
    legend(H1(2),{'Option Price (Black Shcoles)'},'Location','NorthEast');
    legend(H2,{'Probability'},'Location','SouthWest');
    grid on;
set(h,'WindowStyle','Docked');

%%
h=     figure;
    [AX,H1,H2] = plotyy(ExercisePrices,[VanillaPriceDaily BLSPrices CIDaily] , ExercisePrices,ProbabilityITMDaily);

    xlabel('Exercise Price');
    title('Option prices for a Vanilla option using a 1 year - Daily volatility');
    Axes_YLabels = get(AX,'Ylabel');
    set(Axes_YLabels{1},'String','Option Price') ;

    set(H1(1),'LineStyle','-');
    set(H1(1),'Color','r');
    set(H1(1),'LineWidth',2);

    set(H1(2),'LineStyle','-');
    set(H1(2),'Color','b');
    set(H1(2),'LineWidth',2);

    set(H1(3),'LineStyle','--');
    set(H1(3),'Color','r');
    set(H1(3),'LineWidth',1);

    set(H1(4),'LineStyle',':');
    set(H1(4),'Color','r');
    set(H1(4),'LineWidth',1);

    set(H2,'LineStyle','-');
    set(H2,'Color','g');
    set(H2,'LineWidth',2);

    set(get(AX(2),'Ylabel'),'String','Probability of being In the Money');
    legend(H1,{['Option Price (Monte Carlo)'], ['Option Price (Black Scholes)'], ['99% Confidence interval (Lower)'],['99% Confidence interval (Upper)']},'Location','NorthEast');
    legend(H2,{'Probability'},'Location','SouthWest');
    grid on;
    set(h,'WindowStyle','Docked');
 end;


function f = cumsum(f, m, dim)
%CUMSUM   Indefinite integral of a TRIGTECH.
%   CUMSUM(F) is the indefinite integral of the TRIGTECH F, whose mean
%   is zero, with the constant of integration chosen so that F(-1) = 0.
%   If the mean of F is not zero then an error is thrown since the indefinite
%   integral would no longer be periodic.
%
%   CUMSUM(F, M) will compute the Mth definite integral with the constant of
%   integration chosen so that each intermediary integral evaluates to 0 at
%   -1.
%   Thus, CUMSUM(F, 2) is equivalent to CUMSUM(CUMSUM(F)).
%
%   CUMSUM(F, M, 2) will take the Mth cumulative sum over the columns F an
%   array-valued TRIGTECH.
%
% See also DIFF, SUM.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% If the TRIGTECH G of length n is represented as
%       \sum_{k=-(n-1)/2}^{(n-1)/2} c_k exp(i*pi*kx)
% its integral is represented with a TRIGTECH of length n given by
%       \sum_{k=-(n-1)/2}^{(n-1)/2} b_k exp(i*pi*kx)
% where b_0 is determined from the constant of integration as
%       b_0 = \sum_{k=-(n-1)/2}^{(n-1)/2} (-1)^k/(i*pi*k) c_k;
% with c_0 := 0. The other coefficients are given by
%       b_k = c_k/(i*pi*k).
%
% If the TRIGTECH G of length n is represented as
%       \sum_{k=-n/2+1}^{n/2-1} c_k exp(i*pi*kx) + c(n/2)cos(n*pi/2x)
% then first set c(n) = 0.5*c(n) and define a = [0.5*c(n/2) c] so that we
% have the equivalent expansion:
%       \sum_{k=-n/2}^{n/2} a_k exp(i*pi*kx)
% The integral of this is represented with a TRIGTECH of length n+1 given by
%       \sum_{k=-n/2}^{n/2} b_k exp(i*pi*kx)
% where b_0 is determined from the constant of integration as
%       b_0 = \sum_{k=-n/2}^{n/2} (-1)^k/(i*pi*k) a_k;
% with a_0 := 0. The other coefficients are given by
%       b_k = a_k/(ik).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Trivial case of an empty TRIGTECH:
if ( isempty(f) )
    return
end

if ( nargin < 2 || isempty(m) )
    % Order of integration not passed in. Assume 1 by default:
    m = 1;
elseif ( m == 0 )
    % Nothing to do here!
    return
end

% Sum with respect to the continuous variable by default:
if ( nargin < 3 )
    dim = 1;
end

if ( dim == 1 )
    % Take difference across 1st dimension:
    f = cumsumContinuousDim(f, m);
else
    % Take difference across 2nd dimension:
    f = cumsumFiniteDim(f, m);
end

end

function f = cumsumContinuousDim(f, m)
% CUMSUM over the continuous dimension.

    % Initialize storage:
    c = f.coeffs; % Obtain Fourier coefficients {c_k}
    numCoeffs = size(c,1);

    fIsEven = mod(numCoeffs, 2) == 0;

    % index of constant coefficient
    if fIsEven
       ind = numCoeffs/2 + 1;
    else
       ind = (numCoeffs + 1)/2;
    end

    % Check that the mean of the TRIGtech is zero.  If it is not, then
    % throw an error.
    if ( any(abs(c(ind,:)) > 1e1*vscale(f)*eps) )
        error('CHEBFUN:TRIGTECH:cumsum:meanNotZero', ...
            ['Indefinite integrals are only possible for TRIGTECH objects '...
            'with zero mean.']);
    end

    % Force the mean to be exactly zero.
    if ( fIsEven )
        % Set coeff corresponding to the constant mode to zero:
        c(numCoeffs/2+1,:) = 0;
        % Expand the coefficients to be symmetric (see above discussion).
        c(1,:) = 0.5*c(1,:);
        c = [c; c(1,:)];
        highestDegree = numCoeffs/2;
    else
        c((numCoeffs+1)/2,:) = 0;
        highestDegree = (numCoeffs-1)/2;
    end

    % Loop for integration factor for each coefficient:
    sumIndicies = (-highestDegree:highestDegree).';
    integrationFactor = (-1i./sumIndicies/pi).^m;
    % Zero out the one corresponding to the constant term.
    integrationFactor(highestDegree+1) = 0;
    c = bsxfun(@times,c,integrationFactor);
    % If this is an odd order cumsum and there are an even number of
    % coefficients then zero out the cofficient corresponding to sin(N/2x)
    % term, since this will be zero on the Fourier grid.
    if ( (mod(m, 2) == 1) && fIsEven )
        c(1,:) = 0;
        c(numCoeffs+1,:) = 0;
    end

    % Fix the constant term.
    c(highestDegree+1,:) = -sum(bsxfun(@times,c,(-1).^sumIndicies));

    % If the original TRIGTECH had an even number of coefficients then
    % shrink the coefficent vector corresponding to its indefinite integral
    % back to its original size since it was increased by one above to make
    % the integration code slicker.
    if ( fIsEven )
        c = c(1:end-1,:);
    end

    % Recover values and attach to output:
    f.values = f.coeffs2vals(c);
    f.values(:,f.isReal) = real(f.values(:,f.isReal));

    f.coeffs = c;

    % Simplify (as suggested in Chebfun ticket #128)
    f = simplify(f);

    % Ensure f(-1) = 0:
    lval = get(f, 'lval');
    f.coeffs(1,:) = f.coeffs(1,:) - lval;
    f.values = bsxfun(@minus, f.values, lval);

end

function f = cumsumFiniteDim(f, m)
% CUMSUM over the finite dimension.

    for k = 1:m
        f.values = cumsum(f.values, 2);
        f.coeffs = cumsum(f.coeffs, 2);
    end

end


function f = projectOntoBMCIII( f )
% PROJECTONTOBMCI  Projection onto BMC-III symmetry.
%
% f = projectOntoBMCIII(f) is the orthogonal projection of f onto BMC-III
% symmetry, i.e., a function that is
% 1. even in theta for every even wave number in lambda;
% 2. odd in theta for every odd wave number in lambda;
% Additionally, for all but k=0 wavenumber lambda the resulting projection
% enforces the ballfun is zero at the poles.
%
% The projection is orthogonal, i.e., the correction matrix to fix up the
% structure has the smallest possible Frobenius norm.

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

if isempty( f )
   return
end

% Get the tensor of coefficients
F = f.coeffs;

% Permute F
F = permute(F,[1,3,2]);

% Loop over lambda
for l = 1:size(F,3)
    % Update the matrix
    F(:,:,l) = projectOntoRTheta(F(:,:,l));
end

% Permute back
F = permute(F,[1,3,2]);
f = ballfun(F, 'coeffs');
end

function X = projectOntoRTheta( X )
% Project the matrix of Chebyshev--Fourier coefficients onto a BMC-II
% function. The projection is orthogonal, i.e., the correction matrix to
% fix up the structure has the smallest possible Frobenius norm.

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

zeroMode = floor(n/2)+1;
evenModes = [fliplr(zeroMode-2:-2:1) zeroMode+2:2:n];  % Not including the zero mode
oddModes = [fliplr(zeroMode-1:-2:1) zeroMode+1:2:n];

% First do the zero-periodic mode in theta.
% Enforce the expansion is even in r.
I = eye( m ); A = I(2:2:end,:);
C = A \ ( A * X(:,zeroMode) );
C = I \ C;

% Update coeff matrix:
X(:,zeroMode) = X(:,zeroMode) - C;

% Second do the even-periodic, non-zero modes in theta.
% Enforce these are zero at the pole and that the expansion is even in
% theta
% Vectors [ T_k(0) ]:
A = real( (-1i).^(0:m-1) );   % A * X should be the zero vector.

% Solution to underdetermined system A*(X + Y) = 0 with smallest Frobenius
% norm:
C = A \ ( A * X(:,evenModes) );
C = I \ C;
X(:,evenModes) = X(:,evenModes) - C;

% Now project onto odd-anti-periodic.  Nothing special has to be done at
% the poles since enforcing the expansion is odd in r guarantees that it
% sums to zero.
A = I(1:2:end,:);
C = A \ ( A * X(:,oddModes) );
C = I \ C;
X(:,oddModes) = X(:,oddModes) - C;
end


function C=clustering_coef_bu(G)
%CLUSTERING_COEF_BU     Clustering coefficient
%
%   C = clustering_coef_bu(A);
%
%   The clustering coefficient is the fraction of triangles around a node
%   (equiv. the fraction of node's neighbors that are neighbors of each other).
%
%   Input:      A,      binary undirected connection matrix
%
%   Output:     C,      clustering coefficient vector
%
%   Reference: Watts and Strogatz (1998) Nature 393:440-442.
%
%
%   Mika Rubinov, UNSW, 2007-2010

n=length(G);
C=zeros(n,1);

for u=1:n
    V=find(G(u,:));
    k=length(V);
    if k>=2                 %degree must be at least 2
        S=G(V,V);
        C(u)=sum(S(:))/(k^2-k);
    end
end


function pinv = perm_inverse ( n, p )

%*****************************************************************************80
%
%% PERM_INVERSE computes the inverse of a permutation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of values being permuted.
%    N must be positive.
%
%    Input, integer P(N), describes the permutation.
%    P(I) is the item which is permuted into the I-th place
%    by the permutation.
%
%    Output, integer PINV(N), the inverse permutation.
%

%
%  Check.
%
  perm_check ( n, p );

  pinv(p(1:n)) = 1:n;

  return
end


function a = rutis1 ( )

%*****************************************************************************80
%
%% RUTIS1 returns the RUTIS1 matrix.
%
%  Example:
%
%    6 4 4 1
%    4 6 1 4
%    4 1 6 4
%    1 4 4 6
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A has constant row sums.
%
%    Because it has a constant row sum of 15,
%    A has an eigenvalue of 15, and
%    a (right) eigenvector of ( 1, 1, 1, 1 ).
%
%    A has constant column sums.
%
%    Because it has a constant column sum of 15,
%    A has an eigenvalue of 15, and
%    a (left) eigenvector of ( 1, 1, 1, ..., 1 ).
%
%    A has a repeated eigenvalue.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Output, real A(4,4), the matrix.
%

%
%  Note that the matrix entries are listed by row.
%
  a(1:4,1:4) = [ ...
    6.0,  4.0,  4.0,  1.0; ...
    4.0,  6.0,  1.0,  4.0; ...
    4.0,  1.0,  6.0,  4.0; ...
    1.0,  4.0,  4.0,  6.0 ];

  return
end


function res = smoothPolyline(poly, M)
%SMOOTHPOLYLINE Smooth a polyline using local averaging.
%
%   RES = smoothPolygon(POLY, M)
%   POLY contains the polyline vertices, and M is the size of smoothing
%   (given as the length of the convolution window).
%   Extremities of the polyline are smoothed with reduced window (last and
%   first vertices are kept identical, second and penultimate vertices are
%   smoothed with 3 values, etc.).
%
%   Example
%     img = imread('circles.png');
%     img = imfill(img, 'holes');
%     contours = bwboundaries(img');
%     poly = contours{1}(201:500,:);
%     figure; drawPolyline(poly, 'b'); hold on;
%     poly2 = smoothPolyline(poly, 21);
%     drawPolygon(poly2, 'm');
%
%   See also
%     polygons2d, smoothPolygon, simplifyPolyline, resamplePolyline

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2015-02-17, using Matlab 8.4.0.150421 (R2014b)
% Copyright 2015-2022 INRA - Cepia Software Platform

% compute the number of elements before and after
M1 = floor((M - 1) / 2);
M2 = ceil((M - 1) / 2);

% create convolution vector
v2 = ones(M, 1) / M;

% apply filtering on central part of the polyline
res(:,1) = conv(poly(:,1), v2, 'same');
res(:,2) = conv(poly(:,2), v2, 'same');

% need to recompute the extremities
for i = 1:M1
    i2 = 2 * i - 1;
    res(i, 1) = mean(poly(1:i2, 1));
    res(i, 2) = mean(poly(1:i2, 2));
end
for i = 1:M2
    i2 = 2 * i - 1;
    res(end - i + 1, 1) = mean(poly(end-i2+1:end, 1));
    res(end - i + 1, 2) = mean(poly(end-i2+1:end, 2));
end


function value = c8_le_l2 ( x, y )

%*****************************************************************************80
%
%% C8_LE_L2 := X <= Y for complex values, and the L2 norm.
%
%  Definition:
%
%    The L2 norm can be defined here as:
%
%      C8_NORM_L2(X) = sqrt ( ( real (X) )**2 + ( imag (X) )**2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex X, Y, the values to be compared.
%
%    Output, logical VALUE, is TRUE if X <= Y.
%
  if ( ( real ( x ) )^2 + ( imag ( x ) )^2 ...
    <= ( real ( y ) )^2 + ( imag ( y ) )^2 )
    value = 1;
  else
    value = 0;
  end

  return
end


function x = discrete_sample(p, n)
% Samples from a discrete distribution
%
%   x = discretesample(p, n)
%       independently draws n samples (with replacement) from the
%       distribution specified by p, where p is a probability array
%       whose elements sum to 1.
%
%       Suppose the sample space comprises K distinct objects, then
%       p should be an array with K elements. In the output, x(i) = k
%       means that the k-th object is drawn at the i-th trial.
%
%   Remarks
%   -------
%       - This function is mainly for efficient sampling in non-uniform
%         distribution, which can be either parametric or non-parametric.
%
%       - The function is implemented based on histc, which has been
%         highly optimized by mathworks. The basic idea is to divide
%         the range [0, 1] into K bins, with the length of each bin
%         proportional to the probability mass. And then, n values are
%         drawn from a uniform distribution in [0, 1], and the bins that
%         these values fall into are picked as results.
%
%       - This function can also be employed for continuous distribution
%         in 1D/2D dimensional space, where the distribution can be
%         effectively discretized.
%
%       - This function can also be useful for sampling from distributions
%         which can be considered as weighted sum of "modes".
%         In this type of applications, you can first randomly choose
%         a mode, and then sample from that mode. The process of choosing
%         a mode according to the weights can be accomplished with this
%         function.
%
%   Examples
%   --------
%       % sample from a uniform distribution for K objects.
%       p = ones(1, K) / K;
%       x = discretesample(p, n);
%
%       % sample from a non-uniform distribution given by user
%       x = discretesample([0.6 0.3 0.1], n);
%
%       % sample from a parametric discrete distribution with
%       % probability mass function given by f.
%       p = f(1:K);
%       x = discretesample(p, n);
%

%   Created by Dahua Lin, On Oct 27, 2008
%

%% parse and verify input arguments

assert(isfloat(p), 'discretesample:invalidarg', ...
    'p should be an array with floating-point value type.');

assert(isnumeric(n) && isscalar(n) && n >= 0 && n == fix(n), ...
    'discretesample:invalidarg', ...
    'n should be a nonnegative integer scalar.');

%% main

% process p if necessary

K = numel(p);
if ~isequal(size(p), [1, K])
    p = reshape(p, [1, K]);
end

% construct the bins

edges = [0, cumsum(p)];
s = edges(end);
if abs(s - 1) > eps
    edges = edges * (1 / s);
end

% draw bins

rv = rand(1, n);
c = histc(rv, edges);
ce = c(end);
c = c(1:end-1);
c(end) = c(end) + ce;

% extract samples

xv = find(c);

if numel(xv) == n  % each value is sampled at most once
    x = xv;
else                % some values are sampled more than once
    xc = c(xv);
    d = zeros(1, n);
    dv = [xv(1), diff(xv)];
    dp = [1, 1 + cumsum(xc(1:end-1))];
    d(dp) = dv;
    x = cumsum(d);
end

% randomly permute the sample's order
x = x(randperm(n));


function R=estExpDecayReliability(t,T,method)
%%ESTEXPDECAYRELIABILITY Determine the probability that a particular
%       example of something will be functional after time T given
%       independent the failure times of n other examples under the
%       assumption that the time of failure given something is functional
%       at time zero is exponentially distributed, so the probability of
%       success (it lasting longer than T) given that it works at time zero
%       is Pr{T>t}=exp(-t/theta) for some parameter theta.
%
%INPUTS: t A 1Xn or nX1 vector of the times when something failed. All
%          elements should be >=0.
%        T The desired time after which a failure is acceptable. T>=0.
%   method The estimation approach. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) Use the
%            expected value estimate.
%          1 Use the maximum likelihood estimate. This estimate is biased.
%
%OUTPUTS: R The probability that  the system will still be operational at
%           some point after time T.
%
%This implements the methods of [1]. The basic reliability model is
%R=p*P
%where p is the probability of a failure at time 0 and P is the probability
%of a failure between time 0 and T. THe exponential decay model applies to
%P.
%
%EXAMPLE:
%We demonstrate here that the value of R at time T is more reliable when
%computing using method 0 than method 1 given n=30 samples of the data.
% p=0.9;
% T=2;
% theta=3;
% lambda=1/theta;
% RTrue=p*(1-ExponentialD.CDF(T,lambda))
%
% numRuns=1e4;
% R0=0;
% R1=0;
% for curRun=1:numRuns
%     n=30;
%     t=zeros(n,1);
%     for k=1:n
%         if(rand()<p)%If working at time 0.
%            t(k)=ExponentialD.rand(1,lambda);
%         else
%            t(k)=0;
%         end
%     end
%     R0=R0+estExpDecayReliability(t,T,0);
%     R1=R1+estExpDecayReliability(t,T,1);
% end
% R0=R0/numRuns
% R1=R1/numRuns
%Typically, R0 will be closet to RTrue than R1.
%
%REFERENCES:
%[1] E. L. Pugh, "The best estimate of reliability in the exponential
%    case," Operations Research, vol. 11, no. 1, pp. 57-61, Jan.-Feb. 1963.
%
%June 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(method))
    method=0;
end

n=length(t);
sel=(t~=0);
p=sum(sel)/n;

%Keep only failure times that are >0.
t=t(sel);
n=length(t);

if(n==0)
   R=0;
   return;
end

theta=sum(t)/n;%Equation 7.
switch(method)
    case 0%The expected value solution.
        R=p*(1-T/(n*theta))^(n-1);%Equation 17 (with p inserted).
    case 1%The ML solution.
        R=p*exp(-T/theta);%Equation 6 (with p inserted).
    otherwise
        error('Unknown method specified.')
end
end

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


function [px, py, gx, gy] = position_5_link(z,P)
%[px,py,gx,gy] = POSITION_5_LINK(Z,P)
%
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS:
%
%
%OUTPUTS:
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
%
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);
th4 = z(4,:);
th5 = z(5,:);

nTime = length(th1);
px = zeros(5,nTime);
py = zeros(5,nTime);
gx = zeros(5,nTime);
gy = zeros(5,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

px(4,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
py(4,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);
gx(4,:) = d4.*cos(th4) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
gy(4,:) = d4.*sin(th4) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);

px(5,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
py(5,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);
gx(5,:) = d5.*cos(th5) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
gy(5,:) = d5.*sin(th5) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Setting up advection-diffusion solver
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = please_Update_Adv_Diff_Concentration_Unsplit(C,dt,dx,dy,uX,uY,k)

% C:     concentration
% dt:    time-step
% dx,dy: spatial steps in x and y, respectively
% uX:    x-Component of Velocity
% uY:    y-Component of Velocity
% k:     diffusion coefficient

% Compute Necessary Derivatives (Note: these calculations could be parallalized)
Cx = give_Necessary_Derivative(C,dx,uX,'x');
Cy = give_Necessary_Derivative(C,dy,uY,'y');
Cxx = DD(C,dx,'x');
Cyy = DD(C,dy,'y');

% Update Concentration
%C = C + dt * ( k*(Cxx+Cyy) - uX'.*Cx - uY'.*Cy );

C = C + dt * ( k*(Cxx+Cyy) - uX.*Cx - uY.*Cy );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Computes derivative based on sign of Velocity, u, using UPWIND
% approach
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C_z = give_Necessary_Derivative(C,dz,uZ,string)

C_z = zeros(size(C));
len = length(uZ(:,1));
signs = sign(uZ);

if strcmp(string,'x')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %left side of grid
        if signs(i,1) <= 0
            C_z(i,1) =  ( C(i,2) - C(i,1) ) / (dz);
        else
            C_z(i,1) =  ( C(i,1) - C(i,len) ) / (dz);
        end

        %right side of grid
        if signs(i,len) <= 0
            C_z(i,len) =  ( C(i,1) - C(i,len) ) / (dz);
        else
            C_z(i,len) =  ( C(i,len) - C(i,len-1) ) / (dz);
        end

    end

    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(i,j) <= 0
                C_z(i,j) = ( C(i,j+1) - C(i,j) ) / (dz);
            else
                C_z(i,j) = ( C(i,j) - C(i,j-1) ) / (dz);
            end
        end
    end

    % Ends x-Direction calculation %

elseif strcmp(string,'y')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %bottom of grid
        if signs(1,i) <= 0
            C_z(1,i) =  ( C(2,i) - C(1,i) ) / (dz);
        else
            C_z(1,i) =  ( C(1,i) - C(len,i) ) / (dz);
        end

        %top of grid
        if signs(len,i) <= 0
            C_z(len,i) =  ( C(1,i) - C(len,i) ) / (dz);
        else
            C_z(len,i) =  ( C(len,i) - C(len-1,i) ) / (dz);
        end

    end

    %Standard Upwind
    for i=2:len-1
        for j=1:len
            if signs(i,j) <= 0
                C_z(i,j) = ( C(i+1,j) - C(i,j) ) / (dz);
            else
                C_z(i,j) = ( C(i,j) - C(i-1,j) ) / (dz);
            end
        end
    end

    % Ends y-Direction calculation %

else

    fprintf('\n\n\n ERROR IN FUNCTION D FOR COMPUTING 1ST DERIVATIVE\n');
    fprintf('Need to specify which desired derivative, x or y.\n\n\n');

end

clear signs; clear len;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Finds CENTERED finite difference approximation to 2ND
% DERIVATIVE in z direction, specified by input and 'string'
% Note: It automatically accounts for periodicity of the domain.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function u_zz = DD(u,dz,string)

% u:      velocity
% dz:     spatial step in "z"-direction
% string: specifies which 2ND derivative to take (to enforce periodicity)

len = length(u(:,1));

if strcmp(string,'x')

    %For periodicity on ends
    u_zz(:,1) =  ( u(:,2) - 2*u(:,1)   + u(:,len) )   / (dz^2);
    u_zz(:,len)= ( u(:,1) - 2*u(:,len) + u(:,len-1) ) / (dz^2);

    %Standard Upwind Scheme (Centered Difference)
    for j=2:len-1
        u_zz(:,j) = ( u(:,j+1) - 2*u(:,j) + u(:,j-1) ) / (dz^2);
    end

elseif strcmp(string,'y')

    %For periodicity on ends
    u_zz(1,:) =  ( u(2,:) - 2*u(1,:)   + u(len,:) )   / (dz^2);
    u_zz(len,:)= ( u(1,:) - 2*u(len,:) + u(len-1,:) ) / (dz^2);

    %Standard Upwind Scheme (Centered Difference)
    for j=2:len-1
        u_zz(j,:) = ( u(j+1,:) - 2*u(j,:) + u(j-1,:) ) / (dz^2);
    end

else

    fprintf('\n\n\n ERROR IN FUNCTION DD FOR COMPUTING 2ND DERIVATIVE\n');
    fprintf('Need to specify which desired derivative, x or y.\n\n\n');

end


function [Ex Ey Ez] = MiePECBall(cart, k, R, N)
% The code caculates the time-harmonic scattered field value at point 'cart'
% due to a PEC ball with radius R centered at the origin.
% The incident wave is E=exp(-ikz).
% The formulation is based on H.C. van de Hulst, P123
% 'light scattering by small particles', Dover, 1981
% Input: cart -- cartiesian coordiante (1x3) of the point ouside the ball
%        k    -- wavenumber
%        R    -- radius of the PEC ball
%        N    -- number of terms used in Mie series
% Output: [Ex Ey Ez] -- the scattering electric field
% Author: Jiguang Sun, 02/13/2011, jsun@desu.edu
% Please report bugs to jsun@desu.edu
% Copyright (c) 2011, Jiguang Sun, rights reserved.
% THIS SOFTWARE IS PROVIDED "AS IS".
% Redistribution and use in source and binary forms, with or without
% modification, are permitted.
i = sqrt(-1);
[phi,th,r] = cart2sph(cart(1), cart(2), cart(3)); th = pi/2-th;
if r < R
    % disp('The point is inside the ball!')
    Ex = 0; Ey = 0; Ez = 0;
    return;
end

n = 1:N+2;
x = k*R;
[j(n), ierr(1,n)] = besselj(n - 1 + 1/2, x); j = j*sqrt(pi/(2*x));
[h(n), ierr(1,n)] = besselh(n - 1 + 1/2, 2, x); h = h*sqrt(pi/(2*x));
if any(any(ierr))
    disp('Accuracy error in a Bessel or Hankel function.');
    Ex = 0; Ey = 0; Ez = 0;
    return;
end
% scattering coefficients
an = j(2:N+1)./h(2:N+1);
bn = (j(2:N+1)+k*R*j(1:N)-k*R*j(3:N+2))./(h(2:N+1)+k*R*h(1:N)-k*R*h(3:N+2));
% various terms ralated to Racati-Bessel's functions
n = 1:N+4;
z = k*r; k2r = k^2*r;
[h(n), ierr(2,n)] = besselh(n - 2 + 1/2, 2, z); h = h*sqrt(pi/(2*z));
hn = h(3:N+2);
drh = (h(3:N+2)+k*r*h(2:N+1)-k*r*h(4:N+3))/2;
ddrh=(k2r*h(1:N)+2*k*h(2:N+1)-(1/r+2*k2r)*h(3:N+2)-2*k*h(4:N+3)+k2r*h(5:N+4))/4;
%
if any(any(ierr))
    disp('Accuracy error in a Bessel or Hankel function.')
    Ex = 0; Ey = 0; Ez = 0;
    return;
end
% various terms related to associated Legendre polynomials
Pn(1) = 1;
Pn(2) = cos(th);
for n=2:N-1
    Pn(n+1) = ((2*n-1)*cos(th)*Pn(n)-(n-1)*Pn(n-1))/n;
end
% Pn1 starts from n = 1.
Pn1(1) = -sin(th);
Pn1(2) = -3.0*sin(th)*cos(th);
for n=2:N-1
    Pn1(n+1) = (2.0*n+1)*cos(th)*Pn1(n)/n-(n+1)*Pn1(n-1)/n;
end
% Pn1S starts from n = 1;
Pn1S(1) = -1;
Pn1S(2) = -3.0*cos(th);
for n = 2:N-1
    Pn1S(n+1) = Pn1S(n-1)-(2*n+1)*Pn(n+1);
end

for n = 1:N
    if n == 1
        dPn1(n) = -cos(th);
    elseif n == 2
        dPn1(n) = 3*(2*sin(th)*sin(th)-1);
    else
        dPn1(n) = (2*n-1)/(n-1)*Pn1(n-1)*(-sin(th))+(2*n-1)/(n-1)*cos(th)*dPn1(n-1)-n/(n-1)*dPn1(n-2);
    end
end
% compute M and N
Mrho = 0; Mth = 0; Mphi = 0;
for n = 1:N
    c = (-i)^(n)*(2*n+1)/(n*(n+1));
    Mth = Mth + c*an(n)*hn(n)*Pn1S(n)*cos(phi);
    Mphi = Mphi-c*an(n)*hn(n)*dPn1(n)*sin(phi);
end
Nrho = 0; Nth = 0; Nphi = 0;
for n = 1:N
    c = (-i)^(n)*(2*n+1)/(n*(n+1));
    Nrho = Nrho + bn(n)*c*(ddrh(n)+k^2*r*hn(n))*Pn1(n)*cos(phi)/(k);
    Nth = Nth + bn(n)*c/r*drh(n)*dPn1(n)*cos(phi)/(k);
    Nphi = Nphi + bn(n)*c/r*drh(n)*Pn1S(n)*(-sin(phi))/(k);
end
% compute scattering field in Spherical coordinate
Erho = Mrho + i*Nrho;
Ephi = Mphi + i*Nphi;
Eth = Mth + i*Nth;
% change back to cartisian coordinate
Ex = Erho*sin(th)*cos(phi)+Eth*cos(th)*cos(phi)-Ephi*sin(phi);
Ey = Erho*sin(th)*sin(phi)+Eth*cos(th)*sin(phi)+Ephi*cos(phi);
Ez = Erho*cos(th)-Eth*sin(th);


%% RATE OF CONVERGENCE OF MIXED FINITE ELEMENT METHOD IN 2D
%
% This example is to show the rate of convergence of mixed finite element
% (RT0-P0) approximation of the Poisson equation on the unit square:
%
% $$- \Delta u = f \; \hbox{in } (0,1)^2$$
%
% for the following boundary condition:
%
% # Pure Dirichlet boundary condition. $\Gamma _D = \partial \Omega$.
% # Pure Neumann boundary condition. $\Gamma _N = \partial \Omega$.
% # Mix Dirichlet and Neumann boundary condition. $u=g_D \hbox{ on }\Gamma_D,
% \quad \nabla u\cdot n=g_N \hbox{ on }\Gamma_N. \Gamma _N = \{(x,y): x=0,
% y\in [0,1]\}, \; \Gamma _D = \partial \Omega \backslash \Gamma _N$.
%
% Written by Ming Wang.

%%
clear all; close all;
[node,elem] = squaremesh([0,1,0,1],0.25);
pde = mixBCdata;
option.L0 = 2;
option.maxIt = 4;
option.printlevel = 1;
option.elemType = 'RT0-P0';
% option.solver = 'uzawapcg';
option.solver = 'dmg';

%% Pure Dirichlet boundary condition.
% option.plotflag = 0;
bdFlag = setboundary(node,elem,'Dirichlet');
mfemPoisson3(node,elem,pde,bdFlag,option);

%% Pure Neumann boundary condition.
% option.plotflag = 0;
bdFlag = setboundary(node,elem,'Neumann');
mfemPoisson(node,elem,pde,bdFlag,option);

%% Mix Dirichlet and Neumann boundary condition.
option.plotflag = 1;
option.solver = 'uzawapcg';
bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
mfemPoisson(node,elem,pde,bdFlag,option);

%% Conclusion
%
% The optimal rates of convergence for u and sigma are observed, namely,
% 1st order for L2 norm of u, L2 norm of sigma and H(div) norm of sigma.
% The 2nd order convergent rates between two discrete functions ||uI-uh||
% and ||sigmaI-sigmah|| are known as superconvergence.
%
% dmg and uzawapcg converges uniformly in all cases. Distributive MG is two
% times faster than uzawapcg.


function lattice_rule_test07 ( )

%*****************************************************************************80
%
%% LATTICE_RULE_TEST07 tests LATTICE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994, page 18.
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LATTICE_RULE_TEST07\n' );
  fprintf ( 1, '  LATTICE applies a lattice rule to integrate\n' );
  fprintf ( 1, '  a function over the unit hypercube.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The spatial dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  The lattice rule order M will vary.\n' );

  z(1:dim_num) = [ 1, 2 ];
  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  i4vec_print ( dim_num, z, '  The lattice generator vector:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '         I         M    EXACT       ESTIMATE    ERROR\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    m = prime ( 3 * i );

    quad = lattice ( dim_num, m, z, @f_01_2d );

    exact = e_01_2d ( dim_num, a, b );

    error = abs ( exact - quad );

    fprintf ( 1, '  %8d  %8d  %10.6f  %10.6f  %10.6e\n', i, m, exact, quad, error );

  end

  return
end


function g = gauss(x, m, P)

% GAUSS Gaussian distribution value
%   GAUSS(X, M, P) gives the probability density at point X, following a
%   Gaussian or Normal distribution with mean M and covariances matrix P.
%
%   For X vector, the evaluation happens at a single point denoted by X,
%   and the result is a positive scalar in the range [0 , 1].
%
%   For X matrix, the evaluation happens at each row of X, and the result
%   is a row-vector of scalars, one for each column of X.

%   Copyright 2013 Joan Sola

d = size(x,1);
n = size(x,2);
MD2 = zeros(1,n);

if (size(m,1) == d) && (size(P,1) == d) && (size(P,2) == d)

    z = x - repmat(m, 1, n);
    for i = 1:n
        MD2(1,i) = z(:,i)' * P^-1 * z(:,i);
    end
    a = sqrt((2*pi)^d * det(P));
    g = exp(-0.5 * MD2) / a;

else
    error ('Input sizes don''t match')
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