%GSP_DEMO_GRAPH_TV Reconstruction of missing sample on a graph using TV
%
%   In this demo, we try to reconstruct missing sample of a piece-wise
%   smooth signal on a graph. To do so, we will minimize the well-known TV
%   norm defined on the graph.
%
%   For this example, you need the unlocbox. You can download it here:
%   http://unlocbox.sourceforge.net/download
%
%   We express the recovery problem as a convex optimization problem of the
%   following form:
%
%   ..   argmin   ||grad(x)||_1   s. t. ||Mx-b||_2 < epsilon
%
%   .. math:: arg \min_x  \|\nabla(x)\|_1 \text{ s. t. } \|Mx-b\|_2 \leq \epsilon
%
%   Where b represents the known measurements, M is an operator
%   representing the mask and $\epsilon$ is the radius of the l2 ball.
%
%   We set
%
%   * $f_1(x)=||\nabla x ||_1$
%     We define the prox of $f_1$ as:
%
%     .. prox_{f1,gamma} (z) = argmin_{x} 1/2 ||x-z||_2^2  +  gamma  ||grad(z)||_1
%
%     .. math:: prox_{f1,\gamma} (z) = arg \min_{x} \frac{1}{2} \|x-z\|_2^2 +  \gamma \| \nabla z \|_1
%
%   * $f_2$ is the indicator function of the set S define by $||Mx-b||_2 < \epsilon$
%     We define the prox of $f_2$ as
%
%     .. prox_{f2,gamma} (z) = argmin_{x} 1/2 ||x-z||_2^2  +  gamma i_S( x ),
%
%     .. math:: prox_{f2,\gamma} (z) = arg \min_{x} \frac{1}{2} \|x-z\|_2^2   + i_S(x) ,
%
%     with $i_S(x)$ is zero if x is in the set S and infinity otherwise.
%     This previous problem has an identical solution as:
%
%     .. argmin_{z} ||x - z||_2^2   s.t.  ||b - M z||_2 < epsilon
%
%     .. math:: arg \min_{z} \|x - z\|_2^2   \hspace{1cm} such \hspace{0.25cm} that \hspace{1cm} \|Mz-b\|_2 \leq \epsilon
%
%     It is simply a projection on the B2-ball.
%
%   Results
%   -------
%
%   .. figure::
%
%      Original signal on graph
%
%      This figure shows the original signal on graph.
%
%   .. figure::
%
%      Depleted signal on graph
%
%      This figure shows the signal on graph after the application of the
%      mask and addition of noise. Half of the vertices are set to 0.
%
%   .. figure::
%
%      Reconstructed signal on graph usign TV
%
%      This figure shows the reconstructed signal thanks to the algorithm.
%
%   Comparison with Tikhonov regularization
%   ---------------------------------------
%
%   We can also use the Tikhonov regularizer that will promote smoothness.
%   In this case, we solve:
%
%   ..   argmin   ||grad(x)||_2^2   s. t. ||Mx-b||_2 < epsilon
%
%   .. math:: arg \min_x \tau \|\nabla(x)\|_2^2 \text{ s. t. } \|Mx-b\|_2 \leq \epsilon
%
%   The result is presented in the following figure:
%
%   .. figure::
%
%      Reconstructed signal on graph using Tikhonov
%
%      This figure shows the reconstructed signal thanks to the algorithm.
%

% Author: Nathanael Perraudin
% Date: 4th March 2014

%% Initialisation
clear;
close all;
% Load toolbox
init_unlocbox();

%% Important parameters
% size of the graph for the demo
N = 250;
% probability of having a label on a vertex.
p = 0.1;
verbose = 1;    % verbosity level
sigma = 0.0;

%% Create a graph
%  paramgraph.distribute = 1;
% G = gsp_sensor(N, paramgraph);
G = gsp_sensor(N);
G = gsp_community(N);

% for a path with irregular weights and distances
% G = gsp_path(N);
% G = gsp_update_coordinates(G, G.coords + [rand(G.N, 1), zeros(G.N, 1)]);
% G = gsp_update_weights(G, G.W .* exp(-gsp_distanz(G.coords').^2));

G = gsp_adj2vec(G);
G = gsp_estimate_lmax(G);
G = gsp_compute_fourier_basis(G);

% x_0 = (1 + sign(G.U(:,4))) / 2;
x_0 = round((1 + sign(G.U(:,2))) * 2+(1 + sign(G.U(:,3))) +(1 + sign(G.U(:,4))) / 2);
% x_0 = round(linspace(0, 5, N))';
x_0 = G.info.node_com;

%% Set the labels
% create the mask
% ind_obs = boolean(full(sparse(1:4:G.N, 1, 1, G.N, 1)));
ind_obs = rand(G.N, 1) < p;
ind_unobs = not(ind_obs);

%applying the Mask to the data
x_obs = ind_obs.*(x_0+sigma*randn(G.N,1));

%% Method 1: Graph TV
% setting different parameter for the simulation
param_solver.verbose = verbose;  % display parameter
param_solver.tol = 1e-5;
param_solver.maxit = 4000;

tic;
sol_tv = gsp_regression_tv(G, ind_obs, x_obs, 0, param_solver);
toc

%% Method 2: Tikhonov
tic;
G2 = G;
G2.L = G.L^3;
%sol_tik = gsp_regression_tik(G, ind_obs, x_obs, 0, param_solver);
sol_tik = gsp_regression_tik(G2, ind_obs, x_obs, 0, param_solver);
toc

%% Method 3: My idea: inpaint using graph TV on one variable that is l-2
%% away from another one that is smooth
%%
% x, y are primal variables: z is the dual
%      min_x,y,z ||grad(x_all)||_1 + a/2||x-y||^2 + b/2 ||grad(y_all)||^2
%
% grad(x_all) = grad([x;x_obs]) = A*x + A_obs * x_obs = A*x - b
%
% only rows of gradient corresponding to unknown values used:
%      min_x,y,z ||Ax - b||_1  + a/2||x-y||^2 + b/2 ||Ay - b||^2
% Get rid of operator A by using dual variable:
%      min_x,y,z ||z - b||_1   + a/2||x-y||^2 + b/2 ||Ay - b||^2
% Think of all primal variables as one vector w = [x;y] containing ONLY
% the elements on the unlabeled nodes

% choose parameters for solving the problem:
alpha = .1;
beta = 1;

% compute the Diff matrix of the graph
G = gsp_adj2vec(G);

% operators based on the Diff
A_obs = G.Diff(:, ind_obs);
A_unobs = G.Diff(:, not(ind_obs));
n_unobs = nnz(not(ind_obs));
zeros_x = zeros(n_unobs, 1);
zeros_y = zeros_x;

% graph TV term is the l-1 on the dual:
b = - A_obs * x_obs(ind_obs);
f1_params.y = b;
f1.eval = @(z) norm(z - b, 1);
f1.prox = @(z, gamma) prox_l1(z, gamma, f1_params);
f1.L = @(w) A_unobs * w(1:n_unobs);
f1.Lt = @(z) [A_unobs' * z; zeros_y];
f1.norm_L = normest(A_unobs);
%f1.norm_L = sqrt(G.lmax);

% use prox for the second function
% w = [x;y], w0 = [x0;y0]
% prox_g(x) = ((1+g)x + gy)/(1+2g)
f2.prox = @(w, gamma) [ ((1+alpha*gamma) * w(1:n_unobs) + ...
    alpha*gamma     * w(n_unobs+1:end))/(1+2*alpha*gamma);...
    (alpha*gamma     * w(1:n_unobs) + ...
    (1+alpha*gamma) * w(n_unobs+1:end))/(1+2*alpha*gamma)];
f2.eval = @(w) alpha/2 * norm(w(1:n_unobs) - w(n_unobs+1:end))^2;

% use gradient for the last function
AtA = A_unobs' * A_unobs;
Atb = A_unobs' * b;
f3.grad = @(w) [zeros_x; beta * (AtA*w(n_unobs+1:end) - Atb)];
f3.eval = @(w) beta/2 * norm(A_unobs * w(n_unobs+1:end) - b)^2;
f3.beta = beta * normest(AtA);

%% solve the problem
param_solver.verbose = 1;
param_solver.algo = 'FBF_PRIMAL_DUAL';
param_solver.normalized_timestep = 0.99;
tic;
[sol_mine, info_sol] = solvep([sol_tv(not(ind_obs));  sol_tik(not(ind_obs))], {f1,f2,f3}, param_solver);
toc

sol_mine_x = x_obs;
sol_mine_x(ind_unobs) = sol_mine(1:n_unobs);
sol_mine_y = x_obs;
sol_mine_y(ind_unobs) = sol_mine(n_unobs+1:end);

%% Method 4: L-1 of Lx ||Lx||_1
tic;
param_solver = rmfield(param_solver, 'algo');
sol_Lx_l1 = gsp_regression_Lx_l1(G, ind_obs, x_obs, 0, param_solver);
toc

%% Compute the errors
ans_tv = round(sol_tv);
ans_tik = round(sol_tik);
ans_mine = round(sol_mine_x);
ans_Lxl1 = round(sol_Lx_l1);

err_tv      = nnz(ans_tv ~= x_0) / G.N
err_tik     = nnz(ans_tik ~= x_0) / G.N
err_mine    = nnz(ans_mine ~= x_0) / G.N
err_Lxl1    = nnz(ans_Lxl1 ~= x_0) / G.N

%% Print the result
paramplot.show_edges = 1;
val_lims = lin_map([-.05, 1.05], [min([x_0; sol_tv; sol_tik]), max([x_0; sol_tv; sol_tik])], [0, 1]);

%% Plot the original graph
if not(strcmp(G.type, 'path'))
    figure(1)
    paramplot.vertex_highlight = ind_obs;
    gsp_plot_signal(G, x_0, paramplot)
    caxis(val_lims);
    title('Original signal: highlighted known values')

    % % Let show depleted graph
    % figure(2)
    % gsp_plot_signal(G,depleted_graph_value,paramplot)
    % caxis([-1 1])
    % title('Measurement')
    % Let show the reconstructed graph
    figure(3)
    paramplot.vertex_highlight = (ans_tv ~= x_0);
    gsp_plot_signal(G, ans_tv, paramplot)
    caxis(val_lims)
    title('TV solution: highlighted mistakes')

    % Let show the reconstructed graph
    figure(4)
    paramplot.vertex_highlight = (ans_tik ~= x_0);
    gsp_plot_signal(G, ans_tik, paramplot)
    caxis(val_lims)
    title('Tikhonov solution: highlighted mistakes')

    % Let show the reconstructed graph
    figure(5)
    paramplot.vertex_highlight = (ans_mine ~= x_0);
    gsp_plot_signal(G, ans_mine, paramplot)
    caxis(val_lims)
    title('My solution: highlighted mistakes')

    % Let show the reconstructed graph
    figure(6)
    paramplot.vertex_highlight = (ans_Lxl1 ~= x_0);
    gsp_plot_signal(G, ans_Lxl1, paramplot)
    caxis(val_lims)
    title('||Lx||_1 solution: highlighted mistakes')

    %%
else strcmp(G.type, 'path')
    figure;
    plot(G.coords(:, 1), sol_tv)
    hold on;
    plot(G.coords(:, 1), sol_tik)
    plot(G.coords(:, 1), x_0);
    plot(G.coords(:, 1), sol_Lx_l1)
    plot(G.coords(ind_obs, 1), x_0(ind_obs), 'o');
    legend('TV', 'Tikhonov', '||Lx||_1', 'true');
end

figure; plot(sol_mine_x)
hold on; plot(sol_mine_y)


function p = normal2(mu, Sigma, dom)
%NORMAL2   Bivariate normal distribution.
%   NORMAL2(MU, SIGMA, DOMAIN) returns the joint probability density for two
%   variables whose means are in the vector MU and whose covariance is the 2x2
%   positive definite matrix SIGMA. The result is a chebfun2 defined on the
%   (finite) domain DOMAIN.
%
%   If DOMAIN is not supplied, the domain is taken to be large enough to make
%   the density essentially zero at the boundary.
%
%   Example:
%      p = cheb.normal2([1,0], [1 1;1 5]);
%      surf(p)

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

if ( any(eig(Sigma) < 0) || norm(Sigma - Sigma') > 0 )
    error('CHEB:NORMAL2:covariance:nonSymPosDef', ...
        'Covariance matrix must be symmetric positive definite.')
end

if nargin < 3
    sig = svd(Sigma);
    dom = [ mu(1)+4*[-sig(1) sig(1)], mu(2)+4*[-sig(1) sig(1)] ];
end

x = chebfun2(@(x,y) x,dom);
y = chebfun2(@(x,y) y,dom);
invSig = inv(Sigma);
x = x - mu(1);
y = y - mu(2);
z = invSig(1,1)*x.^2 + (invSig(1,2)+invSig(2,1))*x.*y + invSig(2,2)*y.^2;
const = 2*pi*sqrt(det(Sigma));

p = exp(-z/2)/const;

end


function [tscr] = triscr2(pp,tt)
%TRISCR2 calc. area-len. ratios for triangles in a 2-simplex
%triangulation in the two-dimensional plane.
%   [SCR2] = TRISCR2(VERT,TRIA) returns the area-len. ratios
%   where SCR2 is a T-by-1 vector, VERT is a V-by-2 array of
%   XY coordinates, and TRIA is a T-by-3 array of
%   vertex indexing, where each row defines a triangle, such
%   that VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(
%   TRIA(II,3),:) are the coordinates of the II-TH triangle.
%
%   See also TRIAREA, TRIANG2, TRIBAL2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 17/01/2017

%--------------------------- compute signed area-len. ratios
    scal = 4.0 * sqrt(3.0) / 3.0;

    area = triarea(pp,tt) ;             % also error checks!

    lrms = sum((pp(tt(:,2),:)        ...
              - pp(tt(:,1),:)).^2,2) ...
         + sum((pp(tt(:,3),:)        ...
              - pp(tt(:,2),:)).^2,2) ...
         + sum((pp(tt(:,3),:)        ...
              - pp(tt(:,1),:)).^2,2) ;

    lrms =(lrms / 3.0) .^ 1.00 ;

    tscr = scal * area ./ lrms ;

end


function L = gaussianNoiseLogLikelihood(noise, mu, varsigma, y)

% GAUSSIANNOISELOGLIKELIHOOD Log likelihood of the data under the GAUSSIAN noise model.
% FORMAT
% DESC returns the log likelihood of a data set under the  Gaussian noise model.
% ARG noise : the noise structure for which the log likelihood is required.
% ARG mu : input mean locations for the log likelihood.
% ARG varSigma : input variance locations for the log likelihood.
% ARG y : target locations for the log likelihood.
%
% SEEALSO : gaussianNoiseParamInit, gaussianNoiseLikelihood, noiseLogLikelihood
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

N = size(mu, 1);
D = size(mu, 2);
varsigma = varsigma + noise.sigma2;
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
arg = (y - mu);
arg = arg.*arg./varsigma;

L = - 0.5*sum(sum(log(varsigma))) ...
    - 0.5*sum(sum(arg)) ...
    - 0.5*N*D*log(2*pi);


function [sigma,shrinkage]=cov1para(x,shrink)

% function sigma=cov1para(x)
% x (t*n): t iid observations on n random variables
% sigma (n*n): invertible covariance matrix estimator
%
% Shrinks towards one-parameter matrix:
%    all variances are the same
%    all covariances are zero
% if shrink is specified, then this value is used for shrinkage

% This version: 04/2014

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is released under the BSD 2-clause license.

% Copyright (c) 2014, Olivier Ledoit and Michael Wolf
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in the
% documentation and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% de-mean returns
[t,n]=size(x);
meanx=mean(x);
x=x-meanx(ones(t,1),:);
%x = bsxfun(@minus,x,meanx);

% compute sample covariance matrix
sample=(1/t).*(x'*x);

% compute prior
%meanvar=mean(diag(sample));
meanvar = trace(sample)/n;
prior=meanvar*eye(n);

%if (nargin < 2 | shrink == -1) % compute shrinkage parameters

  % what we call p
  y=x.^2;
  phiMat=y'*y/t-sample.^2;
  phi=sum(sum(phiMat));

  % what we call r is not needed for this shrinkage target

  % what we call c
  %gamma=norm(sample-prior,'fro')^2;
  gamma = sample-prior;
  gamma = sum(gamma(:).^2);

  % compute shrinkage constant
  kappa=phi/gamma;
  shrinkage=max(0,min(1,kappa/t));

%else % use specified number
%  shrinkage=shrink;
%end

% compute shrinkage estimator
sigma=shrinkage*prior+(1-shrinkage)*sample;


function [A,b,x] = spikes(n,t_max)
%SPIKES Test problem with a "spiky" solution.
%
% [A,b,x] = spikes(n,t_max)
%
% Artificially generated discrete ill-posed problem.
%
% The solution x consists of a unit step at t = .5, and a pulse train
% of spikes of decrasing magnitude at t = .5, 1.5, 2.5, ...
%
% The parameter t_max is optional; its default value is 5.
% It controls the length of the pulse train.

% Per Christian Hansen, IMM, 04/21/97.

% Initialization.
if (nargin == 1), t_max = 5; end
del = t_max/n;

% Compute the matrix A.
[t,sigma] = meshgrid(del:del:t_max,del:del:t_max);
A = sigma./(2*sqrt(pi*t.^3)).*exp(-(sigma.^2)./(4*t));

% Compute the right-hand side b and the solution x.
if (nargout > 1)
  heights = 2*ones(t_max,1); heights(1) = 25;
  heights(2) = 9; heights(3) = 5; heights(4) = 4; heights(5) = 3;
  x = zeros(n,1); n_h = 1;
  peak = 0.5/t_max; peak_dist = 1/t_max;
  if (peak < 1)
    n_peak = round(peak*n); x(n_peak) = heights(n_h);
    x(n_peak+1:n) = ones(n-n_peak,1);
    peak = peak + peak_dist; n_h = n_h + 1;
  end
  while (peak < 1)
    x(round(peak*n)) = heights(n_h);
    peak = peak + peak_dist; n_h = n_h + 1;
  end
  b = A*x;
end


%QUESTION NO:3

% d)Design and Train a feedforward network for the following problem:
%   Addition: Consider a 4-input and 3-output problem, where the output
%   should be the result of the sum of two 2-bit input numbers.

clear
inp=[0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1;0 0 0 0 1 1 1 1 0 0 0 0 1 1 1 1;...
    0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 1;0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1];
out=[0 0 0 0 0 0 0 1 0 0 1 1 0 1 1 1; 0 0 1 1 0 1 1 0 1 1 0 0 1 0 0 1; ...
     0 1 0 1 1 0 1 0 0 1 0 1 1 0 1 0];
network=newff([0 1;0 1; 0 1; 0 1],[6 3],{'logsig','logsig'});
network=init(network);
y=sim(network,inp);
network.trainParam.epochs = 500;
network=train(network,inp,out);
y=sim(network,inp);
Layer1_Weights=network.iw{1};
Layer1_Bias=network.b{1};
Layer2_Weights=network.lw{2};
Layer2_Bias=network.b{2};
Layer1_Weights
Layer1_Bias
Layer2_Weights
Layer2_Bias
Actual_Desired=[y' out'];
Actual_Desired


function a = covar2 ( m, n, b, a )

%*****************************************************************************80
%
%% COVAR2 returns the covariance matrix for a rectangular matrix.
%
%  Discussion:
%
%    M should be at least N, and B has maximal rank N.
%    The covariance matrix is defined as
%
%      A = inverse ( B' * B ).
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of B.
%    N <= M is required.
%
%    Input, real B(M,N), the matrix whose covariance matrix
%    is desired.
%
%    Output, real A(N,N), the covariance matrix of B.
%
%    Output, integer IERROR, error flag.
%    IERROR = 0 for no error,
%    IERROR is nonzero to mean that A is singular.
%
  if ( m < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVAR2 - Fatal error!\n' );
    fprintf ( 1, '  M < N\n' );o
    error ( 'COVAR2 - Fatal error!' );
  end
%
%  Compute B' * B.
%
  btb(1:n,1:n) = b(1:m,1:n)' * b(1:m,1:n);
%
%  Factor the matrix.
%
  [ btb, pivot, error ] = r8mat_gefa ( btb, n );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVAR2 - Fatal error!\n' );
    fprintf ( 1, '  The matrix B'' * B does not have full rank.\n' );
    fprintf ( 1, '  This means that B does not have rank N.\n' );
    error ( 'COVAR2 - Fatal error!' );
  end
%
%  Solve for A by solving the equations ( B' * B ) * A = I
%  one column at a time.
%
  a = identity ( n, n );

  job = 0;

  for j = 1 : n
    a(1:n,j) = r8mat_gesl ( btb, n, pivot, a(1:n,j), job );
  end

  return
end


function varargout = crosslegtable(X)
% Cross-leg table function
%
%   CROSSLEGTABLE([x1, x2]) returns the value of the Cross-legged
%   table function at the specified points. [x1] and [x2] may be vectors.
%   The search domain is
%
%               -10 < x_i < 10
%
%   The global minimum is found on the planes x = 0 and y = 0, with
%
%                   fmin = -1.

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 20/Jul/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;  % # dims
        varargout{2} = [-10, -10]; % LB
        varargout{3} = [+10, +10]; % UB
        varargout{4} = NaN; % solution (too complicated)
        varargout{5} = -1; % function value at solution

    % otherwise, output function value
    else

        % keep values in the serach interval
        X(X < -10) = inf;     X(X > 10) = inf;

        % split input vector X into x1, x2
        if size(X, 1) == 2
            x1 = X(1, :);        x2 = X(2, :);
        else
            x1 = X(:, 1);        x2 = X(:, 2);
        end

        % output function value
        varargout{1} = -(abs(sin(x1).*sin(x2).*exp(abs(100 - sqrt(x1.^2 + x2.^2)/pi))) + 1).^(-0.1);

    end

end


function [deltaPoseNoisy] = poseSubNoisy(p_j , p_i, sigmaT, sigmaR, isUniform)
% pose_sub = (p_j , p_i) = inv(p_i) * p_j + noise
% returns the planar roto-translation that tranform pj in pi
% plus noise (sigmaT is the std of the noise on the cartesian components,
% sigmaR is the std of the noise added to the angle before wrapping)

if nargin < 5
   isUniform = 0; % Gaussian noise by default
end

if nargin == 2
  sigmaT = 0;
  sigmaR = 0;
end

p_j = p_j(:);
p_i = p_i(:);

rho_i = p_i(1:2);
rho_j = p_j(1:2);
th_i = p_i(3);
th_j = p_j(3);

R = rot2D(th_i);

delta_th = wrapToPi(th_j - th_i);

if(abs(delta_th) > pi)
    disp('Error in pose_sub')
    disp(delta_th)
end

deltaPose = [ R' * (rho_j   -  rho_i)
    delta_th  ];

if isUniform == 0 % Gaussian noise
    noise = [sigmaT*randn(2,1) ;  sigmaR*randn];
else % uniform noise in the interval [-sigma, + sigma]
    noise = [sigmaT*2*(rand(2,1)-0.5*ones(2,1)) ; sigmaR*2*(rand-0.5)];
end

deltaPoseNoisy = deltaPose + noise;
deltaPoseNoisy(3) = wrapToPi(deltaPoseNoisy(3)); % wrap angle


function [A,C] = fd_bilinear_coefficients(mn,mx,side,V)
  % FD_BILINEAR_COEFFICIENTS  Given a grid from minimum corner mn to maximum
  % corner mx, with `side` nodes on each side, compute a matrix `A` so that
  % `V=A*C` if `C` are the node centers.
  %
  % [A,C] = fd_bilinear_coefficients(mn,mx,side,V)
  %
  % Inputs:
  %   mn  3d position of minimum corner
  %   mx  3d position of maximum corner
  %   side  number of nodes in each dimension [x y]
  %   V  #V by 2 list of query points
  % Outputs:
  %   A  x*y by #V matrix
  %   C  x*y by 3 list of node center positions
  %

  [X,Y] = meshgrid( ...
    linspace(mn(1),mx(1),side(1)), ...
    linspace(mn(2),mx(2),side(2)));
  C = [X(:) Y(:)];
  r = (mx-mn)./(side-1);

  I = floor(bsxfun(@times,bsxfun(@rdivide,bsxfun(@minus,V,mn),mx-mn),side-1));
  A = sparse(size(V,1),prod(side));
  for x = 1:2
    for y = 1:2
      Ixy = sub2ind(side([2 1]),I(:,2)+x,I(:,1)+y);
      Sxy = C(Ixy,:);
      Axy = prod(bsxfun(@minus,r,abs(V-Sxy)),2)/prod(r);
      A = A + sparse((1:size(V,1))',Ixy,Axy,size(V,1),size(C,1));
    end
  end

end


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

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

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

[c,p] = max(sigmoid(X*all_theta'),[],2);

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

end


function [x,minCost]=convexQuadProgIntegerNoConst2D(Q,b,nonNeg)
%%CONVEXQUADPROGINTEGERNOCONST2D Perform quadratic programming on a 2D
%       convex problem with no constraints except that the solutions must
%       be integers that are either unconstrained or are non-negative.
%       This functions solves the optimization problem
%         minimize_x x'*Q*x+2*b'*x
%         subject to x is a 2X1 set of integers (optionally non-negative
%                    integers).
%
%INPUTS: Q An 2X2 real, positive definite symmetric matrix.
%        b An 2X1 real vector.
%   nonNeg This is true if all the x elements should be non-negative and
%          false otherwise. The default if omitted or an empty matrix is
%          passed is false.
%
%OUTPUTS: x The 2X1 optimal solution (or an empty matrix if the optimal
%           cost is infinite).
%   minCost The cost of the optimal constrained solution. If the cost isn't
%           finite, then this is an empty matrix.
%
%One could implement the branch-and bound algorithm for large scale
%problems as in [1]. However, in the 2D case, there are so few branches, it
%isn't worth it. Instead, we simply branch as in [1] (with a
%simple modification for non-negative constraints) and we traverse all 8
%hypotheses and then choose the lowest cost one. In [1], when branching,
%one creates smaller Q and b submatrices to optimize over rather than
%redoing the entire problem with an added eualaity constraint. In the 2D
%case, fixing an index means that the subproblem is just optimizing a scalar
%quadratic equation. Thus, we don't need to explicitely form submatrices;
%we just directly evaluate the scalar solution to the quadratic.
%
%EXAMPLE 1:
%Here is a simple example. We get the unconstrained solution. Then, we get
%the integer constrained solution and see that the cost is higher. Finally,
%we get the integer constrained solution with non-negativity constraints.
%That is NOT just clipping the integer constrained solution to 0. To show
%that, we compute the cost of the clipped solution and we see that it is
%much larger than the solution returned by the function.
% Q=[32,  20;
%    20, 22];
% b=[-178;-1200];
% [xOpt,minCost]=convexQuadProgEqConst(Q,b)
% nonNeg=false;
% [xOptInt,minCostInt]=convexQuadProgIntegerNoConst2D(Q,b,nonNeg)
% nonNeg=true;
% [xNonNegInt,minCostNonNegInt]=convexQuadProgIntegerNoConst2D(Q,b,nonNeg)
% %We show that the zero-clipped solution is much higher than the one with
% %non-negativity constraints.
% xTest=max(xOptInt,0);
% testCost=xTest'*Q*xTest+2*b'*xTest
%
%EXAMPLE 2:
%One might be tempted the think that one can always obtain the globally
%optimal solution by solving the unconstrained problem and then just
%rouning the result or by just trying the 4 points obtained by taking the
%floor and ceiling of each element of the unconstrained solution. However,
%that is not the case. In this example, we show the unconstrained solution
%and then the optimal integer constrained differ by more than 1 index.
% Q=[25, 0.3;
%   0.3, 0.01];
% b=[-156.9;
%    -3.31];
% xUnconst=convexQuadProgEqConst(Q,b)
% [x,minCost]=convexQuadProgIntegerNoConst2D(Q,b)
% %Now, we show that the cost of just rouning up or down the non-integer part
% %of xUnconst does not produce a globally optimal solution. Only the first
% %element of xUnConst is not an integer, so we only need to consider points
% %rounding that one up or down (as opposed to 4 points rounding each
% %element).
% xJustRoundUp=ceil(xUnconst);
% costJustRoundUp=xJustRoundUp'*Q*xJustRoundUp+2*b'*xJustRoundUp
% xJustRoundDown=floor(xUnconst);
% costJustRoundDown=xJustRoundDown'*Q*xJustRoundDown+2*b'*xJustRoundDown
%
%REFERENCES:
%[1] F. Koerner, "An efficient branch and bound algorithm to solve the
%    quadratic integer programming problem," Computing, vol. 30, pp. 253-
%    260, Sep. 1983.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(nonNeg))
    nonNeg=false;
end

xOpt=convexQuadProgEqConst(Q,b);

%Impose the non-negativity constraint. When branching on each value, if the
%non-negativity constraint was enforce,d both branches will be the same.
%However, we aren't going to bother checking to see whether a branch should
%be skipped, because there are so few branches.
if(nonNeg)
    xOpt=max(xOpt,0);
end

x=[];
minCost=Inf;

%Branch on the first index.
for x1=[floor(xOpt(1)),ceil(xOpt(1))]
    %The part of the cost function that depends only on x1 and not x2
    %(which is fixed).
    constCostTerm=2*b(1)*x1+Q(1,1)*x1^2;
    %The optimal solution for x2 given x1 and no constraints.
    x2Node=-(b(2)+Q(1,2)*x1)/(Q(2,2));
    if(nonNeg)
        %Apply a non-negativity constraint.
        x2Node=max(x2Node,0);
    end

    %Now, branch on the second index. With it fixed, we just evaluate the
    %cost (There is nothing else to optimize).
    for x2=[floor(x2Node),ceil(x2Node)]
        costCur=2*b(2)*x2+2*Q(1,2)*x1*x2+Q(2,2)*x2^2+constCostTerm;
        if(costCur<minCost)
            minCost=costCur;
            x=[x1;x2];
        end
    end
end

%Now branch on the second index.
for x2=[floor(xOpt(2)),ceil(xOpt(2))]
    %The part of the cost function that depends only on x2 and not x1
    %(which is fixed).
    constCostTerm=2*b(2)*x2+Q(2,2)*x2^2;
    %The optimal solution for x1 given x2 and no constraints.
    x1Node=-(b(1)+Q(1,2)*x2)/(Q(1,1));

    if(nonNeg)
        %Apply a non-negativity constraint.
        x1Node=max(x1Node,0);
    end

    %Now, branch on the first index.
    for x1=[floor(x1Node),ceil(x1Node)]
        costCur=2*b(1)*x1+Q(1,1)*x1^2+2*Q(1,2)*x1*x2+constCostTerm;
        if(costCur<minCost)
            minCost=costCur;
            x=[x1;x2];
        end
    end
end
end

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


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%
for i=1:size(X,1)
    tempMin=inf;
    for j=1:K
        temp =sum((centroids(j,:)-X(i,:)).^2);
        if temp<tempMin
           idx(i)=j;
           tempMin = temp;
        end
    end

end

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

end


%% newton raphson example
% Find the Darcy friction factor for pipe flow using the Colebrook
% equation.
fprintf('\n**************************************************\n')
fprintf('NON-LINEAR SYSTEM OF EQUATIONS - PIPE FLOW EXAMPLE\n')
fprintf('**************************************************\n')
%% References:
% * http://en.wikipedia.org/wiki/Darcy_friction_factor_formulae
% * http://en.wikipedia.org/wiki/Darcy%E2%80%93Weisbach_equation
% * http://en.wikipedia.org/wiki/Moody_chart
% * http://www.mathworks.com/matlabcentral/fileexchange/35710-iapwsif97-functional-form-with-no-slip
%% inputs:
p = 0.68; % [MPa] water pressure (100 psi)
dp = -0.068*1e6; % [Pa] pipe pressure drop (10 psi)
T = 323; % [K] water temperature
D = 0.10; % [m] pipe hydraulic diameter
L = 100; % [m] pipe length
roughness = 0.00015; % [m] cast iron pipe roughness
rho = 1./IAPWS_IF97('v_pT',p,T); % [kg/m^3] water density (988.1 kg/m^3)
mu = IAPWS_IF97('mu_pT',p,T); % [Pa*s] water viscosity (5.4790e-04 Pa*s)
Re = @(u) rho*u*D/mu; % Reynolds number
%% governing equations
% Use Colebrook and Darcy-Weisbach equation to solve for pipe flow.

% friction factor (Colebrook eqn.)
residual_friction = @(u, f) 1/sqrt(f) + 2*log10(roughness/3.7/D + 2.51/Re(u)/sqrt(f));
% pressure drop (Darcy-Weisbach eqn.)
residual_pressdrop = @(u, f) rho*u^2*f*L/2/D + dp;
% residuals
fun = @(x) [residual_friction(x(1),x(2)), residual_pressdrop(x(1),x(2))];
%% solve
x0 = [1,0.01]; % initial guess
fprintf('\ninitial guess: u = %g[m/s], f = %g\n',x0) % display initial guess
options = optimset('TolX',1e-12); % set TolX
[x, resnorm, f, exitflag, output, jacob] = newtonraphson(fun, x0, options);
fprintf('\nexitflag: %d, %s\n',exitflag, output.message) % display output message
%% results
fprintf('\nOutputs:\n')
properties = {'Pressure','Pressure-drop','Temperature','Diameter','Length', ...
    'roughness','density','viscosity','Reynolds-number','speed','friction'};
units = {'[Pa]','[Pa]','[C]','[cm]','[m]','[mm]','[kg/m^3]','[Pa*s]','[]','[m/s]','[]'};
values = {p*1e6,dp,T-273.15,D*100,L,roughness*1000,rho,mu,Re(x(1)),x(1),x(2)};
fprintf('%15s %10s %10s\n','Property','Unit','Value')
results = [properties; units; values];
fprintf('%15s %10s %10.4g\n',results{:})
%% comparison
% solve using Haaland
Ntest = 10;
u0 = linspace(x(1)*0.1, x(1)*10, Ntest); % [m/s]
Re0 = Re(u0);
f0 = (1./(-1.8*log10((roughness/D/3.7)^1.11 + 6.9./Re0))).^2;
u0 = sqrt(-dp/rho./(f0*L/2/D));
% plot
plot(u0, f0, '-', u0, x(2)*ones(1,Ntest), '--', x(1)*ones(1,Ntest), f0, '--')
grid
title('Pipe flow solution using Haaland equation.')
xlabel('water speed, u [m/s]'),ylabel('friction factor, f')
legend('f_{Haaland}',['f = ',num2str(x(2))], ['u = ',num2str(x(1)),' [m/s]'])
%% LSQ Curve Fitting
fprintf('\n**********************************************\n')
fprintf('LEAST SQUARES CURVE FITTING WITH NEWTONRAPHSON\n')
fprintf('**********************************************\n')
% independent variables
[x,y] = meshgrid(0:10,0:10);
% bivariate distribution
bivar = @(x1,x2,sig,u1,u2)1/2/pi/sig^2*exp(-1/2*(((x1-u1).^2+(x2-u2).^2)/sig));
sigma = 3; ux = 4; uy = 5; % std dev, x & y means
z = bivar(x,y,sigma,ux,uy); % dist
% plot
figure,contour(x,y,z),hold('all')
title('lsq curve-fitting bivariate pdf with newtonraphson')
xlabel('x-coord'),ylabel('y-coord') % axes titles
grid,colorbar % show colorbar and grid
z_meas = z + (2*rand(11)-1)/1e4; % measured data
% fitting function
lsqfitfun = @(c)z_meas-bivar(x,y,c(1),c(2),c(3));
% fit coefficients to fun
c0 = [1,2,3]; % initial guess
fprintf('\ninitial guess: sigma = %g, ux = %g, uy = %g\n',c0) % display initial guess
options = optimset('TolX',1e-12); % set TolX
[c, ~, ~, exitflag, output] = newtonraphson(lsqfitfun, c0, options);
fprintf('\nexitflag: %d, %s\n',exitflag, output.message) % display output message
fprintf('\ncurve-fit coefficients: sigma=%g, ux=%g, uy=%g\n',c)
lines = plot(repmat(c(2),1,11),0:10,'--',0:10,repmat(c(3),1,11),'--', ...
    c(2),c(3),'o');
set(lines,'LineWidth',2);
legend('bivariate distribution','u_x','u_y','center')


function [ grid_level, grid_point ] = cc_levels_constrained ( dim_num, ...
  q_max, alpha, level_min, level_max, grid_num, point_num )

%*****************************************************************************80
%
%% CC_LEVELS_CONSTRAINED: CC grids with constrained levels.
%
%  Discussion:
%
%    The constraint on the levels of the 1D Clenshaw Curtis rule in
%    spatial dimension I is:
%
%      LEVEL_MIN(I) <= LEVEL(I) <= LEVEL_MAX(I)
%
%    The constraint on the collection of levels making up a rule is:
%
%      Sum ( 1 <= I <= DIM_NUM ) ALPHA(I) * LEVEL(I) <= Q_MAX.
%
%    The relationship of level to order is roughly
%
%      ORDER = 2**LEVEL+1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real Q_MAX, the maximum values of
%    Q, the sum of the weighted orders in each spatial coordinate.
%
%    Input, real ALPHA(DIM_NUM), the weight factors for
%    the orders in each spatial dimension.
%
%    Input, integer LEVEL_MIN(DIM_NUM), LEVEL_MAX(DIM_NUM), the minimum
%    and maximum values of the level of the 1D Clenshaw Curtis rule
%    in each spatial dimension.
%
%    Input, integer GRID_NUM, the number of Clenshaw Curtis
%    grids in the constraint set.
%
%    Input, integer POINT_NUM, the total number of points in the grids.
%
%    Output, integer GRID_LEVEL(DIM_NUM,GRID_NUM), contains, for each
%    grid, the level of the Clenshaw-Curtis rule in each dimension.
%
%    Output, real GRID_POINT(DIM_NUM,POINT_NUM), contains
%    a list of all the abscissas of all the rules, listed one grid at
%    a time.  If a point occurs in several grids, it will be listed
%    several times.
%
  point_num = 0;
  grid_num = 0;

  level_1d = [];
  more = 0;

  while ( 1 )

    [ level_1d, more ] = vector_constrained_next4 ( dim_num, alpha, level_min, ...
      level_max, level_1d, q_max, more );

    if ( ~more )
      break
    end

    order_1d = cc_level_to_order ( dim_num, level_1d );

    order_nd = prod ( order_1d(1:dim_num) );

    grid_point(1:dim_num,point_num+1:point_num+order_nd) = cc_grid ( dim_num, ...
      order_1d, order_nd );

    point_num = point_num + order_nd;

    grid_num = grid_num + 1;
    grid_level(1:dim_num,grid_num) = level_1d(1:dim_num);

  end

  return
end


function varargout = ellipsoidMesh(elli, varargin)
%ELLIPSOIDMESH Convert a 3D ellipsoid to face-vertex mesh representation.
%
%   [V, F] = ellipsoidMesh(ELLI)
%   ELLI is given by:
%   [XC YC ZC  A B C  PHI THETA PSI],
%   where (XC, YC, ZC) is the ellipsoid center, A, B and C are the half
%   lengths of the ellipsoid main axes, and PHI THETA PSI are Euler angles
%   representing ellipsoid orientation, in degrees.
%
%
%   See also
%   meshes3d, drawEllipsoid, sphereMesh, inertiaEllipsoid
%

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

%% Default values

% number of meridians
nPhi    = 32;

% number of parallels
nTheta  = 16;

%% Extract input arguments

% Parse the input (try to extract center coordinates and radius)
if nargin == 0
    % no input: assumes ellipsoid with default shape
    elli = [0 0 0 5 4 3 0 0 0];
end

% default set of options for drawing meshes
options = {'FaceColor', 'g', 'linestyle', 'none'};

while length(varargin) > 1
    switch lower(varargin{1})
        case 'nphi'
            nPhi = varargin{2};

        case 'ntheta'
            nTheta = varargin{2};

        otherwise
            % assumes this is drawing option
            options = [options varargin(1:2)]; %#ok<AGROW>
    end

    varargin(1:2) = [];
end

%% Parse numerical inputs

% Extract ellipsoid parameters
xc  = elli(:,1);
yc  = elli(:,2);
zc  = elli(:,3);
a   = elli(:,4);
b   = elli(:,5);
c   = elli(:,6);
k   = pi / 180;
ellPhi   = elli(:,7) * k;
ellTheta = elli(:,8) * k;
ellPsi   = elli(:,9) * k;

%% Coordinates computation

% convert unit basis to ellipsoid basis
sca     = createScaling3d(a, b, c);
rotZ    = createRotationOz(ellPhi);
rotY    = createRotationOy(ellTheta);
rotX    = createRotationOx(ellPsi);
tra     = createTranslation3d([xc yc zc]);

% concatenate transforms
trans   = tra * rotZ * rotY * rotX * sca;

%% parametrisation of ellipsoid

% spherical coordinates
theta   = linspace(0, pi, nTheta+1);
phi     = linspace(0, 2*pi, nPhi+1);

% convert to cartesian coordinates
sintheta = sin(theta);
x = cos(phi') * sintheta;
y = sin(phi') * sintheta;
z = ones(length(phi),1) * cos(theta);

% transform mesh vertices
[x, y, z] = transformPoint3d(x, y, z, trans);

% convert to FV mesh
[vertices, faces] = surfToMesh(x, y, z, 'xPeriodic', false, 'yPeriodic', true);

% format output
varargout = formatMeshOutput(nargout, vertices, faces);


function gauss_seidel_test02 ( )

%*****************************************************************************80
%
%% GAUSS_SEIDEL_TEST02 tests GAUSS_SEIDEL2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_TEST02:\n' );

  it_num = 400;
  n = 20;

  x_exact = ( 1 : n )';

  l = sparse ( 2:n,   1:n-1, -1.0, n, n );
  d = sparse ( 1:n,   1:n,    2.0, n, n );
  u = sparse ( 1:n-1, 2:n,   -1.0, n, n );

  a = l + d + u;

  b = a * x_exact;

  x = zeros ( n, 1 );
  x_plot(1:n,1) = x;

  step = 1 : it_num + 1;
  e = nan ( it_num+1, 1 );
  xm = nan ( it_num+1, 1 );

  e(1,1) = ( norm ( a * x - b ) ).^2;

  for it = 1 : it_num

    x_new = gauss_seidel2 ( n, l, d, u, b, x );

    e(it+1,1) = ( norm ( a * x_new - b ) ).^2;
    x_plot(1:n,it+1) = x_new;
%
%  Display the error.
%
    figure ( 1 )
    plot ( step, log ( e ), 'm-*' )
    title ( 'Log (Error^2)' )
    xlabel ( 'Step' )
    ylabel ( 'Error' )
    grid
%
%  Display the motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;

    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Energy' )
    grid
%
%  Update the solution
%
    x = x_new;

  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );

  return
end


function [ vX ] = SolveProxTvAdmm( vY, mD, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX ] = SolveProxTvAdmm( vY, mD, paramLambda, numIterations )
% Solves the Prox of the Total Variation (TV) Norm using Alternating
% Direction Method of Multipliers (ADMM) Method.
% Basically solves the problem given by:
% $$ \arg \min_{ x \in \mathbb{R}^{n} } \frac{1}{2} {\left\| x - y \right|}_{2}^{2} + \lambda {\left\| D x \right\|}_{1} $$
% Input:
%   - vX                -   input Vector.
%                           Initialization of the iterative process.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia ADMM - https://en.wikipedia.org/wiki/Augmented_Lagrangian_method#Alternating_direction_method_of_multipliers.
% Remarks:
%   1.  Using vanilla ADMM with no optimization of the parameter or
%       smoothing.
%   2.  Matrix Factorization caching according to "Matrix Inversion Lemma"
%       (See S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein -
%       Distributed Optimization and Statistical Learning via the
%       Alternating Direction Method of Multipliers Page 28). Basically:
%       (mA.' * mA + paramRho * I)^(-1) = (1 / paramRho) + (1 / (paramRho * paramRho)) * mA.' * (I + (1 /
%       paramRho) * mA * mA.')^(-1) * mA
% Known Issues:
%   1.  A
% TODO:
%   1.  Pre calculate decomposition of the Linear System.
% Release Notes:
%   -   1.0.000     27/11/2019  Royi Avital
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

paramRho = 5;

mI = eye(size(vY, 1));
mC = decomposition(mI + paramRho * (mD.' * mD), 'chol');

vX = vY;
vZ = ProxL1(mD * vX, paramLambda / paramRho);
vU = mD * vX - vZ;

for ii = 2:numIterations

    vX = mC \ (vY + (paramRho * mD.' * (vZ - vU)));
    vZ = ProxL1(mD * vX + vU, paramLambda / paramRho);
    vU = vU + mD * vX - vZ;

end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding
vX = max(vX - lambdaFactor, 0) + min(vX + lambdaFactor, 0);

end


% Butterworth IIR Filter
srate=1000; % sample rate
npts=2000; %npts in signal
Nyquist=srate/2; %Nyquist frequency
lpf=300; %low-pass frequency
order=5; %filter order
t=[0:npts-1]/srate; %time scale for plot
x=(rand(npts,1)*2)-1; % raw data from -1 to +1

[b,a]=butter(order,lpf/Nyquist); %create filter coefficients

filtered_data=filter(b,a,x); % filter using 'b' and 'a' coefficients

% Calculate FFT
fftpts=npts; % number points in FFT
hpts=fftpts/2; % half number of FFT points
binwidth=srate/fftpts;
f=[0:binwidth:srate-binwidth];
x_fft=abs(fft(x))/hpts; %scaled FFT of original signal
filtered_fft=abs(fft(filtered_data))/hpts; %scaled FFT of filtered signal

subplot(2,2,1)
plot(t,x);
title('Raw Time Series');
subplot(2,2,3)
plot(t,filtered_data);
title('Filtered Time Series');
xlabel('Time (s)');
subplot(2,2,2)
plot(f(1:hpts),x_fft(1:hpts));
title('Raw FFT');
subplot(2,2,4)
plot(f(1:hpts),filtered_fft(1:hpts));
title('Filtered FFT');
xlabel('Frequency (Hz)');


function [tfr,t,f] = tfrwv(x,t,N,trace);
%TFRWV	Wigner-Ville time-frequency distribution.
%	[TFR,T,F]=TFRWV(X,T,N,TRACE) computes the Wigner-Ville distribution
%	of a discrete-time signal X,
%	or the cross Wigner-Ville representation between two signals.
%
%	X     : signal if auto-WV, or [X1,X2] if cross-WV.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR   : time-frequency representation. When called without
%	        output arguments, TFRWV runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); tfrwv(sig);
%
%	See also all the time-frequency representations listed in
%	the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least one parameter required');
end;
[xrow,xcol] = size(x);

if (nargin == 1),
 t=1:xrow; N=xrow ; trace=0;
elseif (nargin == 2),
 N=xrow ; trace=0;
elseif (nargin == 3),
 trace = 0;
end;

if (N<0),
 error('N must be greater than zero');
end;

[trow,tcol] = size(t);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

tfr= zeros (N,tcol);
if trace, disp('Wigner-Ville distribution'); end;
for icol=1:tcol,
 ti= t(icol); taumax=min([ti-1,xrow-ti,round(N/2)-1]);
 tau=-taumax:taumax; indices= rem(N+tau,N)+1;
 tfr(indices,icol) = x(ti+tau,1) .* conj(x(ti-tau,xcol));
 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1),
  tfr(tau+1,icol) = 0.5 * (x(ti+tau,1) * conj(x(ti-tau,xcol))  + ...
                           x(ti-tau,1) * conj(x(ti+tau,xcol))) ;
 end;
 if trace, disprog(icol,tcol,10); end;
end;
tfr= fft(tfr);
if (xcol==1), tfr=real(tfr); end ;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrwv');
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function F = Gaus_filter(I,sigma)

R = ceil(3*sigma);
for i = -R:R,
    for j = -R:R,
        M(i+ R+1,j+R+1) = exp(-(i*i+j*j)/2/sigma/sigma)/(2*pi*sigma*sigma);
    end
end
M = M/sum(sum(M));  % normalize
F = xconv2(I,M);


%% inpainting RobustPCA example: moon picture corrupted with some text
addpath('../');

% read image and add the mask
img = double(imread('moon.tif'))/255;
img = img(141:140+256, 51:50+256);
msk = zeros(size(img));
msk(65:192,65:192) = imresize(imread('text.png'), 0.5);
img_corrupted = img;
img_corrupted(msk > 0) = nan;

fprintf(1, '%d corrupted entries\n', nnz(isnan(img_corrupted)));

% create a matrix X from overlapping patches
ws = 16; % window size
no_patches = size(img, 1) / ws;
X = zeros(no_patches^2, ws^2);
k = 1;
for i = (1:no_patches*2-1)
    for j = (1:no_patches*2-1)
        r1 = 1+(i-1)*ws/2:(i+1)*ws/2;
        r2 = 1+(j-1)*ws/2:(j+1)*ws/2;
        patch = img_corrupted(r1, r2);
        X(k,:) = patch(:);
        k = k + 1;
    end
end

% apply Robust PCA
lambda = 0.02; % close to the default one, but works better
tic
[L, S] = RobustPCA(X, lambda, 1.0, 1e-5);
toc

% reconstruct the image from the overlapping patches in matrix L
img_reconstructed = zeros(size(img));
img_noise = zeros(size(img));
k = 1;
for i = (1:no_patches*2-1)
    for j = (1:no_patches*2-1)
        % average patches to get the image back from L and S
        % todo: in the borders less than 4 patches are averaged
        patch = reshape(L(k,:), ws, ws);
        r1 = 1+(i-1)*ws/2:(i+1)*ws/2;
        r2 = 1+(j-1)*ws/2:(j+1)*ws/2;
        img_reconstructed(r1, r2) = img_reconstructed(r1, r2) + 0.25*patch;
        patch = reshape(S(k,:), ws, ws);
        img_noise(r1, r2) = img_noise(r1, r2) + 0.25*patch;
        k = k + 1;
    end
end
img_final = img_reconstructed;
img_final(~isnan(img_corrupted)) = img_corrupted(~isnan(img_corrupted));

% show the results
figure;
subplot(2,2,1), imshow(img_corrupted), title('Corrupted image')
subplot(2,2,2), imshow(img_final), title('Recovered image')
subplot(2,2,3), imshow(img_reconstructed), title('Recovered low-rank')
subplot(2,2,4), imshow(img_noise), title('Recovered sparse')

fprintf(1, 'ws=%d\tlambda=%f\trank(L)=%d\tcard(S)=%d\terr=%f\n', ...
       ws, lambda, rank(L), nnz(S), norm(img - img_final, 'fro'));


function yval = r8poly_der_val ( n, poly_cof, xval )

%*****************************************************************************80
%
%% R8POLY_DER_VAL evaluates the derivative of a polynomial in standard form.
%
%  Discussion:
%
%    A polynomial in standard form, with coefficients POLY_COF(*),
%    may be written:
%
%      P(X) = POLY_COF(1)
%           + POLY_COF(2) * X
%           ...
%           + POLY_COF(N) * X**(N-1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the polynomial.
%
%    Input, real POLY_COF(1:N), the polynomial coefficients.
%    POLY_COF(1) is the constant term, and POLY_COF(N) is the coefficient of
%    X**(N-1).
%
%    Input, real XVAL, a value where the derivative of the
%    polynomial is to be evaluated.
%
%    Output, real YVAL, the value of the derivative of the
%    polynomial at XVAL.
%
  yval = ( n - 1 ) * poly_cof(n);

  for i = n-1 : -1 : 2
    yval = yval * xval + ( i - 1 ) * poly_cof(i);
  end

  return
end


function  test_softmax_classifier()

    clc;
    clear;
    close all;

    %% Set algorithms
    if 0
        algorithms = gd_solver_list('ALL');
    else
        algorithms = gd_solver_list('LS');
    end

    %% prepare dataset
    if 1
        n_per_class = 100;    % # of samples
        d = 3;      % # of dimensions
        l = 5;      % # of classes
        std = 0.15; % standard deviation

        data = multiclass_data_generator(n_per_class, d, l, std);
        n = length(data.y_train);
        d = d + 1; % adding '1' row for intersect

        % train data
        x_train = [data.x_train; ones(1,n)];
        % transform class label into label logical matrix
        y_train = zeros(l,n);
        for j=1:n
            y_train(data.y_train(j),j) = 1;
        end

        % test data
        x_test = [data.x_test; ones(1,n)];
        % transform class label into label logical matrix
        y_test = zeros(l,n);
        for j=1:n
            y_test(data.y_test(j),j) = 1;
        end

        lambda = 0.0001;
        w_opt = zeros(d*l,1);

    else
        % load real-world data
        data = importdata('../data/mnist/6000_data_0.001.mat');
        x_train = data.x_trn;
        y_train = data.y_trn;
        x_test = data.x_tst;
        y_test= data.y_tst;
        d = size(x_train,1);
        n = length(y_train);
        lambda = data.lambda;

        w_opt = data.w_opt;
        l = data.L;
    end

    % set plot_flag
    if d > 4
        plot_flag = false;  % too high dimension
    else
        plot_flag = true;
    end

    %% define problem definitions
    problem = softmax_regression(x_train, y_train, x_test, y_test, l, lambda);

    % initialize
    w_init = randn(d*l,1);

    w_list = cell(length(algorithms),1);
    info_list = cell(length(algorithms),1);

    %% calculate solution
    if norm(w_opt)
    else
        % calculate solution
        w_opt = problem.calc_solution(1000);
    end
    f_opt = problem.cost(w_opt);
    fprintf('f_opt: %.24e\n', f_opt);

    %% perform algorithms
    for alg_idx=1:length(algorithms)
        fprintf('\n\n### [%02d] %s ###\n\n', alg_idx, algorithms{alg_idx});

        clear options;
        % general options for optimization algorithms
        options.w_init = w_init;
        options.tol_gnorm = 1e-10;
        options.max_iter = 100;
        options.verbose = true;
        options.f_opt = f_opt;
        options.store_w = true;

        switch algorithms{alg_idx}
            case {'SD-STD'}

                options.step_alg = 'fix';
                options.step_init = 1;
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-SCALE-EXACT'}

                options.sub_mode = 'SCALING';
                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'Newton-STD'}

                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'Newton-DAMP'}

                options.sub_mode = 'DAMPED';
                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'Newton-CHOLESKY'}

                options.sub_mode = 'CHOLESKY';
                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'CG-PRELIM'}

                options.sub_mode = 'PRELIM';
                options.step_alg = 'exact';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-BKT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-EXACT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'exact';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-PRECON-EXACT'}

                options.sub_mode = 'PRECON';
                % diagonal scaling
                options.M = diag(diag(A));
                options.step_alg = 'exact';
                options.beta_alg = 'PR';

                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'NCG-BTK'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'NCG-WOLFE'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'strong_wolfe';
                options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'BFGS-H-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-H-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-B-BKT'}

                options.step_alg = 'backtracking';
                options.update_mode = 'B';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-B-EXACT'}

                options.step_alg = 'exact';
                options.update_mode = 'B';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'DAMPED-BFGS-BKT'}

                options.step_alg = 'backtracking';
                options.update_mode = 'Damping';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'DAMPED-BFGS-EXACT'}

                options.step_alg = 'exact';
                options.update_mode = 'Damping';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'L-BFGS-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'BB'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = bb(problem, options);

            case {'SGD'}

                options.batch_size = 1;
                options.step = 0.1 * options.batch_size;
                %options.step_alg = 'decay';
                options.step_alg = 'fix';

                [w_list{alg_idx}, info_list{alg_idx}] = sgd(problem, options);

            otherwise
                warn_str = [algorithms{alg_idx}, ' is not supported.'];
                warning(warn_str);
                w_list{alg_idx} = '';
                info_list{alg_idx} = '';
        end

    end

    fprintf('\n\n');

    %% plot all
    close all;

    % display iter vs cost/gnorm
    display_graph('iter','cost', algorithms, w_list, info_list);
    display_graph('iter','gnorm', algorithms, w_list, info_list);

    % draw convergence sequence
    w_history = cell(1);
    cost_history = cell(1);
    for alg_idx=1:length(algorithms)
        w_history{alg_idx} = info_list{alg_idx}.w;
        cost_history{alg_idx} = info_list{alg_idx}.cost;
    end
    draw_convergence_sequence(problem, w_opt, algorithms, w_history, cost_history);

    % display classification results
    y_pred_list = cell(length(algorithms),1);
    accuracy_list = cell(length(algorithms),1);
    for alg_idx=1:length(algorithms)
        % predict class
        y_pred_list{alg_idx} = problem.prediction(w_list{alg_idx});
        % calculate accuracy
        accuracy_list{alg_idx} = problem.accuracy(y_pred_list{alg_idx});
        fprintf('Classificaiton accuracy: %s: %.4f\n', algorithms{alg_idx}, accuracy_list{alg_idx});
    end

    % convert logial matrix to class label vector
    [~, y_train] = max(y_train, [], 1);
    [~, y_test] = max(y_test, [], 1);
    if plot_flag
        display_classification_result(problem, algorithms, w_list, y_pred_list, accuracy_list, x_train, y_train, x_test, y_test);
    end

end


function out = sig_prbs(n,m)
% Pseudo-random binary signal (PRBS)
%
%% Syntax
%  out = sig_prbs(n,m)
%
%% Description
% Function generates the pseudo-random binary signal (PRBS) with the uniform
% amplitude , see R. Iserman: Prozessidentifikation, 1974, Springer.
%
% Input:
% * n ... the length of the internal register
% * m ... the number of samples per the width of the narrowest
%         impulse of PRBS
%
% Output:
% * out ... PRBS signal values
%
% See also:
% sig_prs_minmax
%
%%

registry1=0:11;
registry2=[0 2 3 4 5 6 7 8 9 10 11 15 20 40 100];
store=ones(n,1);

out(1:m,1)=store(n);

for i=2:2^n-1
    storex=store(registry1(n))+store(registry2(n));
    for j=0:n-2
        store(n-j)=store(n-j-1);
    end
    store(1)=storex;
    if store(1)==2
        store(1)=0;
    end
    for j=(i-1)*m+1:m*i
        out(j,1)=2*store(n)-1;
    end
end


function pdf = truncated_normal_a_pdf ( x, mu, sigma, a )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_A_PDF evaluates the lower truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%
%    Input, real MU, S, the mean and standard deviation of the
%    parent Normal distribution.
%
%    Input, real A, the lower truncation limit.
%
%    Output, real PDF, the value of the PDF.
%
  alpha = ( a - mu ) / sigma;
  xi = ( x - mu ) / sigma;

  alpha_cdf = normal_01_cdf ( alpha );
  xi_pdf = normal_01_pdf ( xi );

  pdf = xi_pdf / ( 1.0 - alpha_cdf ) / sigma;

  return
end


function [c,s,r] = Givens(x,y)
%
%  computes c,s,r such that
%
%  [c  s] [x] = [r]
%  [-s c] [y]   [0]
%
%  with c*c + s*s = 1;
%
   if (y == 0),
      c = 1; s = 0; r = x;
   else
      if (abs(x) >= abs(y))
         t = y/x; r = sqrt(1 + t*t);
         c = 1/r;
         s = t*c;
         r = x*r;
      else
         t = x/y; r = sqrt(1 + t*t);
         s = 1/r;
         c = t*s;
         r = y*r;
      end
   end


function a = mirt_dctn(a)
%MIRT_DCTN N-D (multidimensional) discrete cosine transform.
%   B = MIRT_DCTN(A) returns the discrete cosine transform of A.
%   The array B is the same size as A and contains the
%   discrete cosine transform coefficients.
%
%   This function works much faster than Matlab standard
%   dct (for 1D case) or dct2 (for 2D case), and also allows the ND input.
%   The function takes advantage of 1) FFT 2) fast permutation through indicies
%   3) persistent precomputation (coefficients and indicies are precomputed during
%   the first run).
%
%   This transform can be inverted using MIRT_IDCTN.
%   Author: Andriy Myronenko, see www.bme.ogi.edu/~myron
%   revised on May 06, 2010
%
%   See also MIRT_IDCT.

persistent siz ww ind isreala;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check input
if (nargin == 0) || isempty(a) ,
    error('Insufficient input');
end

isreala=isreal(a);
ndim=ndims(a);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check for the row vector
transpose=0;
if (ndim==2) && (size(a,1)==1)
    transpose=1; a=a';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check if the variable size has changed and we need to
%%% precompute weights and indicies

precompute=0;
if  ~exist('siz','var'),
    precompute=1;
elseif abs(numel(siz)-ndims(a))>0
    precompute=1;
elseif sum(abs(siz-size(a)),2)>0,
    precompute=1;
elseif isreala~=isreal(a),
    precompute=1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Precompute weights and indicies
if precompute,
    siz=size(a);
    ndim=ndims(a);

    for i=1:ndim,
        n=siz(i);

        ww{i} = 2*exp(((-1i*pi)/(2*n))*(0:n-1)')/sqrt(2*n);
        ww{i}(1) = ww{i}(1) / sqrt(2);
        ind{i}=bsxfun(@plus, [(1:2:n) fliplr(2:2:n)]', 0:n:n*(prod(siz)/n-1));
        if (siz(i)==1), break; end;
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Actual multidimensional DCT. Handle 1D and 2D cases
%%% separately, because .' is much faster than shiftdim.

% check for 1D or 2D cases
if ndim==2
    if (min(siz)==1),
        a=dct(a,ww{1},ind{1});if transpose, a=a'; end;  % 1D case
    else a = dct(dct(a,ww{1},ind{1}).',ww{2},ind{2}).'; % 2D case
    end
else
    % ND case (3D and higher)
    for i=1:ndim
        a=reshape(dct(reshape(a,siz(1),[]),ww{i},ind{i}), siz); % run dct along vectors (1st dimension)
        siz=[siz(2:end) siz(1)];                   % circular shift of size array by 1
        a=shiftdim(a,1);                           % circular shift dimensions by 1
    end
end

function a=dct(a,ww,ind)
%DCT  Discrete cosine transform 1D (operates along first dimension)

isreala=isreal(a);
k=1; if ~isreala, ia = imag(a); a = real(a); k=2; end; % check if complex

% k=1 (if 'a' is real) and 2 (if 'a' is complex)
for i=1:k
     a=a(ind);     % rearange
     a = fft(a);  % ifft
     a = real(bsxfun(@times,  ww, a)); % multiply weights

    % check if the data is not real
    if ~isreala,
        if i==1, ra = a; a = ia; clear ia;  % proceed to imaginary part
        else a = complex(ra,a); end;        % finalize the output
    end

end


function [ vWm, vWc, scalingFactor ] = CalcSigmaPointsWeights( paramAlpha, paramBeta, paramKappa, stateOrder )
% ----------------------------------------------------------------------------------------------- %
% [ vWm, vWc, scalingFactor ] = CalcSigmaPointsWeights( paramAlpha, paramBeta, paramKappa, stateOrder )
%   Calculates the weights for the mean, covariance and scaling factor of
%   the Unscented Transform.
% Input:
%   - paramAlpha    -   Parameter Alpha.
%                       Influences how far the Sigma Points are form the
%                       mean.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, 1].
%   - paramBeta     -   Parameter Beta.
%                       For Gaussian PDF the optimal value is 2.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: NA.
%   - paramKappa    -   Parameter Kappa.
%                       Influences how far the Sigma Points are form the
%                       mean.
%                       Type: 'Single' / 'Double'.
%                       Range: [0, inf).
%   - stateOrder    -   State Vector  Dimension.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
% Output:
%   - vWm           -   Mean Weights Vector.
%                       Weights used for calculation of the mean of the
%                       transformed data.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vWc           -   Covariance Weights Vector.
%                       Weights used for calculation of the covariance of
%                       the transformed data.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - scalingFctr   -   Scaling Factor.
%                       Parameter of the Generalized Unscented Transform
%                       which controls
%                       Equals to 'size(hF(mX(:, 1), 1)'.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
% References
%   1.  Unscented Transform (Wikipedia) - https://en.wikipedia.org/wiki/Unscented_transform.
%   2.  Lecture 5: Unscented Kalman Filter and General Gaussian Filtering (Simo Sarkka).
%   3.  Robot Mapping - Unscented Kalman Filter (Cyrill Stachniss).
% Remarks:
%   1.  This method of parameters is often called Generalized / Scaled
%       Unscented Kalman Filter.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     24/08/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

numSigmaPoints  = (2 * stateOrder) + 1;

vWm = zeros(numSigmaPoints, 1);
vWc = zeros(numSigmaPoints, 1);

paramLambda = ((paramAlpha * paramAlpha) * (stateOrder + paramKappa)) - stateOrder;

vWm(1)      = paramLambda / (stateOrder + paramLambda);
vWm(2:end)  = 1 / (2 * (stateOrder + paramLambda));
vWc(1)      = vWm(1) + (1 - (paramAlpha * paramAlpha) + paramBeta);
vWc(2:end)  = vWm(2:end);

scalingFactor = sqrt(stateOrder + paramLambda);

end


function distance_map_in_meters = distance_in_meters_cityscapes(...
    depth_map_in_meters, camera_parameters_file)
%DISTANCE_IN_METERS_CITYSCAPES  Compute air thickness, i.e. distance of depicted
%object from camera center, expressed in meters, as a dense map with same
%resolution as the image, using a dense depth map and intrinsic camera
%parameters as inputs.

% Retrieve relevant intrinsic camera parameters: focal length and optical
% center, both expressed in pixel coordinates.
[~, f_x, c_x, c_y] = camera_parameters_cityscapes(camera_parameters_file);

% Compute medium (i.e. air) thickness in meters from depth. The derivation of
% the formula in the final lines of code is based on similar triangles.

[height, width] = size(depth_map_in_meters);
[X, Y] = meshgrid(1:width, 1:height);

distance_map_in_meters = depth_map_in_meters .*...
    sqrt((f_x ^ 2 + (X - c_x) .^ 2 + (Y - c_y) .^ 2) / f_x ^ 2);

end


function a = combin ( alpha, beta, n )

%*****************************************************************************80
%
%% COMBIN returns the combinatorial matrix.
%
%  Formula:
%
%    If ( I = J ) then
%      A(I,J) = ALPHA + BETA
%    else
%      A(I,J) = BETA
%
%  Example:
%
%    N = 5, ALPHA = 2, BETA = 3
%
%    5 3 3 3 3
%    3 5 3 3 3
%    3 3 5 3 3
%    3 3 3 5 3
%    3 3 3 3 5
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    det ( A ) = ALPHA**(N-1) * ( ALPHA + N * BETA ).
%
%    LAMBDA(1:N-1) = ALPHA,
%    LAMBDA(N) = ALPHA + N * BETA.
%
%    The eigenvector associated with LAMBDA(N) is (1,1,1,...,1)/sqrt(N).
%
%    The other N-1 eigenvectors are simply any (orthonormal) basis
%    for the space perpendicular to (1,1,1,...,1).
%
%    A is nonsingular if ALPHA /= 0D+00 and ALPHA + N * BETA /= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    Example 3.25,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, New York, 1969, page 53,
%    LC: QA263.G68.
%
%    Donald Knuth,
%    The Art of Computer Programming,
%    Volume 1, Fundamental Algorithms, Second Edition,
%    Addison-Wesley, Reading, Massachusetts, 1973, page 36.
%
%  Parameters:
%
%    Input, real ALPHA, BETA, scalars that define A.
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a(1:n,1:n) = beta;

  for i = 1 :n
    a(i,i) = a(i:i) + alpha;
  end

  return
end


function [J grad] = nnCostFunction(nn_params, ...
                                   input_layer_size, ...
                                   hidden_layer_size, ...
                                   num_labels, ...
                                   X, y, lambda)
%NNCOSTFUNCTION Implements the neural network cost function for a two layer
%neural network which performs classification
%   [J grad] = NNCOSTFUNCTON(nn_params, hidden_layer_size, num_labels, ...
%   X, y, lambda) computes the cost and gradient of the neural network. The
%   parameters for the neural network are "unrolled" into the vector
%   nn_params and need to be converted back into the weight matrices.
%
%   The returned parameter grad should be a "unrolled" vector of the
%   partial derivatives of the neural network.
%

% Reshape nn_params back into the parameters Theta1 and Theta2, the weight matrices
% for our 2 layer neural network
Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
                 hidden_layer_size, (input_layer_size + 1));

Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
                 num_labels, (hidden_layer_size + 1));

% Setup some useful variables

m = size(X, 1);
X = [ones(size(X,1),1), X];

% You need to return the following variables correctly
J = 0;
Theta1_grad = zeros(size(Theta1));
Theta2_grad = zeros(size(Theta2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the code by working through the
%               following parts.
%
% Part 1: Feedforward the neural network and return the cost in the
%         variable J. After implementing Part 1, you can verify that your
%         cost function computation is correct by verifying the cost
%         computed in ex4.m
%
% Part 2: Implement the backpropagation algorithm to compute the gradients
%         Theta1_grad and Theta2_grad. You should return the partial derivatives of
%         the cost function with respect to Theta1 and Theta2 in Theta1_grad and
%         Theta2_grad, respectively. After implementing Part 2, you can check
%         that your implementation is correct by running checkNNGradients
%
%         Note: The vector y passed into the function is a vector of labels
%               containing values from 1..K. You need to map this vector into a
%               binary vector of 1's and 0's to be used with the neural network
%               cost function.
%
%         Hint: We recommend implementing backpropagation using a for-loop
%               over the training examples if you are implementing it for the
%               first time.
%
% Part 3: Implement regularization with the cost function and gradients.
%
%         Hint: You can implement this around the code for
%               backpropagation. That is, you can compute the gradients for
%               the regularization separately and then add them to Theta1_grad
%               and Theta2_grad from Part 2.
%

Z2 = X*Theta1';
A2 = sigmoid(Z2);

A2 = [ones(size(A2,1),1), A2];
Z3 = A2*Theta2';

A3 = sigmoid(Z3);

% Y = zeros(m, max(y(:)));
% Y(y,:)

Y = dummyvar(y);

J = sum(sum(-1.*Y.*log(A3)-(1-Y).*log(1-A3),2))/m;

reg = (sum(sum(Theta1(:,2:end).^2)) + sum(sum(Theta2(:,2:end).^2)))*lambda/(2*m);
J = J+reg;

delta3 = A3 - Y;

Theta2_grad = Theta2_grad + (delta3'*A2)/m;

delta2 = delta3*Theta2(:,2:end).*sigmoidGradient(Z2);

Theta1_grad = Theta1_grad+(delta2'*X)/m;

Theta1_grad(:,2:end) = Theta1_grad(:,2:end) + lambda*Theta1(:,2:end)/m;
Theta2_grad(:,2:end) = Theta2_grad(:,2:end) + lambda*Theta2(:,2:end)/m;
% -------------------------------------------------------------

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

% Unroll gradients
grad = [Theta1_grad(:) ; Theta2_grad(:)];

end


function variance = nakagami_variance ( a, b, c )

%*****************************************************************************80
%
%% NAKAGAMI_VARIANCE returns the variance of the Nakagami PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B
%    0.0 < C
%
%    Output, real VARIANCE, the variance of the PDF.
%
  t1 = gamma ( c + 0.5 );
  t2 = gamma ( c );

  variance = b * b * ( 1.0 - t1 * t1 / ( c * t2 * t2 ) );

  return
end


function z = spadaptdemo
% SPADAPTDEMO   Simple example of dimension-adaptive interpolation
%    A 2D-example for dimension-adaptive multi-linear sparse grid
%    interpolation using the Chebyshev grid and vectorized processing
%    of the model function.
%
%    See also SPINTERP, SPVALS.

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.1
% Date   : September 29, 2003

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

% The Branin function is used here
f = inline(['(5/pi*x-5.1/(4*pi^2)*x.^2+y-6).^2 + 10*(1-1/(8*pi))*' ...
	    ' cos(x)+10']);

% Define objective box
range = [-5 10; 0 15];

% Define problem dimension
d = 2;

% Create full grid for plotting
gs = 33;
[X,Y] = meshgrid(linspace(range(1,1),range(1,2),gs),...
								 linspace(range(2,1),range(2,2),gs));

% Set options: Switch vectorized processing on.
options = spset('Vectorized', 'on','RelTol', 1e-2, ...
								'GridType', 'Chebyshev', ...
								'DimensionAdaptive', 'on', ...
								'DimadaptDegree', 1, ...
                'MinPoints', 10);

% Compute sparse grid weights over range
z = spvals(f, d, range, options);

% Compute inpterpolated values at full grid
ip = spinterp(z, X, Y);

% Plot original function, interpolation, and error
subplot(2,2,1);
mesh(X,Y,f(X,Y));
title('original');
axis tight;

subplot(2,2,2);
mesh(X,Y,ip);
title('interpolated');
axis tight;

subplot(2,2,3);
mesh(X,Y,abs(f(X,Y)-ip));
title('absolute error');
axis tight;

subplot(2,2,4);
plotindices(z);
title('resulting index sets');


function x = fibonacci_lattice_q_nodes ( k )

%*****************************************************************************80
%
%% FIBONACCI_LATTICE_Q_NODES returns Fibonacci lattice nodes in 2D.
%
%  Discussion:
%
%    Because this is a standard lattice rule, it is really only suited
%    for functions which are periodic, of period 1, in both X and Y.
%
%    The number of nodes returned is
%
%      M = fibonacci ( k ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994,
%    ISBN: 0198534728,
%    LC: QA311.S56
%
%  Parameters:
%
%    Input, integer K, the index of the Fibonacci number to be used.
%    K must be at least 3.
%
%    Output, real X(2,M), the nodes.
%
  dim_num = 2;

  m = fibonacci ( k );

  z(1) = 1;
  z(2) = fibonacci ( k - 1 );

  for j = 0 : m - 1
    x(1:dim_num,j+1) = mod ( j * z(1:dim_num) / m, 1.0 );
  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% KNOCK-OUT BARRIER OPTION PRICE COMPARISON (RUN SCRIPT)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Compare Methods For Barrier Options Under Black Scholes Model
%              This script compares accuracy/CPU of the following methods,
%                   Monte Carlo
%                   PROJ
%
% Author:      Justin Kirkby
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);
addpath('../../PROJ/LEVY/RN_CHF')
addpath('../../PROJ/LEVY/Helper_Functions')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
S_0    = 100;   %Initial price
W      = 100;   %Strike            %NOTE: no error handling in place for extreme values of W (increase grid if strike falls outside)
r      = 0.05;  %Interest rate
q      = 0.02;  %dividend yield
T      = 1;     %Time (in years)
call   = 1;     %For call use 1 (else, its a put)
down   = 1;     %down-out or up-out (down=1 => down-and-out)
H      = 90;    %barrier (Knock-Out)
M      = 52;    %number of discrete monitoring points
rebate = 0;     % rebate paid immediately upon passing the barrier (knocking-out)

sigma = 0.15;  % volatility of diffusion

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = 1; params = {}; params.sigmaBSM = sigma;
modelInput = getModelInput(model, T/M, r, q, params);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Reference Price (Using PROJ)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../PROJ/LEVY/Barrier_Options')
L1_ref = 20; N_ref = 2^17;
alpha_ref = getTruncationAlpha(T, L1_ref, modelInput, model);
price_ref = PROJ_Barrier(N_ref, alpha_ref, call, down, S_0, W, H, M, r, q, modelInput.rnCHF, T, rebate);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  PROJ Method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = 2^12;    % grid roughly centered on [c1 - alph, c1 + alph]
L1 = 10;
alpha = getTruncationAlpha(T, L1, modelInput, model);

tic
price_PROJ = PROJ_Barrier(N, alpha, call, down, S_0, W, H, M, r, q, modelInput.rnCHF, T, rebate);
time_PROJ = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Monte Carlo Pricer (Exact Sim, Multi-Step)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NOTE: Variance reduction techniques should be used in practice
addpath('../../Monte_Carlo/')
addpath('../../Monte_Carlo/Barrier')
N_sim = 5*10^5;  % number of paths
mult = 3;   % multiplier for simulation (see below) to reduce bias

tic
M_mult = M*mult;  %time partitioning to reduce bias
Spath = Simulate_Jump_Diffusion_func( N_sim, M_mult + 1, T, S_0, r, q, sigma);

[price_MC, stdErr] = Price_MC_Barrier_Strikes_func(Spath, call, down, H, W, M, mult, rebate, r, T);

price_MC_L = price_MC - 2*stdErr; price_MC_U = price_MC + 2*stdErr;
time_MC = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% COMPARE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n---------------------------------------------\n')
fprintf('Method      |    Price    |    Err   |  CPU \n')
fprintf('---------------------------------------------\n')
fprintf('Reference   | %.8f  |          |       \n', price_ref)
fprintf('---------------------------------------------\n')
fprintf('PROJ        | %.8f  | %.2e | %.4f \n', price_PROJ, abs(price_ref-price_PROJ), time_PROJ)
fprintf('---------------------------------------------\n')
fprintf('MC-Exact    |[%.3f,%.3f]| %.2e | %.4f \n', price_MC_L, price_MC_U, abs(price_ref-price_MC), time_MC)
fprintf('---------------------------------------------\n')


function sample = SampleGaussian(mu, sigma)

% sample from the Gaussian distribution specifed by mean value mu and standard deviation sigma
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

 sample = mu + sigma*randn(1,1);


function X = fast_pca(in_X, K)

in_X = in_X - repmat(mean(in_X),size(in_X,1),1);
[U, S, ~] = rsvd(in_X, K);
K = min(size(S,2),K);
X = U(:,1:K)*diag(sqrt(diag(S(1:K,1:K))));
X = X./repmat(sqrt(sum(X.*X,2)),1,K);
end

function [U,S,V] = rsvd(A,K)
%-------------------------------------------------------------------------------------
% random SVD
% Extremely fast computation of the truncated Singular Value Decomposition, using
% randomized algorithms as described in Halko et al. 'finding structure with randomness
%
% usage :
%
%  input:
%  * A : matrix whose SVD we want
%  * K : number of components to keep
%
%  output:
%  * U,S,V : classical output as the builtin svd matlab function
%-------------------------------------------------------------------------------------
% Antoine Liutkus  (c) Inria 2014

[M,N] = size(A);
P = min(2*K,N);
X = randn(N,P);
Y = A*X;
W1 = orth(Y);
B = W1'*A;
[W2,S,V] = svd(B,'econ');
U = W1*W2;
K=min(K,size(U,2));
U = U(:,1:K);
S = S(1:K,1:K);
V=V(:,1:K);
end


function test_HyperTangents()

xVec = 0:0.001:0.5;
sF=1;
coeff=40;

for i=1:length(xVec)

    x = xVec(i);

    if x < 0.1

        uX_Tar(i) = sF*( tanh(coeff*(0.05-x) ) );

    elseif x <= 0.2

        uX_Tar(i) = sF*( tanh(coeff*(x-0.15) ) );

    elseif x < 0.3

        uX_Tar(i) = sF*( tanh(coeff*(0.25-x) ) );

    elseif x<0.40

        uX_Tar(i) = sF*( tanh(coeff*(x-0.35) ) );

    else
        uX_Tar(i) = sF*( tanh(coeff*(0.45-x) ) );
    end

end

plot(xVec,uX_Tar,'.'); hold on;


function M = euclideancomplexfactory(m, n)
% Returns a manifold struct to optimize over complex m-by-n matrices.
%
% function M = euclideancomplexfactory(m, n)
%
% Returns M, a structure describing the vector space of complex m-by-n
% matrices, as a manifold for Manopt.
%
% The complex plane is here viewed as R^2. The inner product between two
% m-by-n matrices A and B is given by: real(trace(A'*B)). This choice
% guides the proper definition of gradient and Hessian for this geometry.
% This is not the classical Euclidean inner product for complex matrices;
% it is a real inner product.
%
% See also: euclideanfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 7, 2015.
% Contributors:
% Change log:

    if ~exist('n', 'var') || isempty(n)
        n = 1;
    end

    M.name = @() sprintf('Vector space C^(%dx%d)', m, n);

    M.dim = @() 2*m*n;

    M.inner = @(x, d1, d2) real(d1(:)'*d2(:));

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) norm(x-y, 'fro');

    M.typicaldist = @() sqrt(m*n);

    M.proj = @(x, d) d;

    M.egrad2rgrad = @(x, g) g;

    M.ehess2rhess = @(x, eg, eh, d) eh;

    M.tangent = M.proj;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

	M.log = @(x, y) y-x;

    M.hash = @(x) ['z' hashmd5([real(x(:)) ; imag(x(:))])];

    M.rand = @() (randn(m, n) + 1i*randn(m, n))/sqrt(2);

    M.randvec = @randvec;
    function u = randvec(x) %#ok<INUSD>
        u = randn(m, n) + 1i*randn(m, n);
        u = u / norm(u, 'fro');
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(m, n);

    M.transp = @(x1, x2, d) d;

    M.pairmean = @(x1, x2) .5*(x1+x2);

    mn = m*n;
    M.vec = @(x, u_mat) [real(u_mat(:)) ; imag(u_mat(:))];
    M.mat = @(x, u_vec) reshape(u_vec(1:mn), [m, n]) + 1i*reshape(u_vec((mn+1):end), [m, n]);
    M.vecmatareisometries = @() true;

end


function signal = logCompression(signal, a, normalise)
%LOGCOMPRESSION   Log compress an input signal.
%
% DESCRIPTION:
%       logCompression compresses the input signal using the expression
%       signal = log10(1 + a*signal)./log10(1 + a)
%
% USAGE:
%       signal = logCompression(signal, a)
%       signal = logCompression(signal, a, normalise)
%
% INPUTS:
%       signal      - input signal
%       a           - compression factor
%
% OPTIONAL INPUTS
%       normalise   - Boolean controlling whether the maximum of the input
%                     signal is normalised to unity before compression
%                     (default = false). If set to true, the original
%                     magnitude is restored after compression.
%
% OUTPUTS:
%       signal      - log compressed signal
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 24th February 2011
%       last update - 24th February 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check for optional normalise input
if nargin == 2
    normalise = false;
end

% compress signal
if normalise
    mx = max(signal(:));
    signal = mx*(log10(1 + a*signal./mx)./log10(1 + a));
else
    signal = log10(1 + a*signal)./log10(1 + a);
end


function varargout = alldist2(varargin)
% VL_ALLDIST2  Pairwise distances
%  D = VL_ALLDIST2(X,Y) returns the pairwise distance matrix D of the
%  columns of S1 and S2, yielding
%
%    D(i,j) = sum (X(:,i) - Y(:,j)).^2
%
%  VL_ALLDIST2(X) returns the pairwise distance matrix fo the columns of
%  S, yielding
%
%    D(i,j) = sum (X(:,i) - X(:,j)).^2
%
%  VL_ALLDIST2(...,'METRIC') changes the computed distance. Supported
%  values for METRIC are
%
%   METRIC  D(i,j)
%   --------------------------------------------------------
%    LINF   max |X  - Y|
%    L2     sum (X  - Y).^2
%    L1     sum |X  - Y|
%    L0     sum (X ~= Y)
%    CHI2   sum (X  - Y).^2 ./ (X + Y)
%    HELL   sum (X^.5 - Y^.5) .^ 2
%
%  (Notice that the standard definition of chi2 is half of what is
%  computed here).
%
%  VL_ALLDIST2(...,'KERNEL') computes the following 'kernels' K:
%
%   KERNEL  K(i,j)
%   ---------------------------------------------------------
%    KL2    sum X .* Y
%    KL1    sum min (X, Y)
%    KCHI2  2 * sum (X .* Y) ./ (X + Y)
%    KHELL  (X .* Y) .^ 0.5
%
%  The constant are chosen so that D(i,j) = K(i,i) + K(j,j) - 2 K(i,j)
%  where D is the metric corresponding to the kenrel (if the arguments
%  are non-negative vectors). Each kernel can be interpreted as the
%  inner product inducing the corresponding metric in an embedding of
%  the real space into an approrpiate reproducing Kenrel Hilbert
%  space.
%
%  VL_ALLDIST2() supports several storage classes. X and Y must have the
%  same storage class. The sotrage class of D is promoted to reduce
%  the chance of overvlow, but this is not checked.
%
%    X & Y class      D class
%   ---------------------------
%    UINT8            UINT32
%     INT8             INT32
%    UINT16           UINT32
%     INT16            INT32
%    UINT32           UINT32
%     INT32            INT32
%    SINGLE           SINGLE
%    DOUBLE           DOUBLE
%
%  Warning: Both chi2 and kchi2 use integer math when presented with
%  integer data types. This can easily result in zeros where you did
%  not expect them.
%
%  See also: VL_HELP().
[varargout{1:nargout}] = vl_alldist2(varargin{:});


function r83p_print ( n, a, title )

%*****************************************************************************80
%
%% R83P_PRINT prints a R83P matrix.
%
%  Discussion:
%
%    The R83P storage format stores a periodic tridiagonal matrix as
%    a 3 by N array, in which each row corresponds to a diagonal, and
%    column locations are preserved.  The matrix value
%    A(1,N) is stored as the array entry A(3,N), and the matrix value
%    A(N,1) is stored as the array entry A(1,1).
%
%  Example:
%
%    Here is how a R83P matrix of order 5 would be stored:
%
%      A51 A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54 A15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A(3,N), the R83P matrix.
%
%    Input, string TITLE, a title.
%
  r83p_print_some ( n, a, 1, 1, n, n, title );

  return
end


function a = daub10 ( n )

%*****************************************************************************80
%
%% DAUB10 returns the DAUB10 matrix.
%
%  Discussion:
%
%    The DAUB10 matrix is the Daubechies wavelet transformation matrix
%    with 10 coefficients.
%
%    Note that in the reference, the coefficient 0.0775714938400459
%    is given incorrectly, with the "8" misrepresented as a "0".
%
%  Properties:
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gilbert Strang, Truong Nguyen,
%    Wavelets and Filter Banks,
%    Wellesley-Cambridge Press, 1997,
%    ISBN: 0-9614088-7-1,
%    LC: TK7872.F5S79 / QA403.3.S87
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 10, and a multiple of 2.
%
%    Output, real A(N,N), the matrix.
%
  c = [ ...
    0.1601023979741929, ...
    0.6038292697971895, ...
    0.7243085284377726, ...
    0.1384281459013203, ...
   -0.2422948870663823, ...
   -0.0322448695846381, ...
    0.0775714938400459, ...
   -0.0062414902127983, ...
   -0.0125807519990820, ...
    0.0033357252854738 ];

  if ( n < 10 || mod ( n, 2 ) ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DAUB10 - Fatal error!\n' );
    fprintf ( 1, '  N must be at least 10 and a multiple of 2.\n' );
    error ( 'DAUB10 - Fatal error!' );
  end

  a = zeros ( n, n );

  for i = 1 : 2 : n - 1

    a(i,i)                  =    c(1);
    a(i,i+1)                =    c(2);
    a(i,i4_wrap(i+2,1,n))   =    c(3);
    a(i,i4_wrap(i+3,1,n))   =    c(4);
    a(i,i4_wrap(i+4,1,n))   =    c(5);
    a(i,i4_wrap(i+5,1,n))   =    c(6);
    a(i,i4_wrap(i+6,1,n))   =    c(7);
    a(i,i4_wrap(i+7,1,n))   =    c(8);
    a(i,i4_wrap(i+8,1,n))   =    c(9);
    a(i,i4_wrap(i+9,1,n))   =   c(10);

    a(i+1,i)                =   c(10);
    a(i+1,i+1)              =  - c(9);
    a(i+1,i4_wrap(i+2,1,n)) =    c(8);
    a(i+1,i4_wrap(i+3,1,n)) =  - c(7);
    a(i+1,i4_wrap(i+4,1,n)) =    c(6);
    a(i+1,i4_wrap(i+5,1,n)) =  - c(5);
    a(i+1,i4_wrap(i+6,1,n)) =    c(4);
    a(i+1,i4_wrap(i+7,1,n)) =  - c(3);
    a(i+1,i4_wrap(i+8,1,n)) =    c(2);
    a(i+1,i4_wrap(i+9,1,n)) =  - c(1);

  end

  return
end


function order = level_to_order_open ( dim_num, level )

%*****************************************************************************80
%
%% LEVEL_TO_ORDER converts a level to an order for open rules.
%
%  Discussion:
%
%    Sparse grids can naturally be nested.  A natural scheme is to use
%    a series of one-dimensional rules arranged in a series of "levels"
%    whose order roughly doubles with each step.
%
%    The arrangement described here works naturally for the Fejer Type 1,
%    Fejer Type 2, Newton Cotes Open, Newton Cotes Half Open,
%    and Gauss-Patterson rules.  It also can be used, partially, to describe
%    the growth of Gauss-Legendre rules.
%
%    The idea is that we start with LEVEL = 0, ORDER = 1 indicating the single
%    point at the center, and for all values afterwards, we use the relationship
%
%      ORDER = 2**(LEVEL+1) - 1.
%
%    The following table shows how the growth will occur:
%
%    Level    Order
%
%    0          1
%    1          3 =  4 - 1
%    2          7 =  8 - 1
%    3         15 = 16 - 1
%    4         31 = 32 - 1
%    5         63 = 64 - 1
%
%    For the Fejer Type 1, Fejer Type 2, Newton Cotes Open,
%    Newton Cotes Open Half, and Gauss-Patterson rules, the point growth is
%    nested.  If we have ORDER points on a particular LEVEL, the next level
%    includes all these old points, plus ORDER+1 new points, formed in the
%    gaps between successive pairs of old points plus an extra point at each
%    end.
%
%    Level    Order = New + Old
%
%    0          1   =  1  +  0
%    1          3   =  2  +  1
%    2          7   =  4  +  3
%    3         15   =  8  +  7
%    4         31   = 16  + 15
%    5         63   = 32  + 31
%
%    If we use a series of Gauss-Legendre rules, then there is almost no
%    nesting, except that the central point is shared.  If we insist on
%    producing a comparable series of such points, then the "nesting" behavior
%    is as follows:
%
%    Level    Order = New + Old
%
%    0          1   =  1  +  0
%    1          3   =  2  +  1
%    2          7   =  6  +  1
%    3         15   = 14  +  1
%    4         31   = 30  +  1
%    5         63   = 62  +  1
%
%    Moreover, if we consider ALL the points used in such a set of "nested"
%    Gauss-Legendre rules, then we must sum the "NEW" column, and we see that
%    we get roughly twice as many points as for the truly nested rules.
%
%    In this routine, we assume that a vector of levels is given,
%    and the corresponding orders are desired.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL(DIM_NUM), the nesting level.
%
%    Output, integer ORDER(DIM_NUM,1), the order (number of points) of the rule.
%
  order = zeros ( dim_num, 1 );

  for dim = 1 : dim_num

    if ( level(dim) < 0 )
      order(dim,1) = -1;
    elseif ( level(dim) == 0 )
      order(dim,1) = 1;
    else
      order(dim,1) = 2^( level(dim) + 1 ) - 1;
    end

  end

  return
end