%% Machine Learning Online Class
%  Exercise 5 | Regularized Linear Regression and Bias-Variance
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     linearRegCostFunction.m
%     learningCurve.m
%     validationCurve.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% =========== Part 1: Loading and Visualizing Data =============
%  We start the exercise by first loading and visualizing the dataset.
%  The following code will load the dataset into your environment and plot
%  the data.
%

% Load Training Data
fprintf('Loading and Visualizing Data ...\n')

% Load from ex5data1:
% You will have X, y, Xval, yval, Xtest, ytest in your environment
load ('ex5data1.mat');

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

% Plot training data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =========== Part 2: Regularized Linear Regression Cost =============
%  You should now implement the cost function for regularized linear
%  regression.
%

theta = [1 ; 1];
J = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Cost at theta = [1 ; 1]: %f '...
         '\n(this value should be about 303.993192)\n'], J);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =========== Part 3: Regularized Linear Regression Gradient =============
%  You should now implement the gradient for regularized linear
%  regression.
%

theta = [1 ; 1];
[J, grad] = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Gradient at theta = [1 ; 1]:  [%f; %f] '...
         '\n(this value should be about [-15.303016; 598.250744])\n'], ...
         grad(1), grad(2));

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =========== Part 4: Train Linear Regression =============
%  Once you have implemented the cost and gradient correctly, the
%  trainLinearReg function will use your cost function to train
%  regularized linear regression.
%
%  Write Up Note: The data is non-linear, so this will not give a great
%                 fit.
%

%  Train linear regression with lambda = 0
lambda = 0;
[theta] = trainLinearReg([ones(m, 1) X], y, lambda);

%  Plot fit over the data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
hold on;
plot(X, [ones(m, 1) X]*theta, '--', 'LineWidth', 2)
hold off;

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =========== Part 5: Learning Curve for Linear Regression =============
%  Next, you should implement the learningCurve function.
%
%  Write Up Note: Since the model is underfitting the data, we expect to
%                 see a graph with "high bias" -- slide 8 in ML-advice.pdf
%

lambda = 0;
[error_train, error_val] = ...
    learningCurve([ones(m, 1) X], y, ...
                  [ones(size(Xval, 1), 1) Xval], yval, ...
                  lambda);

plot(1:m, error_train, 1:m, error_val);
title('Learning curve for linear regression')
legend('Train', 'Cross Validation')
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 150])

fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =========== Part 6: Feature Mapping for Polynomial Regression =============
%  One solution to this is to use polynomial regression. You should now
%  complete polyFeatures to map each example into its powers
%

p = 8;

% Map X onto Polynomial Features and Normalize
X_poly = polyFeatures(X, p);
[X_poly, mu, sigma] = featureNormalize(X_poly);  % Normalize
X_poly = [ones(m, 1), X_poly];                   % Add Ones

% Map X_poly_test and normalize (using mu and sigma)
X_poly_test = polyFeatures(Xtest, p);
X_poly_test = bsxfun(@minus, X_poly_test, mu);
X_poly_test = bsxfun(@rdivide, X_poly_test, sigma);
X_poly_test = [ones(size(X_poly_test, 1), 1), X_poly_test];         % Add Ones

% Map X_poly_val and normalize (using mu and sigma)
X_poly_val = polyFeatures(Xval, p);
X_poly_val = bsxfun(@minus, X_poly_val, mu);
X_poly_val = bsxfun(@rdivide, X_poly_val, sigma);
X_poly_val = [ones(size(X_poly_val, 1), 1), X_poly_val];           % Add Ones

fprintf('Normalized Training Example 1:\n');
fprintf('  %f  \n', X_poly(1, :));

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% =========== Part 7: Learning Curve for Polynomial Regression =============
%  Now, you will get to experiment with polynomial regression with multiple
%  values of lambda. The code below runs polynomial regression with
%  lambda = 0. You should try running the code with different values of
%  lambda to see how the fit and learning curve change.
%

lambda = 1;
[theta] = trainLinearReg(X_poly, y, lambda);

% Plot training data and fit
figure(1);
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
plotFit(min(X), max(X), mu, sigma, theta, p);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
title (sprintf('Polynomial Regression Fit (lambda = %f)', lambda));

figure(2);
[error_train, error_val] = ...
    learningCurve(X_poly, y, X_poly_val, yval, lambda);
plot(1:m, error_train, 1:m, error_val);

title(sprintf('Polynomial Regression Learning Curve (lambda = %f)', lambda));
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 100])
legend('Train', 'Cross Validation')

fprintf('Polynomial Regression (lambda = %f)\n\n', lambda);
fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =========== Part 8: Validation for Selecting Lambda =============
%  You will now implement validationCurve to test various values of
%  lambda on a validation set. You will then use this to select the
%  "best" lambda value.
%

[lambda_vec, error_train, error_val] = ...
    validationCurve(X_poly, y, X_poly_val, yval);

close all;
plot(lambda_vec, error_train, lambda_vec, error_val);
legend('Train', 'Cross Validation');
xlabel('lambda');
ylabel('Error');

fprintf('lambda\t\tTrain Error\tValidation Error\n');
for i = 1:length(lambda_vec)
	fprintf(' %f\t%f\t%f\n', ...
            lambda_vec(i), error_train(i), error_val(i));
end

fprintf('Program paused. Press enter to continue.\n');
pause;


% function [IDX_SC, Uk, Dk, time_SC] = SC(G,lap_type)
%
% This is an implementation of the classical Spectral CLustering algorithm
% using either:
% - the combinatorial Laplacian if lap_type='combinatorial' (in this case,
% the features are not normalized);
% - or the normalized Laplacian if lap_type='normalized' (in this case,
% the features are normalized).
% See the paper cited below for many references on this algorithm.
%
% Copyright (C) 2016 Nicolas Tremblay, Gilles Puy.
% This file is part of the CSCbox (Compressive Spectral Clustering toolbox)
%
% The CSCbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% The CSCbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
% If you use this toolbox please kindly cite
%     N. Tremblay, G. Puy, R. Gribonval and P. Vandergheynst.
%     Compressive Spectral Clustering.
%     ArXiv e-prints:1602.02018 Feb. 2016.

function [IDX_SC, Uk, Dk, time_SC] = SC(G,lap_type)

normBOOL=strcmp(lap_type,'normalized');
G = gsp_create_laplacian(G,lap_type);

% eigs
if isfield(G, 'G.Dk'), % if it was already calculated, do not do it again
    Dk=G.Dk;
    Uk=G.Uk;
    time_SC.eigs=G.time_SC_eigs;
else % run eigs
    tic;
    opts.isreal=1;opts.issym=1;opts.maxit=10000;
    [Uk,Dk]=eigs(G.L,G.k,'SA',opts);Dk=diag(Dk);
    time_SC.eigs=toc;
end

if normBOOL % kmeans on Yk
    tic;
    Yk=Uk./repmat(sqrt(sum(Uk.^2,2)),1,G.k);
    IDX_SC = kmeans(Yk, G.k,'Replicates',20);
    time_SC.kmeans=toc;
else % kmeans on Uk
    tic;
    IDX_SC = kmeans(Uk, G.k,'Replicates',20);
    time_SC.kmeans=toc;
end

time_SC.total=time_SC.eigs+time_SC.kmeans;


% StackExchange Signal Processing Q75231
% https://dsp.stackexchange.com/questions/75231
% Image Denoising Using Total Variation (TV) Minimization with ADMM
% References:
%   1.
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes
% - 1.0.000     18/05/2021
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

CONV_SHAPE_FULL     = 1;
CONV_SHAPE_SAME     = 2;
CONV_SHAPE_VALID    = 3;

%% Simulation Parameters

imageFileName = 'Lena256.png';

paramLambda = 0.025; %<! TV Norm

numIterations   = 250;
noiseStd        = 10 / 255;

%% Generate Data

mI = im2double(imread(imageFileName));

numRows = size(mI, 1);
numCols = size(mI, 2);

mY = mI + (noiseStd * randn(numRows, numCols));
vY = mY(:);

vXInit  = zeros(numRows * numCols, 1);
% Generate the Diff Operator (2D Gradient) by Finite Differences
mD      = CreateGradientOperator(numRows, numCols);

% Objective Function
hObjFun = @(vX) (0.5 * sum( (vX - vY) .^ 2)) + (paramLambda * sum(abs(mD * vX)));

% Analysis
solverIdx       = 0;
cMethodString   = {};

mObjFunValMse   = zeros(numIterations, 1);
mSolMse         = zeros(numIterations, 1);

%% Display Data

figureIdx = figureIdx + 1;

hFigure = figure('Position', [100, 100, 542, 642]);
hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [010, 326, 256, 256]);
hImgObj = image(repmat(mI, [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Reference Image']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [276, 326, 256, 256]);
hImgObj = image(repmat(mY, [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Input Image (Noisy)']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

%% Solution by CVX

solverString = 'CVX';

% cvx_solver('SDPT3'); %<! Default, Keep numRows low
% cvx_solver('SeDuMi');
cvx_solver('Mosek'); %<! Can handle numRows > 500, Very Good!
% cvx_solver('Gurobi');

hRunTime = tic();

cvx_begin('quiet')
    % cvx_precision('best');
    variable vX(numRows * numCols);
    minimize( (0.5 * sum_square(vX - vY)) + (paramLambda * norm(mD * vX, 1)) );
cvx_end

runTime = toc(hRunTime);

DisplayRunSummary(solverString, hObjFun, vX, runTime, cvx_status);

sCvxSol.vXCvx     = vX(:);
sCvxSol.cvxOptVal = hObjFun(vX);

%% Display Result

hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [010, 010, 256, 256]);
hImgObj = image(repmat(reshape(vX, numRows, numCols), [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Denoised Image - CVX']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

%% Solution by ADMM
%{
Solving:

$$ \arg \min_{ x \in \mathbb{R}^{n} } \frac{1}{2} {\left\| x - y \right|}_{2}^{2} + \lambda {\left\| D x \right\|}_{1} $$
%}

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['ADMM'];

hRunTime = tic();

[vX, mX] = SolveProxTvAdmm(vXInit, vY, mD, paramLambda, 'numIterations', numIterations);

runTime = toc(hRunTime);

DisplayRunSummary(cLegendString{solverIdx}, hObjFun, vX, runTime);

[mObjFunValMse, mSolMse] = UpdateAnalysisData(mObjFunValMse, mSolMse, mX, hObjFun, sCvxSol, solverIdx);

%% Display Results

hAxes   = axes(hFigure, 'Units', 'pixels', 'Position', [276, 010, 256, 256]);
hImgObj = image(repmat(reshape(vX, numRows, numCols), [1, 1, 3]));
set(get(hAxes, 'Title'), 'String', {['Denoised Image - ADMM']}, ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTick', [], 'XTickLabel', []);
set(hAxes, 'YTick', [], 'YTickLabel', []);

if(generateFigures == ON)
    % saveas(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

figureIdx = figureIdx + 1;

hFigure = DisplayComparisonSummary(numIterations, mObjFunValMse, mSolMse, cLegendString, figPosLarge, lineWidthNormal, fontSizeTitle, fontSizeAxis);

if(generateFigures == ON)
    % saveas(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function f = laplacian(f)
%LAPLACIAN   Scalar laplacian of a SPHEREFUN.
%   L = LAPLACIAN(F) computes the spherical Laplacian of the SPHEREFUN F.
%
% This is gvien, in intrinsic spherical coordinates as
%
% laplacian(F) = d_{theta} d_{theta} F + tan(theta) d_{theta} F +
%                1/sin(theta) + d_{lambda} d_{lambda} F
%
% where 0 <= theta <= pi is the co-latitude variable and
% -pi <= lambda <= pi  is the longitude variable.
%
% See also GRADIENT, DIVERGENCE.

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

% We implement the computation of the Laplacian using the spherefun/diff
% command.  This function computes tangential derivatives in the x, y, and
% z directions, so that the Laplacian can be implemented as we usually
% think of it in 3D, i.e.
% laplacian(f) = f_xx + f_yy + f_zz.

% Compute the derivatives
fxx = diff(f, 1, 2);
fyy = diff(f, 2, 2);
fzz = diff(f, 3, 2);

% Determine which derivative has the most coefficients in its
% representation for the columns and rows.
[mxx,nxx] = length(fxx);
[myy,nyy] = length(fyy);
[mzz,nzz] = length(fzz);

m = max([mxx myy mzz]);
n = max([nxx nyy nzz]);

% Ensure m and n are even:
m = m + mod(m, 2);
n = n + mod(n, 2);

% Construct a spherefun by sampling the derivatives on the same size grid.
f = spherefun(sample(fxx, m, n/2) + sample(fyy, m, n/2) + ...
               sample(fzz, m, n/2));

end


function G = rand_inverse_wishart(m, v, H_inv_upper_chol)

% function G = rand_inverse_wishart(m, v, H_inv_upper_chol)
% rand_inverse_wishart  Pseudo random matrices drawn from an
% inverse Wishart distribution
% G = rand_inverse_wishart(m, v, H_inv_upper_chol)
% Returns an m-by-m matrix drawn from an inverse-Wishart distribution.
%
% INPUTS:
%     m:          dimension of G and H_inv_upper_chol.
%     v:          degrees of freedom, greater or equal than m.
%     H_inv_chol: upper cholesky decomposition of the inverse of the
%                 matrix parameter.
%                 The upper cholesky of the inverse is requested here
%                 in order to avoid to recompute it at every random draw.
%                 H_inv_upper_chol = chol(inv(H))
% OUTPUTS:
%     G:          G ~ IW(m, v, H) where H = inv(H_inv_upper_chol'*H_inv_upper_chol)
%                 or, equivalently, using the correspondence between Wishart and
%                 inverse-Wishart: inv(G) ~ W(m, v, S) where
%                 S = H_inv_upper_chol'*H_inv_upper_chol = inv(H)
%
% SPECIAL REQUIREMENT
%     none
%

% Copyright (C) 2003-2009 Dynare Team
%
% This file is part of Dynare.
%
% Dynare is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% Dynare 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 Dynare.  If not, see <http://www.gnu.org/licenses/>.

X = randn(v, m) * H_inv_upper_chol;

% At this point, X'*X is Wishart distributed
% G = inv(X'*X);

% Rather compute inv(X'*X) using the SVD
[U,S,V] = svd(X, 0);
SSi = 1 ./ (diag(S) .^ 2);
G = (V .* repmat(SSi', m, 1)) * V';


function [ p, l, u ] = plu_plu ( n, pivot )

%*****************************************************************************80
%
%% PLU_PLU returns the PLU factors of the PLU matrix.
%
%  Example:
%
%    Input:
%
%      N = 5
%      PIVOT = ( 1, 3, 3, 5, 5 )
%
%    Output:
%
%      P:
%
%          1             0            0             0            0
%          0             0            1             0            0
%          0             1            0             0            0
%          0             0            0             0            1
%          0             0            0             1            0
%
%      L:
%
%         1              0            0             0            0
%         0.25           1            0             0            0
%         0.125          0.375        1             0            0
%         0.0625         0.1875       0.3125        1            0
%         0.03125        0.09375      0.15625       0.21875      1
%
%      U:
%
%        11             12           13            14           15
%         0             22           23            24           25
%         0              0           33            34           35
%         0              0            0            44           45
%         0              0            0             0           55
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer PIVOT(N), the list of pivot rows.  PIVOT(I)
%    must be a value between I and N, reflecting the choice of
%    pivot row on the I-th step.  For no pivoting, set PIVOT(I) = I.
%
%    Output, real P(N,N), L(N,N), U(N,N), the P, L and U factors
%    of A, as defined by Gaussian elimination with partial pivoting.
%    P is a permutation matrix, L is unit lower triangular, and U
%    is upper trianguler.
%

%
%  Check that the pivot vector is legal.
%
  for i = 1 : n

    if ( pivot(i) < i )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PLU_PLU - Fatal error!\n' );
      fprintf ( 1, '  PIVOT(%d) = %d\n', i, pivot(i) );
      fprintf ( 1, '  but PIVOT(I) must be no less than I!\n' );
      error ( 'PLU_PLU - Fatal error!' )
    elseif ( n < pivot(i) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PLU_PLU - Fatal error!\n' );
      fprintf ( 1, '  PIVOT(%d) = %d\n', i, pivot(i) );
      fprintf ( 1, '  but PIVOT(I) must be no greater than N\n' );
      fprintf ( 1, '  and N = %d\n', n );
      error ( 'PLU_PLU - Fatal error!' )
    end

  end
%
%  Compute U.
%
  u = zeros ( n, n );
  for i = 1 : n
    for j = 1 : n

      if ( i <= j )
        u(i,j) = 10 * i + j;
      else
        u(i,j) = 0.0;
      end

    end
  end
%
%  Compute L.
%
  l = zeros ( n, n );
  for i = 1 : n
    for j = 1 : n

      if ( i < j )
        l(i,j) = 0.0;
      elseif ( j == i )
        l(i,j) = 1.0;
      else
        l(i,j) = ( 2 * j - 1 ) / 2^i;
      end

    end
  end
%
%  Compute P.
%
  p = zeros ( n, n );
  for i = 1 : n
    for j = 1 : n
      if ( i == j )
        p(i,j) = 1.0;
      else
        p(i,j) = 0.0;
      end
    end
  end
%
%  Apply the pivot permutations, in reverse order.
%
  for i = n : -1 : 1

    if ( pivot(i) ~= i )
      for j = 1 : n
        temp          = p(i,j);
        p(i,j)        = p(pivot(i),j);
        p(pivot(i),j) = temp;
      end
    end

  end

  return
end


function [lng] = spm_lg_gamma(p,b)
% Log of generalised gamma function
% FORMAT [lng] = spm_lg_gamma(p,b)
%
% p       - dimension parameter
% b       - degrees of freedom type parameter
%__________________________________________________________________________
%
% References:
% * Bayesian Inference in Statistical Analysis, Box & Tiao, 1992, p. 427.
% * Aspects of Multivariate Statistical Theory, R.J. Muirhead, p. 62.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_lg_gamma.m 2784 2009-02-24 19:11:20Z guillaume $

if b <= (p-1)/2
   warning('Parameter out of range');
   lng = NaN;
   return
end

lng = (p*(p-1)/2) * gammaln(0.5);
for alpha = 1:p
  lng = lng + gammaln(b+0.5*(alpha-1));
end


% Author: Jai Juneja
% Date: 12/02/2013
%
% Function transforms a point p_r from robot's local frame to the global
% frame.
%
% Inputs:
%   r = [x y alpha]'	:   Robot frame
%   p_r                 :	Point in robot's frame (2*n matrix for n points)
%
% Outputs:
%   p_g                 :   Point in global frame (2*n matrix)
%   Optional:
%   PG_r                :   Jacobian of p_g wrt. robot frame
%   PG_pr               :   Jacobian of p_g wrt. p_r
function [p_g, PG_r, PG_pr] = transToGlobal(r, p_r)

    r_pos = r(1:2);     % Robot's position
    r_ang = r(3);       % Robot's orientation

    % Transformation matrix:
    R = [cos(r_ang), -sin(r_ang) ; ...
        sin(r_ang), cos(r_ang)];

    % Hence point p_r tranformed to global co-ordinate frame is:
    p_g = R * p_r;
    p_g = [p_g(1, :) + r_pos(1); p_g(2, :) + r_pos(2)];

    if nargout > 1      % Compute Jacobians
        pr_x = p_r(1); pr_y = p_r(2);

        PG_r = [...
            1,  0,  - pr_y*cos(r_ang) - pr_x*sin(r_ang); ...
            0,  1,  pr_x*cos(r_ang) - pr_y*sin(r_ang)];

        PG_pr = R;
    end
end


% SOLVE_LINEAR_ELASTICITY: Solve a linear elasticity problem on a NURBS domain.
%                          For a planar domain it is the plane strain model.
%
% The function solves the linear elasticity problem
%
%      - div (sigma(u)) = f    in Omega = F((0,1)^n)
%      sigma(u) \cdot n = g    on Gamma_N
%                     u = h    on Gamma_D
%
% with   sigma(u) = mu*(grad(u) + grad(u)^t) + lambda*div(u)*I.
%
%   u:          displacement vector
%   sigma:      Cauchy stress tensor
%   lambda, mu: Lame' parameters
%   I:          identity tensor
%
% USAGE:
%
%  [geometry, msh, space, u] = solve_linear_elasticity (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - nmnn_sides:   sides with Neumann boundary condition (may be empty)
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - press_sides:  sides with pressure boundary condition (may be empty)
%    - symm_sides:   sides with symmetry boundary condition (may be empty)
%    - lambda_lame:  first Lame' parameter
%    - mu_lame:      second Lame' parameter
%    - f:            source term
%    - h:            function for Dirichlet boundary condition
%    - g:            function for Neumann condition (if nmnn_sides is not empty)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree of the spline functions.
%    - regularity: continuity of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry: geometry structure (see geo_load)
%  msh:      mesh object that defines the quadrature rule (see msh_cartesian)
%  space:    space object that defines the discrete basis functions (see sp_vector)
%  u:        the computed degrees of freedom
%
% See also EX_LIN_ELAST_HORSESHOE for an example.
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011, 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [geometry, msh, sp, u] = ...
              solve_linear_elasticity (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

% Construct geometry structure
geometry = geo_load (geo_name);
degelev  = max (degree - (geometry.nurbs.order-1), 0);
nurbs    = nrbdegelev (geometry.nurbs, degelev);
[rknots, zeta, nknots] = kntrefine (nurbs.knots, nsub-1, nurbs.order-1, regularity);

nurbs    = nrbkntins (nurbs, nknots);
geometry = geo_load (nurbs);

% Construct msh structure
rule     = msh_gauss_nodes (nquad);
[qn, qw] = msh_set_quad_nodes (geometry.nurbs.knots, rule);
msh      = msh_cartesian (geometry.nurbs.knots, qn, qw, geometry);

% Construct space structure
space_scalar = sp_nurbs (nurbs, msh);
scalar_spaces = repmat ({space_scalar}, 1, msh.rdim);
sp = sp_vector (scalar_spaces, msh);
clear space_scalar scalar_spaces

% Assemble the matrices
mat    = op_su_ev_tp (sp, sp, msh, lambda_lame, mu_lame);
rhs    = op_f_v_tp (sp, msh, f);

% Apply Neumann boundary conditions
for iside = nmnn_sides
% Restrict the function handle to the specified side, in any dimension, gside = @(x,y) g(x,y,iside)
  gside = @(varargin) g(varargin{:},iside);
  dofs = sp.boundary(iside).dofs;
  rhs(dofs) = rhs(dofs) + op_f_v_tp (sp.boundary(iside), msh.boundary(iside), gside);
end

% Apply pressure conditions
for iside = press_sides
  msh_side = msh_eval_boundary_side (msh, iside);
  sp_side  = sp_eval_boundary_side (sp, msh_side);

  x = cell (msh_side.rdim, 1);
  for idim = 1:msh_side.rdim
    x{idim} = reshape (msh_side.geo_map(idim,:,:), msh_side.nqn, msh_side.nel);
  end
  pval = reshape (p (x{:}, iside), msh_side.nqn, msh_side.nel);

  rhs(sp_side.dofs) = rhs(sp_side.dofs) - op_pn_v (sp_side, msh_side, pval);
end

% Apply symmetry conditions
symm_dofs = [];
for iside = symm_sides
  if (~strcmpi (sp.transform, 'grad-preserving'))
    error ('The symmetry condition is only implemented for spaces with grad-preserving transform')
  end
  msh_side = msh_eval_boundary_side (msh, iside);
  for idim = 1:msh.rdim
    normal_comp(idim,:) = reshape (msh_side.normal(idim,:,:), 1, msh_side.nqn*msh_side.nel);
  end

  parallel_to_axes = false;
  for ind = 1:msh.rdim
    ind2 = setdiff (1:msh.rdim, ind);
    if (all (all (abs (normal_comp(ind2,:)) < 1e-10)))
      symm_dofs = union (symm_dofs, sp.boundary(iside).dofs(sp.boundary(iside).comp_dofs{ind}));
      parallel_to_axes = true;
      break
    end
  end
  if (~parallel_to_axes)
    error ('solve_linear_elasticity: We have only implemented the symmetry condition for boundaries parallel to the axes')
  end

end

% Apply Dirichlet boundary conditions
u = zeros (sp.ndof, 1);
[u_drchlt, drchlt_dofs] = sp_drchlt_l2_proj (sp, msh, h, drchlt_sides);
u(drchlt_dofs) = u_drchlt;

int_dofs = setdiff (1:sp.ndof, [drchlt_dofs, symm_dofs]);
rhs(int_dofs) = rhs(int_dofs) - mat (int_dofs, drchlt_dofs) * u_drchlt;

% Solve the linear system
u(int_dofs) = mat(int_dofs, int_dofs) \ rhs(int_dofs);

end


function [xmin, fmin] = golden(npd, f, ax, bx, cx, tol, varargin)
%
%GOLDEN   Minimize function of one variable using golden section search
%
%   [xmin, fmin] = golden(npd, f, ax, bx, cx, tol) computes a local minimum
%   of f. xmin is the computed local minimizer of f and fmin is
%   f(xmin). xmin is computed to an relative accuracy of TOL.
%
%   The parameters ax, bx and cx must satisfy the following conditions:
%   ax < bx < cx, f(bx) < f(ax) and f(bx) < f(cx).
%
%   xmin satisfies ax < xmin < cx. golden is guaranteed to succeed if f
%   is continuous between ax and cx
%
%   Roman Geus, ETH Zuerich, 9.12.97
%
%   ATI -- added "npd" argument & made private to KDE class
C = (3-sqrt(5))/2;
R = 1-C;

x0 = ax;
x3 = cx;
if (abs(cx-bx) > abs(bx-ax)),
  x1 = bx;
  x2 = bx + C*(cx-bx);
else
  x2 = bx;
  x1 = bx - C*(bx-ax);
end
f1 = feval(f,x1,npd,varargin{:});
f2 = feval(f,x2,npd,varargin{:});

k = 1;
while abs(x3-x0) > tol*(abs(x1)+abs(x2)),
%  fprintf(1,'k=%4d, |a-b|=%e\n', k, abs(x3-x0));
  if f2 < f1,
    x0 = x1;
    x1 = x2;
    x2 = R*x1 + C*x3;   % x2 = x1+c*(x3-x1)
    f1 = f2;
    f2 = feval(f,x2,npd,varargin{:});
  else
    x3 = x2;
    x2 = x1;
    x1 = R*x2 + C*x0;   % x1 = x2+c*(x0-x2)
    f2 = f1;
    f1 = feval(f,x1,npd,varargin{:});
  end
  k = k+1;

%  [x0,x1,x2,x3,f1,f2]
end

if f1 < f2,
  xmin = x1;
  fmin = f1;
else
  xmin = x2;
  fmin = f2;
end


%%
% Simple example of gradient flow in metric space.

rep = '../results/gradflow-metric/';
[~,~] = mkdir(rep);

f = @(x,y)x.^2 + y.^2;
Gx = @(x,y)2*x;
Gy = @(x,y)2*y;

% background image
N = 256;
tx = linspace(-.05,1,N);
ty = linspace(-.08,.5,N);
[Y,X] = meshgrid(ty,tx);
F = f(X,Y);
clf; hold on;
imagesc(tx,ty,F');
contour(tx,ty,F', 15, 'k');

% for the implicit stepping
N1 = 2048;
tx1 = linspace(min(tx),max(tx),N1);
ty1 = linspace(min(ty),max(ty),N1);
[Y1,X1] = meshgrid(ty1,tx1);

plist = [1 1];
plist = [1 1.3 1.5 1.8 2 3 5 10 Inf];

gmode = 'explicit';
gmode = 'implicit';

tau = .25; niter = 70;

lgd = {};
for ip=1:length(plist)
    p=plist(ip);

    % conjugate exponent
    q = conjexp(p);
    ri = @(r,p)sign(r).*abs(r).^(1/(p-1));

    grad = @(gx,gy)ri([gx, gy],p);
    if p==1
        grad = @(gx,gy)[gx.*(gx==max(gx,gy)), gy.*(gy==max(gx,gy))];
    elseif p==Inf
        grad = @(gx,gy)[sign(gx),sign(gy)];
    end
    grad = @(gx,gy)grad(gx,gy)*norm([gx,gy],q)/norm(grad(gx,gy),q);

    % initial point
    x = .95; y = .45;
    U = []; V = [];
    % do the descent
    for i=1:niter
        U(end+1) = x; V(end+1) = y;
        switch gmode
            case 'explicit'
                g = grad(Gx(x,y), Gy(x,y));
                x = x - tau*g(1);
                y = y - tau*g(2);
            case 'implicit'
                E = ( abs(X1-x).^p + abs(Y1-y).^p ).^(2/p) + tau * ( X1.^2 + Y1.^2 );
                if p==Inf
                    E = max(abs(X1-x),abs(Y1-y)).^2 + tau * ( X1.^2 + Y1.^2 );
                end
                [tmp,i] = min(E(:));
                x = X1(i); y = Y1(i);
        end
    end

    lgd{end+1} = ['p=' num2str(p)];
    c = (ip-1)/(length(plist)-1);
    plot(U,V, '.-', 'Color', [c 0 1-c], 'LineWidth', 2, 'MarkerSize', 23);
    colormap parula(256);
    axis image; axis off;
    axis([min(tx) max(tx) min(ty) max(ty)]);
    drawnow;

end

saveas(gcf, [rep 'gradflow-' gmode '.eps'], 'epsc');


function [wc,w0,a0,ak,bk,c0,ck]=get_harmonics(y,pas)
%[wc,w0,a0,ak,bk,c0,ck]=get_harmonics(y,pas)
% given a signal x(t),y=x(t)
% the function get_harmoniques returns the pulse w0(rd/s) of the signal y(t)
% the coefficients ak (a1,a2,...)  and bk (b1,b2,...)  of the trigonomical
% representation  of the  signal y(t):
% y(t)= a0/2+ a1.cos(w0t)+b1.sin(w0t)+a2.cos(2w0t) +b2*sin(t*w0)+...
% it also returns the coefficients c0 and ck of the complex representation
% y(t)= ...+c(-1).exp(-jw0t)+c(0)+c(1).exp(jw0t)+c(2).exp(2jw0t)+...
% wc is the interval pulse;
% to plot the amplitude spectrum: stem(wc,abs(ck))
%[wc,w0,a0,ak,bk,c0,ck]=get_harmoniques(y,pas)
% be a signal x(t).
% For more theorical details look at www.azzimalik.com
N=length(y);
Y=fft(y);
if size(Y,2)==1;
    Y=Y';
end
w=2*(0:(N-1))*pi/(N*pas);
w0=w(2);
nc2=fix(N/2);
if nc2~=N/2;
    nc3=nc2+1;
    wc=[fliplr(-w(2:nc2+1)) w(1:nc2+1)];
    Yc=[Y(nc2+2:N) Y(1:nc2+1)];Yc=Yc/N;
else
    nc3=nc2;
    wc=[fliplr(-w(2:nc2+1)) w(1:nc2+1)];
    Yc=[Y(nc2+1:N) Y(1:nc2+1)];Yc=Yc/N;
end
%-------------------------------------------------------------
N1=length(Yc);
for l=2:nc2+1
    ak(l-1)=real(Y(l)+Y(N-l+2))/N;
    bk(l-1)=-imag(Y(l)-Y(N-l+2))/N;
    ckp(l-1)=(ak(l-1)-j*bk(l-1))/2;
    ckm(l-1)=(ak(l-1)+j*bk(l-1))/2;
end
a0=2*Y(1)/N;
c0=a0/2;ck=[fliplr(ckm) c0 ckp];


function h = p31_h ( n, x )

%*****************************************************************************80
%
%% P31_H evaluates the Hessian for problem 31.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  m = 5;
  a = [ 4.0, 4.0, 4.0, 4.0; ...
        1.0, 1.0, 1.0, 1.0; ...
        8.0, 8.0, 8.0, 8.0; ...
        6.0, 6.0, 6.0, 6.0; ...
        3.0, 7.0, 3.0, 7.0 ]';

  c = [ 0.1, 0.2, 0.2, 0.4, 0.6 ]';

  for ii = 1 : n
    for jj = 1 : n
      for j = 1 : m
        d = c(j) + sum ( ( x(1:n) - a(1:n,j) ).^2 );
        h(ii,jj) = h(ii,jj) - 8.0 * ( x(ii) - a(ii,j) ) * ( x(jj) - a(jj,j) ) / d^3;
        if ( ii == jj )
          h(ii,jj) = h(ii,jj) + 2.0 / d^2;
        end
      end
    end
  end

  return
end


function phi = basis_brick8 ( n, p )

%*****************************************************************************80
%
%% BASIS_BRICK8: BRICK8 basis functions at natural coordinates.
%
%  Discussion:
%
%      8------7        t  s
%     /|     /|        | /
%    5------6 |        |/
%    | |    | |        0-------r
%    | 4----|-3
%    |/     |/
%    1------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(3,N), natural coordinates of evaluation
%    points.
%
%    Output, real PHI(8,N), the basis function values.
%
  phi(1,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(2,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(3,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(4,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 - p(3,1:n) );
  phi(5,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) );
  phi(6,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 - p(2,1:n) ) .* ( 1.0 + p(3,1:n) );
  phi(7,1:n) =  ( 1.0 + p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) );
  phi(8,1:n) =  ( 1.0 - p(1,1:n) ) .* ( 1.0 + p(2,1:n) ) .* ( 1.0 + p(3,1:n) );

  phi(1:8,1:n) = phi(1:8,1:n) / 8.0;

  return
end


function [a,b,alpha,p,chiopt,Cab,Calphap]=...
    wtls_line(xin,yin,uxin,uyin,guck);
% function [a,b,alpha,p,chiopt,Cab,Calphap]=...
%   wtls_line(xin,yin,uxin,uyin,guck);
%
% weighted total least squares (wtls) fit of a straigth line
% to a set of points with uncertainties in both coordinates
%
% input:        xin     abscissa vector
%               yin     ordinate vector
%               uxin    (standard) uncertainties of xin, same size as xin
%               uyin    (standard) uncertainties of yin, same size as yin
%               guck    flag, if >0, chisquare is plotted over whole
%                       alpha-interval, optimum alpha is shown as red +
%
% output:       a, b    usual straight line parameters
%                       y=a*x+b
%               alpha,p more stable parametrisation
%                       y*cos(alpha)-x*sin(alpha)-p=0
%                       alpha: slope angle in radians
%                       p: distance of straight line from (0,0)
%                       conversion: a=tan(alpha),b=p/cos(alpha)
%               chiopt  minimum chisquare found
%               Cab     covariances, [var(a),var(b),cov(a,b)]
%               Calphap covariances, [var(alpha),var(p),cov(alpha,p)]
%
% algorithm:    M. Krystek & M. Anton
%               Physikalisch-Technische Bundesanstalt Braunschweig, Germany
%               Meas. Sci. Tech. 18 (2007), pp3438-3442
%
% tested for Matlab 6 and Matlab 7
% testdata: script file pearson_york_testdata.m
%
% 2007-03-08

tol=1e-6; %"tolerance" parameter of fnimbnd, see there

global ux uy x y
% inputs
if nargin<5,guck=0;end
% force column vectors
x=xin(:);
y=yin(:);
ux=uxin(:);
uy=uyin(:);
% "initial guess":
p0=polyfit(x,y,1);
alpha0=atan(p0(1));
% one-dimensional search, use p=p^
[alphaopt,chiopt,exitflag] = ...
    fminbnd(@chialpha,alpha0-pi/2,alpha0+pi/2,optimset('TolX',tol));
% get optimum p from alphaopt
alpha=alphaopt;
uk2=ux.^2*sin(alpha)^2+uy.^2*cos(alpha)^2;
u2=1./mean(1./uk2);
w=u2./uk2;
xbar=mean(w.*x);
ybar=mean(w.*y);
p=ybar*cos(alpha)-xbar*sin(alpha);
% convert to a, b parameters y=a*x+b
a=sin(alpha)/cos(alpha);
b=p/cos(alpha);
% --- uncertainty calculation, covariance matrix = 2*inv(Hessian(chi2)) ---
n=length(x);
vk=y.*cos(alpha)-x*sin(alpha)-p;
vka=-y*sin(alpha)-x*cos(alpha);
vkaa=-vk-p;
fk=vk.*vk;
fka=2*vk.*vka;
fkaa=2*(vka.^2+vk.*vkaa);
gk=uk2;
gka=2*sin(alpha)*cos(alpha)*(ux.^2-uy.^2);
gkaa=2*(ux.^2-uy.^2)*(cos(alpha)^2-sin(alpha)^2);
Hpp=2*n/u2;
Halphap=-2*sum((vka.*gk-gka.*vk)./gk.^2);
Halphaalpha=...
    sum(fkaa./gk-2*fka.*gka./gk.^2+2*gka.^2.*fk./gk.^3-gkaa.*fk./gk.^2);
NN=2/(Hpp*Halphaalpha-Halphap^2);
var_p=NN*Halphaalpha;
var_alpha=NN*Hpp;
cov_alphap=-NN*Halphap;
Calphap=[var_alpha,var_p,cov_alphap];
% ------ convert to a & b covariance matrix, following DIN 1319 (4)
var_a=var_alpha/cos(alpha)^4;
var_b=(var_alpha*p*p*sin(alpha)^2+var_p*cos(alpha)^2+...
    2*cov_alphap*p*sin(alpha)*cos(alpha))/cos(alpha)^4;
cov_ab=(var_alpha*p*sin(alpha)+cov_alphap*cos(alpha))/cos(alpha)^4;
Cab=[var_a,var_b,cov_ab];
% ------ end of uncertainty calculation -----------------------------------
%
%-------------------- plotting section ------------------------------------
if guck~=0
    alphatest=linspace(alpha-pi/2,alpha+pi/2,1000);
    for k=1:length(alphatest),chitest(k)=chialpha(alphatest(k));end
    plot(alphatest,chitest,alphaopt,chiopt,'+r')
    xlabel('alpha')
    ylabel('\chi^2')
    grid on
    disp([mfilename,' :: paused, hit any key to continue'])
    pause
end
% ------------------ end of plotting section ------------------------------
return
% ------------------ subfunction ------------------------------------------
function chi=chialpha(alpha)
    global ux uy x y
    uk2=ux.^2*sin(alpha)^2+uy.^2*cos(alpha)^2;
    u2=1./mean(1./uk2);
    w=u2./uk2;
    xbar=mean(w.*x);
    ybar=mean(w.*y);
    p=ybar*cos(alpha)-xbar*sin(alpha);
    chi=sum((y*cos(alpha)-x*sin(alpha)-p).^2./uk2);
return
% ----------- end of subfunction ------------------------------------------


function [theta,b,QMin]=LTSRegression(x,y,h)
%%LTSREGRESSION Perform least trimmed squares (LTS) regression using the
%               fast, approximate algorithm of [1] that is identified as
%               best for <600 data points. This means find theta and b that
%               minimizes
%               sum_{i=1}^h r_i^2
%               where r_i is the ith largest value of
%               abs(y(i)-theta*x(:,i)-b)
%               This type of regression is robust to outliers.
%
%INPUTS: x,y These are the real samples of data. x is an xDimXN set of N
%            vectors and y is a set of corresponding scalar values. The
%            regression searches for theta and b to fit a line such that
%            y=theta'*x(:,i)+b.
%          h The trim value of the estimator. This must be between
%            fix((N+xDim+2)/2) and n. The default if omitted or an empty
%            matrix is passed is the value that allows for the highest
%            breakdown value, which is fix((N+xDim+2)/2).
%
%OUTPUTS: theta An xDimX1 vector. The multivariate slope of the line.
%             b The scalar y intercept of the data.
%          QMin The residual of the data.
%
%The algorithm described in [1] without the y intercept adjustment is used.
%
%EXAMPLE:
%This is similar to an example in [1]. There is a distribution that one
%might expect to be well approximated with a line corrupted by a minority
%of samples of a different distribution at the side. The plot is zoomed in
%so not all samples are shown. The standard linear regression result is
%shown for comparison.
% x=100*randn(1,800/2);
% y=x+1+randn(1,800/2);
%
% xy=GaussianD.rand(200/2,[50;0],25*eye(2,2));
% x=[x,xy(1,:)];
% y=[y,xy(2,:)];
%
% figure(1)
% clf
% hold on
% scatter(x,y,'b','filled')
%
% coeffs=multilinRegress([x;y]);
% numPoints=100;
% xEst=linspace(-200,200,numPoints);
% yEst=xEst*coeffs(1)+coeffs(2);
% plot(xEst,yEst,'--k','linewidth',2)
% [theta,b]=LTSRegression(x,y);
% yEst=theta*xEst+b;
% plot(xEst,yEst,'-r','linewidth',2)
% axis([-30-20,70+20,-30-20,50+20])%Zoom in.
% legend('Samples','Least Squares Fit','LTS Fit','location','NorthWest')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] P. J. Rousseeuw and V. Katrien, "Computing LTS regression for large
%    data sets," Data Mining and Knowledge Discovery, vol. 12, no. 1, pp.
%    26-45, Jan. 2006.
%
%August 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(x,2);

x=[x;ones(1,n)];

y=y(:);

p=size(x,1);%p>=2

if(nargin<3||isempty(h))
    h=fix((n+p+1)/2);
end

if(h<fix((n+p+1)/2)||h>n)
   error('Invalid value of h provided.')
end

totalStarts=binomial(n,p);
Q3Heap=BinaryHeap(10,false);
for k=1:min(totalStarts,500)
    if(totalStarts<500)
        %Do all possible initializations.
        idx=unrankCombination(k-1,n,p)+1;
    else
        %Get a random subset of points in x.
        idx=randCombination(n,p)+1;
    end

    xCur=x(:,idx);
    yCur=y(idx);
    %If what we get is not full rank, then we just use a
    %pseudoinverse. This skips issues of determining which and how
    %many additional columns to add.
    if(rank(xCur)<p)
        theta0=pinv(xCur)'*yCur;
    else
        theta0=xCur'\yCur;
    end

    for i=1:3
        r0=y'-sum(bsxfun(@times,x,theta0),1);
        [~,idx]=sort(abs(r0),'ascend');
        idx=idx(1:h);

        xCur=x(:,idx);
        yCur=y(idx);
        theta0=pinv(xCur)'*yCur;
    end

    r2=(y'-sum(bsxfun(@times,x,theta0),1)).^2;
    Q3=sum(mink(r2,h));

    if(Q3Heap.heapSize()<10)
        Q3Heap.insert(Q3,idx);
    else
        topEl=Q3Heap.getTop();
        if(topEl.key>Q3)
            Q3Heap.deleteTop();
            Q3Heap.insert(Q3,idx);
        end
    end
end

%For the 10 Q3 values in the heap, iterate the C-step in Section 2
%until convergence.
theta0List=zeros(p,10);
QList=zeros(10,1);
for k=1:10
    topEl=Q3Heap.getTop();
    Q3Heap.deleteTop();

    idx=topEl.value;
    xCur=x(:,idx);
    yCur=y(idx);
    theta0=pinv(xCur)'*yCur;
    idxOld=zeros(h,1);

    %It was noted that convergence should be for fewer than 10
    %iterations, so we make the maximum 10.
    for curIter=1:10
        if(all(idx(:)==idxOld(:)))
            break;
        end
        r0=y'-sum(bsxfun(@times,x,theta0),1);
        [~,idx]=sort(abs(r0),'ascend');
        idx=idx(1:h);

        xCur=x(:,idx);
        yCur=y(idx);
        theta0=pinv(xCur)'*yCur;
    end

    r2=(y'-sum(bsxfun(@times,x,theta0),1)).^2;
    Q=sum(mink(r2,h));

    QList(k)=Q;
    theta0List(:,k)=theta0;
end

[QMin,idx]=min(QList);
theta0Min=theta0List(:,idx);

theta=theta0Min(1:(p-1));
b=theta0Min(p);
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 rbfBasis = rbfComputeBasis( X, k, cluster, scale, show )
% Get locations and sizes of radial basis functions for use in rbf network.
%
% Radial basis function are a simple, fast method for universal function
% approximation / regression. The idea is to find a mapping X->y where X
% and y are continuous real variables. The mapping is linear over features
% of X: y=rbfWeight*features(X). The features are of the form:
%  f_j(x) = exp(-||x-mu_j||_2^2 / 2 sig_j^2 ).
% The number of basis functions controls the complexity of the network. Too
% many basis functions can lead to overfitting, too few to bad
% interpolation. Rbf networks are trained in two phases.  First, the radial
% basis functions are found in an unsupervised manner (using only Xtrain),
% for example by clustering Xtrain.  Next, given the basis functions,
% Xtrain and ytrain, the basis weights are found by solving the system:
%  rbfWeight * features(Xtrain) = ytrain
% At this point, to interpolate any new points, Xtest, use:
%  ytest = rbfWeight * features(Xtest)
% The code below achieves all three steps:
%  rbfBasis  = rbfComputeBasis( Xtrain, k, cluster, scale, show );
%  rbfWeight = rbfComputeFtrs(Xtrain,rbfBasis) \ ytrain;
%  ytest     = rbfComputeFtrs(Xtest,rbfBasis) * rbfWeight;
% Note, in the returned rbfBasis struct there are a number of flags that
% control how the rbf features are computed. These can be altered to
% achieve the desired effect.
%
% For an in depth discussion of rbf networks see:
%  Christopher M. Bishop. "Neural Networks for Pattern Recognition"
%
% USAGE
%  rbfBasis = rbfComputeBasis( X, k, [cluster], [scale], [show] )
%
% INPUTS
%  X           - [N x d] N points of d dimensions each
%  k           - number of basis functions to use
%  cluster     - [1]: Computes cluster centers for use as rbf functions.
%              - 0: Evenly centered basis functions (ok for small d)
%  scale       - [5] Alter computed value of sigma by given factor
%                set larger for smoother results, too small -> bad interp
%  show        - [0] will display results in figure(show)
%                if show<0, assumes X is array Nxs^2 of N sxs patches
%
% OUTPUTS
%  rfbBasis
%   .d          - feature vector size
%   .k          - number of basis functions actually used
%   .mu         - [d x k] rbf centers
%   .vars       - [1 x k] rbf widths
%   .var        - rbf average width
%   .globalVar  - [1] if true use single average var for rbfs
%   .constant   - [0] if true include extra basis with constant activation
%   .normalize  - [0] if true normalize overall rbf response to sum to 1
%
% EXAMPLE
%
% See also RBFDEMO, RBFCOMPUTEFTRS
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.50
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<2 || isempty(k)); error('k not specified'); end
if( nargin<3 || isempty(cluster)); cluster=1;  end
if( nargin<4 || isempty(scale)); scale=5;  end
if( nargin<5 || isempty(show)); show=0;  end
[N, d] = size(X);

if( cluster )
  %%% CLUSTERS subsample, run kmeans
  maxN=5000; if( N>maxN );  X=X(randSample(N,maxN),:);  N=maxN;  end
  prm.nTrial=5; prm.display=0;
  [IDX,mu] = kmeans2( X, k, prm );
  mu = mu'; k = size(mu,2);
else
  %%% GRID generate locations evenly spaced on grid
  if( d>4 ); error('d too high. curse of dimensionality..'); end
  nBPer = round( k ^ (1/d) );  k = nBPer ^ d; rg=[min(X)' max(X)'];
  del=(rg(:,2)-rg(:,1))/(nBPer-1); rg=rg+[-del del]/2;
  loc=cell(1,d);  for i=1:d; loc{i}=linspace(rg(i,1),rg(i,2),nBPer); end
  grid=cell(1,d); if(d>1); [grid{:}]=ndgrid(loc{:}); else grid=loc; end
  mu=zeros(d,k);   for i=1:d; mu(i,:) = grid{i}(:); end
end

%%% Set var to be equal to average distance of neareast neighbor.
dist = pdist2( mu', mu' );
dist = dist + realmax * eye( k );
vars = min(dist)* scale;
var  = mean(vars);
vars = max( vars, var/100 );

%%% store results
rbfBasis = struct('d',d, 'k',k, 'mu',mu, 'vars',vars, 'var',var, ...
  'globalVar',1, 'constant',0, 'normalize',0);

%%% optionally display
if( abs(show) )
  if( show<0 ) % if images can display
    siz = sqrt(d);
    I = clusterMontage( reshape(X,siz,siz,N), IDX, 25, 1 );
    figure(-show); clf; montage2( I );
    figure(-show+1); clf; montage2(reshape(mu,siz,siz,[]));
  elseif( d==1 ) % 1D data
    figure(show); clf; hold on;
    minX = min(X,[],1 );  maxX = max(X,[],1 );
    xs = linspace( minX, maxX, 500 )';
    for i=1:k
      ys = exp( -(xs-mu(i)).^2 / 2 / var  );
      plot( xs, ys );
    end
  elseif( d==2 ) % 2D data
    figure(show); clf;
    minX = min(X,[],1 );  maxX = max(X,[],1 );
    xs1 = linspace(minX(1),maxX(1),25);
    xs2 = linspace(minX(2),maxX(2),25);
    [xs1,xs2] = ndgrid( xs1, xs2 );
    xs = [xs1(:) xs2(:)]; n = size(xs,1);
    for i=1:k
      mui = repmat(mu(:,i),[1 n])';
      ys = exp( - sum( ((xs - mui)).^2, 2 ) / 2 / var );
      surf( xs1, xs2, reshape(ys,size(xs1)) );
      hold on;
    end;
  elseif( d==3 ) % 3D data (show data+centers)
    figure(show); clf; hold on;
    scatter3( X(:,1),X(:,2),X(:,3),12,'filled');
    scatter3( mu(1,:),mu(2,:),mu(3,:),30,'filled');
  end
end


function Q=quatMult(quat2,quat1,handed)
%%QUATMULT  Multiply two quaternions together. Quaternions are an extension
%           of complex numbers. The type of product performed here is
%           deemed the Grassman product. Unit-magnitude quaternions are
%           often used to represent rotations and the multiplication
%           quat2*quat1 is the orientation obtained by rotating by quat1
%           and then rotating by quat2. Quaternion multiplication is not
%           commutative. The quaternions can be either left or
%           right-handed.
%
%INPUTS:quat2 A 4XN set of N quaternions, where the first element in each
%             column is the scalar part of the quaternion (sometimes called
%             q0 or q4) and the next three elements are the (hypercomplex)
%             vector part. That is, the hypercomplex quaternion given by
%             quat2(:,1) can be written in hypercomplex, non-vector form
%             as quat2(1,1)+i*quat2(2,1)+j*quat2(3,1)+k*quat2(4,1), where
%             i, j, and k are all roots of -1. When considering unit
%             quaternions, this means that it has the form
%             [cos(theta/2);sin(theta/2)u'], where theta is an angle and u
%             is a 3X1 unit vector.
%       quat1 A 4XN set of N quaternions that are to be left-multiplied by
%             the corresponding quaternions in quat2.
%     handed The handedness of the quaternions. If omitted or an empty
%            matrix is passed,, it is assumed that the quaternions are
%            right-handed (the standard). Possible values are
%            'right' The default if omitted. The quaternion multiplication
%                    is assumed right-handed (standard).
%            'left'  The quaternion multiplication is assumed left-handed.
%                    This is used in someplaces, including the reference
%                    from Shuster, below.
%
%OUTPUTS: Q   The 4XN matrix of quaternions products such that
%             Q(:,i)=quat2(:,i)*quat1(:,1), where the multiplication
%             operation is quaternion multiplication.
%
%Properties of quaternions including multiplication are described in [1].
%Details of rotation using unit quaternions are given in [2].
%
%The difference in multiplication between right-handed and left-handed
%quaternions lies in the sign used for the cross product.
%
%A quaternion of the form q(1)+i*q(2)+j*q(3)+k*q(4) that obeys right-handed
%multiplication rules supports the following rules for multiplication of i,
%j, and k, where an item in a row is multiplied by an item in the column to
%get the result:
%  i,  j, k
%i -1, k,-j
%j -k,-1, i
%k  j,-i,-1
%On the other hand, left-handed multiplication rules flip the signs of the
%off-diagonal terms:
%  i,  j, k
%i -1,-k, j
%j  k,-1,-i
%k -j, i,-1
%
%REFERENCES:
%[1] Weisstein, Eric W. "Quaternion." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/Quaternion.html
%[2] M. D. Shuster, "A survey of attitude representations," The Journal of
%    the Astronautical Sciences, vol. 41, no. 4, pp. 439-517, Oct.-Dec.
%    1993.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The number of quaternions.
N=size(quat2,2);

if(nargin<3||isempty(handed))
    handed='right';
end

Q=zeros(4,N);
%The handedness switches the sign of the cross product
switch(handed)
    case 'right'
        for curQuat=1:N
            s1=quat1(1,curQuat);
            v1=quat1(2:4,curQuat);
            s2=quat2(1,curQuat);
            v2=quat2(2:4,curQuat);

            Q(:,curQuat)=[s1*s2-dot(v1,v2);
                          s1*v2+s2*v1+cross(v2,v1)];
        end
    case 'left'
        %Flip the algebra to left-handed for the formula below.
         for curQuat=1:N
            s1=quat1(1,curQuat);
            v1=quat1(2:4,curQuat);
            s2=quat2(1,curQuat);
            v2=quat2(2:4,curQuat);

            Q(:,curQuat)=[s1*s2-dot(v1,v2);
                          s1*v2+s2*v1-cross(v2,v1)];
        end
    otherwise
        error('Invalid handedness provided.')
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 [x, c, funVal, ValueL]=mtLogisticC(A, y, z, opts)
%
%%
% Function mtLogisticC:
%      Logistic Loss for Multi-task Learning
%             with the (group) L1/Lq-norm Constraint
%
%% Problem
%
%  min \sum_i - weight_i * log (p_i) + z * sum_j ||x^j||_q
%
%  p_i= 1/ (1+ exp(-y_i (x_l' * a_i + c_l) ) ) denotes the probability
%
%  a_i denotes a training sample,
%      and a_i' corresponds to the i-th row of the data matrix A
%
%  x^j denotes the j-th row of x
%  x_l denotes the l-th column of x, for the l-th task
%  c_l is the intercept for the l-th task
%
%  y_i (either 1 or -1) is the response
%
%  weight_i denotes the weight for the i-th sample
%  In this implementation, we assume weight_{il}=1/m
%
%  For the case that the multi tasks share the same data
%  matrix, please refer to the functions:
%            mcLeastR and mcLogisticR.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size m x 1)
%  z -        L1/Lq norm regularization parameter (z >=0)
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         The obtained weight of size n x k
%  c-         The obtained intercept if size 1 x k
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on February 19, 2010.
%
%% Related papers
%
% [1]  Jun Liu, Shuiwang Ji, and Jieping Ye, Multi-Task Feature Learning
%      Via Efficient L2,1-Norm Minimization, UAI, 2009
%
% [2]  Jun Liu, Lei Yuan, Songcan Chen and Jieping Ye, Multi-Task Feature Learning
%      Via Efficient L2,1-Norm Minimization, Technical Report ASU, 2009.
%
%% Related functions:
%
%  sll_opts, initFactor, pathSolutionLeast
%  mtLeastR, mtLeastC, mtLogisticR, ep21d
%
%%

%% Verify and initialize the parameters
%%
if (nargin <4)
    error('\n Inputs: A, y, z, and opts.ind should be specified!\n');
end

[m,n]=size(A);

if (length(y) ~=m)
    error('\n Check the length of y!\n');
end

if (z<=0)
    error('\n z should be positive!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

%% Detailed initialization
%%

% Initialize ind and q
if ~isfield(opts,'ind')
    error('\n In mtLeastR, .ind should be specified');
else
    ind=opts.ind;
    k=length(ind)-1;

    if ind(k+1)~=m
        error('\n Check opts.ind');
    end
end

% Initialize q
if (~isfield(opts,'q'))
    q=2; opts.q=2;
else  % currently, we only implement q=2
    q=opts.q;
    if (q~=2)
        error('\n Currently, we only implement the case q=2');
    end
end

%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Starting point initialization

p_flag=(y==1);                  % the indices of the postive samples

for i=1:k
    ind_i=(ind(i)+1):ind(i+1);  % indices for the i-th group

    m1(1,i)=sum(p_flag(ind_i));      % the total number of the positive samples
    m2(1,i)=length(ind_i)-m1(1,i);   % the total number of the negative samples
end

% initialize a starting point
if opts.init==2
    x=zeros(n,k); c=zeros(1,k);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if ( size(x,1)~=n || size(x,2)~=k )
            error('\n Check the input .x0');
        end
    else
        x=zeros(n,k);
    end

    if isfield(opts,'c0')
        c=opts.c0;

        if ( length(c)~=k )
            error('\n Check the input .c0');
        end
    else
        c=log(m1./m2);
    end
end

Ax=zeros(m,1); % m x 1
% compute Ax: Ax_i= A_i * x_i
for i=1:k
    ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
    m_i=ind(i+1)-ind(i);          % number of samples in the i-th group

    if (opts.nFlag==0)
        Ax(ind_i,1)=A(ind_i,:)* x(:,i);
    elseif (opts.nFlag==1)
        invNu=x(:,i)./nu; mu_invNu=mu * invNu;
        Ax(ind_i,1)=A(ind_i,:)*invNu -repmat(mu_invNu, m_i, 1);
    else
        Ax(ind_i,1)=A(ind_i,:)*x(:,i)-repmat(mu*x(:,i), m, 1);
        Ax(ind_i,1)=Ax./nu(ind_i,1);
    end
end

%% The main program
% The Armijo Goldstein line search schemes

if (opts.lFlag==0)

    lambda0=0;
    % a guess of the root

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/m; % the intial guess of the Lipschitz continuous gradient

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);
    cp=c;         ccp=zeros(1,k);

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;   sc=c + beta* ccp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % aa= - diag(y) * (A * s + sc)
        vec_sc=zeros(m,1);
        for i=1:k
            ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
            vec_sc(ind_i,1)=sc(i);
        end
        aa=- y.*(As+ vec_sc);

        % fun_s is the logistic loss at the search point
        bb=max(aa,0);
        fun_s= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / m;

        % compute prob=[p_1;p_2;...;p_m]
        prob=1./( 1+ exp(aa) );

        % b= - diag(y) * (1 - prob)
        b= -y.*(1-prob) / m;

        gc=zeros(1,k);                     % the gradient of c
        for i=1:k
            ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
            gc(1,k)=sum(b(ind_i));
        end

        % compute g= AT b, the gradient of x
        for i=1:k
            ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group

            if (opts.nFlag==0)
                tt =A(ind_i,:)'*b(ind_i,1);
            elseif (opts.nFlag==1)
                tt= A(ind_i,:)'*b(ind_i,1) - sum(b(ind_i,1)) * mu';
                tt=tt./nu(ind_i,1);
            else
                invNu=b(ind_i,1)./nu(ind_i,1);
                tt=A(ind_i,:)'*invNu - sum(invNu)*mu';
            end

            g(:,i)= tt;
        end

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        cp=c;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L; c= sc- gc/L;

            % L1/Lq-norm regularized projection
            [x, lambda, zf_step]=ep21d(v, n, k, z, lambda0);
            lambda0=lambda;

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute Ax: Ax_i= A_i * x_i
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                m_i=ind(i+1)-ind(i);          % number of samples in the i-th group

                if (opts.nFlag==0)
                    Ax(ind_i,1)=A(ind_i,:)* x(:,i);
                elseif (opts.nFlag==1)
                    invNu=x(:,i)./nu; mu_invNu=mu * invNu;
                    Ax(ind_i,1)=A(ind_i,:)*invNu -repmat(mu_invNu, m_i, 1);
                else
                    Ax(ind_i,1)=A(ind_i,:)*x(:,i)-repmat(mu*x(:,i), m, 1);
                    Ax(ind_i,1)=Ax./nu(ind_i,1);
                end
            end

            % aa= - diag(y) * (A * x + c)
            vec_sc=zeros(m,1);
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                vec_sc(ind_i,1)=c(i);
            end
            aa=- y.*(Ax+ vec_sc);

            % fun_s is the logistic loss at the search point
            bb=max(aa,0);
            fun_x= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / m;

            r_sum=(norm(v,'fro')^2 + norm(c-sc,2)^2) / 2;
            l_sum=fun_x - fun_s - sum(sum(v.* g)) - (c-sc)* gc';

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + <v, g> + <c ,gc>
            %                           + L/2 * (<v,v> + <c-sc,c-sc> )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;

        xxp=x-xp;   ccp=c-cp;

        funVal(iterStep)=fun_x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% Adaptive line search scheme

if (opts.lFlag==1)

    lambda0=0;
    % a guess of the root

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/m; % the intial guess of the Lipschitz continuous gradient

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);
    cp=c;             ccp=zeros(1,k);
    % t is the upper bound of absolute value of x

    gamma=1;
    % we shall set the value of gamma = L,
    % and L is appropriate for the starting point

    for iterStep=1:opts.maxIter
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp;   sc=c + beta* ccp;
                As=Ax + beta* (Ax-Axp);
            else
                alpha= (-1+ sqrt(5)) / 2;  beta=0;
                s=x; sc= c;
                As=Ax;
            end

            % aa= - diag(y) * (A * s + sc)
            vec_sc=zeros(m,1);
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                vec_sc(ind_i,1)=sc(i);
            end
            aa=- y.*(As+ vec_sc);

            % fun_s is the logistic loss at the search point
            bb=max(aa,0);
            fun_s= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / m;

            % compute prob=[p_1;p_2;...;p_m]
            prob=1./( 1+ exp(aa) );

            % b= - diag(y) * (1 - prob)
            b= -y.*(1-prob) / m;

            gc=zeros(1,k);                     % the gradient of c
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                gc(1,k)=sum(b(ind_i));
            end

            % compute g= AT b, the gradient of x
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group

                if (opts.nFlag==0)
                    tt =A(ind_i,:)'*b(ind_i,1);
                elseif (opts.nFlag==1)
                    tt= A(ind_i,:)'*b(ind_i,1) - sum(b(ind_i,1)) * mu';
                    tt=tt./nu(ind_i,1);
                else
                    invNu=b(ind_i,1)./nu(ind_i,1);
                    tt=A(ind_i,:)'*invNu - sum(invNu)*mu';
                end

                g(:,i)= tt;
            end

            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L; cnew= sc- gc/L;

            % projection
            [xnew, lambda, zf_step]=ep21d(v, n, k, z, lambda0);
            lambda0=lambda;

            v=xnew-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute Axnew = A *xnew: Ax_i= A_i * xnew_i
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                m_i=ind(i+1)-ind(i);          % number of samples in the i-th group

                if (opts.nFlag==0)
                    Axnew(ind_i,1)=A(ind_i,:)* xnew(:,i);
                elseif (opts.nFlag==1)
                    invNu=xnew(:,i)./nu; mu_invNu=mu * invNu;
                    Axnew(ind_i,1)=A(ind_i,:)*invNu -repmat(mu_invNu, m_i, 1);
                else
                    Axnew(ind_i,1)=A(ind_i,:)*xnew(:,i)-repmat(mu*xnew(:,i), m, 1);
                    Axnew(ind_i,1)=Axnew./nu(ind_i,1);
                end
            end

            % aa= - diag(y) * (A * x + c)
            vec_sc=zeros(m,1);
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                vec_sc(ind_i,1)=cnew(i);
            end
            aa=- y.*(Axnew+ vec_sc);

            % fun_x is the logistic loss at the new approximate solution
            % xnew
            bb=max(aa,0);
            fun_x= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / m;

            r_sum=(norm(v,'fro')^2 + norm(cnew-sc,2)^2) / 2;
            l_sum=fun_x - fun_s - sum(sum(v.* g)) - (cnew-sc)* gc';

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + <v, g> + <c ,gc>
            %                           + L/2 * (<v,v> + <c-sc,c-sc>  + <t-s_t, t-s_t>)
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;     Axp=Ax;
        x=xnew;   Ax=Axnew;
        cp=c;      c=cnew;
        % update x, t, c, and Ax

        xxp=x-xp;  ccp=c-cp;

        funVal(iterStep)=fun_x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end


%Tristan Ursell
%Relative Noise Transform
%(c) November 2012
%
%Iout=relnoise(Iin,sz,sigma);
%Iout=relnoise(Iin,sz,sigma,'field');
%[Iout,Ivar]=relnoise(Iin,sz,sigma,...);
%[Iout,Ivar,Imean]=relnoise(Iin,sz,sigma,...);
%
%Iin = the input image, of any numerical class.
%
%sz = (3 < sz < min(size(Iin))) is the size of the filter block used to
%calculate means and variances.  This value must be odd.
%
%sigma (sigma > 0) is the weighting parameter that defines the standard
%deviation relative to the filter block's standard deviation around which
%the center pixel will be Gaussian weighted. Setting sigma = 1 weights the
%current pixel using the STD of the current filter block. Lower values
%bring the current pixel closer to the mean, while high values are more
%tolerant of variations.  As sigma -> Inf, Iout = Iin.
%
%The field 'plot' will create an output plot comparing this transform to
%the original image, a Gaussian blur with STD = sz/2, and median filter
%with block size equal to sz.  At first glance, this filter appears similar
%to a median transform, but it does a better job of preserving local
%intensity extrema.  Comparison with the median filter requires the Image
%Processing Toolbox, but the rest of the script does not.
%
%The field 'disk' or 'square' will choose between using a disk or square
%filter block shape, where sz is the disk diameter or square side length.
%The default is square.
%
%The field 'custom' may be followed by a user-defined logical matrix or
%strel, e.g. relnoise(Iin,sz,sigma,'custom',strel('line',30,45)).  In this
%case 'sz' will be unused.
%
%Iout is the transformed output image.
%
%Ivar is the variance of the pixel intensities in the filter block at every
%point in the image -- essentially the spatially varying variance of the
%image.
%
%Imean is the mean smoothed image using the filter block, equivalent to a
%convolution averaging filter with the specified neighborhood.
%
%see also: wiener2  filter2
%
%Example:
%
%Iin=imread('spot_test.tif');
%
%Iout=relnoise(Iin,3,0.5,'square','plot');
% %OR
%Iout=relnoise(Iin,3,0.5,'custom',strel('line',30,0),'plot');
%
%figure;
%subplot(1,2,1)
%imagesc(Iout-double(Iin))
%title('What was removed from the original image.')
%axis equal tight
%box on
%
%subplot(1,2,2)
%imagesc(abs(fftshift(fft(Iout-double(Iin)))))
%title('FFT of difference between original and filtered images.')
%axis equal tight
%box on
%

function [varargout]=relnoise(Iin,sz,sigma,varargin)

%Nans
Inan=isnan(Iin);
if sum(Inan(:))>0
    error('Input matrix contains NaNs.')
end

%convert type
Iin=double(Iin);

%check filter size
if or(sz<1,sz>min(size(Iin)))
    error('The filter size is out of bounds.')
end

if mod(sz,2)~=1
    error('The filter size must be an odd integer.')
end

%parse field input
f1=find(strcmp('plot',varargin),1);
f2=find(strcmp('disk',varargin),1);
f3=find(strcmp('custom',varargin),1);

%choose plot option
if ~isempty(f1)
    plotq=1;
else
    plotq=0;
end

%choose filter type
if ~isempty(f3)
    hood_temp=varargin{f3+1};
    if strcmp(class(hood_temp),'strel')
        hood=hood_temp.getnhood;
    else
        hood=hood_temp;
    end

    sz=round(mean(size(hood)));
elseif ~isempty(f2)
    %disk filter block
    Xdisk = ones(sz,1)*(-(sz-1)/2:(sz-1)/2);
    Ydisk = (-(sz-1)/2:(sz-1)/2)'*ones(1,sz);
    Zdisk = sqrt(Xdisk.^2 + Ydisk.^2);

    hood=zeros(sz,sz);
    hood(Zdisk<=(sz-1)/2)=1;
else
    %square filter block
    hood=ones(sz,sz);
end

%convert hood class
hood=single(hood);

%calcualte means and variances
hood_sz=sum(hood(:));

%perform convolultion and normalization
Imean0=conv2(Iin,hood,'same')/hood_sz;
Inorm=conv2(ones(size(Iin)),hood,'same')/hood_sz;
Imean=Imean0./Inorm;
Ivar=conv2(Iin.^2,hood,'same')./Inorm*1/hood_sz-Imean.^2;

%compute weight matrix
W=exp(-(Iin-Imean).^2./(2*sigma^2*Ivar));

%correct for zero variance pixels
W(Ivar==0)=0;

%compute output image
if sigma==0
    Iout=Imean;
else
    Iout=Iin.*W+(1-W).*Imean;
end

%handle outputs
if nargout==1
    varargout{1}=Iout;
elseif nargout==2
    varargout{1}=Iout;
    varargout{2}=Ivar;
elseif nargout==3
    varargout{1}=Iout;
    varargout{2}=Ivar;
    varargout{3}=Imean;
elseif nargout==0
else
    error('Incorrect number of output arguments.')
end

%plot comparisons
if plotq==1

    figure;
    subplot(2,2,1)
    imagesc(Iin)
    xlabel('X')
    ylabel('Y')
    box on
    axis equal tight
    title('Original Image')

    subplot(2,2,2)
    imagesc(Iout)
    xlabel('X')
    ylabel('Y')
    box on
    axis equal tight
    title(['Relative Noise Reduction (this filter), size = ' num2str(sz) ', sigma = ' num2str(sigma)])

    subplot(2,2,3)
    %look for image processing toolbox
    boxes=ver;
    gotit=strfind([boxes.Name],'Image Processing Toolbox');
    if ~isempty(gotit)
        Iout2=medfilt2(Iin,[sz,sz],'symmetric');
    else
        disp('Sorry, you do not have the Image Processing Toolbox.')
        disp('The medfilt2 comparison image cannot be generated.')
        disp('Disabling `plot` will stop this message.')
        Iout2=Imean;
    end
    imagesc(Iout2)
    xlabel('X')
    ylabel('Y')
    box on
    axis equal tight

    if ~isempty(gotit)
        title(['Median Filter of size ' num2str(sz)])
    else
        title(['Mean Filter of size ' num2str(sz)])
    end

    %construct Gaussian filter (without Image Processing Toolbox)
    sz2=round(3/2*sz);
    Xgauss = ones(sz2,1)*(-(sz2-1)/2:(sz2-1)/2);
    Ygauss = (-(sz2-1)/2:(sz2-1)/2)'*ones(1,sz2);
    Zgauss = exp(-(Xgauss.^2+Ygauss.^2)/(2*(sz/2)^2));
    Zgauss = Zgauss/sum(Zgauss(:));
    Iout3=conv2(Iin,Zgauss,'same');

    subplot(2,2,4)
    imagesc(Iout3)
    xlabel('X')
    ylabel('Y')
    box on
    axis equal tight
    title(['Gaussian Blur with STD = ' num2str(sz/2)])
    pause(0.1)
end


function [errRadians] = hyperSam(a, b)
% HYPERSAM Computes the spectral angle error (in radians) between two vectors
%
% Usage
%   [errRadians] = hyperSam(a, b)
% Inputs
%   a - Vector 1.
%   b - Vector 2.
% Outputs
%   errRadians - angle between vectors a and b in radians

[p,N] = size(a);
errRadians = zeros(1,N);
for k=1:N
    tmp = a(:,k);
    errRadians(k) = acos(dot(tmp, b)/ (norm(b) * norm(tmp)));
end
return;


function x = project_exp(v)
% PROJECT_EXP    Project points onto the exponential cone.
%
%   When v is a 1 x 3 vector, project_exp(v) is the projection
%   of v onto the exponential cone. When v is an n x 3 vector,
%   project_exp(v) projects each row of v onto the cone
%   in a vectorized fashion.
%
%   For reference, the exponential cone and its dual are given by
%     Kexp   = { (x,y,z) | ye^(x/y) <= z, y > 0 }
%     Kexp^* = { (u,v,w) | u < 0, -ue^(v/u) <= ew } cup { (0,v,w) | v,w >= 0 }

    r = v(:,1); s = v(:,2); t = v(:,3);
    x = nan(size(v));

    % v in cl(Kexp)
    idx = ( (s.*exp(r./s) <= t & s > 0) | (r <= 0 & s == 0 & t >= 0) );
    x(idx,:) = v(idx,:);

    % -v in Kexp^*
    idx = ( (-r < 0 & r.*exp(s./r) <= -exp(1).*t) | (-r == 0 & -s >= 0 & -t >= 0) );
    x(idx,:) = 0;

    % special case with analytical solution
    idx = (r < 0 & s < 0);
    x(idx,:) = v(idx,:);
    x(idx,2) = max(x(idx,2),0);
    x(idx,3) = max(x(idx,3),0);

    % minimize ||x - v||^2 subject to se^{r/s} = t via primal-dual Newton method
    % these components are computed serially, so much slower
    idx = find(isnan(x(:,1)));

    g     = @(w)   w(2)*exp(w(1)/w(2)) - w(3);
    gradg = @(w)   [ exp(w(1)/w(2)); exp(w(1)/w(2))*(1 - w(1)/w(2)); -1 ];

    alpha = 0.001; beta = 0.5;

    for i = 1:length(idx)
        disp('newton')
        u = v(idx(i),:)';
        u(2) = max(u(2),1); u(3) = max(u(3),1);
        y = 1; % dual variable

        r = @(w,z) [ w - v(idx(i),:)' + z*gradg(w); g(w) ];

        for iter = 1:100
            KKT = [ eye(3)+y*hessg(u), gradg(u) ; gradg(u)', 0 ];
            z = KKT \ -r(u,y);
            du = z(1:3);
            dy = z(4);

            % backtracking line search
            t = 1;
            ustep = u + t*du; ystep = y + t*dy;
            while ustep(2) < 0 || (norm(r(ustep, ystep)) > (1 - alpha*t)*norm(r(u, y)))
                t = beta*t;
                ustep = u + t*du; ystep = y + t*dy;
            end

            u = ustep; y = ystep;

            if abs(g(u)) < 1e-8 && norm(r(u,y)) <= 1e-8
                x(idx(i),:) = u;
                break;
            end
        end
    end
end

function h = hessg(w)
    r = w(1); s = w(2); t = w(2);
    h = exp(r/s)*[ 1/s,    -r/s^2,   0;
                   -r/s^2, r^2/s^3,  0;
                   0,      0,        0 ];
end


% Check whether a graph is regular, i.e. whether every node has the same degree.
%
% INPUTS: adjacency matrix, nxn
% OUTPUTS: Boolean, 0 or 1
%
% Note: Defined for unweighted graphs only.
% GB: last updated, Sep 23, 2012

function S=isRegular(adj)

S=false;

degs=sum(adj>0); % remove weights and sum columns

if degs == degs(1)*ones(size(degs)); S = true; end


function p_mendousse = mendousse(x, t, source_freq, p0, c0, rho0, BonA, alpha_0)
%MENDOUSSE   Compute Mendousse's solution for nonlinear wave propagation in viscous media.
%
% DESCRIPTION:
%       mendousse calculates the propagation of a monofrequency plane wave
%       source in a thermoviscous medium with absorption given by
%       alpha_0*f^2. The solution is taken from Eq. (264) in Chapter 4 of
%       Nonlinear acoustics by Hamilton and Blackstock (2008). The infinite
%       sum is adaptively truncated when the moving average of the previous
%       five sum terms is smaller than a predefined convergence percentage
%       (0.01 percent by default).
%
% USAGE:
%       p_mendousse = mendousse(x, t, source_freq, p0, c0, rho0, BonA, alpha_0)
%
% INPUTS:
%       x           - position [m]
%       t           - time [s]
%       source_freq - frequency of plane wave [Hz]
%       p0          - source pressure [Pa]
%       c0          - medium sound speed [m/s]
%       rho0        - medium density [kg/m^3]
%       BonA        - nonlinearity parameter B/A
%       alpha_0     - absorption coefficient [dB/(MHz^2 cm)]
%
% OUTPUTS:
%       p_mendousse - calculated pressure field
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 21st February 2011
%       last update - 17th June 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/>.

% stop the summation when the current term contributes less than this amount
CONVERGENCE_PERCENTAGE = 0.01;

% minimum number of sum terms to use
MIN_NUM_SUM_TERMS = 20;

% number of pressure values to use in the moving average
MOVING_AV = 5;

% calculate the mach number
mach_num = p0/(rho0*c0^2);

% calculate the nonlinearity coefficient
beta = 1 + BonA/2;

% convert the absorption to nepers per meter
alpha_0 = db2neper(alpha_0, 2);

% calculate the absorption parameter
alpha = alpha_0*(2*pi*source_freq)^2;

% calculate the Goldberg number
goldberg_num = beta*mach_num*2*pi*source_freq/(c0*alpha);

% preallocate output variable
p_mendousse = zeros(1, length(x));

% calculate series
for loop_index = 1:length(x)

    % ---------------------------------------------------------------------
    % compute numerator summation
    % ---------------------------------------------------------------------

    % initialise loop variables
    p_term1 = 0;                    % pressure
    sum_term_contrib = 1;           % percentage contribution of current sum term
    n = 1;                          % summation index variable
    p_prev = zeros(1, MOVING_AV);   % moving average variable

    % loop the sum until it reaches an acceptable level of convergence
    while (sum_term_contrib > CONVERGENCE_PERCENTAGE) || (n < MIN_NUM_SUM_TERMS)

        % compute next sum term and add to the total
        p_term1 = p_term1 + (-1)^(n + 1) * n * besseli(n, goldberg_num/2) * exp(-n^2*alpha*x(loop_index)) * sin(n*2*pi*source_freq*t(loop_index));

        % update moving average of the contribution of sum term in a fifo
        % paradigm
        p_prev = circshift(p_prev, [0, 1]);
        p_prev(1) = p_term1;
        sum_term_contrib = 100*abs((p_term1 - mean(p_prev))/p_term1);

        % update summation variable
        n = n + 1;

    end

    % ---------------------------------------------------------------------
    % compute denominator summation
    % ---------------------------------------------------------------------

    % initialise loop variables
    p_term2 = 0;                    % pressure
    sum_term_contrib = 1;           % percentage contribution of current sum term
    n = 1;                          % summation index variable
    p_prev = zeros(1, MOVING_AV);   % moving average variable

    % loop the sum until it reaches an acceptable level of convergence
    while (sum_term_contrib > CONVERGENCE_PERCENTAGE) || (n < MIN_NUM_SUM_TERMS)

        % compute next sum term and add to the total
        p_term2 = p_term2 + (-1)^(n) * besseli(n, goldberg_num/2) * exp(-n^2*alpha*x(loop_index)) * cos(n*2*pi*source_freq*t(loop_index));

        % update moving average of the contribution of sum term in a fifo
        % paradigm
        p_prev = circshift(p_prev, [0, 1]);
        p_prev(1) = p_term2;
        sum_term_contrib = 100*abs((p_term2 - mean(p_prev))/p_term2);

        % update summation variable
        n = n + 1;

    end

    % ---------------------------------------------------------------------
    % apply scaling parameters and store the value of p
    % ---------------------------------------------------------------------

    % check if the numerator is zero
    if p_term1 ~= 0
        p_term1 = 4*p_term1./goldberg_num;
    else
        p_term1 = 0;
    end
    p_mendousse(loop_index) = p_term1 / (besseli(0, goldberg_num/2) + 2*p_term2);

end

% scale output by p0
p_mendousse = p0.*p_mendousse;


function [A,M] = assemblematrix3(node,elem,lumpflag)
%% ASSEMBLEMATRIX3 matrix for diffusion and reaction
%
% [A,M] = ASSEMBLEMATRIX3(node,elem) return the stiffness matrix and the
% mass matrix.
%
% [A,M] = ASSEMBLEMATRIX3(node,elem,1) return the stiffness matrix and the
% lumped mass matrix. Note that in the output M is a vector not a matrix. A
% sparse diagonal matrix using M as diaongal can be obtained by
% spdiags(M,0,N,N);
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

N = size(node,1);
A = sparse(N,N);
M = sparse(N,N);

%% Compute geometric quantities and gradient of local basis
[Dphi,volume] = gradbasis3(node,elem);

%% Assemble stiffness matrix
for i = 1:4
    for j = i:4
        Aij = dot(Dphi(:,:,i),Dphi(:,:,j),2).*volume;
        ii = double(elem(:,i));
        jj = double(elem(:,j));
        if (j==i)
            A = A + sparse(ii,jj,Aij,N,N);
        else
            A = A + sparse([ii;jj],[jj;ii],[Aij; Aij],N,N);
        end
        if (nargout > 1) && (~exist('lumpflag','var') || lumpflag == 0 )
            if (j==i)
                M = M + sparse(ii,jj,volume/10,N,N);
            else
                M = M + sparse([ii;jj],[jj;ii],[volume/20; volume/20],N,N);
            end
        end
    end
end

%% Assemble mass matrix by mass lumping
if exist('lumpflag','var') && lumpflag==1 && nargout > 1
    M = accumarray(elem(:),[volume;volume;volume;volume]/4,[N,1]);
end


function S = oblateSurfaceArea(elli, varargin)
%OBLATESURFACEAREA  Approximated surface area of an oblate ellipsoid.
%
%   S = oblateSurfaceArea(R1,R2)
%
%   Example
%   oblateSurfaceArea
%
%   See also
%   geom3d, ellipsoidSurfaceArea, prolateSurfaceArea
%

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

%% Parse input argument

if size(elli, 2) == 7
    R1 = elli(:, 4);
    R2 = elli(:, 5);

elseif size(elli, 2) == 1 && ~isempty(varargin)
    R1 = elli(:, 1);
    R2 = varargin{1};
end

assert(R1 < R2, 'First radius must be smaller than second radius');

% surface theorique d'un ellipsoide oblate
% cf http://fr.wikipedia.org/wiki/Ellipso%C3%AFde_de_r%C3%A9volution
e = sqrt(R2.^2 - R1.^2) ./ R2;
S = 2 * pi * R2.^2 + pi * R1.^2 * log((1 + e) ./ (1 - e)) ./ e;


% Chapter 3 - Complex Iterative Maps.
% Program 3a - Julia Sets.
% Copyright Birkhauser 2013. Stephen Lynch.

% Plot the Julia set J(0,1.1) (Figure 3.1(d)).
clear
k=15;niter=2^k;
x=zeros(1,niter);y=zeros(1,niter);
x1=zeros(1,niter);y1=zeros(1,niter);
a=0;b=1.1;
x(1)=real(0.5+sqrt(0.25-(a+1i*b)));
y(1)=imag(0.5+sqrt(0.25-(a+1i*b)));
% Check that the point is unstable.
isunstable=2*abs(x(1)+1i*y(1))

hold on
for n=1:niter
    x1=x(n);y1=y(n);
    u=sqrt((x1-a)^2+(y1-b)^2)/2;v=(x1-a)/2;
    u1=sqrt(u+v);v1=sqrt(u-v);
    x(n+1)=u1;y(n+1)=v1;
    if y(n)<b
        y(n+1)=-y(n+1);
    end
    if rand < .5
       x(n+1)=-u1;y(n+1)=-y(n+1);
    end
end

fsize=15;
plot(x,y,'k.','MarkerSize',1)
set(gca,'XTick',-1.6:0.4:1.6,'FontSize',fsize)
set(gca,'YTick',-1.2:0.4:1.2,'FontSize',fsize)
xlabel('Re z','FontSize',fsize)
ylabel('Im z','FontSize',fsize)
hold off

% End of Program 3a.


#!/usr/bin/octave -q
% Copyright 2018 Marc René Schädler
%
% This file is part of the mobile hearing aid prototype project
% The the mobile hearing aid prototype project is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
%
% The mobile hearing aid prototype project 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 the mobile hearing aid prototype project. If not, see http://www.gnu.org/licenses/.

close all
clear
clc

[signal, fs] = audioread('/tmp/inout.wav');

analysis_length = 100; % ms

analysis_samples = floor(fs.*analysis_length./1000);
analysis_frames = size(signal,1)./analysis_samples;

level_in = -inf*ones(analysis_frames,2);
level_out = -inf*ones(analysis_frames,2);

freq_in = zeros(analysis_frames,2);
freq_out = zeros(analysis_frames,2);

analysis_window = hanning(analysis_samples);
analysis_window = analysis_window ./ sqrt(mean(analysis_window.^2));

for i=1:analysis_frames
  in1 = signal(1+(i-1).*analysis_samples:i.*analysis_samples,1) .* analysis_window;
  in2 = signal(1+(i-1).*analysis_samples:i.*analysis_samples,2) .* analysis_window;
  out1 = signal(1+(i-1).*analysis_samples:i.*analysis_samples,3) .* analysis_window;
  out2 = signal(1+(i-1).*analysis_samples:i.*analysis_samples,4) .* analysis_window;

  level_in(i,1) = 10*log10(mean(in1.^2));
  level_in(i,2) = 10*log10(mean(in2.^2));
  level_out(i,1) = 10*log10(mean(out1.^2));
  level_out(i,2) = 10*log10(mean(out2.^2));

  [~, maxidx_in1] = max(abs(fft(in1)));
  [~, maxidx_in2] = max(abs(fft(in2)));
  [~, maxidx_out1] = max(abs(fft(out1)));
  [~, maxidx_out2] = max(abs(fft(out2)));

  freq_in(i,1) = (maxidx_in1-1)./analysis_samples.*fs;
  freq_in(i,2) = (maxidx_in2-1)./analysis_samples.*fs;
  freq_out(i,1) = (maxidx_out1-1)./analysis_samples.*fs;
  freq_out(i,2) = (maxidx_out2-1)./analysis_samples.*fs;
end

freq_bins = logspace(log10(88.4),log10(16000),16);
freq_color = hsv(length(freq_bins)).*0.8;

figure('Position',[0 0 800 800],'Visible','on');
sample_levels = [-80 -60 -40 -20 0];

for i=1:2
  subplot(2,2,i);
  plot([0 0],[-100 10],'k');
  hold on;
  plot([-100  10],[  0   0],'k');
  plot([-100  10],[-100 10],'k');
  plot([-100 -10],[ -80 10],'k--');
  plot([-100 -30],[ -60 10],'k--');
  plot([-100 -50],[ -40 10],'k--');
  plot([-100 -70],[ -20 10],'k--');
  plot([-100 -90],[   0 10],'k--');

  out_mean = nan(length(freq_bins),length(sample_levels));

  h = [];
  for j=1:length(freq_bins)
    if j == 1
      freq_mask = freq_in(:,i) >= freq_bins(j)  & freq_in(:,i) < freq_bins(j+1);
    elseif j==length(freq_bins)
      freq_mask = freq_in(:,i) >= freq_bins(j-1) & freq_in(:,i) < freq_bins(j);
    else
      freq_mask = freq_in(:,i) >= freq_bins(j-1) & freq_in(:,i) < freq_bins(j+1);
    end
    x = level_in(freq_mask,i);
    y = level_out(freq_mask,i);
    validmask = x > -100 & y > -100 & ~isnan(x) & ~isnan(y) & ~isinf(x) & ~isinf(y);
    x = x(validmask);
    y = y(validmask);
    c = freq_color(j,:);
    if length(x) > 6
      p = polyfit(x,y,4);
      out_mean(j,:) = polyval(p,sample_levels);
    end
    scatter(x,y,20,c);
    if mod(j-1,2) == 0
      h(j) = plot(-90:10,polyval(p,-90:10),'Color',c);
    end
  end
  legend(h,num2str(round(freq_bins(2:2:end).')),'location','southeast');

  xlim([-100 10]);
  ylim([-100 10]);
  xlabel('Input / dB FS');
  ylabel('Output / dB FS');

  subplot(2,2,i+2);

  plot(log([50 16000]),[0 0],'k');
  hold on;

  level_color = hsv(length(sample_levels)).*0.8;

  h = [];
  for j=1:length(sample_levels)
    c = level_color(j,:);
    plot(log(freq_bins(1:end-1)),sample_levels(j).*ones(length(freq_bins)-1,1),'--','Color',c);
    h(j) = plot(log(freq_bins),out_mean(:,j),'-','Color',c);
  end

  xlim(log([50 16000]));
  ylim([-100 10]);
  xlabel('Frequency / Hz');
  ylabel('Levels / dB');
  set(gca,'xtick',log(freq_bins(2:2:end-1)));
  set(gca,'xticklabel',round(freq_bins(2:2:end-1)));
  legend(h,num2str(sample_levels.'),'location','southeast');
end

set(gcf,'PaperUnits','inches','PaperPosition',[0 0 8 8].*1.4);
print('-depsc2','-r300','inout.eps');


function ex9bvp
%EX9BVP  Example 9 of the BVP tutorial.
%   This boundary value problem is the subject of Chapter 8 of
%   C.C. Lin and L.A. Segel, Mathematics Applied to Deterministic
%   Problems in the Natural Sciences, SIAM, Philadelphia, 1988.
%   The ODEs
%
%      v' = (C - 1)/n
%      C' = (vC - min(x,1))/eta
%
%   are solved on the interval [0, lambda].  The boundary conditions
%   are v(0) = 0, C(lambda) = 1, and continuity of v(x) and C(x) at
%   x = 1.  Accordingly, this is a three-point BVP that must be
%   reformulated for solution with the two-point BVP solver BVP4C.
%   This reformulation involves introducing unknowns y_1(x) for v
%   and y_2(x) for C on the interval 0 <= x <= 1 and unknowns y_3(x)
%   for v and y_4(x) for C on 1 <= x <= lambda. A new independent
%   variable is introduced for the second interval, tau =
%   (x - 1)/(lambda - 1), so that it also ranges from 0 to 1. The
%   differential equations for the four unknowns are then solved
%   on the interval [0, 1].  The continuity conditions on v and C
%   become boundary conditions on the new unknowns.  A plot of v(x)
%   and C(x) on [0, lambda] involves plotting the new unknowns over
%   the subintervals.
%
%   The quantity of most interest is the emergent osmolarity Os =
%   1/v(lambda).  The parameters are related to another parameter
%   kappa by eta = lambda^2/(n*kappa^2).  Lin and Segel develop an
%   approximate solution for Os valid for "small" n.  Here the BVP is
%   solved for a range of kappa when lambda = 2 and n = 0.005.  The
%   computed Os is compared to the approximation of Lin and Segel.

% Copyright 1999, The MathWorks, Inc.

n = 5e-2;
lambda = 2;

options = [];   % place holder

sol = bvpinit(linspace(0,1,5),[1 1 1 1]);

fprintf(' kappa    computed Os  approximate Os \n')
for kappa = 2:5
  eta = lambda^2/(n*kappa^2);

  sol = bvp4c(@ex9ode,@ex9bc,sol,options,n,lambda,eta);

  K2 = lambda*sinh(kappa/lambda)/(kappa*cosh(kappa));
  approx = 1/(1 - K2);
  computed = 1/sol.y(3,end);
  fprintf('  %2i    %10.3f    %10.3f \n',kappa,computed,approx);
end

% v and C are computed separately on 0 <= x <= 1 and 1 <= x <= lambda.
% A change of independent variable is used for the second interval,
% which must then be undone to obtain the corresponding mesh.
x = [sol.x sol.x*(lambda-1)+1];
y = [sol.y(1:2,:) sol.y(3:4,:)];

clf reset
plot(x,y(1,:),x,y(2,:),'--')
legend('v(x)','C(x)')
title('A three-point BVP.')
xlabel(['\lambda = ',num2str(lambda),', \kappa = ',num2str(kappa),'.'])
ylabel('v and C')
shg

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

function dydx = ex9ode(x,y,n,lambda,eta)
%EX9ODE  ODE function for Example 9 of the BVP tutorial.
dydx = [ (y(2) - 1)/n
         (y(1)*y(2) - x)/eta
         (lambda - 1)*(y(4) - 1)/n
         (lambda - 1)*(y(3)*y(4) - 1)/eta ];

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

function res = ex9bc(ya,yb,n,lambda,eta)
%EX9BC  Boundary conditions for Example 9 of the BVP tutorial.
res = [ ya(1)
        yb(4) - 1
        yb(1) - ya(3)
        yb(2) - ya(4)];