function M = multinomialdoublystochasticfactory(n)
% Manifold of n-by-n doubly-stochastic matrices with positive entries.
%
% function M = multinomialdoublystochasticfactory(n)
%
% M is a Manopt manifold structure to optimize over the set of n-by-n
% matrices with (strictly) positive entries and such that the entries of
% each column and each row sum to one.
%
% Points on the manifold and tangent vectors are represented naturally as
% matrices of size n. The Riemannian metric imposed on the manifold is the
% Fisher metric, that is, if X is a point on the manifold and U, V are two
% tangent vectors:
%
%     M.inner(X, U, V) = <U, V>_X = sum(sum(U.*V./X)).
%
% The retraction here provided is only first order. Consequently, the
% slope test in the checkhessian tool is only valid at points X where the
% gradient is zero. Furthermore, if some entries of X are very close to
% zero, this may cause numerical difficulties that can also lead to a
% failed slope test. More generally, it is important that the solution of
% the optimization problem should have positive entries, sufficiently far
% away from zero to avoid numerical issues.
%
% The file is based on developments in the research paper
% A. Douik and B. Hassibi, "Manifold Optimization Over the Set
% of Doubly Stochastic Matrices: A Second-Order Geometry"
% ArXiv:1802.02628, 2018.
%
% Link to the paper: https://arxiv.org/abs/1802.02628.
%
% Please cite the Manopt paper as well as the research paper:
% @Techreport{Douik2018Manifold,
%   Title   = {Manifold Optimization Over the Set of Doubly Stochastic
%              Matrices: {A} Second-Order Geometry},
%   Author  = {Douik, A. and Hassibi, B.},
%   Journal = {Arxiv preprint ArXiv:1802.02628},
%   Year    = {2018}
% }
%
% See also: multinomialsymmetricfactory multinomialfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Ahmed Douik, March 6, 2018.
% Contributors: Nicolas Boumal
% Change log:
%
%    Apr. 24, 2018 (AD):
%        Changed pinv() to a particular solution to the equation.
%
%    July 24, 2018 (AD):
%        A bugfix related to the pinv() change, with effects in many places.
%
%    Sep.  6, 2018 (NB):
%        Removed M.exp() as it was not implemented.
%
%    Aug. 19, 2019 (AD, BM, NB):
%        Fixed typos in comments; replaced some linear solves with pcg
%        for efficiency when n is large. By default, pcg is used:
%        comments in the code indicate other possibilities and how they
%        differ. Added maxDSiters to doubly_stochastic argument.
%        The main change has been to factor out these linear solves.

    e = ones(n, 1);

    maxDSiters = 100 + 2*n;

    function [alpha, beta] = mylinearsolve(X, b)
        % zeta = sparse(A)\b; % sparse might not be better perf.-wise.
        % where A = [eye(n) X ; X' eye(n)];
        %
        % For large n use the pcg solver instead of \.
        % [zeta, ~, ~, iter] = pcg(sparse(A), b, 1e-6, 100);
        %
        % Even faster is to create a function handle
        % computing A*x (x is a given vector).
        % Make sure that A is not created, and X is only
        % passed with mylinearsolve and not A.
        [zeta, ~, ~, iter] = pcg(@mycompute, b, 1e-6, 100);

        function Ax = mycompute(x)
            xtop = x(1:n,1);
            xbottom = x(n+1:end,1);
            Axtop = xtop + X*xbottom;
            Axbottom = X'*xtop + xbottom;
            Ax = [Axtop; Axbottom];
        end

        alpha = zeta(1:n, 1);
        beta = zeta(n+1:end, 1);
    end

    M.name = @() sprintf('%dx%d doubly-stochastic matrices with positive entries', n, n);

    M.dim = @() (n-1)^2;

    % Fisher metric
    M.inner = @iproduct;
    function ip = iproduct(X, eta, zeta)
        ip = sum((eta(:).*zeta(:))./X(:));
    end

    M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

    M.dist = @(X, Y) error('multinomialdoublystochasticfactory.dist not implemented yet.');

    % The manifold is not compact as a result of the choice of the metric,
    % thus any choice here is arbitrary. This is notably used to pick
    % default values of initial and maximal trust-region radius in the
    % trustregions solver.
    M.typicaldist = @() n;

    % Pick a random point on the manifold
    M.rand = @random;
    function X = random()
        Z = abs(randn(n, n));     % Random point in the ambient space
        X = doubly_stochastic(Z, maxDSiters); % Projection on the Manifold
    end

    % Pick a random vector in the tangent space at X.
    M.randvec = @randomvec;
    function eta = randomvec(X) % A random vector in the tangent space
        % A random vector in the ambient space
        Z = randn(n, n);
        % Projection of the vector onto the tangent space
        b = [sum(Z, 2) ; sum(Z, 1)'];
        [alpha, beta] = mylinearsolve(X, b);
        eta = Z - (alpha*e' + e*beta').*X;
        % Normalizing the vector
        nrm = M.norm(X, eta);
        eta = eta / nrm;
    end

    % Projection of vector eta in the ambient space to the tangent space.
    M.proj = @projection;
    function etaproj = projection(X, eta) % Projection of the vector eta in the ambeint space onto the tangent space
        b = [sum(eta, 2) ; sum(eta, 1)'];
        [alpha, beta] = mylinearsolve(X, b);
        etaproj = eta - (alpha*e' + e*beta').*X;
    end

    M.tangent = M.proj;
    M.tangent2ambient = @(X, eta) eta;

    % Conversion of Euclidean to Riemannian gradient
    M.egrad2rgrad = @egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad) % projection of the euclidean gradient
        mu = (X.*egrad);
        b = [sum(mu, 2) ; sum(mu, 1)'];
        [alpha, beta] = mylinearsolve(X, b);
        rgrad = mu - (alpha*e' + e*beta').*X;
    end

    % First-order retraction
    M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end
        Y = X.*exp(t*(eta./X));
        Y = doubly_stochastic(Y, maxDSiters);
        Y = max(Y, eps);
    end

    % Conversion of Euclidean to Riemannian Hessian
    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, eta)

        % computing the directional derivative of the Riemannian
        % gradient
        gamma = egrad.*X;
        gammadot = ehess.*X + egrad.*eta;

        b = [sum(gamma, 2) ; sum(gamma, 1)'];
        bdot = [sum(gammadot, 2) ; sum(gammadot, 1)'];
        [alpha, beta] = mylinearsolve(X, b);
        [alphadot, betadot] = mylinearsolve(X, bdot- [eta*beta; eta'*alpha]);

        S = (alpha*e' + e*beta');
        deltadot = gammadot - (alphadot*e' + e*betadot').*X- S.*eta;

        % projecting gamma
        delta = gamma - S.*X;

        % computing and projecting nabla
        nabla = deltadot - 0.5*(delta.*eta)./X;
        rhess = projection(X, nabla);
    end

    % Miscellaneous manifold functions
    M.hash = @(X) ['z' hashmd5(X(:))];
    M.lincomb = @matrixlincomb;
    M.zerovec = @(X) zeros(n, n);
    M.transp = @(X1, X2, d) projection(X2, d);
    M.vec = @(X, U) U(:);
    M.mat = @(X, u) reshape(u, n, n);
    M.vecmatareisometries = @() false;

end


function [data clustPoints idx centers slopes lengths] = generateData( ...
    slope, ...
    slopeStd, ...
    numClusts, ...
    xClustAvgSep, ...
    yClustAvgSep, ...
    lengthAvg, ...
    lengthStd, ...
    lateralStd, ...
    totalPoints ...
    )
% generateData Generates 2D data for clustering; data is created along
%              straight lines, which can be more or less parallel depending
%              on slopeStd argument.
%
% Inputs:
%        slope - Base direction of the lines on which clusters are based.
%     slopeStd - Standard deviation of the slope; used to obtain a random
%                slope variation from the normal distribution, which is
%                added to the base slope in order to obtain the final slope
%                of each cluster.
%    numClusts - Number of clusters (and therefore of lines) to generate.
% xClustAvgSep - Average separation of line centers along the X axis.
% yClustAvgSep - Average separation of line centers along the Y axis.
%                line centers along each dimension.
%    lengthAvg - The base length of lines on which clusters are based.
%    lengthStd - Standard deviation of line length; used to obtain a random
%                length variation from the normal distribution, which is
%                added to the base length in order to obtain the final
%                length of each line.
%   lateralStd - "Cluster fatness", i.e., the standard deviation of the
%                distance from each point to the respective line, in both x
%                and y directions; this distance is obtained from the
%                normal distribution.
%  totalPoints - Total points in generated data (will be
%                randomly divided among clusters).
%
% Output:
%         data - Matrix (totalPoints x 2) with the generated data
%  clustPoints - Vector (numClusts x 1) containing number of points in each
%                cluster
%          idx - Vector (totalPoints x 1) containing the cluster indices of
%                each point
%      centers - Matrix (numClusts x 2) containing centers from where
%                clusters were generated
%       slopes - Vector (numClusts x 1) containing the effective slopes
%                used to generate clusters
%      lengths - Vector (numClusts x 1) containing the effective lengths
%                used to generate clusters
%
% ----------------------------------------------------------
% Usage example:
%
%   [data cp idx] = generateData(1, 0.5, 5, 15, 15, 5, 1, 2, 200);
%
% This creates 5 clusters with a total of 200 points, with a base slope
% of 1 (std=0.5), separated in average by 15 units in both x and y
% directions, with average length of 5 units (std=1) and a "fatness" or
% spread of 2 units.
%
% To take a quick look at the clusters just do:
%
%   scatter(data(:,1), data(:,2), 8, idx);
% ----------------------------------------------------------
%
%  N. Fachada
%  Instituto Superior Técnico
%  Aug 10, 2010
%  Updated: Aug 2, 2012

% Make sure totalPoints >= numClusts
if totalPoints < numClusts
    error('Number of points must be equal or larger than the number of clusters.');
end;

% Determine number of points in each cluster
clustPoints = abs(randn(numClusts, 1));
clustPoints = clustPoints / sum(clustPoints);
clustPoints = round(clustPoints * totalPoints);

% Make sure totalPoints is respected
while sum(clustPoints) < totalPoints
    % If one point is missing add it to the smaller cluster
    [C,I] = min(clustPoints);
    clustPoints(I(1)) = C + 1;
end;
while sum(clustPoints) > totalPoints
    % If there is one extra point, remove it from larger cluster
    [C,I] = max(clustPoints);
    clustPoints(I(1)) = C - 1;
end;

% Make sure there are no empty clusters
emptyClusts = find(clustPoints == 0);
if ~isempty(emptyClusts)
    % If there are empty clusters...
    numEmptyClusts = size(emptyClusts, 1);
    for i=1:numEmptyClusts
        % ...get a point from the largest cluster and assign it to the
        % empty cluster
        [C,I] = max(clustPoints);
        clustPoints(I(1)) = C - 1;
        clustPoints(emptyClusts(i)) = 1;
    end;
end;

% Initialize data matrix
data = zeros(sum(clustPoints), 2);

% Initialize idx (vector containing the cluster indices of each point)
idx = zeros(totalPoints, 1);

% Initialize lengths vector
lengths = zeros(numClusts, 1);

% Determine cluster centers
xCenters = xClustAvgSep * numClusts * (rand(numClusts, 1) - 0.5);
yCenters = yClustAvgSep * numClusts * (rand(numClusts, 1) - 0.5);
centers = [xCenters yCenters];

% Determine cluster slopes
slopes = slope + slopeStd * randn(numClusts, 1);

% Create clusters
for i=1:numClusts
    % Determine length of line where this cluster will be based
    lengths(i) = abs(lengthAvg + lengthStd*randn);
    % Determine how many points have been assigned to previous clusters
    sumClustPoints = 0;
    if i > 1
        sumClustPoints = sum(clustPoints(1:(i - 1)));
    end;
    % Create points for this cluster
    for j=1:clustPoints(i)
        % Determine where in the line the next point will be projected
        position = lengths(i) * rand - lengths(i) / 2;
        % Determine x coordinate of point projection
        delta_x = cos(atan(slopes(i))) * position;
        % Determine y coordinate of point projection
        delta_y = delta_x * slopes(i);
        % Get point distance from line in x coordinate
        delta_x = delta_x + lateralStd * randn;
        % Get point distance from line in y coordinate
        delta_y = delta_y + lateralStd * randn;
        % Determine the actual point
        data(sumClustPoints + j, :) = [(xCenters(i) + delta_x) (yCenters(i) + delta_y)];
    end;
    % Update idx
    idx(sumClustPoints + 1 : sumClustPoints + clustPoints(i)) = i;
end;


function v = rowNorm(A)
%ROWNORM  norm of each row
%   V = ROWNORM(A) returns the Euclidean norm of each row of A. When A is
%   an M*N matrix, the output, V, will be a M*1 vector.
%
%   Babak taati May 4, 2005
%   revised May 19, 2009

if nargin ~= 1
    error('Requires one input arguments.')
end

v = sqrt(sum(A.*A, 2));


function bti_test02 ( )

%*****************************************************************************80
%
%% BTI_TEST02 tests BURGERS_TIME_INVISCID.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTI_TEST02\n' );
  fprintf ( 1, '  Test BURGERS_TIME_INVISCID\n' );

  method = 2;
  nx = 81;
  nt = 200;
  t_max = 2.0;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Method: 2, Upwind conservative.\n' );
  fprintf ( 1, '  Initial condition: expansion\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_inviscid ( method, @ic_expansion, nx, nt, t_max, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 2 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers, inviscid, initial expansion, upwind conservative' )

  filename = 'bti_test02.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


%% ODF Estimation from EBSD data
%
%%
% In order to discuss ODF estimation from individual orientation data we
% start by loading an EBSD data set

mtexdata copper

plot(ebsd,ebsd.orientations)

%%
% of copper. The orientation distribution function can now be computed by

odf = calcDensity(ebsd('copper').orientations)

plotSection(odf,'contourf')
mtexColorMap LaboTeX
mtexColorbar

%%
% The function <orientation.calcODF.html calcODF> implements the ODF
% estimation from EBSD data in MTEX. The underlying statistical method is
% called kernel density estimation, which can be seen as a generalized
% histogram. To be more precise, let's $\psi : SO(3) \to R$ be a radially
% symmetric, unimodal model ODF. Then the kernel density estimator for the
% individual orientation data $o_1,o_2,\ldots,o_M$ is defined as
%
% $$f(o) = \frac{1}{M} \sum_{i=1}^{M} \psi(o o_i^{-1})$$
%
% The choice of the model ODF $\psi$ and in particular its halfwidth has a
% great impact in the resulting ODF. If no halfwidth is specified the
% default halfwidth of 10 degrees is selected.
%
%% Automatic halfwidth selection
%
% MTEX includes an automatic halfwidth selection algorithm which is called
% by the command <orientation.calcKernel.html calcKernel>. To work
% properly, this algorithm needs spatially independent EBSD data as in the
% case of this dataset of very rough EBSD measurements (only one
% measurement per grain).

% try to compute an optimal kernel
psi = calcKernel(ebsd.orientations)

%%
% In the above example, the EBSD measurements are spatial dependent and the
% resulting halfwidth is too small. To avoid this problem we have to perform
% grain reconstruction first and then estimate the halfwidth from the
% grains.

% grains reconstruction
grains = calcGrains(ebsd);

% correct for to small grains
grains = grains(grains.grainSize>5);

% compute optimal halfwidth from the meanorientations of grains
psi = calcKernel(grains('co').meanOrientation)

% compute the ODF with the kernel psi
odf = calcDensity(ebsd('co').orientations,'kernel',psi)

%%
% Once an ODF is estimated all the functionality MTEX offers for
% <ODFCharacteristics.html ODF analysis> and <ODFPlot.html ODF
% visualization> is available.

h = [Miller(1,0,0,odf.CS),Miller(1,1,0,odf.CS),Miller(1,1,1,odf.CS)];
plotPDF(odf,h,'antipodal','silent')

%% Effect of halfwidth selection
%
% As mentioned above a proper halfwidth selection is crucial for ODF
% estimation. The following simple numerical experiment illustrates the
% dependency between the kernel halfwidth and the estimated error.
%
% Let's start with a model ODF and simulate some individual orientation data.

modelODF = fibreODF(Miller(1,1,1,crystalSymmetry('cubic')),xvector);
ori = discreteSample(modelODF,10000)

%%
% Next we define a list of kernel halfwidth ,

hw = [1*degree, 2*degree, 4*degree, 8*degree, 16*degree, 32*degree];

%%
% estimate for each halfwidth an ODF and compare it to the original ODF.

e = zeros(size(hw));
for i = 1:length(hw)

  odf = calcDensity(ori,'halfwidth',hw(i),'silent');
  e(i) = calcError(modelODF, odf);

end

%%
% After visualizing the estimation error we observe that its value is large
% either if we choose a very small or a very large halfwidth.
% In this specific example, the optimal halfwidth seems to be about 4
% degrees.

close all
plot(hw/degree,e)
xlabel('halfwidth in degree')
ylabel('esimation error')


%  Figure 5.45  Feedback Control of Dynamic Systems, 6e
%                   Franklin, Powell, Emami

clf
numG=[1 0];
denG=[1 1 4];
K1=0:.1:8;   % K = 2 at breakin point
K2=100;
K=[K1 K2];
axis('square')
r=rlocus(numG,denG,K);    % roots vs. K
plot(r,'-'),nicegrid;
axis([-6 2 -4 4])
hold on
p=roots(denG);             % find and plot OL poles
z=roots(numG);             % find and plot OL zeros
plot(z(1),0,'o')
plot(real(p(1)),imag(p(1)),'x',real(p(2)),imag(p(2)),'x')
title('Fig. 5.45  Root Locus vs. K_T')
xlabel('Re(s)')
ylabel('Im(s)')
hold off
axis('normal')


function [s,lon,lat] = m_geodesic(lon1,lat1,lon2,lat2,Npoints,varargin)
% M_GEODESIC - Compute points along geodesics of an ellipsoidal earth.
%
%       [S,LON,LAT] = m_geodesic(lon1,lat1,lon2,lat2,Npoints,spheroid)
%
% lat1 = GEODETIC latitude of first point (degrees)
% lon1 = longitude of first point (degrees)
% lat2, lon2 = second point (degrees)
% Npoints = number of points along geodesic
% spheroid = (Optional) spheroid, defaults to 'wgs84'
% S = distance in meters
% LON,LAT = points on geodesics.
%
% Note that inputs can be the same size, or a mixture of scalars and matrices.
% However, output curves will be on columns of a matrix (i.e. form of
% inputs will not be preserved).
%
% See M_FDIST, M_IDIST, M_LLDIST

% R. pawlowicz (rich@ocgy.ubc.ca) 10/Jan/2005
%
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.

% Do the equivalent of meshgrid-type calls to make all
% matrix sizes match. To do this we can have no more than
% two different numbers in any particular dimension, and one
% of these has to be a '1'.
allsize=[size(lon1);size(lat1);size(lon2);size(lat2)];
i1=ones(1,size(allsize,2));
for k=1:size(allsize,2)
 rs=unique(allsize(:,k));
 if length(rs)==2 && rs(1)==1
   j1=i1;j1(k)=rs(2);
   if allsize(1,k)==1,lon1=repmat(lon1,j1); end
   if allsize(2,k)==1,lat1=repmat(lat1,j1); end
   if allsize(3,k)==1,lon2=repmat(lon2,j1); end
   if allsize(4,k)==1,lat2=repmat(lat2,j1); end
 elseif length(rs)>2
  error('incompatible array sizes!');
 end
end

% Get the distances and bearings

[S,A12,A21]=m_idist(lon1,lat1,lon2,lat2,varargin{:});

s=[0:Npoints-1]'/(Npoints-1)*S(:)';

[lon,lat]=m_fdist(lon1(:)',lat1(:)',A12(:)',s,varargin{:});


function geometry_test0415 ( )

%*****************************************************************************80
%
%% TEST0415 tests LINES_IMP_INT_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 April 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0415\n' );
  fprintf ( 1, '  LINES_IMP_INT_2D finds the intersection of\n' );
  fprintf ( 1, '  two lines written in implicit form.\n' );
  fprintf ( 1, '\n' );
%
%  x + 2y - 4 = 0
%
  a1 =  1.0;
  b1 =  2.0;
  c1 = -4.0;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Line 1 coefficients:  %8f  %8f  %8f\n', a1, b1, c1 );
%
%  x - y - 1 = 0
%
  a2 =  1.0;
  b2 = -1.0;
  c2 = -1.0;
  fprintf ( 1, '  Line 2 coefficients:  %8f  %8f  %8f\n', a2, b2, c2 );

  [ ival, p ] = lines_imp_int_2d ( a1, b1, c1, a2, b2, c2 );

  if ( ival == 1 )
    fprintf ( 1, '  Intersection at %8f  %8f\n', p(1:dim_num) )
  elseif ( ival == 0 )
    fprintf ( 1, '  Lines are parallel, no intersection.\n' );
  elseif ( ival == 2 )
    fprintf ( 1, '  Lines are coincident.\n' );
  else
    fprintf ( 1, '  Unknown return value of ival = %d\n', ival );
  end
%
%  2x + 4y - 1 = 0
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Line 1 coefficients:  %8f  %8f  %8f\n', a1, b1, c1 );

  a2 =  2.0;
  b2 = +4.0;
  c2 = -1.0;
  fprintf ( 1, '  Line 2 coefficients:  %8f  %8f  %8f\n', a2, b2, c2 );

  [ ival, p ] = lines_imp_int_2d (a1, b1, c1, a2, b2, c2 );

  if ( ival == 1 )
    fprintf ( 1, '  Intersection at %8f  %8f\n', p(1:dim_num) );
  elseif ( ival == 0 )
    fprintf ( 1, '  Lines are parallel, no intersection.\n' );
  elseif ( ival == 2 )
    fprintf ( 1, '  Lines are coincident.\n' );
  else
    fprintf ( 1, '  Unknown return value of ival = %d\n', ival );
  end
%
%  -3x - 6y +12 = 0
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Line 1 coefficients:  %8f  %8f  %8f\n', a1, b1, c1 );

  a2 =  -3.0;
  b2 =  -6.0;
  c2 = +12.0;
  fprintf ( 1, '  Line 2 coefficients:  %8f  %8f  %8f\n', a2, b2, c2 );

  [ ival, p ] = lines_imp_int_2d  ( a1, b1, c1, a2, b2, c2 );

  if ( ival == 1 )
    fprintf ( 1, '  Intersection at %8f  %8f\n', p(1:dim_num) );
  elseif ( ival == 0 )
    fprintf ( 1, '  Lines are parallel, no intersection.\n' );
  elseif ( ival == 2 )
    fprintf ( 1, '  Lines are coincident.\n' );
  else
    fprintf ( 1, '  Unknown return value of ival = %d\n', ival );
  end

  return
end


% Plot steady state objects
gSS = reshape(ggSS,I,2);
g_a = (lla(2) / (lla(1) + lla(2))) * gSS(:,1) ...
    + (lla(1) / (lla(1) + lla(2))) * gSS(:,2);

figure
hold on
f = bar(a / (wSS * (1 - ttau) * zAvg),g_a,'histc');
sh=findall(gcf,'marker','*'); delete(sh);
set(f,'FaceColor',[.03,.24,.8],'EdgeColor',[.8,.24,.03]);
xlim([min(a / (wSS * (1 - ttau) * zAvg)) .8*max(a / (wSS * (1 - ttau) * zAvg))])
xlabel('Net worth to average income','interpreter','latex')
ylabel('Mass of households','interpreter','latex')
title('Steady State Distribution of Assets','interpreter','latex','fontsize',14)
grid on
alpha(0.9)
set(gcf,'color','w')
hold off

% Plot steady state MPCs
tau_MPC = 1;
J = 2;
N_MPC = 21;
dt_MPC = tau_MPC / (N_MPC - 1);

C_stacked = zeros(I*J,N_MPC );
ctilde = zeros(I*J,1);
C_stacked(:,N_MPC) = 0;

B = (1/dt_MPC) * speye(I*J) - A;
c_stacked = reshape(cSS,I*J,1);

for n=N_MPC-1:-1:1
    vec = c_stacked + C_stacked(:,n+1)/dt_MPC;
    C_stacked(:,n) = B\vec;
end

C = reshape(C_stacked(:,1),I,J);
MPC = (C(2:I,:) - C(1:I-1,:)) ./ (aa(2:I,:) - aa(1:I-1,:));

figure
hold on
plot(a(2:I) / (wSS * (1 - ttau) * zAvg),MPC(:,1),'linewidth',1.5,'linestyle','-','color',[.03,.24,.8])
plot(a(2:I) / (wSS * (1 - ttau) * zAvg),MPC(:,2),'linewidth',1.5,'linestyle','-','color',[.8,.24,.03])
plot(a(2:I) / (wSS * (1 - ttau) * zAvg),ones(I-1,1)*MPC(.8*I-1,2),'linewidth',1.5,'linestyle','--','color','k')
xlim([min(a(2:I) / (wSS * (1 - ttau) * zAvg)) .8*max(a / (wSS * (1 - ttau) * zAvg))])
xlabel('Net worth to average income','interpreter','latex')
ylabel('MPC','interpreter','latex')
text(3,.05,'$\leftarrow z=0 $','Fontsize',20,'interpreter','latex','Color',[.8,.24,.03]);
text(1,0.25,'$\leftarrow z=1 $','Fontsize',20,'interpreter','latex','Color',[.03,.24,.8]);
set(gcf,'color','w')
title('MPCs in steady state','interpreter','latex','fontsize',14)
alpha(0.7)
grid on
hold off
drawnow


function [res] = lmc(x)

%LMC calculates the left medcouple, a robust measure of
%left tail weight
%
% The left medcouple is described in:
%    Brys, G., Hubert, M. and Struyf, A. (2006),
%    "Robust Measures of Tail Weight",
%    Computational Statistics and Data Analysis,
%    50 (No 3), 733-759.
%
% For the up-to-date reference, please consult the website:
%    wis.kuleuven.be/stat/robust.html
%
% Required input arguments:
%    x : Data matrix (rows=observations, columns=variables)
%
% I/O:
%    result=lmc(x);
%
% Example:
%    result = lmc([chi2rnd(5,1000,1) trnd(3,1000,1)]);
%
% The output of LMC is a vector containing the left medcouple
%     for each column of the data matrix x
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Guy Brys
% Last Update: 17/03/2006

if (nargin<1)
    error('No input arguments')
end
if (size(x,1)==1)
    x = x';
end
for (i=1:size(x,2))
    res(i) = -mc(x(x(:,i)<=prctile(x(:,i),50),i));
end


function call_price=european_call_div(S, K, r, sigma, time_to_maturity, dividend_times, dividend_amounts)

%--------------------------------------------------------------------------
%
% DESCRIPTION:
%
% European option price with an underlying stock paying a discrete-time
% dividend
%
%
% Reference:
%
% John Hull, "Options, Futures and other Derivative Securities",
% Prentice-Hall, second edition, 1993.
%
%--------------------------------------------------------------------------
%
% INPUTS:
%
%  S:                   spot price
%  K:                   exercice price
%  r:                   interest rate
%  y:                   continous payout
%  sigma:               volatility
%  time_to_maturity:    time to maturity
%  dividend_times:      periods of dividend payment
%  dividend_amounts:    amounts of dividend payment
%
%--------------------------------------------------------------------------
%
% OUTPUT:
%
% call_price: price of a call option
%
%--------------------------------------------------------------------------
%
% Author:  Paolo Z., February 2012
%
%--------------------------------------------------------------------------

adjusted_S = S;

for ( i=1:max(size(dividend_times)) )
    if (dividend_times(i)<=time_to_maturity)
        adjusted_S = adjusted_S-dividend_amounts(i)*exp(-r*dividend_times(i));
    end
end

call_price = bs_european_call(adjusted_S,K,r,sigma,time_to_maturity);


function dcor = distcorr(x,y)

% This function calculates the distance correlation between x and y.
% Reference: http://en.wikipedia.org/wiki/Distance_correlation
% Date: 18 Jan, 2013
% Author: Shen Liu (shen.liu@hotmail.com.au)

% Check if the sizes of the inputs match
if size(x,1) ~= size(y,1);
    error('Inputs must have the same number of rows')
end

% Delete rows containing unobserved values
N = any([isnan(x) isnan(y)],2);
x(N,:) = [];
y(N,:) = [];

% Calculate doubly centered distance matrices for x and y
a = pdist2(x,x);
mcol = mean(a);
mrow = mean(a,2);
ajbar = ones(size(mrow))*mcol;
akbar = mrow*ones(size(mcol));
abar = mean(mean(a))*ones(size(a));
A = a - ajbar - akbar + abar;

b = pdist2(y,y);
mcol = mean(b);
mrow = mean(b,2);
bjbar = ones(size(mrow))*mcol;
bkbar = mrow*ones(size(mcol));
bbar = mean(mean(b))*ones(size(b));
B = b - bjbar - bkbar + bbar;

% Calculate squared sample distance covariance and variances
dcov = sum(sum(A.*B))/(size(mrow,1)^2);

dvarx = sum(sum(A.*A))/(size(mrow,1)^2);
dvary = sum(sum(B.*B))/(size(mrow,1)^2);

% Calculate the distance correlation
dcor = sqrt(dcov/sqrt(dvarx*dvary));


% This program illustrates various examples presented in
% 'Advanced Mathematics and Mechanics Applications Using MATLAB'
% 3rd Ed., CRC Press, 2003, by Howard Wilson, Louis Turcotte and
% David Halpern. The program is executed by typing rundynam.
% Then select the individually numbered examples. The separate
% program modules are listed below.
%
% rundynam   main program which calls the other modules
% beamwave   vibrating beam with initial deflection
% beszeros   zeros of integer order Bessel functions
% circwave   circular membrane with an oscillating force
% elipmodes  elliptic membrane modes by Moler's method
% forcmove   moving force on a semi-infinite string
% rectwave   rectangular membrane with an oscillating force
% runchain   falling elastic chain with one end fixed
% runpenl    pendulum with large amplitude oscillations
% runtop     spinning and nutating top motion
% smdplot    spring-mass-damper with harmonic applied force
% stringmo   vibrating string shaken at one end
% strngwave  vibrating string with initial deflection
% trusvibs   natural frequencies of a pin-connected truss


function r = revaltrig(zz, zj, fj, wj, form)
%REVALTRIG   Evaluate rational function in trigonometric barycentric form.
%   R = REVALTRIG(ZZ, ZJ, FJ, WJ, FORM) returns R (vector of floats), the
%   values of the trigonometric barycentric rational function of form FORM
%   with support points ZJ, function values FJ, and barycentric weights WJ
%   evaluated at the points ZZ.
%
% See also AAATRIG, PRZTRIG, REVAL.

% Copyright 2018 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.
m = numel(zj);
zv = zz(:);                             % vectorize zz if necessary

% Define basis functions
if strcmp(form,'even')
    cst = @(x) cot(x);
elseif strcmp(form,'odd')
    cst = @(x) csc(x);
end

CC = cst(bsxfun(@minus, zv, zj.')/2); % Cauchy matrix
r = (CC*(wj.*fj))./(CC*wj);             % vector of values

if strcmp(form,'odd')
r(isinf(real(zv/1i)) & real(zv/1i)>0) = sum(wj.*fj.*exp(-1i*zj/2))./sum(wj.*exp(-1i*zj/2));
r(isinf(real(zv/1i)) & real(zv/1i)<0) = sum(wj.*fj.*exp(+1i*zj/2))./sum(wj.*exp(+1i*zj/2));
elseif strcmp(form,'even')
r(isinf(real(zv/1i))) = sum(wj.*fj)./sum(wj);
end

% Deal with NaN:
ii = find(isnan(r));
for jj = 1:length(ii)
    if ( isnan(zv(ii(jj))) || ~any(zv(ii(jj)) == zj) )
        % r(NaN) = NaN is fine.
        % The second case may happen if r(zv(ii)) = 0/0 at some point.
    else
        % Clean up values NaN = inf/inf at support points.
        % Find the corresponding node and set entry to correct value:
        r(ii(jj)) = fj(zv(ii(jj)) == zj);
    end
end

% Reshape to input format:
r = reshape(r, size(zz));

end % End of REVALTRIG().


function K = Abrarov2010CPC(x,y,tau,N)
% This is an approximation of the Voigt function within the Humlicek
% regions 3 and 4. The approximation is one given by S.M. Abrarov et. al.
% "High-accurace approximation of the complex probability function by
% Fourier expansion of exponential multiplier" (2010).
% x = sqrt(ln(2))*(nu - nu0)/alphaD
% y = sqrt(ln(2))*alphaL/alphaD
% where 'ln' denotes the natural log, nu the wavenumber, nu0 the wavenumber
% at center, alphaD and alphaL the Doppler and Lorentzian half-width at
% half-maximum.
% Suggested values for N & tau are 23 and 12 respectively.

if nargin == 2
    tau = 12;
    N = 23;
end

K = zeros(size(x));
a = zeros(1,N);
for c = 1:length(x)
    summation = 0;
    for n = 0:N
        a(n+1) = 2*sqrt(pi)/tau*exp(-(n*pi/tau)^2);
        first = ((1i*n*pi*tau+tau^2*y)*(1-exp(-(1i*n*pi+tau*y))*cos(tau*x(c))) + exp(-(1i*n*pi+tau*y))*tau^2*x(c)*sin(tau*x(c)))/(tau^2*x(c)^2 - (n*pi - 1i*tau*y)^2);
        second = ((1i*n*pi*tau-tau^2*y)*(1-exp(1i*n*pi-tau*y)*cos(tau*x(c))) - exp(1i*n*pi-tau*y)*tau^2*x(c)*sin(tau*x(c)))/(tau^2*x(c)^2 - (n*pi + 1i*tau*y)^2);
        summation = summation + a(n+1)*(first - second);
    end
    third = (y-exp(-(tau*y))*(y*cos(tau*x(c))-x(c)*sin(tau*x(c))))/(2*sqrt(pi)*(x(c)^2+y^2));
    K(c) = summation/(2*sqrt(pi)) - a(1)*third;
end


function [xk,niter,residuals,outputData,opts] =NESTA(A,At,b,muf,delta,opts)
% [xk,niter,residuals,outputData] =NESTA(A,At,b,muf,delta,opts)
%
% Solves a L1 minimization problem under a quadratic constraint using the
% Nesterov algorithm, with continuation:
%
%     min_x || U x ||_1 s.t. ||y - Ax||_2 <= delta
%
% Continuation is performed by sequentially applying Nesterov's algorithm
% with a decreasing sequence of values of  mu0 >= mu >= muf
%
% The primal prox-function is also adapted by accounting for a first guess
% xplug that also tends towards x_muf
%
% The observation matrix A is a projector
%
% Inputs:   A and At - measurement matrix and adjoint (either a matrix, in which
%               case At is unused, or function handles).  m x n dimensions.
%           b   - Observed data, a m x 1 array
%           muf - The desired value of mu at the last continuation step.
%               A smaller mu leads to higher accuracy.
%           delta - l2 error bound.  This enforces how close the variable
%               must fit the observations b, i.e. || y - Ax ||_2 <= delta
%               If delta = 0, enforces y = Ax
%               Common heuristic: delta = sqrt(m + 2*sqrt(2*m))*sigma;
%               where sigma=std(noise).
%           opts -
%               This is a structure that contains additional options,
%               some of which are optional.
%               The fieldnames are case insensitive.  Below
%               are the possible fieldnames:
%
%               opts.xplug - the first guess for the primal prox-function, and
%                 also the initial point for xk.  By default, xplug = At(b)
%               opts.U and opts.Ut - Analysis/Synthesis operators
%                 (either matrices of function handles).
%               opts.normU - if opts.U is provided, this should be norm(U)
%                   otherwise it will have to be calculated (potentially
%                   expensive)
%               opts.MaxIntIter - number of continuation steps.
%                 default is 5
%               opts.maxiter - max number of iterations in an inner loop.
%                 default is 10,000
%               opts.TolVar - tolerance for the stopping criteria
%               opts.stopTest - which stopping criteria to apply
%                   opts.stopTest == 1 : stop when the relative
%                       change in the objective function is less than
%                       TolVar
%                   opts.stopTest == 2 : stop with the l_infinity norm
%                       of difference in the xk variable is less
%                       than TolVar
%               opts.TypeMin - if this is 'L1' (default), then
%                   minimizes a smoothed version of the l_1 norm.
%                   If this is 'tv', then minimizes a smoothed
%                   version of the total-variation norm.
%                   The string is case insensitive.
%               opts.Verbose - if this is 0 or false, then very
%                   little output is displayed.  If this is 1 or true,
%                   then output every iteration is displayed.
%                   If this is a number p greater than 1, then
%                   output is displayed every pth iteration.
%               opts.fid - if this is 1 (default), the display is
%                   the usual Matlab screen.  If this is the file-id
%                   of a file opened with fopen, then the display
%                   will be redirected to this file.
%               opts.errFcn - if this is a function handle,
%                   then the program will evaluate opts.errFcn(xk)
%                   at every iteration and display the result.
%                   ex.  opts.errFcn = @(x) norm( x - x_true )
%               opts.outFcn - if this is a function handle,
%                   then then program will evaluate opts.outFcn(xk)
%                   at every iteration and save the results in outputData.
%                   If the result is a vector (as opposed to a scalar),
%                   it should be a row vector and not a column vector.
%                   ex. opts.outFcn = @(x) [norm( x - xtrue, 'inf' ),...
%                                           norm( x - xtrue) / norm(xtrue)]
%               opts.AAtinv - this is an experimental new option.  AAtinv
%                   is the inverse of AA^*.  This allows the use of a
%                   matrix A which is not a projection, but only
%                   for the noiseless (i.e. delta = 0) case.
%               opts.USV - another experimental option.  This supercedes
%                   the AAtinv option, so it is recommended that you
%                   do not define AAtinv.  This allows the use of a matrix
%                   A which is not a projection, and works for the
%                   noisy ( i.e. delta > 0 ) case.
%                   opts.USV should contain three fields:
%                   opts.USV.U  is the U from [U,S,V] = svd(A)
%                   likewise, opts.USV.S and opts.USV.V are S and V
%                   from svd(A).  S may be a matrix or a vector.
%
%  Outputs:
%           xk  - estimate of the solution x
%           niter - number of iterations
%           residuals - first column is the residual at every step,
%               second column is the value of f_mu at every step
%           outputData - a matrix, where each row r is the output
%               from opts.outFcn, if supplied.
%           opts - the structure containing the options that were used
%
% Written by: Jerome Bobin, Caltech
% Email: bobin@acm.caltech.edu
% Created: February 2009
% Modified (version 1.0): May 2009, Jerome Bobin and Stephen Becker, Caltech
% Modified (version 1.1): Nov 2009, Stephen Becker, Caltech
%
% NESTA Version 1.1
%   See also Core_Nesterov

if nargin < 6, opts = []; end
if isempty(opts) && isnumeric(opts), opts = struct; end

%---- Set defaults
fid = setOpts('fid',1);
Verbose = setOpts('Verbose',true);
function printf(varargin), fprintf(fid,varargin{:}); end
MaxIntIter = setOpts('MaxIntIter',5,1);
TypeMin = setOpts('TypeMin','L1');
TolVar = setOpts('tolvar',1e-5);
[U,U_userSet] = setOpts('U', @(x) x );
if ~isa(U,'function_handle')
    Ut = setOpts('Ut',[]);
else
    Ut = setOpts('Ut', @(x) x );
end
xplug = setOpts('xplug',[]);
normU = setOpts('normU',[]);  % so we can tell if it's been set

residuals = []; outputData = [];
AAtinv = setOpts('AAtinv',[]);
USV = setOpts('USV',[]);
if ~isempty(USV)
    if isstruct(USV)
        Q = USV.U;  % we can't use "U" as the variable name
                    % since "U" already refers to the analysis operator
        S = USV.S;
        if isvector(S), s = S; %S = diag(s);
        else s = diag(S); end
        %V = USV.V;
    else
        error('opts.USV must be a structure');
    end
end

% -- We can handle non-projections IF a (fast) routine for computing
%    the psuedo-inverse is available.
%    We can handle a nonzero delta, but we need the full SVD
if isempty(AAtinv) && isempty(USV)
    % Check if A is a partial isometry, i.e. if AA' = I
    z = randn(size(b));
    if isa(A,'function_handle'), AAtz = A(At(z));
    else AAtz = A*(A'*z); end
    if norm( AAtz - z )/norm(z) > 1e-8
        error('Measurement matrix A must be a partial isometry: AA''=I');
    end
end

% -- Find a initial guess if not already provided.
%   Use least-squares solution: x_ref = A'*inv(A*A')*b
% If A is a projection, the least squares solution is trivial
if isempty(xplug) || norm(xplug) < 1e-12
    if ~isempty(USV) && isempty(AAtinv)
        AAtinv = Q*diag( s.^(-2) )*Q';
    end
    if ~isempty(AAtinv)
        if delta > 0 && isempty(USV)
            error('delta must be zero for non-projections');
        end
        if isa(AAtinv,'function_handle')
            x_ref = AAtinv(b);
        else
            x_ref = AAtinv * b;
        end
    else
        x_ref = b;
    end

    if isa(A,'function_handle')
        x_ref=At(x_ref);
    else
        x_ref = A'*x_ref;
    end

    if isempty(xplug)
        xplug = x_ref;
    end
    % x_ref itself is used to calculate mu_0
    %   in the case that xplug has very small norm
else
    x_ref = xplug;
end

% use x_ref, not xplug, to find mu_0
if isa(U,'function_handle')
    Ux_ref = U(x_ref);
else
    Ux_ref = U*x_ref;
end
switch lower(TypeMin)
    case 'l1'
        mu0 = 0.9*max(abs(Ux_ref));
    case 'tv'
        mu0 = ValMUTv(Ux_ref);
end

% -- If U was set by the user and normU not supplied, then calcuate norm(U)
if U_userSet && isempty(normU)
    % simple case: U*U' = I or U'*U = I, in which case norm(U) = 1
    z = randn(size(xplug));
    if isa(U,'function_handle'), UtUz = Ut(U(z)); else UtUz = U'*(U*z); end
    if norm( UtUz - z )/norm(z) < 1e-8
        normU = 1;
    else
        z = randn(size(Ux_ref));
        if isa(U,'function_handle')
            UUtz = U(Ut(z));
        else
            UUtz = U*(U'*z);
        end
        if norm( UUtz - z )/norm(z) < 1e-8
            normU = 1;
        end
    end

    if isempty(normU)
        % have to actually calculate the norm
        if isa(U,'function_handle')
            [normU,cnt] = my_normest(U,Ut,length(xplug),1e-3,30);
            if cnt == 30, printf('Warning: norm(U) may be inaccurate\n'); end
        else
            [mU,nU] = size(U);
            if mU < nU, UU = U*U'; else UU = U'*U; end
            % last resort is to call MATLAB's "norm", which is slow
            if norm( UU - diag(diag(UU)),'fro') < 100*eps
                % this means the matrix is diagonal, so norm is easy:
                normU = sqrt( max(abs(diag(UU))) );
            elseif issparse(UU)
                normU = sqrt( normest(UU) );
            else
                if min(size(U)) > 2000
                    % norm(randn(2000)) takes about 5 seconds on my PC
                    printf('Warning: calculation of norm(U) may be slow\n');
                end
                normU = sqrt( norm(UU) );
            end
        end
    end
    opts.normU = normU;
end

niter = 0;
Gamma = (muf/mu0)^(1/MaxIntIter);
mu = mu0;
Gammat= (TolVar/0.1)^(1/MaxIntIter);
TolVar = 0.1;

for nl=1:MaxIntIter

    mu = mu*Gamma;
    TolVar=TolVar*Gammat;    opts.TolVar = TolVar;
    opts.xplug = xplug;
    if Verbose, printf('\tBeginning %s Minimization; mu = %g\n',opts.TypeMin,mu); end
    [xk,niter_int,res,out,optsOut] = Core_Nesterov(...
        A,At,b,mu,delta,opts);

    xplug = xk;
    niter = niter_int + niter;

    residuals = [residuals; res];
    outputData = [outputData; out];

end
opts = optsOut;

%---- internal routine for setting defaults
function [var,userSet] = setOpts(field,default,mn,mx)
    var = default;
    % has the option already been set?
    if ~isfield(opts,field)
        % see if there is a capitalization problem:
        names = fieldnames(opts);
        for i = 1:length(names)
            if strcmpi(names{i},field)
                opts.(field) = opts.(names{i});
                opts = rmfield(opts,names{i});
                break;
            end
        end
    end
    if isfield(opts,field) && ~isempty(opts.(field))
        var = opts.(field);  % override the default
        userSet = true;
    else
        userSet = false;
    end
    % perform error checking, if desired
    if nargin >= 3 && ~isempty(mn)
        if var < mn
            printf('Variable %s is %f, should be at least %f\n',...
                field,var,mn); error('variable out-of-bounds');
        end
    end
    if nargin >= 4 && ~isempty(mx)
        if var > mx
            printf('Variable %s is %f, should be at least %f\n',...
                field,var,mn); error('variable out-of-bounds');
        end
    end
    opts.(field) = var;
end

%---- internal routine for setting mu0 in the tv minimization case
function th=ValMUTv(x)

    N = length(x);n = floor(sqrt(N));
    Dv = spdiags([reshape([-ones(n-1,n); zeros(1,n)],N,1) ...
            reshape([zeros(1,n); ones(n-1,n)],N,1)], [0 1], N, N);
        Dh = spdiags([reshape([-ones(n,n-1) zeros(n,1)],N,1) ...
            reshape([zeros(n,1) ones(n,n-1)],N,1)], [0 n], N, N);
        D = sparse([Dh;Dv]);

    Dhx = Dh*x;
    Dvx = Dv*x;

    sk = sqrt(abs(Dhx).^2 + abs(Dvx).^2);
    th = max(sk);

end

end %-- end of NESTA function

%%%%%%%%%%%% POWER METHOD TO ESTIMATE NORM %%%%%%%%%%%%%%%
% Copied from MATLAB's "normest" function, but allows function handles, not just sparse matrices
function [e,cnt] = my_normest(S,St,n,tol, maxiter)
%MY_NORMEST Estimate the matrix 2-norm via power method.
    if nargin < 4, tol = 1.e-6; end
    if nargin < 5, maxiter = 20; end
    if isempty(St)
        St = S;  % we assume the matrix is symmetric;
    end
    x = ones(n,1);
    cnt = 0;
    e = norm(x);
    if e == 0, return, end
    x = x/e;
    e0 = 0;
    while abs(e-e0) > tol*e && cnt < maxiter
       e0 = e;
       Sx = S(x);
       if nnz(Sx) == 0
          Sx = rand(size(Sx));
       end
       e = norm(Sx);
       x = St(Sx);
       x = x/norm(x);
       cnt = cnt+1;
    end
end


function normals = faceNormal(nodes, faces)
%FACENORMAL Compute normal vector of faces in a 3D mesh.
%
%   NORMALS = faceNormal(VERTICES, FACES)
%   VERTICES is a set of 3D points (as a N-by-3 array), and FACES is either
%   a N-by-3 index array or a cell array of indices. The function computes
%   the normal vector of each face.
%   The orientation of the normal is defined by the sign of cross product
%   between vectors joining vertices 1 to 2 and 1 to 3.
%
%
%   Example
%     [v e f] = createIcosahedron;
%     normals1 = faceNormal(v, f);
%     centros1 = faceCentroids(v, f);
%     figure; drawMesh(v, f);
%     hold on; axis equal; view(3);
%     drawVector3d(centros1, normals1);
%
%     pts = rand(50, 3);
%     hull = minConvexHull(pts);
%     normals2 = faceNormal(pts, hull);
%
%   See also
%   meshes3d, drawMesh, convhull, convhulln, drawVector3d

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2006-07-05
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

% deprecation warning
warning('geom3d:deprecated', ...
    [mfilename ' is deprecated, use ''meshFaceNormals'' instead']);

if isnumeric(faces)
    % compute vector of first edges
	v1 = nodes(faces(:,2),1:3) - nodes(faces(:,1),1:3);
    v2 = nodes(faces(:,3),1:3) - nodes(faces(:,1),1:3);

%     % normalize vectors
%     v1 = normalizeVector3d(v1);
%     v2 = normalizeVector3d(v2);

    % compute normals using cross product (nodes have same size)
	normals = normalizeVector3d(cross(v1, v2, 2));

else
    % initialize empty array
    normals = zeros(length(faces), 3);

    for i = 1:length(faces)
        face = faces{i};
        % compute vector of first edges
        v1 = nodes(face(2),1:3) - nodes(face(1),1:3);
        v2 = nodes(face(3),1:3) - nodes(face(1),1:3);

%         % normalize vectors
%         v1 = normalizeVector3d(v1);
%         v2 = normalizeVector3d(v2);

        % compute normals using cross product
        normals(i, :) = normalizeVector3d(cross(v1, v2, 2));
    end
end


function pce_burgers ( )

%*****************************************************************************80
%
%% PCE_BURGERS applies the polynomial chaos expansion to the Burgers equation.
%
%  Discussion:
%
%    The time-dependent viscous Burgers equation to be solved is:
%
%      du/dt = - d ( u*(1/2-u)) /dx + nu d2u/dx2
%
%    with boundary conditions
%
%      u(-3.0) = 0.0, u(+3.0) = 1.0.
%
%    The viscosity nu is assumed to be an uncertain quantity with
%    normal distribution of known mean and variance.
%
%    A polynomial chaos expansion is to be used, with Hermite polynomial
%    basis functions h(i,x), 0 <= i <= n.
%
%    Because the first two Hermite polynomials are simply 1 and x,
%    we have that
%
%      nu = nu_mean * h(0,x) + nu_variance * h(1,x).
%
%    We replace the time derivative by an explicit Euler approximation,
%    so that the equation now describes the value of U(x,t+dt) in terms
%    of known data at time t.
%
%    Now assume that the solution U(x,t) can be approximated
%    by the truncated expansion:
%
%      U(x,t) = sum ( 0 <= i <= n ) c(i,t) * h(i,x)
%
%    In the equation, we replace U by its expansion, and then multiply
%    successively by each of the basis functions h(*,x) to get a set of
%    n+1 equations that can be used to determine the values of c(i,t+dt).
%
%    This process is repeated until the desired final time is reached.
%
%    At any time, the coefficients c(0,t) contain information definining
%    the expected value of u(x,t) at that time, while the higher order coefficients
%    can be used to deterimine higher moments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2012
%
%  Author:
%
%    Original FORTRAN90 version by Gianluca Iaccarino.
%    This MATLAB version is by John Burkardt.
%
%  Local parameters:
%
%    Local, real DT, the timestep.
%
%    Local, real DX, the spacing between grid points.
%
%    Local, integer N, the number of intervals in the spatial domain.
%
%    Local, real NUMEAN, the mean of viscosity.
%
%    Local, real NUVARIANCE, the variance of viscosity.
%
%    Local, integer P, the order of the PC expansion.
%
%    Local, real T, the current time.
%
%    Local, real TF, the final integration time.
%
%    Local, real U1(N+1,P+1), the PCE representation at the current time.
%
%    Local, real U2(N+1,P+1), the PCE representation for the next time.
%
%    Local, real X(N+1,1), the grid points.
%
  p = 5;
  n = 32;
  nt = 2000;
  ti = 0.0;
  tf = 2.0;
  dt = ( tf - ti ) / nt;
  numean = 0.25;
  nuvariance = 0.08;

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PCE_BURGERS\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Polynomial Chaos Expansion\n' );
  fprintf ( 1, '  1D Burgers equation\n' );
  fprintf ( 1, '  Original version by Gianluca Iaccarino\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  PCE order       = %d\n', p );
  fprintf ( 1, '  Number of cells = %d\n', n );
  fprintf ( 1, '  Time step       = %g\n', dt );
  fprintf ( 1, '  Initial time    = %g\n', ti );
  fprintf ( 1, '  Final time      = %g\n', tf );
  fprintf ( 1, '  Viscosity Mean  = %g\n', numean );
  fprintf ( 1, '  Viscosity Var   = %g\n', nuvariance );
  fprintf ( 1, '\n' );
%
%  Define some numerical parameters.
%
  dx = 6.0 / n;
  conv = dt / ( 2.0 * dx );

  visc = zeros ( p + 1, 1 );
  visc(1) = numean * dt / ( dx * dx );
  visc(2) = nuvariance * dt / ( dx * dx );
%
%  Define a uniform grid.
%
  x = ( linspace ( - 3.0, + 3.0, n + 1 ) )';
%
%  Set the initial conditions.
%
  u1 = zeros ( n + 1, p + 1 );
  u1(1:n+1,1) = 0.5 + x(1:n+1,1) / 6.0;

  u2 = zeros ( n + 1, p + 1 );
%
%  Time integration.
%
  t1 = ti;
%
%  Write the current solution.
%
  output_filename = 'burgers.history.txt';
  output_unit = fopen ( output_filename, 'wt' );
  fprintf ( output_unit, '----------\n' );
  fprintf ( output_unit, 'T = %g\n', t1 );
  for i = 1 : n + 1
    fprintf ( output_unit, '  %10g', x(i) );
    for k = 1 : p + 1
      fprintf ( output_unit, '  %10g', u1(i,k) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Time integration.
%
  for it = 1 : nt

    t2 = ( ( nt - it ) * ti   ...
         + (      it ) * tf ) ...
         / ( nt      );
%
%  Boundary conditions.
%
    u2(1,1:p+1) = 0.0;
    u2(n+1,1) = 1.0;
    u2(n+1,2:p+1) = 0.0;

    for k = 1 : p + 1

      dp = he_double_product_integral ( k - 1, k - 1 );

      for ix = 2 : n
%
%  Viscous term.
%
        term1 = visc(1) * ( u1(ix+1,k) - 2.0 * u1(ix,k) + u1(ix-1,k) );
        i = 2;
        for j = 1 : p + 1
          tp = he_triple_product_integral ( i - 1, j - 1, k - 1 );
          term1 = term1 + visc(i) * ( u1(ix+1,j) - 2.0 * u1(ix,j) + u1(ix-1,j) ) * tp / dp;
        end
%
%  Convective term.
%
        term2 = - conv * 0.5 * ( u1(ix+1,k) - u1(ix-1,k) );
        for j = 1 : p + 1
          for i = 1 : p + 1
            tp = he_triple_product_integral ( i - 1, j - 1, k - 1 );
            term2 = term2 + ( conv * u1(ix,i) * ( u1(ix+1,j) - u1(ix-1,j) ) * tp ) / dp;
          end
        end

        u2(ix,k) = u1(ix,k) + term1 + term2;

      end

    end

    t1 = t2;
    u1(1:n+1,1:p+1) = u2(1:n+1,1:p+1);
%
%  Print solution every 100 time steps.
%
    if ( mod ( it, 100 ) == 0 )
      fprintf ( output_unit, '----------\n' );
      fprintf ( output_unit, 'T = %g\n', t1 );
      for i = 1 : n + 1
        fprintf ( output_unit, '  %10g', x(i) );
        for k = 1 : p + 1
          fprintf ( output_unit, '  %10g', u1(i,k) );
        end
        fprintf ( output_unit, '\n' );
      end
    end

  end

  fclose ( output_unit );
  fprintf ( 1, '  Time history in "%s".\n', output_filename );
%
%  Compute the mean and variance.
%
  umean(1:n+1,1) = u1(1:n+1,1);

  uvariance = zeros ( n + 1, 1 );

  for i = 1 : n + 1
    for j = 2 : p + 1
      dp = he_double_product_integral ( j - 1, j - 1 );
      uvariance(i) = uvariance(i) + u1(i,j).^2 * dp;
    end
  end
%
%  Save the solution at the final time.
%
  output_filename = 'burgers.moments.txt';
  output_unit = fopen ( output_filename, 'wt' );
  fprintf ( output_unit, ' X E[U] Var[U]\n' );
  for i = 1 : n + 1
    fprintf ( output_unit, '  %18.8g  %18.8g  %18.8g\n', ...
      x(i), umean(i), uvariance(i) );
  end
  fclose ( output_unit );
  fprintf ( 1, '  Moments in "%s".\n', output_filename );

  output_filename = 'burgers.modes.txt';
  output_unit = fopen ( output_filename, 'wt' );
  fprintf ( output_unit, ' X U_0 ... U_P \n' );
  for i = 1 : n + 1
    fprintf ( output_unit, '  %10g', x(i) );
    for k = 1 : p + 1
      fprintf ( output_unit, '  %10g', u1(i,k) );
    end
    fprintf ( output_unit, '\n' );
  end
  fclose ( output_unit );
  fprintf ( 1, '  Final modes in "%s".\n', output_filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PCE_BURGERS:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [X,Y,Z]=cone0(z,t,p)
% [X,Y,Z]=cone0 defines a non-orthogonal
% coordinate system using a conical surface
% with a planar base
X=z.*tan(t).*cos(p); Y=z.*tan(t).*sin(p); Z=z;


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% z-Transform properties

%linearity

syms n z

x1=n^2;
x2=2^n;
a1=3;
a2=4;

Le=a1*x1+a2*x2;
Left=ztrans(Le,z)

X1=ztrans(x1);
X2=ztrans(x2);
Right=a1*X1+a2*X2


function [OBrientest] = OBrientest(X,alpha)
%O'Brien's Test for Homogeneity of Variances.
%[In the Obrien's test the data are transforming to
%yij = ((nj-1.5)*nj*((xij-mean(xj))**2)-((0.5)*(var(xj))*(nj-1)))/((nj-1)*(nj-2))
%and uses the F distribution performing an one-way ANOVA using y as the
%dependent variable (O'Brien, 1979)].
%
%   Syntax: function [OBrientest] = OBrientest(X,alpha)
%
%     Inputs:
%          X - data matrix (Size of matrix must be n-by-2; data=column 1, sample=column 2).
%       alpha - significance level (default = 0.05).
%     Outputs:
%          - Sample variances vector.
%          - Whether or not the homoscedasticity was met.
%
%    Example: From the example 10.1 of Zar (1999, p.180), to test the O'Brien's
%             homoscedasticity of data with a significance level = 0.05.
%
%                                 Diet
%                   ---------------------------------
%                       1       2       3       4
%                   ---------------------------------
%                     60.8    68.7   102.6    87.9
%                     57.0    67.7   102.1    84.2
%                     65.0    74.0   100.2    83.1
%                     58.6    66.3    96.5    85.7
%                     61.7    69.8            90.3
%                   ---------------------------------
%
%           Data matrix must be:
%            X=[60.8 1;57.0 1;65.0 1;58.6 1;61.7 1;68.7 2;67.7 2;74.0 2;66.3 2;69.8 2;
%            102.6 3;102.1 3;100.2 3;96.5 3;87.9 4;84.2 4;83.1 4;85.7 4;90.3 4];
%
%     Calling on Matlab the function:
%             OBrientest(X)
%
%       Answer is:
%
% The number of samples are: 4
%
% ----------------------------
% Sample    Size      Variance
% ----------------------------
%   1        5         9.3920
%   2        5         8.5650
%   3        4         7.6567
%   4        5         8.3880
% ----------------------------
%
% O'Brien's Test for Equality of Variances F=0.0171, df1= 3, df2=15
% Probability associated to the F statistic = 0.9968
% The associated probability for the F test is larger than 0.05
% So, the assumption of homoscedasticity was met.
%

%  Created by A. Trujillo-Ortiz and R. Hernandez-Walls
%             Facultad de Ciencias Marinas
%             Universidad Autonoma de Baja California
%             Apdo. Postal 453
%             Ensenada, Baja California
%             Mexico.
%             atrujo@uabc.mx
%
%  April 18, 2003.
%
%  To cite this file, this would be an appropriate format:
%  Trujillo-Ortiz, A. and R. Hernandez-Walls. (2003). Obrientest: O'Brien's test for
%    homogeneity of variances. A MATLAB file. [WWW document]. URL http://www.mathworks.com/
%    matlabcentral/fileexchange/loadFile.do?objectId=3335&objectType=FILE
%
%  References:
%
%  O'Brien, R. G. (1979), A General ANOVA Method for Robust Tests of
%           Additive Models for Variances. Journal of the American
%           Statistical Association, 74:877-880.
%  Zar, J. H. (1999), Biostatistical Analysis (2nd ed.).
%           NJ: Prentice-Hall, Englewood Cliffs. p. 180.
%

if nargin < 2,
   alpha = 0.05;
end

Y=X;
k=max(Y(:,2));
fprintf('The number of samples are:%2i\n\n', k);

%O'Brien Procedure.
n=[];s2=[];Z=[];
indice=Y(:,2);
for i=1:k
   Ye=find(indice==i);
   eval(['Y' num2str(i) '=Y(Ye,1);']);
   eval(['mY' num2str(i) '=mean(Y(Ye,1));']);
   eval(['n' num2str(i) '=length(Y' num2str(i) ') ;'])
   eval(['s2' num2str(i) '=(std(Y' num2str(i) ').^2) ;'])
   eval(['xn= n' num2str(i) ';'])
   eval(['xs2= s2' num2str(i) ';'])
   eval(['Z' num2str(i) '= ((n' num2str(i) ' - 1.5)*(n' num2str(i) ')*((Y' num2str(i) ' - mY' num2str(i) ').^2)-((0.5)*(s2' num2str(i) ')*(n' num2str(i) ' - 1)))/((n' num2str(i) ' - 1)*(n' num2str(i) ' - 2));']);
   eval(['x= Z' num2str(i) ';']);
   n=[n;xn];s2=[s2;xs2];Z=[Z;x];
end

for i=1:k
   if n(i)==2
      error('Requires sample sizes greater than two. Please, redefine the data matrix.');
   end
end

Y=[Z Y(:,2)];

fprintf('-----------------------------\n');
disp(' Sample    Size      Variance')
fprintf('-----------------------------\n');
for i=1:k
   fprintf('   %d       %2i         %.4f\n',i,n(i),s2(i))
end
fprintf('-----------------------------\n');
disp(' ')

C=(sum(Y(:,1)))^2/length(Y(:,1)); %correction term.
SST=sum(Y(:,1).^2)-C; %total sum of squares.
dfT=length(Y(:,1))-1; %total degrees of freedom.

indice=Y(:,2);
for i=1:k
   Ye=find(indice==i);
   eval(['A' num2str(i) '=Y(Ye,1);']);
end

A=[];
for i=1:k
   eval(['x =((sum(A' num2str(i) ').^2)/length(A' num2str(i) '));']);
   A=[A,x];
end

SSA=sum(A)-C; %sample sum of squares.
dfA=k-1; %sample degrees of freedom.
SSE=SST-SSA; %error sum of squares.
dfE=dfT-dfA; %error degrees of freedom.
MSA=SSA/dfA; %sample mean squares.
MSE=SSE/dfE; %error mean squares.
F=MSA/MSE; %F-statistic.
v1=dfA;df1=v1;
v2=dfE;df2=v2;

P = 1 - fcdf(F,v1,v2);  %probability associated to the F-statistic.

fprintf('O''Brien''s Test for Equality of Variances F=%3.4f, df1=%2i, df2=%2i\n', F,df1,df2);
fprintf('Probability associated to the F statistic = %3.4f\n', P);

if P >= alpha;
  fprintf('The associated probability for the F test is equal or larger than% 3.2f\n', alpha);
  fprintf('So, the assumption of homoscedasticity was met.\n');
else
  fprintf('The associated probability for the F test is smaller than% 3.2f\n', alpha);
  fprintf('So, the assumption of homoscedasticity was not met.\n');
end


function point = circle3dPoint(circle, pos)
%CIRCLE3DPOINT Coordinates of a point on a 3D circle from its position.
%
%   output = circle3dPoint(input)
%
%   Example
%   % Draw some points on a 3D circle
%     figure('color','w'); hold on; view(130,-10);
%     circle = [10 20 30 50 90 45 0];
%     drawCircle3d(circle)
%     % origin point
%     pos1 = 0;
%     drawPoint3d(circle3dPoint(circle, pos1), 'ro')
%     % few points regularly spaced
%     drawPoint3d(circle3dPoint(circle, 10:10:40), '+')
%     % Draw point opposite to origin
%     drawPoint3d(circle3dPoint(circle, 180), 'k*')
%
%
%   See also
%   circles3d, circle3dPosition
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-06-21,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

pos=pos(:);

% extract circle coordinates
xc  = circle(1);
yc  = circle(2);
zc  = circle(3);
r   = circle(4);

theta   = circle(5);
phi     = circle(6);
psi     = circle(7);

% convert position to angle
t = pos * pi / 180;

% compute position on base circle
x   = r * cos(t);
y   = r * sin(t);
z   = zeros(length(pos),1);
pt0 = [x y z];

% compute transformation from local basis to world basis
trans   = localToGlobal3d(xc, yc, zc, theta, phi, psi);

% compute points of transformed circle
point   = transformPoint3d(pt0, trans);


function [val,idx] = of_PCMARE(obs,sim,idx)
% of_MARE Calculates a version of the Mean Absolute Relative Error (MARE)
% of simulated streamflow as a percentage of the MARE of the mean
% observed flow. Ignores time steps with negative flow values.  Adds a
% constant e of 1/100 of mean(obs) to avoid issues with zero flows
% (Pushpalatha et al., 2012).

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% In:
% obs       - time series of observations       [nx1]
% sim       - time series of simulations        [nx1]
% idx       - optional vector of indices to use for calculation, can be
%               logical vector [nx1] or numeric vector [mx1], with m <= n
%
% Out:
% val       - objective function value          [1x1]
% idx       - vector of indices used for calculation

% Pushpalatha, R.; Perrin, C.; le Moine, N. and Andréassian V. (2012). "A
% review of efficiency criteria suitable for evaluating low-flow
% simulations". Journal of Hydrology. 420-421, 171-182.
% doi:10.1016/j.jhydrol.2011.11.055

%% Check inputs and select timesteps
if nargin < 2
    error('Not enugh input arguments')
end

if nargin < 3; idx = []; end
[sim, obs, idx] = check_and_select(sim, obs, idx);

%% Find the constant e
m = mean(obs);
e = m/100;

%% Apply constant and transform flows
obs = obs+e;
sim = sim+e;

%% Calculate metric
MARE = mean(abs((sim-obs)/obs));
MARE_mean = mean(abs((sim-m)/m));

val = MARE/MARE_mean;
end


function c = p_polynomial_coefficients ( n )

%*****************************************************************************80
%
%% P_POLYNOMIAL_COEFFICIENTS: coefficients of Legendre polynomials P(n,x).
%
%  First terms:
%
%     1
%     0     1
%    -1/2   0      3/2
%     0    -3/2    0     5/2
%     3/8   0    -30/8   0     35/8
%     0    15/8    0   -70/8    0     63/8
%    -5/16  0    105/16  0   -315/16   0    231/16
%     0   -35/16   0   315/16   0   -693/16   0    429/16
%
%     1.00000
%     0.00000  1.00000
%    -0.50000  0.00000  1.50000
%     0.00000 -1.50000  0.00000  2.5000
%     0.37500  0.00000 -3.75000  0.00000  4.37500
%     0.00000  1.87500  0.00000 -8.75000  0.00000  7.87500
%    -0.31250  0.00000  6.56250  0.00000 -19.6875  0.00000  14.4375
%     0.00000 -2.1875   0.00000  19.6875  0.00000 -43.3215  0.00000  26.8125
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996.
%
%  Parameters:
%
%    Input, integer N, the highest order polynomial to evaluate.
%    Note that polynomials 0 through N will be evaluated.
%
%    Output, real C(1:N+1,1:N+1), the coefficients of the Legendre polynomials
%    of degree 0 through N.  Each polynomial is stored as a row.
%
  if ( n < 0 )
    c = [];
    return
  end

  c(1:n+1,1:n+1) = 0.0;

  c(1,1) = 1.0;

  if ( n <= 0 )
    return
  end

  c(2,2) = 1.0;

  for i = 2 : n
    c(i+1,1:i-1) =                (   - i + 1 ) * c(i-1,1:i-1) / ( i );
    c(i+1,2:i+1) = c(i+1,2:i+1) + ( i + i - 1 ) * c(i  ,1:i  ) / ( i );
  end

  return
end


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)

m = size(X, 1); % Number of training examples

% You need to return the following variables correctly
p = zeros(m, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%

prob = sigmoid(X * theta);
pos = find(prob >= 0.5);
p(pos,1) = 1;

% =========================================================================

end


function [ x, w ] = rule_adjust ( a, b, c, d, n, x, w )

%*****************************************************************************80
%
%% RULE_ADJUST maps a quadrature rule from [A,B] to [C,D].
%
%  Discussion:
%
%    Most quadrature rules are defined on a special interval, like
%    [-1,1] or [0,1].  To integrate over an interval, the abscissas
%    and weights must be adjusted.  This can be done on the fly,
%    or by calling this routine.
%
%    If the weight function W(X) is not 1, then the W vector will
%    require further adjustment by the user.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the endpoints of the definition interval.
%
%    Input, real C, D, the endpoints of the integration interval.
%
%    Input, integer N, the number of abscissas and weights.
%
%    Input, real X(N), the abscissas.
%
%    Input, real W(N), the weights.
%
%    Output, real X(N), the adjusted abscissas.
%
%    Output, real W(N), the adjusted weights.
%
  x(1:n) = ( ( b - x(1:n)     ) * c   ...
           + (     x(1:n) - a ) * d ) ...
           / ( b          - a );

  w(1:n) = ( ( d - c ) / ( b - a ) ) * w(1:n);

  return
end


% KM_DEMO_KPCA_U Kernel principal component analysis (KPCA) on a U-shaped
% two-dimensional data set.
%
% This program implements the example shown in Figure 2.4 of "Kernel
% Methods for Nonlinear Identification, Equalization and Separation of
% Signals", Ph.D. dissertation by S. Van Vaerenbergh.
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2010.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

close all
clear

%% PARAMETERS

N_split = [30, 30, 60];		% number of data points in 2 straight parts and curve of "U"
corners = [3 4 2 4];	% corners: x1 x2 y1 y2
nvar = 0.05;	% noise variance

kernel.type = 'gauss';
kernel.par =  .5;
numeig = 4;	% number of eigenvalues to plot

Ntest = [50,50];	% number of test grid divisions, in each dimension
border = [0 6 0 6];	% test grid border

%% PROGRAM
tic

%% generate data
N = sum(N_split);
N1 = N_split(1); N2 = N_split(2); N3 = N_split(3);

X1 = [linspace(corners(1),corners(2),N1)' repmat(corners(3),N1,1)];	% lower straight part
X2 = [linspace(corners(1),corners(2),N2)' repmat(corners(4),N2,1)];	% upper straight part

angles = linspace(pi/2,3*pi/2,N3)';
rad = (corners(4)-corners(3))/2;
m3 = [corners(1) corners(3)+rad];
X3 = repmat(m3,N3,1) + rad*[cos(angles) sin(angles)];

n = nvar*randn(N,2);
X = [X1; X2; X3] + n;

%% generate test grid data
Nt1 = Ntest(1); Nt2 = Ntest(2);
Xtest = zeros(Nt1*Nt2,2);
absc = linspace(border(1),border(2),Nt1);
ordi = linspace(border(3),border(4),Nt2);
for i=1:Nt1,
	for j=1:Nt2,
		Xtest((i-1)*Nt2+j,:) = [absc(i) ordi(j)];
	end
end

%% calculate kernel principal components and projections of Xtest
[E,v] = km_kpca(X,numeig,kernel.type,kernel.par);

Kt = km_kernel(X,Xtest,kernel.type,kernel.par);	% kernels of test data set
Xtestp = E'*Kt;	 % projections of test data set on the principal directions

Y = cell(numeig,1);
for i=1:numeig,
 	Y{i} = reshape(Xtestp(i,:),Nt2,Nt1);	% shape into 2D grid
end

toc
%% OUTPUT

figure;
plot(X(:,1),X(:,2),'.')
axis(border)

% fireworks!
for i=1:numeig,
	figure; hold on
	[C,h] = contourf(-interp2(Y{i},2),25);
	plot(X(:,1)/border(2)*(Nt1*4-1)+1,X(:,2)/border(4)*(Nt2*4-1)+1,'o',...
		'MarkerFaceColor','White','MarkerEdgeColor','Black')
	set(gca, 'XTick', [])
	set(gca, 'YTick', [])
end


function [ x, error_norm, iter, flag ]  = sor ( A, x, b, w, max_it, tol )

%*****************************************************************************80
%
%% SOR solves the linear system Ax=b using the Successive Over-Relaxation Method.
%
%  Discussion:
%
%    When the parameter W
%    is set to 1, this is equivalent to the Gauss-Seidel iteration.
%
%  Reference:
%
%    Richard Barrett, Michael Berry, Tony Chan, James Demmel,
%      June Donato, Jack Dongarra, Victor Eijkhout, Roidan Pozo,
%      Charles Romine, Henk van der Vorst
%    Templates for the Solution of Linear Systems: Building Blocks for
%      Iterative Methods,
%    SIAM Publications, 1993.
%
%  Parameters:
%
%    Input, real A(N,N), the symmetric positive definite matrix.
%
%    Input, real X(N), the initial guess vector.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, real W, the relaxation scalar, between 0 and 2.
%
%    Input, integer MAX_IT, the maximum number of iterations.
%
%    Input, real TOL, an error tolerance.
%
%    Output, real X(N), the solution.
%
%    Output, real ERROR_NORM, the norm of the error.
%
%    Output, integer ITER, the number of iterations performed.
%
%    Output, integer FLAG, the return flag.
%    0 = the solution was found to within the specified tolerance.
%    1 = a satisfactory solution was not found.  The iteration limit
%        was exceeded.
%

%
%  Initialization.
%
  flag = 1;
  iter = 0;

  bnrm2 = norm ( b );
  if ( bnrm2 == 0.0 )
    bnrm2 = 1.0
  end

  r = b - A * x;
  error_norm = norm ( r ) / bnrm2;
  errorhist = [ ];
  errorhist(1) = error_norm;

  if ( error_norm < tol )
    flag = 0;
    return
  end
%
%  Split the matrix.
%
  [ M, N, b ] = split ( A, b, w, 2 );

  for iter = 1 : max_it

    x_1 = x;
%
%  Update the approximation.
%
    x = M \ ( N * x + b );
%
%  Compute the error.
%
    error_norm = norm ( x - x_1 ) / norm ( x );
    errorhist(iter+1) = error_norm;
%
%  Check for convergence.
%
    if ( error_norm <= tol )
      flag = 0;
      break
    end

  end
%
%  Restore the right hand side.
%
  b = b / w;

  error_norm = errorhist;

  return
end


%%%% discretization of direct space, to be used for plotting the field
%%%% distribution; X,Y = vectors to be used for FFT; Xi,Yi = vectors to be
%%%% used for interpolation
function [X,Y,Xi,Yi]=prcellgrid(a1,a2,N1,N2)
X=[]; Y=[]; %spatial coordinates after FFT
for l=1:N1
    for m=1:N2
        X(l,m)=(l-1-(N1-1)/2)*a1(1)/N1 + (m-1-(N2-1)/2)*a2(1)/N2;
        Y(l,m)=(l-1-(N1-1)/2)*a1(2)/N1 + (m-1-(N2-1)/2)*a2(2)/N2;
    end
end

M=501;
Xi=[]; Yi=[]; %finer discretization for field interpolation
for l=1:M
    for m=1:M
        Xi(l,m)=(l-1-(M-1)/2)*a1(1)/M + (m-1-(M-1)/2)*a2(1)/M;
        Yi(l,m)=(l-1-(M-1)/2)*a1(2)/M + (m-1-(M-1)/2)*a2(2)/M;
    end
end


function [area,centroids] = calcVoronoiArea(v,varargin)
% compute the spherical area of the Voronoi decomposition
%
% Input
%  v - @vector3d
%
% Output
%  area - area of the corresponding Voronoi cells
%  centroids - centroid of the voronoi cell
%
% Options
% incomplete -

v = reshape(v,[],1);
N = length(v);

% in case of antipodal symmetry - add antipodal points
antipodal = v.antipodal || check_option(varargin, 'antipodal');
if antipodal
  v.antipodal = false;
  [v,~,IC] = unique([v;-v],'noSymmetry');
end

[V,C] = calcVoronoi(v);

nd = ~cellfun('isempty',C);
v = v.subSet(nd);

last = cumsum(cellfun('prodofsize',C(nd)));

left = [C{nd}];
shift = 2:last(end)+1;             % that is the shift
shift(last) = [0;last(1:end-1)]+1; % and the last gets the first
right = left(shift);

center = cumsum([1 diff(shift)>1]);

va = v.subSet(center);
vb = V.subSet(left);
vc = V.subSet(right);    % next vertex around

% calculate the area for each triangle around generator (va)
A = real(sphericalTriangleArea(va,vb,vc));

if nargout>1
  [x,y,z]= double(A.*(va+vb+vc));
  x = full(sparse(center,1,x,length(S2G),1));
  y = full(sparse(center,1,y,length(S2G),1));
  z = full(sparse(center,1,z,length(S2G),1));
  centroids = vector3d(x,y,z);
end

% accumulate areas of spherical triangles around generator
A = full(sparse(center,1,A,length(v),1));

area = zeros(size(nd));
area(nd) = A(1:nnz(nd));

if antipodal
  idx = ( accumarray(IC,ones(size(IC))) == 2 ); % find all double occurences
  area(idx) = area(idx)/2; % halve their weight
  area = area(IC); % go back to original order
  area = area(1:N); % only the original nodes
end


% Peak of Wigner-Ville Frequency Estimation
%
% Estimates the instantaneous frequency of the input signal by
% extracting the peaks of the Wigner-Ville distribution.
%
%
% Usage:
%
%     ife = wvpe( signal, lag_window_length, time_res [, fft_length] )
%
% Parameters:
%
%    signal
%
%	  Input one dimensional signal to be analysed. An analytic signal
%	  is required for this function, however, if signal is real, a
%	  default analytic transformer routine will be called from this
%	  function before computing tfd.
%
%    lag_window_length
%
%	  This is the data window length and controls the size of
%	  the kernel used for analysis (lag_window_length must be
%	  odd). The kernel used will be defined from -(lag_window_length+1)/2
%	  to +(lag_window_length+1)/2 in both time and lag dimensions.
%
%    time_res
%
%	  The number of time samples to skip between successive slices.
%	  Default vaule is 1.
%
%    fft_length
%
%	  Zero-padding at the FFT stage of the analysis may be specified by
%	  giving an fft_length larger than lag_window_length. If fft_length
%	  is not specified, or is smaller than the lag_window_length, then the
%	  next highest power of two above lag_window_length is used. If
%	  fft_length is not a power of two, the next highest power of two is
%	  used.
%
%     tfd
%
%	  The computed time-frequency distribution. size(tfd) will
%	  return [a, b], where a is the next largest power of two above
%	  fft_length, and b is floor(length(signal)/time_res) - 1.
%
%
%
%  See Also: wvd
% TFSAP 7.0
% Copyright Prof. B. Boashash
% Qatar University, Doha
% email: tfsap.research@gmail.com


function dz = doublePendulumDynamics(~,z,u,P)
%DZ = DOUBLEPENDULUMDYNAMICS(T,Z,U,P)
%
%FUNCTION:  This function computes the dynamics of a double
%    pendulum, and is designed to be called from ode45. The
%    model allows for arbitrary mass and inertia for each
%    link, but no friction or actuation
%
%INPUTS:
%    t = time. Dummy input for ode45. Not used.
%    z = [4xn] matrix of states.
%    u = [2xn] matrix of inputs
%    P = struct of parameters
%OUTPUTS:
%    dz = [4xn] matrix of state derivatives
%
%NOTES:
%    This file was automatically generated by writeDoublePendulumDynamics

m1 = P.m1; %link one mass
m2 = P.m2; %link two mass
g  = P.g ; %gravity
l1 = P.l1; %link one length
l2 = P.l2; %link two length
I1 = P.I1; %link one moment of inertia about its center of mass
I2 = P.I2; %link two moment of inertia about its center of mass
d1 = P.d1; %distance between link one center of mass and parent joint
d2 = P.d2; %distance between link two center of mass and parent joint

th1 = z(1,:); %link one absolute angle
dth1 = z(2,:); %link one angular rate
th2 = z(3,:); %link two absolute angle
dth2 = z(4,:); %link two angular rate

u1 = u(1,:); %torque acting on link 1 wrt ground
u2 = u(2,:); %torque acting on link 2 wrt ground

f1 = u1 - m2.*(d2.*dth2.^2.*cos(th2) + dth1.^2.*l1.*cos(th1)).*(d2.*sin(th2) + l1.*sin(th1)) + m2.*(d2.*dth2.^2.*sin(th2) + dth1.^2.*l1.*sin(th1)).*(d2.*cos(th2) + l1.*cos(th1)) - g.*m2.*(d2.*cos(th2) + l1.*cos(th1)) - d1.*g.*m1.*cos(th1);
f2 = u2 - d2.*g.*m2.*cos(th2) + d2.*dth1.^2.*l1.*m2.*sin(th1 - th2);

M11 = - I1 - d1.^2.*m1.*cos(th1).^2 - d1.^2.*m1.*sin(th1).^2 - l1.*m2.*cos(th1).*(d2.*cos(th2) + l1.*cos(th1)) - l1.*m2.*sin(th1).*(d2.*sin(th2) + l1.*sin(th1));
M12 = - I2 - d2.*m2.*cos(th2).*(d2.*cos(th2) + l1.*cos(th1)) - d2.*m2.*sin(th2).*(d2.*sin(th2) + l1.*sin(th1));
M21 = -d2.*l1.*m2.*cos(th1 - th2);
M22 = - I2 - d2.^2.*m2;

D = M11.*M22 - M12.*M21;

ddth1 = (f2.*M12 - f1.*M22)./D;
ddth2 = -(f2.*M11 - f1.*M21)./D;

dz = [...
    dth1; %derivative of link one absolute angle
    ddth1; %derivative of link one angular rate
    dth2; %derivative of link two absolute angle
    ddth2; %derivative of link two angular rate
];
end


function [dy,dh] = checkgrad(X_struct, f, e, varargin)

% checkgrad checks the derivatives in a function, by comparing them to finite
% differences approximations. The partial derivatives and the approximation
% are printed and the norm of the diffrence divided by the norm of the sum is
% returned as an indication of accuracy.
%
% usage: checkgrad('f', X, e, P1, P2, ...)
%
% where X is the argument and e is the small perturbation used for the finite
% differences. and the P1, P2, ... are optional additional parameters which
% get passed to f. The function f should be of the type
%
% [fX, dfX] = f(X, P1, P2, ...)
%
% where fX is the function value and dfX is a vector of partial derivatives.
%
% d - norm error
% dy - analytic
% dh - numerical
%
% Carl Edward Rasmussen, 2001-08-01.

[X_vec, X_template] = struct2vector(X_struct);

% [y, dy] = feval(f, vector2struct(X_vec + val*grad, X_template), varargin{:});
tic
[y, dy_struct] = feval(f, X_struct, varargin{:});
y = sum(y);
dy = struct2vector(dy_struct);
time1 = toc;

% keyboard

fig = figure;
dh = nan(length(X_vec),1) ;
t = cputime;
n_char = 0;
time_step = 5;
DIRECTION = 'forwards';
% DIRECTION = 'backwards';
% if length(X_vec) > 50000
%
% %   SKIP = 10;
% %   START = 1;%14400;%1;
%
%   SKIP = 2000;
%   START = 1;%14400;%1;
%
% %   SKIP = 1000;
% %   START = 30000;%14400;%1;
% else
%   SKIP = 1;%100;
%   START = 1;%4300;%1;
% end

START = 1;
n = max(40, 30/time1) % Evalulate for ~15 seconds
SKIP = ceil(length(X_vec)/n);

% START = 20210;
% SKIP = 1;

if strcmp(DIRECTION, 'backwards');
  idx = length(X_vec):-SKIP:1;
elseif strcmp(DIRECTION, 'forwards');
  idx = START:SKIP:length(X_vec);
end

% idx = length(X_vec):-1:(length(X_vec)-8);
% DIRECTION = 'backwards';
% SKIP = 1;

for j = idx

  dx = zeros(length(X_vec),1);
  dx(j) = dx(j) + e;                               % perturb a single dimension

  [y2] = feval(f, vector2struct(X_vec + dx, X_template), varargin{:});
  [y1] = feval(f, vector2struct(X_vec - dx, X_template), varargin{:});

  y1 = sum(y1(:));
  y2 = sum(y2(:));

  dh(j) = (y2 - y1)/(2*e);

  if ((cputime - t) > time_step) || (j == idx(end))
    t = cputime;
    figure(fig)
    clf
    subplot(2,1,1);
    if strcmp(DIRECTION, 'backwards');
      plot(j:SKIP:length(dy), dy(j:SKIP:end), 'bx-'); hold on;
      plot(j:SKIP:length(dy), dh(j:SKIP:end), 'r-');
    elseif strcmp(DIRECTION, 'forwards');
      plot(START:SKIP:j, dy(START:SKIP:j), 'bx-'); hold on;
      plot(START:SKIP:j, dh(START:SKIP:j), 'r-');
    end
    legend('analytical', 'numerical');
    ylabel('gradient');
    subplot(2,1,2);
    if strcmp(DIRECTION, 'backwards');
      plot(j:SKIP:length(dy), dy(j:SKIP:end) - dh(j:SKIP:end), 'gx-');
    elseif strcmp(DIRECTION, 'forwards');
      plot(START:SKIP:j, dy(START:SKIP:j) - dh(START:SKIP:j), 'gx-');
    end
    ylabel('error')
    drawnow

    s = [num2str(100*j/length(X_vec), '%0.1f'), '%, '];
    if n_char + length(s) >= 80
      fprintf('\n')
      n_char = 0;
    end
    fprintf('%s', s);
    n_char = n_char + length(s);

  end
end
fprintf('\n');

% 'numerical:'
% dh(idx)'
%
% 'analytical:'
% dy(idx)'

'delta:'
err = dy(idx)' - dh(idx)';

'mult:'
mult = dy(idx)' ./ dh(idx)';

for p = [1, 5, 20, 50, 90, 95, 99, 99.9, 99.99]
  fprintf('%g percentile err: \t%e\n', p, prctile(abs(err), p));
end

% % disp([dy dh])                                          % print the two vectors
% d = norm(dh-dy)/norm(dh+dy);       % return norm of diff divided by norm of sum


function phasediag
% Phase diagram visualisation
%    using MATLAB expm
%
%   $Ekkehard Holzbecher  $Date: 2006/04/15 $
%--------------------------------------------------------------------------
T = 10;                % maximum time
C = [-1 1; 1 -3];      % matrix
f = [1; 0];            % input vector
cc = 1;                % initial concentrations (absolute value of the vector)
N = 60;                % discretization of time
M = 16;                % no. of trajectories

%----------------------execution & output----------------------------------
equilibrium = -(inv(C)*f);
t = linspace (0,T,N);
for angle = linspace (0,pi+pi,M)
    c0 = equilibrium + cc*[sin(angle); cos(angle)]; c = c0;
    for i = 2:N
        E = expm(C*t(i));
        c = [c E*c0-(eye(size(C,1))-E)*inv(C)*f];
    end
    plot (c(1,:)',c(2,:)'); hold on;
end

plot (equilibrium(1),equilibrium(2),'s');
xlabel ('variable 1'); ylabel ('variable 2')
title ('phase diagram')


function nsub = subset_enum ( n )

%*****************************************************************************80
%
%% SUBSET_ENUM enumerates the subsets of a set with N elements.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of elements in the set.
%    N must be at least 0.
%
%    Output, integer NSUB, the number of distinct elements.
%
  nsub = 2^n;

  return
end


function perf=mse(e)
%
% calculate the mean squared error of the given errors
%
%  'perf = mse(E);'
%
% see also:
%    mae, linf, trimmedmse
%

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

perf = sum(sum(e.^2)) / numel(e);


function s = fastSampen(y,m,r)
% INPUTS
%  y     num_var x num_samples
%  m     template length
%  r     radius
% OUTPUTS
%  s     Sample Entropy
%
% Written by Shamim Nemati <shamim.nemati@alum.mit.edu>
%
% 01-31-2018 : modified by Giulia Da Poian <giulia.dap@gmail.com>, see
%              comments in the code
%              NOTE : Sample entropy quantifies the likelihood that a
%              sequence of m consecutive data points that matches another
%              sequence of the same length (match within a tolerance of r)
%              will still match the other sequence when their length is
%              increased of one sample (sequences of length m + 1);
%              References :
%              1. Richman JS, Moorman JR. Physiological time-series
%              analysis using approximate entropy and sample entropy.
%              American Journal of Physiology-Heart and Circulatory
%              Physiology. 2000 Jun 1;278(6):H2039-49.
%              2. Humeau-Heurtier A. The multiscale entropy
%              algorithm and its variants: A review. Entropy. 2015 May
%              12;17(5):3110-23.
% LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information

% Ensure y is a row vector
if size(y, 1) > size(y,2)
    y = y';
end

xx = convert_to_lagged_form(y, m)';   % Giulia: replaced m-1 with m to match SampEn definition
Dxx = pdist(xx,'chebychev');

yy = convert_to_lagged_form(y, m+1)'; % Giulia: replaced m with m+1 to match SampEn definition
Dyy = pdist(yy,'chebychev');

A = mean( Dxx < r ) ;
B = mean( Dyy < r );

s = -log(B/A);

function yy = convert_to_lagged_form(y, k)
% Create an observation vector yy(:,t) containing the last k values of y, newest first
% e.g., k=2, y = (a1 a2 a3)     yy  = a2 a3
%                (b1 b2 b3)           b2 b2
%                                     a1 a2
%                                     b1 b2
[s, T] = size(y);
bs = s*ones(1,k);
yy = zeros(k*s, T-k+1);
for i=1:k,     yy(block(i,bs), :) = y(:, k-i+1:end-i+1); end

function sub = block(blocks, block_sizes)
% BLOCK Return a vector of subscripts corresponding to the specified blocks.
% sub = block(blocks, block_sizes)
%
% e.g., block([2 5], [2 1 2 1 2]) = [3 7 8].
blocks = blocks(:)';
block_sizes = block_sizes(:)';
skip = [0 cumsum(block_sizes)];
start = skip(blocks)+1;
fin = start + block_sizes(blocks) - 1;
sub = [];
for j=1:length(blocks)
    sub = [sub start(j):fin(j)];
end


function value = i4_gcdb ( i, j, k )

%*****************************************************************************80
%
%% I4_GCDB finds the greatest common divisor of the form K**N of two numbers.
%
%  Discussion:
%
%    Note that if J is negative, I4_GCDB will also be negative.
%    This is because it is likely that the caller is forming
%    the fraction I/J, and so any minus sign should be
%    factored out of J.
%
%    If I and J are both zero, I4_GCDB is returned as 1.
%
%    If I is zero and J is not, I4_GCDB is returned as J,
%    and vice versa.
%
%    If I and J are nonzero, and have no common divisor of the
%    form K**N, I4_GCDB is returned as 1.
%
%    Otherwise, I4_GCDB is returned as the largest common divisor
%    of the form K**N shared by I and J.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, two numbers whose greatest common divisor K**N
%    is desired.
%
%    Input, integer K, the possible divisor of I and J.
%
%    Output, integer VALUE, the greatest common divisor of
%    the form K^N shared by I and J.
%
  value = 1;
%
%  If both I and J are zero, I4_GCDB is 1.
%
  if ( i == 0 && j == 0 )
    value = 1;
    return
  end
%
%  If just one of I and J is zero, I4_GCDB is the other one.
%
  if ( i == 0 )
    value = j;
    return
  elseif ( j == 0 )
    value = i;
    return
  end
%
%  Divide out K as long as you can.
%
  if ( 0 < j )
    value = 1;
  else
    value = -1;
  end

  while

    if ( mod ( i, k ) ~= 0 || mod ( j, k ) ~= 0 )
      break
    end

    value = value * k;
    i = i / k;
    j = j / k;

  end

  return
end


function result = is_symmetric_matrix(A)
%
% Symmetric Matrices
%
% is_symmetric_matrix(A) determines if the matrix A is a symmetric
% matrix. An error is returned if a matrix that is not square is attempted
% to be determined for symmetry.

matrix_size = size(A);

m = matrix_size(1,1);
n = matrix_size(1,2);

if m ~= n
    error('Only square matrices can be symmetric.');
else
    if A == A'
        result = 1;
    else
        result = 0;
    end
end


function [ vX ] = ProjectOntoHalfSpace( vY, vA, valB )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = OrthogonalProjectionOntoConvexSets( cProjFun, vY, numIterations, stopThr )
%   Solves \arg \min_{x} 0.5 || x - y ||, s.t. x \in \bigcap {C}_{i} using
%   Dykstra's Projection Algorithm.
% Input:
%   - mA            -   Model Matrix.
%                       Input model matrix.
%                       Structure: Matrix (m x n).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - vX            -   Solution Vector.
%                       The solution to the optimization problem..
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Orthogonal Projection onto a Half Space - https://math.stackexchange.com/questions/318740.
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     19/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

valR = (vA.' * vY) - valB;

if(valR > 0)
    vX = vY - ((valR / (vA.' * vA)) * vA);
else
    vX = vY;
end

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%Fourier Transfrom properties

%	convolution

%x1(t)=u(t)-u(t-2)   and x2(t)=u(t)-u(t-4)

% F^-1{X1(w)X2(w)}
syms t w
x1=heaviside(t)-heaviside(t-2);
x2=heaviside(t)-heaviside(t-4);
X1=fourier(x1,w);
X2=fourier(x2,w);
right =ifourier(X1*X2,t);
ezplot(right,[0 8]);

% convolution of x1(t) with x2(t)
figure
t1=0:.01:2;
t2=2.01:.01:4;
x1=[ones(size(t1)) zeros(size(t2))];
x2=ones(size([t1 t2]));
y=conv(x1,x2)*.01;
plot(0:.01:8,y);


function f = p01_f ( x )

%*****************************************************************************80
%
%% P01_F evaluates the objective function for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = ( x - 2.0 ) * ( x - 2.0 ) + 1.0;

  return
end


% MultiQuadrics scattered data INTERPOLATION.
%
% SYNTAX: [fi, coefficients] = mq_interpolation( r, f, ri, c )
%
%          where r and f are the scattered argument and function
%          values, fi is the data interpolated at ri, c is the
%          multiquadric parameter and coefficients is a vector
%          containing the coefficients of the MQ expansion.
%
%         Examples:
%         N =  51; c = 0.2;
%         x = 2*pi*sort( rand([1 N]) ); f = sin(x);
%         xi = linspace(0,2*pi,N);
%        [fi,coefficients] = mq_interpolation(x(:),f(:),xi(:),c);
%         figure(1), plot(x,f,xi,fi,'o')
%
%	  N = 51; c = 0;
%	  x = 2*rand([1 N]) - 1; y = 2*rand([1 N]) - 1; r = [x(:) y(:)];
%	  for i = 1:N, f(i) = exp( -x(i)^2 -y(i)^2 ); end
%	  xi = linspace(-1.1,1.1,N); yi = linspace(-1.1,1.1,N);
%	 [Xi,Yi] = meshgrid(xi,yi);
%	  for i = 1:N*N, ri(i,:) = [Xi(i) Yi(i)]; end
%	 [fi,coefficients] = mq_interpolation(r,f(:),ri,c);
%	  S = reshape(fi,N,N);
%	  figure(2)
%	  mesh(xi,yi,S), hold on, plot3(x,y,f,'o'), hold off

% Written by Orlando Camargo Rodriguez

function [fi,coefficients] = mq_interpolation(r,f,ri,c)

fi           = [];
coefficients = [];

N = length( r(:,1) );

for i = 1:N

    rj_minus_ri = ( r(i,:)'*ones([1 N]) )' - r;

    Phi(:,i) = sqrt( sum( rj_minus_ri.^2 , 2 ) + c*c );

end

coefficients = Phi\f(:); % Calculate the coefficients through Gaussian elimination

Ni = length( ri(:,1) );

for i = 1:Ni

    ri_minus_r = ( ri(i,:)'*ones([1 N]) )' - r;

    phi_at_ri = sqrt( sum( ri_minus_r.^2 , 2 ) + c*c );

    fi(i) = coefficients'*phi_at_ri(:);

end


 function y = mod0(x,b)
%function y = mod0(x,b)
%	the usual mod function returns values in the range [0,b)
%	whereas this mod function returns values in the range [-b/2,b/2)
%	which frequently arises in signal processing problems!
%	Jeff Fessler

y = mod(x + b/2, b) - b/2;


function par_est2a
% parameter estimation with derivatives                      Holzbecher September 2005
% using differential equations
% Idea from FEMLAB - there R instead of Q
% see COMSOL News, Nr. 1, 2005, page 15
global xfit cfit Q

% specify fitting data
xfit = [0.05:0.1:0.95];
cfit = [0.9256859756097451       0.7884908536585051       0.6665396341462926...
        0.559832317073104        0.4683689024389414       0.39214939024380824...
        0.33117378048770196      0.28544207317062964      0.25495426829258294      0.23971036585356142];
Q = -2;
D = fzero(@myfun,1.8);
display (['Best fit for D = ' num2str(D)]);
x = [0:0.01:1];
plot (xfit,cfit,'o',x,-(Q/D/2)*x.*x + (Q/D)*x + 1,'-');
legend ('given','modelled');
xlabel ('x'); ylabel ('c');

function f = myfun(D);
global xfit cfit Q

options = bvpset;
% solve diffusion equation for c with c(0)=1 and dc/dx(1)=0
solinit = bvpinit([0 xfit 1],@guess);
c = bvp4c (@mat4ode,@mat4bc,solinit,options,Q/D,1);
%plot (c.x,c.y(1,:),'r',xfit,-(Q/D/D)*xfit.*xfit+(Q/D)*xfit+ones(1,size(xfit,2)));

% solve Poisson equation for dc/dD (cD) with boundary conditions
solinit = bvpinit([0 xfit 1],@guess1);
cD = bvp4c (@mat4ode,@mat4bc,solinit,options,Q/D/D,0);

% specify function f to vanish
f = 2*(c.y(1,2:size(c.y,2)-1)-cfit)*cD.y(1,2:size(c.y,2)-1)';

function dydx = mat4ode(x,y,Q,c0)
dydx = [y(2); -Q];
% ------------------------------------------------------------
function res = mat4bc(y0,y1,Q,c0)
res = [y0(1)-c0; y1(2)];
% ------------------------------------------------------------
function v = guess(x)
v = [x*(x-2)+1; 2*(x-1)];
% ------------------------------------------------------------
function v = guess1(x)
v = [x*(x-2); 2*(x-1)];


  function [fwhm_best, costs, im_best] = ...
	fwhm_match(true_image, blurred_image, fwhms)
%|function [fwhm_best, costs, im_best] = ...
%|	fwhm_match(true_image, blurred_image, fwhms)
%|
%| given a blurred_image of a true_image, find the FHWM of a Gaussian kernel
%| that, when convolved to the true_image, yields the smoothed image
%| that best matches blurred_image.
%|
%| the set of FWHM values given in the array fwhms is tried.
%|
%| Copyright 2001-8-30, Jeff Fessler, University of Michigan

if nargin == 1 && streq(true_image, 'test'), fwhm_match_test, return, end
if nargin < 2, ir_usage, end

if nargin < 3
	fwhms = 0:0.5:4;
end

costs = zeros(size(fwhms));
cost_min = Inf;
for ii=1:length(fwhms)
	fwhm = fwhms(ii);
	kern = gaussian_kernel(fwhm);
	psf = kern * kern';
	tmp = conv2(true_image, psf, 'same');
	costs(ii) = norm(tmp(:) - blurred_image(:)) / norm(true_image(:));
	if costs(ii) < cost_min
		im_best = tmp;
	end
end

[dummy ibest] = min(costs);
if ibest == 1 || ibest == length(fwhms)
	warning 'need wider range of fwhms'
end
fwhm_best = fwhms(ibest);

% fwhm_match_test
function fwhm_match_test

% pyramidal PSF to stress the approach
psf1 = [0:5 4:-1:0]; psf1 = psf1 / sum(psf1); psf = psf1' * psf1;
true_image = zeros(128); true_image(64:96,64:96) = 1;
blurred_image = conv2(true_image, psf, 'same');
im plc 2 2
im(1, true_image, 'True Image')
im(2, blurred_image, 'Blurred Image')

fwhms = [2:0.25:8];
[fwhm_best, costs] = fwhm_match(true_image, blurred_image, fwhms);
np = length(psf);	ip = -(np-1)/2:(np-1)/2;
kern = gaussian_kernel(fwhm_best);
nk = length(kern);	ik = -(nk-1)/2:(nk-1)/2;

if im
	im subplot 3
	plot(fwhms, costs, 'c-o', fwhm_best, min(costs), 'yx')
	xlabel FWHM, ylabel Cost, title 'Cost vs FWHM'
	im subplot 4
	plot(ip, psf1, '-o', ik, kern(:), '-+')
	xlabel pixel, title 'PSF profile: actual and Gaussian fit'
end


function ex3bvp
%EX3BVP  Example 3 of the BVP tutorial.
%   This is the example for D02KAF from the NAG library.  D02KAF is a code
%   for Sturm-Liouville problems that takes advantage of special features of
%   the problem.  The task is to find the fourth  eigenvalue of Mathieu's
%   equation
%
%      y'' + (lambda -2*q*cos(2*x))*y = 0
%
%   on the interval [0, pi] with boundary conditions y'(0) = 0, y'(pi) = 0.
%   The parameter q = 5.
%
%   A code that exploits fully the special nature of the Sturm-Liouville
%   problem can compute a specific eigenvalue.  Of course using BVP4C we can
%   only compute an eigenvalue near to a guessed value.  We can make it much
%   more likely that we compute the desired eigenfunction by supplying a
%   guess that has the correct qualitative behavior.  We use here the same
%   guess lambda = 15 as the example.  The eigenfunction is determined only
%   to a constant multiple, so the normalizing condition y(0) = 1 is used to
%   specify a particular solution.
%
%   Plotting the solution on the mesh found by BVP4C does not result in a
%   smooth graph.  The solution S(x) is continuous and has a continuous
%   derivative.  It can be evaluated inexpensively using DEVAL at as many
%   points as necessary to get a smooth graph.

% Copyright 2002, The MathWorks, Inc.

% BVPINT is used to form an initial guess for a mesh of 10 equally
% spaced points.  The guess cos(4x) for y(x) and its derivative as guess
% for y'(x) are evaluated in EX3INIT.  The desired eigenvalue is the one
% nearest the guess lambda = 15.  A guess for unknown parameters is the
% (optional) last argument of BVPINIT.
solinit = bvpinit(linspace(0,pi,10),@ex3init,15);

options = bvpset('stats','on');

sol = bvp4c(@ex3ode,@ex3bc,solinit,options);

% BVP4C returns the solution as the structure 'sol'. The computed eigenvalue
% is returned in the field sol.parameters.
fprintf('\n');
fprintf('D02KAF computed lambda = 17.097.\n')
fprintf('bvp4c  computed lambda =%7.3f.\n',sol.parameters)

figure
plot(sol.x,sol.y(1,:),sol.x,sol.y(1,:),'*')
axis([0 pi -1 1])
title('Eigenfunction for Mathieu''s equation.')
xlabel('Solution at mesh points only.')

% Plotting the solution just at the mesh points does not result in a
% smooth graph near the ends of the interval.  The approximate solution
% S(x) is continuous and has a continuous derivative.  DEVAL is used to
% evaluate it at enough points to get a smooth graph.
figure
xint = linspace(0,pi);
Sxint = deval(sol,xint);
plot(xint,Sxint(1,:))
axis([0 pi -1 1])
title('Eigenfunction for Mathieu''s equation.')
xlabel('Solution evaluated on a finer mesh with DEVAL.')

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

function dydx = ex3ode(x,y,lambda)
%EX3ODE  ODE function for Example 3 of the BVP tutorial.
q = 5;
dydx = [              y(2)
         -(lambda - 2*q*cos(2*x))*y(1) ];

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

function res = ex3bc(ya,yb,lambda)
%EX3BC  Boundary conditions for Example 3 of the BVP tutorial.
res = [ ya(2)
        yb(2)
        ya(1) - 1 ];

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

function v = ex3init(x)
%EX3INIT  Guess for the solution of Example 3 of the BVP tutorial.
v = [   cos(4*x)
      -4*sin(4*x) ];