function z = sandwich3x3(x, y)

% SANDWICH3X3 compute x*y*x' provided y is Hermitian and dimensionality is 3x3xN

% Copyright (C) 2017, Donders Centre for Cognitive Neuroimaging, Nijmegen, NL
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% FIXME build in check for hermitianity
z     = complex(zeros(size(x)));
xconj = conj(x);
xabs2 = abs(x).^2;

z(1,1,:,:) = xabs2(1,1,:,:).*y(1,1,:,:)  + ...
             xabs2(1,2,:,:).*y(2,2,:,:)  + ...
             xabs2(1,3,:,:).*y(3,3,:,:)  + ...
             2.*real( x(1,2,:,:).*y(2,1,:,:).*xconj(1,1,:,:) ) + ...
             2.*real( x(1,3,:,:).*y(3,1,:,:).*xconj(1,1,:,:) ) + ...
             2.*real( x(1,3,:,:).*y(3,2,:,:).*xconj(1,2,:,:) );

             %(x(1,2,:,:).*y(2,1,:,:) + x(1,3,:,:).*y(3,1,:,:)).*xconj(1,1,:,:) + ...
             %(x(1,1,:,:).*y(1,2,:,:) + x(1,3,:,:).*y(3,2,:,:)).*xconj(1,2,:,:) + ...
             %(x(1,1,:,:).*y(1,3,:,:) + x(1,2,:,:).*y(2,3,:,:)).*xconj(1,3,:,:);

z(2,1,:,:) = (x(2,1,:,:).*y(1,1,:,:) + x(2,2,:,:).*y(2,1,:,:) + x(2,3,:,:).*y(3,1,:,:)).*xconj(1,1,:,:) + ...
             (x(2,1,:,:).*y(1,2,:,:) + x(2,2,:,:).*y(2,2,:,:) + x(2,3,:,:).*y(3,2,:,:)).*xconj(1,2,:,:) + ...
             (x(2,1,:,:).*y(1,3,:,:) + x(2,2,:,:).*y(2,3,:,:) + x(2,3,:,:).*y(3,3,:,:)).*xconj(1,3,:,:);

z(3,1,:,:) = (x(3,1,:,:).*y(1,1,:,:) + x(3,2,:,:).*y(2,1,:,:) + x(3,3,:,:).*y(3,1,:,:)).*xconj(1,1,:,:) + ...
             (x(3,1,:,:).*y(1,2,:,:) + x(3,2,:,:).*y(2,2,:,:) + x(3,3,:,:).*y(3,2,:,:)).*xconj(1,2,:,:) + ...
             (x(3,1,:,:).*y(1,3,:,:) + x(3,2,:,:).*y(2,3,:,:) + x(3,3,:,:).*y(3,3,:,:)).*xconj(1,3,:,:);

z(1,2,:,:) = conj(z(2,1,:,:));

z(2,2,:,:) = xabs2(2,1,:,:).*y(1,1,:,:)  + ...
             xabs2(2,2,:,:).*y(2,2,:,:)  + ...
             xabs2(2,3,:,:).*y(3,3,:,:)  + ...
             2.*real( x(2,2,:,:).*y(2,1,:,:).*xconj(2,1,:,:) ) + ...
             2.*real( x(2,3,:,:).*y(3,1,:,:).*xconj(2,1,:,:) ) + ...
             2.*real( x(2,3,:,:).*y(3,2,:,:).*xconj(2,2,:,:) );

             %(x(2,2,:,:).*y(2,1,:,:) + x(2,3,:,:).*y(3,1,:,:)).*xconj(2,1,:,:) + ...
             %(x(2,1,:,:).*y(1,2,:,:) + x(2,3,:,:).*y(3,2,:,:)).*xconj(2,2,:,:) + ...
             %(x(2,1,:,:).*y(1,3,:,:) + x(2,2,:,:).*y(2,3,:,:)).*xconj(2,3,:,:);

z(3,2,:,:) = (x(3,1,:,:).*y(1,1,:,:) + x(3,2,:,:).*y(2,1,:,:) + x(3,3,:,:).*y(3,1,:,:)).*xconj(2,1,:,:) + ...
             (x(3,1,:,:).*y(1,2,:,:) + x(3,2,:,:).*y(2,2,:,:) + x(3,3,:,:).*y(3,2,:,:)).*xconj(2,2,:,:) + ...
             (x(3,1,:,:).*y(1,3,:,:) + x(3,2,:,:).*y(2,3,:,:) + x(3,3,:,:).*y(3,3,:,:)).*xconj(2,3,:,:);

z(1,3,:,:) = conj(z(3,1,:,:));

z(2,3,:,:) = conj(z(3,2,:,:));

z(3,3,:,:) = xabs2(3,1,:,:).*y(1,1,:,:)  + ...
             xabs2(3,2,:,:).*y(2,2,:,:)  + ...
             xabs2(3,3,:,:).*y(3,3,:,:)  + ...
             2.*real( x(3,2,:,:).*y(2,1,:,:).*xconj(3,1,:,:) ) + ...
             2.*real( x(3,3,:,:).*y(3,1,:,:).*xconj(3,1,:,:) ) + ...
             2.*real( x(3,3,:,:).*y(3,2,:,:).*xconj(3,2,:,:) );

             %(x(3,2,:,:).*y(2,1,:,:) + x(3,3,:,:).*y(3,1,:,:)).*xconj(3,1,:,:) + ...
             %(x(3,1,:,:).*y(1,2,:,:) + x(3,3,:,:).*y(3,2,:,:)).*xconj(3,2,:,:) + ...
             %(x(3,1,:,:).*y(1,3,:,:) + x(3,2,:,:).*y(2,3,:,:)).*xconj(3,3,:,:);
%
%b1 b2 b7    a1 a2' a4'   b1' b3' b5'
%b3 b4 b8    a2 a3  a5'   b2' b4' b6'
%b5 b6 b9    a4 a5  a6    b7' b8' b9'
%
%b1*a1+b2*a2+b7*a4  b1*a2'+b2*a3+b7*a5  b1*a4'+b2*a5'+b7*a6   b1' b3' b5'
%b3*a1+b4*a2+b8*a4  b3*a2'+b4*a3+b8*a5  b3*a4'+b4*a5'+b8*a6   b2' b4' b6'
%b5*a1+b6*a2+b9*a4  b5*a2'+b6*a3+b9*a5  b5*a4'+b6*a5'+b9*a6   b7' b8' b9'
%
%

%b1*a1*b1'+b2*a2*b1'+b1*a2'*b2'+b2*a3*b2' b1*a1*b3'+b2*a2*b3'+b1*a2'*b4'+b2*a3*b4'
%b3*a1*b1'+b4*a2*b1'+b3*a2'*b2'+b4*a3*b2' b3*a1*b3'+b4*a2*b3'+b3*a2'*b4'+b4*a3*b4'
%
%a1*abs(b1)^2 + a2*(b1'*b2) + a2'*(b1*b2') + a3*abs(b2)^2    a1*b1*b3'    + a2*b2*b3'   + a2'*b1*b4'   + a3*b2*b4'
%a1*b1'*b3    + a2*b1'*b4   + a2'*b2'*b3   + a3*b2'*b4       a1*abs(b3)^2 + a2*(b3'*b4) + a2'*(b3*b4') + a3*abs(b4)^2


function [bcx,bcy] = specific_flow(xbd,ybd)
%poiseuille_flow   Reference problem 5.1 inflow condition
%   [bcx,bcy] = specific_flow(xbd,ybd);
%   input
%          xbd          x coordinate vector
%          ybd          y coordinate vector
%
%   specifies Poiseuille flow boundary condition
%   IFISS function: DJS; 6 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
bcx=1-ybd.*ybd; bcy=0*xbd;
return


function [xPred, SPred, xPropCenPoints]=sqrtDiscCubKalPred(xPrev,SPrev,f,SQ,xi,w,stateDiffTrans,stateAvgFun,stateTrans)
%%SQRTDISCCUBKALPRED Perform the discrete-time prediction step that comes
%                    with the square root implementation of the cubature
%                    Kalman filter with additive process noise.
%
%INPUTS: xPrev The xDim X 1 state estimate at the previous time-step.
%        SPrev The xDim X xDim lower-triangular square root of the state
%              covariance matrix at the previous time-step.
%            f A function handle for the state transition function that
%              takes the state as its parameter.
%           SQ The xDimXxDim lower-triangular square root of the process
%              noise covariance matrix.
%           xi An xDimXnumCubPoints matrix of cubature points. If this and
%              the next parameter are omitted or empty matrices are passed,
%              then fifthOrderCubPoints(xDim+cDim) is used. It is suggested
%              that xi and w be provided to avoid needless recomputation of
%              the cubature points.
%            w A numCubPointsX1 vector of the weights associated with the
%              cubature points. These must be all positive.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states estimates and transforms them
%              however might be necessary. For example, a state continaing
%              angular components will generally need differences between
%              angular components wrapped to the range +/-pi.
%  stateAvgFun An optional function that given an xDimXN matrix of N state
%              estimates and an NX1 vector of weights, provides the
%              weighted average of the state estimates. This is necessary
%              if, for example, states with angular components are
%              averaged.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one should generally
%              want to bind it to the region +/-pi. This is not applied to
%              the output of f.
%
%OUTPUTS: xPred The xDim X 1 predicted state estimate.
%         SPred The xDim X xDim lower-triangular square root of the
%               predicted state covariance estimate.
% xPropCenPoints The centered propagated cubature state points. This matrix
%               is needed if the backwards smoothing step is being applied.
%
%The mathematics behind the function sqrtDiscCubKalPred are described in
%more detail in Section IX of [1] and in [2]. The use of stateDiffTrans,
%stateAvgFun, and stateTrans arises when dealing with state space models
%involving, for example, angles. This is analogous to similar issues that
%arise when dealing with angular measurements as described in [3].
%
%REFERENCES:
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems Magazine,
%    vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] I. Arasaratnam and S. Haykin, "Cubature Kalman filters," IEEE
%    Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269,
%    Jun. 2009.
%[3] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=length(xPrev);

    if(nargin<5||isempty(xi))
        [xi,w]=fifthOrderCubPoints(xDim);
    end

    if(nargin<7||isempty(stateDiffTrans))
        stateDiffTrans=@(x)x;
    end

    if(nargin<8||isempty(stateAvgFun))
        stateAvgFun=@(x,w)calcMixtureMoments(x,w);
    end

    if(nargin<9||isempty(stateTrans))
        stateTrans=@(x)x;
    end

    xDim=length(xPrev);
    numCubPoints=length(w);

    xPropPoints=zeros(xDim,numCubPoints);%Allocate space

    %Calculate the cubature state points
    xPoints=stateTrans(transformCubPoints(xi,xPrev,SPrev));

    %Propagate the cubature state points
    for curP=1:numCubPoints
        xPropPoints(:,curP)=f(xPoints(:,curP));
    end

    %Calculate the predicted state
    xPred=stateAvgFun(xPropPoints,w);

    %Centered, propagated cubature state points
    xDiff=stateDiffTrans(bsxfun(@minus,xPropPoints,xPred));
    xPropCenPoints=bsxfun(@times,xDiff,sqrt(w'));

    %The root prediction covariance.
    SPred=tria([xPropCenPoints, SQ]);
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.


#!/usr/bin/env octave
%% Machine Learning Online Class
%  Exercise 7 | Principle Component Analysis and K-Means Clustering
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     pca.m
%     projectData.m
%     recoverData.m
%     computeCentroids.m
%     findClosestCentroids.m
%     kMeansInitCentroids.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: Find Closest Centroids ====================
%  To help you implement K-Means, we have divided the learning algorithm
%  into two functions -- findClosestCentroids and computeCentroids. In this
%  part, you shoudl complete the code in the findClosestCentroids function.
%
fprintf('Finding closest centroids.\n\n');

% Load an example dataset that we will be using
load('ex7data2.mat');

% Select an initial set of centroids
K = 3; % 3 Centroids
initial_centroids = [3 3; 6 2; 8 5];

% Find the closest centroids for the examples using the
% initial_centroids
idx = findClosestCentroids(X, initial_centroids);

fprintf('Closest centroids for the first 3 examples: \n')
fprintf(' %d', idx(1:3));
fprintf('\n(the closest centroids should be 1, 3, 2 respectively)\n');

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

%% ===================== Part 2: Compute Means =========================
%  After implementing the closest centroids function, you should now
%  complete the computeCentroids function.
%
fprintf('\nComputing centroids means.\n\n');

%  Compute means based on the closest centroids found in the previous part.
centroids = computeCentroids(X, idx, K);

fprintf('Centroids computed after initial finding of closest centroids: \n')
fprintf(' %f %f \n' , centroids');
fprintf('\n(the centroids should be\n');
fprintf('   [ 2.428301 3.157924 ]\n');
fprintf('   [ 5.813503 2.633656 ]\n');
fprintf('   [ 7.119387 3.616684 ]\n\n');

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

%% =================== Part 3: K-Means Clustering ======================
%  After you have completed the two functions computeCentroids and
%  findClosestCentroids, you have all the necessary pieces to run the
%  kMeans algorithm. In this part, you will run the K-Means algorithm on
%  the example dataset we have provided.
%
fprintf('\nRunning K-Means clustering on example dataset.\n\n');

% Load an example dataset
load('ex7data2.mat');

% Settings for running K-Means
K = 3;
max_iters = 10;

% For consistency, here we set centroids to specific values
% but in practice you want to generate them automatically, such as by
% settings them to be random examples (as can be seen in
% kMeansInitCentroids).
initial_centroids = [3 3; 6 2; 8 5];

% Run K-Means algorithm. The 'true' at the end tells our function to plot
% the progress of K-Means
[centroids, idx] = runkMeans(X, initial_centroids, max_iters, true);
fprintf('\nK-Means Done.\n\n');

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

%% ============= Part 4: K-Means Clustering on Pixels ===============
%  In this exercise, you will use K-Means to compress an image. To do this,
%  you will first run K-Means on the colors of the pixels in the image and
%  then you will map each pixel on to it's closest centroid.
%
%  You should now complete the code in kMeansInitCentroids.m
%

fprintf('\nRunning K-Means clustering on pixels from an image.\n\n');

%  Load an image of a bird
A = double(imread('bird_small.png'));

% If imread does not work for you, you can try instead
%   load ('bird_small.mat');

A = A / 255; % Divide by 255 so that all values are in the range 0 - 1

% Size of the image
img_size = size(A);

% Reshape the image into an Nx3 matrix where N = number of pixels.
% Each row will contain the Red, Green and Blue pixel values
% This gives us our dataset matrix X that we will use K-Means on.
X = reshape(A, img_size(1) * img_size(2), 3);

% Run your K-Means algorithm on this data
% You should try different values of K and max_iters here
K = 16;
max_iters = 10;

% When using K-Means, it is important the initialize the centroids
% randomly.
% You should complete the code in kMeansInitCentroids.m before proceeding
initial_centroids = kMeansInitCentroids(X, K);

% Run K-Means
[centroids, idx] = runkMeans(X, initial_centroids, max_iters);

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

%% ================= Part 5: Image Compression ======================
%  In this part of the exercise, you will use the clusters of K-Means to
%  compress an image. To do this, we first find the closest clusters for
%  each example. After that, we

fprintf('\nApplying K-Means to compress an image.\n\n');

% Find closest cluster members
idx = findClosestCentroids(X, centroids);

% Essentially, now we have represented the image X as in terms of the
% indices in idx.

% We can now recover the image from the indices (idx) by mapping each pixel
% (specified by it's index in idx) to the centroid value
X_recovered = centroids(idx,:);

% Reshape the recovered image into proper dimensions
X_recovered = reshape(X_recovered, img_size(1), img_size(2), 3);

% Display the original image
subplot(1, 2, 1);
imagesc(A);
title('Original');

% Display compressed image side by side
subplot(1, 2, 2);
imagesc(X_recovered)
title(sprintf('Compressed, with %d colors.', K));

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


function b = dgesl ( a, lda, n, ipvt, b, job )

%*****************************************************************************80
%
%% DGESL solves a real general linear system A * X = B.
%
%  Discussion:
%
%    DGESL can solve either of the systems A * X = B or A' * X = B.
%
%    The system matrix must have been factored by DGECO or DGEFA.
%
%    A division by zero will occur if the input factor contains a
%    zero on the diagonal.  Technically this indicates singularity
%    but it is often caused by improper arguments or improper
%    setting of LDA.  It will not occur if the subroutines are
%    called correctly and if DGECO has set 0.0 < RCOND
%    or DGEFA has set INFO == 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the output from DGECO or DGEFA.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix A.
%
%    Input, integer IPVT(N), the pivot vector from DGECO or DGEFA.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, integer JOB.
%    0, solve A * X = B;
%    nonzero, solve A' * X = B.
%
%    Output, real B(N), the solution vector.
%

%
%  Solve A * X = B.
%
  if ( job == 0 )

    for k = 1 : n-1

      l = ipvt(k);
      t = b(l);

      if ( l ~= k )
        b(l) = b(k);
        b(k) = t;
      end

      b(k+1:n) = daxpy ( n-k, t, a(k+1:n,k)', 1, b(k+1:n), 1 );

    end

    for k = n : -1 : 1
      b(k) = b(k) / a(k,k);
      t = -b(k);
      b(1:k-1) = daxpy ( k-1, t, a(1:k-1,k)', 1, b(1:k-1), 1 );
    end

  else
%
%  Solve A' * X = B.
%
    for k = 1 : n
      t = b(1:k-1) * a(1:k-1,k);
      b(k) = ( b(k) - t ) / a(k,k);
    end

    for k = n-1 : -1 : 1

      b(k) = b(k) + b(k+1:n) * a(k+1:n,k);
      l = ipvt(k);

      if ( l ~= k )
        [ b(l), b(k) ] = r8_swap ( b(l), b(k) );
      end

    end

  end

  return
end


function [x_best, psi_best, out] = GLM_FY(mapp, lin_sym_solver, x0, options)
% GLM_FY is a Levenberg-Marquardt algorithm for solving systems of
% nonlinear equations :math:`h(x) = 0`, `x` in :math:`R^m`
% using the nonlinear unconstrained minimization :math:`\textrm{min} \psi(x) = 1/2 ||h(x)||^2`
% s.t. `x` in :math:`R^m`.
%
% USAGE:
%
%    [x_best, psi_best, out] = GLM_FY(mapp, lin_sym_solver, x0, options)
%
% INPUTS:
%    mapp:              function handle provides `h(x)` and gradient `h(x)`
%    lin_sym_solver:    function handle for solving the linear system
%    x0:                initial point
%    options:           structure including the parameteres of scheme
%
%                         * .eta - parameter of the scheme
%                         * .MaxNumIter - maximum number of iterations
%                         * .MaxNumMapEval - maximum number of function evaluations
%                         * .MaxNumGmapEval - maximum number of subgradient evaluations
%                         * .TimeLimit - maximum running time
%                         * .epsilon - accuracy parameter
%                         * .x_opt - optimizer
%                         * .psi_opt - optimum
%                         * .adaptive - update lambda adaptively
%                         * .flag_x_error - 1: saves :math:`x_{error}`, 0: do not saves :math:`x_{error}` (default)
%                         * .flag_psi_error - 1: saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                         * .flag_time - 1: saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                         * .Stopping_Crit - stopping criterion
%
%                           1. stop if :math:`||grad|| \leq \epsilon`
%                           2. stop if :math:`||nhxk|| \leq \epsilon`
%                           3. stop if `MaxNumIter` is reached
%                           4. stop if `MaxNumMapEval` is reached
%                           5. stop if `MaxNumGmapEval` is reached
%                           6. stop if `TimeLimit` is reached
%                           7. stop if :math:`||grad|| \leq \textrm{max}(\epsilon, \epsilon^2 * ngradx0)`
%                           8. stop if :math:`||nhxk|| \leq \textrm{max}(\epsilon, \epsilon^2 * nhx0)`
%                           9. stop if (default) :math:`||hxk|| \leq \epsilon` or `MaxNumIter` is reached
%
% OUTPUTS:
%    x_best:            the best approximation of the optimizer
%    psi_best:          the best approximation of the optimum
%    out:               structure including more output information
%
%                         * .T - running time
%                         * .Niter - total number of iterations
%                         * .Nmap - total number of mapping evaluations
%                         * .Ngmap - total number of mapping gradient evaluations
%                         * .merit_func - array including all merit function values
%                         * .x_error - relative error :math:`\textrm{norm}(x_k(:)-x_{opt}(:))/\textrm{norm}(x_{opt})`
%                         * .psi_error - relative error :math:`(\psi_k-\psi_{opt})/(\psi_0-\psi_{opt}))`
%                         * .Status - reason of termination
%
% .. REFERENCE:
% .. J. Fan, Y. Yuan, On  the  quadratic  convergence  of  the Levenberg-Marquardt method without nonsingularity assumption, Computing, 74(1), 23-39  (2005)
% .. Author: - Masoud Ahookhosh, System Biochemistry Group, Luxembourg Center for System Biomedicine, University of Luxembourg, Luxembourg
%            - Update: July 2017, M. Ahookhosh

format longG ;

% ================ Error messages for input and output =================
if nargin > 4
    error('The number of input arguments is more than what is needed');
elseif nargin < 4
    error('The number of input arguments is not enough');
end;

if isempty(mapp)
    error('the function handle mapp has to be defined');
elseif ~isa(mapp,'function_handle')
    error('mapp should be a function handle');
end

if isempty(lin_sym_solver)
    error('the function handle lin_sym_solver has to be defined');
elseif ~isa(lin_sym_solver,'function_handle')
    error('lin_sym_solver should be a function handle');
end

if isempty(x0)
    error('The starting point x0 has to be defined');
elseif ~isa(x0,'numeric')
    error('x0 should be a numeric vector');
end

% =================== initializing the parameters ======================
% ===== user has requested viewing the default values of "options" =====
[eta,epsilon,MaxNumIter,MaxNumMapEval,MaxNumGmapEval, adaptive, ...
    TimeLimit,flag_x_error,flag_psi_error,flag_time,Stopping_Crit] ...
    = Initialization(options);

if ~isa(eta,'numeric') || (eta <= 0)
    error('eta should be numeric and eta in (0,4*delta)');
end

if isfield(options,'x_opt')
    x_opt=options.x_opt;
elseif flag_x_error==1
    error('x_error requires to x_opt be specified');
end

if flag_x_error == 1
    Nxopt      = sqrt(sum(x_opt(:).^2));
    x_error(1) = sqrt(sum((x0(:)-x_opt(:)).^2))/Nxopt;
end

if flag_psi_error == 1
    psi_error(1) = 1;
end

if flag_time == 1
    Time(1) = 0;
end

xk         = x0;
Niter      = 1;
[hxk,ghxk] = mapp(x0);
Nmap       = 1;
Ngmap      = 1;
grad       = ghxk*hxk;
nhxk       = sqrt(sum(hxk.^2));
nhx0       = nhxk;
ngradx0    = sqrt(sum(grad.^2));
I          = eye(length(xk));
psik       = 0.5*nhxk^2;
merit_func = psik;
sigma      = 1e-2;
rho        = 0.2;
gamma      = 0.5; % ???
max_inner  = 5;
StopFlag   = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%% Main body of GLM_FY.m %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T0 = tic;

% ======================= start of the main loop =======================
while ~StopFlag

    muk   = nhxk;
    Hk    = ghxk*ghxk'+muk*I;
    dk    = lin_sym_solver(Hk,grad);
    xk1   = xk+dk;
    hxk1  = mapp(xk1);
    Nmap  = Nmap+1;
    nhxk1 = norm(hxk1);
    if nhxk1 <= gamma*nhxk
        xk    = xk1;
        Niter = Niter+1;
        hxk   = mapp(xk);
        Nmap  = Nmap+1;
        nhxk  = norm(hxk);
        psik  = 0.5*nhxk^2;
    else
        psik1       = 0.5*nhxk1^2;
        alphak      = 1;
        inner_count = 0;
        while ((psik1-psik)>=sigma*alphak*(grad'*dk)&& ...
                inner_count<= max_inner)
            alphak      = rho*alphak;
            xk1         = xk+alphak*dk;
            hxk1        = mapp(xk1);
            Nmap        = Nmap+1;
            nhxk1       = norm(hxk1);
            psik1       = 0.5*nhxk1^2;
            inner_count = inner_count+1;
        end
        xk    = xk1;
        Niter = Niter+1
        nhxk  = nhxk1;
        psik  = psik1;
    end
    [hxk,ghxk] = mapp(xk);
    Nmap       = Nmap+1;
    Ngmap      = Ngmap+1;
    grad       = ghxk*hxk;

    % ================= Gathering output information ===================
    merit_func(Niter) = psik;
    if flag_time == 1
        Time(Niter+1) = toc(T0);
    end

    if flag_x_error == 1
        Nx_opt = norm(x_opt);
        x_error(Niter+1) = sqrt(sum((xk(:)-x_opt(:)).^2))/Nx_opt;
    end

    if flag_psi_error == 1
        psi_error(Niter+1) = (psik-psi_opt)/(psi0-psi_opt);
    end

    % ================== checking stopping criteria ====================
    T = toc(T0);

    [StopFlag,Status] = StopCriterion(grad,nhxk,Niter,Nmap, ...
    Ngmap,MaxNumIter,MaxNumMapEval,MaxNumGmapEval,T,TimeLimit, ...
    epsilon,nhx0,ngradx0,Stopping_Crit);

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Outputs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Status
x_best         = xk;
psi_best       = psik;
out.T          = T;
out.nhx        = nhxk;
out.merit_func = merit_func';
out.Niter      = Niter;
out.Nmap       = Nmap;
out.Ngmap      = Ngmap;
out.Status     = Status;

if flag_x_error == 1
    out.x_error = x_error;
end
if flag_psi_error == 1
    out.psi_error = psi_error;
end
if flag_time == 1
    out.Time = Time;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%% End of GLM_FY.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%