function res=NODDI_erfi(x)
% %erfi(x). The Imaginary error function, as it is defined in Mathematica
% %erfi(z)==erf(iz)/i (z could be complex) using
% %the incomplete gamma function in matlab: gammainc
% %Using "@": erfi = @(x) real(-sqrt(-1).*sign(x).*gammainc(-x.^2,1/2))
% %Note: limit(x->0) erfi(x)/x -> 2/sqrt(pi)
%
% %Example 1:
% x=linspace(0.001,6,100);
% y=exp(-x.^2).*erfi(x)./2./x;
% figure(1), clf;plot(x,y*sqrt(pi))
%
% %Example 2:
% [x,y]=meshgrid(linspace(-3,3,180),linspace(-3,3,180));
% z=x+i*y;
% figure(1), clf;contourf(x,y,log(erfi(z)))
% axis equal;axis off

% MATLAB only:
% xc=5.7;%cut for asymptotic approximation (when x is real)
% res=~isreal(x).*(-(sqrt(-x.^2)./(x+isreal(x))).*gammainc(-x.^2,1/2))+...
%     isreal(x).*real(-sqrt(-1).*sign(x).*((x<xc).*gammainc(-x.^2,1/2))+...
%     (x>=xc).*exp(x.^2)./x/sqrt(pi));

try % FASTER AND COMPATIBLE WITH BOTH MATLAB AND OCTAVE:
    res = Faddeeva_erfi(x);
catch
    if ~moxunit_util_platform_is_octave
    xc=5.7;%cut for asymptotic approximation (when x is real)
    res=~isreal(x).*(-(sqrt(-x.^2)./(x+isreal(x))).*gammainc(-x.^2,1/2))+...
        isreal(x).*real(-sqrt(-1).*sign(x).*((x<xc).*gammainc(-x.^2,1/2))+...
        (x>=xc).*exp(x.^2)./x/sqrt(pi));
    else
        error('Faddeeva_erfi was not build correctly. run Faddeeva_build.m')
    end
end


function auc = aucroc(p_predicted, p_target, freq)

        % Count observations by class
        nTarget     = sum(freq .* p_target);
        nBackground = sum(freq .* (1-p_target));

        % Rank data
        R = tiedrank(p_predicted);  % 'tiedrank' from Statistics Toolbox
        [R_uniq, I, J] = unique(R);
        [R_mean,R_num] = grpstats(freq,R,{'mean','numel'});
        R_freq = R_mean .* R_num;

        % adjust for counts
        rank_start_freq = cumsum(R_freq);
        rank_start_freq = [0; rank_start_freq(1:(end-1))] + 1;
        rank_mean_freq = (2*rank_start_freq+R_freq-1)/2;

        rank_orig = rank_mean_freq(J);

        % Calculate AUC
        %Error = (sum(R(Actual == 1)) - (nTarget^2 + nTarget)/2) / (nTarget * nBackground);
        auc = (sum(rank_orig .* p_target .* freq) - (nTarget^2 + nTarget)/2) / (nTarget * nBackground);


function wavelet_test09 ( )

%*****************************************************************************80
%
%% WAVELET_TEST09 tests DAUB18_TRANSFORM and DAUB18_TRANSFORM_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'WAVELET_TEST09\n' );
  fprintf ( 1, '  DAUB18_TRANSFORM computes the DAUB18 transform of a vector.\n' );
  fprintf ( 1, '  DAUB18_TRANSFORM_INVERSE inverts it.\n' );
%
%  Random data.
%
  n = 16;
  seed = 123456789;
  [ u, seed ] = r8vec_uniform_01 ( n, seed );

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)       D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Constant signal.
%
  n = 8;
  u(1:n) = 1.0;

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Linear signal.
%
  n = 16;
  a_first = 1.0;
  a_last = n;
  u = linspace ( a_first, a_last, n );

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Quadratic data.
%
  n = 8;
  u(1) = 25.0;
  u(2) = 16.0;
  u(3) = 9.0;
  u(4) = 4.0;
  u(5) = 1.0;
  u(6) = 0.0;
  u(7) = 1.0;
  u(8) = 4.0;

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end

  return
end


% sgwt_randmat : Compute random  (Erdos-Renyi model) graph
%
% function A=sgwt_randmat(N,thresh)
%
% Inputs :
% N - number of vertices
% thresh - probability of connection of each edge
%
% Outputs :
% A - adjacency matrix

% This file is part of the SGWT toolbox (Spectral Graph Wavelet Transform toolbox)
% Copyright (C) 2010, David K. Hammond.
%
% The SGWT toolbox 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 SGWT toolbox 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 SGWT toolbox.  If not, see <http://www.gnu.org/licenses/>.

function [A]=sgwt_randmat(N,thresh)
  assert(thresh<=1 && thresh>=0);
  A=rand(N)>1-thresh;
  B=triu(A);
  A=B+B';
  for i=1:size(A,1)
    A(i,i)=0;
  end
  A=sparse(A);


function [ row, col, a, seed ] = wathen_st ( nx, ny, nz_num, seed )

%*****************************************************************************80
%
%% WATHEN_ST: Wathen matrix stored in sparse triplet format.
%
%  Discussion:
%
%    When dealing with sparse matrices in MATLAB, it can be much more efficient
%    to work first with a triple of I, J, and X vectors, and only once
%    they are complete, convert to MATLAB's sparse format.
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    The matrix is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    The local element numbering is
%
%      3--2--1
%      |     |
%      4     8
%      |     |
%      5--6--7
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%    The matrix is symmetric positive definite for any positive values of the
%    density RHO(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2014
%
%  Author:
%
%    Original MATLAB version by Nicholas Higham.
%    Modifications by Tim Davis.
%    Modifications by John Burkardt.
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Input, integer NZ_NUM, the number of values used to describe the matrix.
%
%    Input/output, integer SEED, the random number seed.
%
%    Output, integer ROW(NZ_NUM), COL(NZ_NUM), the row and column indices
%    of the nonzero entries.
%
%    Output, real A(NZ_NUM), the nonzero values.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WATHEN_ST - Fatal error!\n' );
    fprintf ( 1, '  Not enough input.\n' );
    error ( 'WATHEN_ST - Fatal error!' );
  end

  if ( nargin < 2 )
    ny = nx;
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NY was not supplied.  Setting NY = NX = %d.\n', ny );
  end

  if ( nargin < 3 )
    nz_num = wathen_st_size ( nx, ny );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NZ_NUM was not supplied.  NZ_NUM = %d\n', nz_num );
  end

  if ( nargin < 4 )
    seed = 123456789;
  end

  em = [
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0;
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0;
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0;
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0;
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0;
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0;
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0;
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ]';

  row = zeros(nz_num,1);
  col = zeros(nz_num,1);
  a = zeros(nz_num,1);

  node = zeros(8,1);

  k = 0;

  for j = 1 : ny
    for i = 1 : nx

      node(1) = 3 * j * nx + 2 * i + 2 * j + 1;
      node(2) = node(1) - 1;
      node(3) = node(2) - 1;
      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(5) = 3 * ( j - 1 ) * nx + 2 * i + 2 * j - 3;
      node(6) = node(5) + 1;
      node(7) = node(6) + 1;
      node(8) = node(4) + 1;

      [ rho, seed ] = r8_uniform_01 ( seed );
      rho = 100.0 * rho;

      for krow = 1 : 8
        for kcol = 1 : 8
          k = k + 1;
          row(k) = node(krow);
          col(k) = node(kcol);
          a(k) = rho * em(krow,kcol);
        end
      end

    end
  end

  return
end


classdef MW11 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Ma and Wang

%------------------------------- Reference --------------------------------
% Z. Ma and Y. Wang, Evolutionary constrained multiobjective optimization:
% Test suite construction and performance comparisons. IEEE Transactions on
% Evolutionary Computation, 2019, 23(6): 972-986.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 15; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            g = 1 + sum(2*(X(:,obj.M:end) + (X(:,obj.M-1:end-1) - 0.5).^2 - 1).^2,2);
            PopObj(:,1) = g.*X(:,1)*sqrt(1.9999);
            PopObj(:,2) = g.*sqrt(2 - (PopObj(:,1)./g).^2);
            PopCon(:,1) = -(3 - PopObj(:,1).^2 - PopObj(:,2)).*(3 - 2*PopObj(:,1).^2 - PopObj(:,2));
            PopCon(:,2) = (3 - 0.625*PopObj(:,1).^2 - PopObj(:,2)).*(3 - 7*PopObj(:,1).^2 - PopObj(:,2));
            PopCon(:,3) = -(1.62 - 0.18*PopObj(:,1).^2 - PopObj(:,2)).*(1.125 - 0.125*PopObj(:,1).^2 - PopObj(:,2));
            PopCon(:,4) = (2.07 - 0.23*PopObj(:,1).^2 - PopObj(:,2)).*(0.63 - 0.07*PopObj(:,1).^2 - PopObj(:,2));
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1)  = (0:1/(N-1):1)';
            R(:,2)  = 1 - R(:,1);
            R       = R./repmat(sqrt(sum(R.^2,2)/2),1,2);
            c1      = (3 - R(:,1).^2 - R(:,2)).*(3 - 2*R(:,1).^2 - R(:,2));
            c2      = (3 - 0.625*R(:,1).^2 - R(:,2)).*(3 - 7*R(:,1).^2 - R(:,2));
            c3      = (1.62 - 0.18*R(:,1).^2 - R(:,2)).*(1.125 - 0.125*R(:,1).^2 - R(:,2));
            c4      = (2.07 - 0.23*R(:,1).^2 - R(:,2)).*(0.63 - 0.07*R(:,1).^2 - R(:,2));
            invalid = c1<0 | c2>0 | c3<0 | c4>0;
            while any(invalid)
                R(invalid,:) = R(invalid,:).*1.001;
                R(any(R>2.2,2),:) = [];
                c1      = (3 - R(:,1).^2 - R(:,2)).*(3 - 2*R(:,1).^2 - R(:,2));
                c2      = (3 - 0.625*R(:,1).^2 - R(:,2)).*(3 - 7*R(:,1).^2 - R(:,2));
                c3      = (1.62 - 0.18*R(:,1).^2 - R(:,2)).*(1.125 - 0.125*R(:,1).^2 - R(:,2));
                c4      = (2.07 - 0.23*R(:,1).^2 - R(:,2)).*(0.63 - 0.07*R(:,1).^2 - R(:,2));
                invalid = c1<0 | c2>0 | c3<0 | c4>0;
            end
            R = [R;1,1];
            R = R(NDSort(R,1)==1,:);
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [x,y] = meshgrid(linspace(0,2.1,400));
            z     = nan(size(x));
            fes1  = -(3-x.^2-y).*(3-2*x.^2-y) <= 0;
            fes2  = (3-0.625*x.^2-y).*(3-7*x.^2-y) <= 0;
            fes3  = -(1.62-0.18*x.^2-y).*(1.125-0.125*x.^2-y) <= 0;
            fes4  = (2.07-0.23*x.^2-y).*(0.63-0.07*x.^2-y) <= 0;
            z(fes1 & fes2 & fes3 & fes4 & x.^2+y.^2>=2) = 0;
            R = {x,y,z};
        end
    end
end


% op_makePhaseDrift.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,phDrift]=op_makePhaseDrift(in,totalDrift,noise);
%
% DESCRIPTION:
% Add phase drift to a dataset containing multiple averages.  This is generally
% used to generate simulated datasets with phase drift.
%
% INPUTS:
% in         = input data in matlab structure format.
% totalDrift = total amount of phase drift (in degrees) to add over the whole scan.
%               If totalDrift is a scalar, then a constant slope of drift
%               will be added.  If totalDrift is a vector or matrix with dimensions
%               equal to the dimensions of the input data, then this
%               vector specifies the drift applied to each average.
% noise      = the standard deviation of noise to add to the phase drift
%             function.
%
% OUTPUTS:
% out        = Output dataset with phase drift added.
% phDrift     = Vector of phase drift values that were added (in degrees).

function [out,phDrift]=op_makePhaseDrift(in,totalDrift,noise);
%out=op_makedrift(in,totalDrift);

%First make the matrices needed for multiplication
if any(totalDrift)
    if isequal(size(totalDrift),[in.averages/in.subspecs,in.subspecs])
        ph=totalDrift;
    else
        ph=linspace(0,totalDrift,in.averages*in.subspecs);
        ph=reshape(ph,in.subspecs,in.averages);
        ph=ph';
    end
else
    ph=zeros(in.averages,in.subspecs);
end
phnoise=noise*randn(size(ph));
phDrift=ph+phnoise;
%PH=repmat(phDrift',in.sz(1),1);
phDrift_shft=shiftdim(phDrift,-1);
PH=repmat(phDrift_shft,in.sz(1),1,1);

%Now apply the drift to the fids;
fids=in.fids.*exp(-1i*PH*pi/180);

%Now re-calculate specs using ifft
specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

%FILLING IN DATA STRUCTURES
out=in;
out.fids=fids;
out.specs=specs;

%FILLING IN THE FLAGS
out.flags=in.flags;


function [reg_c,rho_c,eta_c] = l_corner(rho,eta,reg_param,U,s,b,method,M)
%L_CORNER Locate the "corner" of the L-curve.
%
% [reg_c,rho_c,eta_c] =
%        l_corner(rho,eta,reg_param)
%        l_corner(rho,eta,reg_param,U,s,b,method,M)
%        l_corner(rho,eta,reg_param,U,sm,b,method,M) ,  sm = [sigma,mu]
%
% Locates the "corner" of the L-curve in log-log scale.
%
% It is assumed that corresponding values of || A x - b ||, || L x ||,
% and the regularization parameter are stored in the arrays rho, eta,
% and reg_param, respectively (such as the output from routine l_curve).
%
% If nargin = 3, then no particular method is assumed, and if
% nargin = 2 then it is issumed that reg_param = 1:length(rho).
%
% If nargin >= 6, then the following methods are allowed:
%    method = 'Tikh'  : Tikhonov regularization
%    method = 'tsvd'  : truncated SVD or GSVD
%    method = 'dsvd'  : damped SVD or GSVD
%    method = 'mtsvd' : modified TSVD,
% and if no method is specified, 'Tikh' is default.  If the Spline Toolbox
% is not available, then only 'Tikh' and 'dsvd' can be used.
%
% An eighth argument M specifies an upper bound for eta, below which
% the corner should be found.

% Per Christian Hansen, IMM, July 26, 2007.
%
%
% This file is part of regtools [1] and covered by the BSD License
%
% Copyright (c) 2008, Per Christian Hansen
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
%
% References:
% [1] http://www.mathworks.com/matlabcentral/fileexchange/file_infos/52-regtools

% Set default regularization method.
if (nargin <= 3)
  method = 'none';
  if (nargin==2), reg_param = (1:length(rho))'; end
else
  if (nargin==6), method = 'Tikh'; end
end

% Set this logical variable to 1 (true) if the corner algorithm
% should always be used, even if the Spline Toolbox is available.
alwayscorner = 0;

% Set threshold for skipping very small singular values in the
% analysis of a discrete L-curve.
s_thr = eps;  % Neglect singular values less than s_thr.

% Set default parameters for treatment of discrete L-curve.
deg   = 2;  % Degree of local smooting polynomial.
q     = 2;  % Half-width of local smoothing interval.
order = 4;  % Order of fitting 2-D spline curve.

% Initialization.
if (length(rho) < order)
  error('Too few data points for L-curve analysis')
end
if (nargin > 3)
  [p,ps] = size(s); [m,n] = size(U);
  beta = U'*b;
  if (m>n), b0 = b - U*beta; end
  if (ps==2)
    s = s(p:-1:1,1)./s(p:-1:1,2);
    beta = beta(p:-1:1);
  end
  xi = beta./s;
end

% Restrict the analysis of the L-curve according to M (if specified).
if (nargin==8)
  index = find(eta < M);
  rho = rho(index); eta = eta(index); reg_param = reg_param(index);
end

if (strncmp(method,'Tikh',4) | strncmp(method,'tikh',4))

  % The L-curve is differentiable; computation of curvature in
  % log-log scale is easy.

  % Compute g = - curvature of L-curve.
  g = lcfun(reg_param,s,beta,xi);

  % Locate the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  [gmin,gi] = min(g);
  reg_c = fminbnd('lcfun',...
    reg_param(min(gi+1,length(g))),reg_param(max(gi-1,1)),...
    optimset('Display','off'),s,beta,xi); % Minimizer.
  kappa_max = - lcfun(reg_c,s,beta,xi); % Maximum curvature.

  if (kappa_max < 0)
    lr = length(rho);
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    f = (s.^2)./(s.^2 + reg_c^2);
    eta_c = norm(f.*xi);
    rho_c = norm((1-f).*beta);
    if (m>n), rho_c = sqrt(rho_c^2 + norm(b0)^2); end
  end

elseif (strncmp(method,'tsvd',4) | strncmp(method,'tgsv',4) | ...
        strncmp(method,'mtsv',4) | strncmp(method,'none',4))

  % Use the adaptive pruning algorithm to find the corner, if the
  % Spline Toolbox is not available.
  if ~exist('splines','dir') | alwayscorner
    %error('The Spline Toolbox in not available so l_corner cannot be used')
    reg_c = corner(rho,eta);
    rho_c = rho(reg_c);
    eta_c = eta(reg_c);
    return
  end

  % Othersise use local smoothing followed by fitting a 2-D spline curve
  % to the smoothed discrete L-curve. Restrict the analysis of the L-curve
  % according to s_thr.
  if (nargin > 3)
    if (nargin==8)       % In case the bound M is in action.
      s = s(index,:);
    end
    index = find(s > s_thr);
    rho = rho(index); eta = eta(index); reg_param = reg_param(index);
  end

  % Convert to logarithms.
  lr = length(rho);
  lrho = log(rho); leta = log(eta); slrho = lrho; sleta = leta;

  % For all interior points k = q+1:length(rho)-q-1 on the discrete
  % L-curve, perform local smoothing with a polynomial of degree deg
  % to the points k-q:k+q.
  v = (-q:q)'; A = zeros(2*q+1,deg+1); A(:,1) = ones(length(v),1);
  for j = 2:deg+1, A(:,j) = A(:,j-1).*v; end
  for k = q+1:lr-q-1
    cr = A\lrho(k+v); slrho(k) = cr(1);
    ce = A\leta(k+v); sleta(k) = ce(1);
  end

  % Fit a 2-D spline curve to the smoothed discrete L-curve.
  sp = spmak((1:lr+order),[slrho';sleta']);
  pp = ppbrk(sp2pp(sp),[4,lr+1]);

  % Extract abscissa and ordinate splines and differentiate them.
  % Compute as many function values as default in spleval.
  P     = spleval(pp);  dpp   = fnder(pp);
  D     = spleval(dpp); ddpp  = fnder(pp,2);
  DD    = spleval(ddpp);
  ppx   = P(1,:);       ppy   = P(2,:);
  dppx  = D(1,:);       dppy  = D(2,:);
  ddppx = DD(1,:);      ddppy = DD(2,:);

  % Compute the corner of the discretized .spline curve via max. curvature.
  % No need to refine this corner, since the final regularization
  % parameter is discrete anyway.
  % Define curvature = 0 where both dppx and dppy are zero.
  k1    = dppx.*ddppy - ddppx.*dppy;
  k2    = (dppx.^2 + dppy.^2).^(1.5);
  I_nz  = find(k2 ~= 0);
  kappa = zeros(1,length(dppx));
  kappa(I_nz) = -k1(I_nz)./k2(I_nz);
  [kmax,ikmax] = max(kappa);
  x_corner = ppx(ikmax); y_corner = ppy(ikmax);

  % Locate the point on the discrete L-curve which is closest to the
  % corner of the spline curve.  Prefer a point below and to the
  % left of the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  if (kmax < 0)
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    index = find(lrho < x_corner & leta < y_corner);
    if ~isempty(index)
      [dummy,rpi] = min((lrho(index)-x_corner).^2 + (leta(index)-y_corner).^2);
      rpi = index(rpi);
    else
      [dummy,rpi] = min((lrho-x_corner).^2 + (leta-y_corner).^2);
    end
    reg_c = reg_param(rpi); rho_c = rho(rpi); eta_c = eta(rpi);
  end

elseif (strncmp(method,'dsvd',4) | strncmp(method,'dgsv',4))

  % The L-curve is differentiable; computation of curvature in
  % log-log scale is easy.

  % Compute g = - curvature of L-curve.
  g = lcfun(reg_param,s,beta,xi,1);

  % Locate the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  [gmin,gi] = min(g);
  reg_c = fminbnd('lcfun',...
    reg_param(min(gi+1,length(g))),reg_param(max(gi-1,1)),...
    optimset('Display','off'),s,beta,xi,1); % Minimizer.
  kappa_max = - lcfun(reg_c,s,beta,xi,1); % Maximum curvature.

  if (kappa_max < 0)
    lr = length(rho);
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    f = s./(s + reg_c);
    eta_c = norm(f.*xi);
    rho_c = norm((1-f).*beta);
    if (m>n), rho_c = sqrt(rho_c^2 + norm(b0)^2); end
  end

else
  error('Illegal method')
end


%H2E Homogeneous to Euclidean
%
% E = H2E(H) is the Euclidean version (K-1xN) of the homogeneous
% points H (KxN) where each column represents one point in P^K.
%
% Reference::
% - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p604.
%
% See also E2H.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function e = h2e(h)

    if isvector(h)
        h = h(:);
    end
    e = h(1:end-1,:) ./ repmat(h(end,:), numrows(h)-1, 1);


function r8poly_lagrange_0_test ( )

%*****************************************************************************80
%
%% R8POLY_LAGRANGE_0_TEST tests R8POLY_LAGRANGE_0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  npol = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8POLY_LAGRANGE_0_TEST\n' );
  fprintf ( 1, '  R8POLY_LAGRANGE_0 evaluates the Lagrange\n' );
  fprintf ( 1, '  factor W(X) at a point.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of data points is %d\n', npol );
%
%  Set the abscissas of the polynomials.
%
  xlo = 0.0;
  xhi = npol - 1;

  xpol = r8vec_even ( npol, xlo, xhi );

  r8vec_print ( npol, xpol, '  Abscissas:' );
%
%  Evaluate W(X).
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X          W(X)\n' );
  fprintf ( 1, '\n' );

  nx = 4 * npol - 1;

  for ival = 1 : nx

    xval = r8vec_even_select ( nx, xlo, xhi, ival );

    w = r8poly_lagrange_0 ( npol, xpol, xval );

    fprintf ( 1, '%12f  %12e\n', xval, w );

  end

  return
end


function res = calc_pot_values(w)
%
%calculate potential values for non obstacle and foal locations in given world
%
r = size(w,1);
c = size(w,2);
for t = size(w,3):-1:1
    for x = 1:r
        for y = 1:c
            if w(x,y,t) == -1
                n = w(x,y,t+1);
                if x ~= 1
                    n = n + w(x-1,y,t+1);
                else
                    n = n + 1;% boundary condition
                end
                if x ~= r
                    n = n + w(x+1,y,t+1);
                else
                    n = n + 1;
                end
                if y ~= 1
                    n = n + w(x,y-1,t+1);
                else
                    n = n + 1;
                end
                if y ~= c
                    n = n + w(x,y+1,t+1);
                else
                    n = n + 1;
                end
                w(x,y,t) = n / 5;% take average of neighbors
            end
        end
    end
end
res = w;
end


function r = randp(lambda)
% randp(lambda) returns Poisson distributed Vector with mean lambda

try
  r = poissrnd(lambda);
  return
end

lambda   = reshape(lambda,1,[]);
aktiv    = find(lambda > 1e-10);
ll       = log(lambda(aktiv));
r(aktiv) = rand(1,length(aktiv));
r(find(lambda < 1e-10)) = 0;

i = 0;
while length(aktiv) > 0
		r(aktiv) = r(aktiv) - exp(i*ll - lambda(aktiv) - gammaln(i+1));
		ind = find(r(aktiv)<=0);
		r(aktiv(ind)) = i;
		aktiv(ind) = [];
		ll(ind) = [];
		i = i + 1;
end


function phi = correct_azimuth(phi)
%CORRECT_AZIMUTH ensures azimuth angle between -pi and +pi-eps
%
%   Usage: phi = correct_azimuth(phi)
%
%   Input parameters:
%       phi     - azimuth / rad. Can be a single value or a matrix.
%
%   Output paramteres:
%       phi     - angle between -pi and +pi-eps / rad
%
%   See also: correct_elevation, get_ir

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Computation ====================================================
% Ensure -2pi <= phi <= 2pi
phi = rem(phi,2*pi);
% Ensure -pi <= phi < pi
phi(phi<-pi) = phi(phi<-pi) + 2*pi;
phi(phi>=pi) = phi(phi>=pi) - 2*pi;


function plotConfMat(varargin)
%PLOTCONFMAT plots the confusion matrix with colorscale, absolute numbers
%   and precision normalized percentages
%
%   usage:
%   PLOTCONFMAT(confmat) plots the confmat with integers 1 to n as class labels
%   PLOTCONFMAT(confmat, labels) plots the confmat with the specified labels
%
%   Vahe Tshitoyan
%   20/08/2017
%
%   Arguments
%   confmat:            a square confusion matrix
%   labels (optional):  vector of class labels

% number of arguments
switch (nargin)
    case 0
       confmat = 1;
       labels = {'1'};
    case 1
       confmat = varargin{1};
       labels = 1:size(confmat, 1);
    otherwise
       confmat = varargin{1};
       labels = varargin{2};
end

confmat(isnan(confmat))=0; % in case there are NaN elements
numlabels = size(confmat, 1); % number of labels

% calculate the percentage accuracies
confpercent = 100*confmat./repmat(sum(confmat, 1),numlabels,1);

% plotting the colors
imagesc(confpercent);
title(sprintf('Accuracy: %.2f%%', 100*trace(confmat)/sum(confmat(:))));
%ylabel('Predicted Class'); xlabel('Target Class');
ylabel('Predicted Class'); xlabel('Target Class');

% set the colormap
colormap(flipud(gray));

% Create strings from the matrix values and remove spaces
textStrings = num2str([confpercent(:), confmat(:)], '%.1f%%\n%d\n');
textStrings = strtrim(cellstr(textStrings));

% Create x and y coordinates for the strings and plot them
[x,y] = meshgrid(1:numlabels);
hStrings = text(x(:),y(:),textStrings(:), ...
    'HorizontalAlignment','center');

% Get the middle value of the color range
midValue = mean(get(gca,'CLim'));

% Choose white or black for the text color of the strings so
% they can be easily seen over the background color
textColors = repmat(confpercent(:) > midValue,1,3);
set(hStrings,{'Color'},num2cell(textColors,2));

% Setting the axis labels
set(gca,'XTick',1:numlabels,...
    'XTickLabel',labels,...
    'YTick',1:numlabels,...
    'YTickLabel',labels,...
    'TickLength',[0 0]);
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% problem 11 - graph of r[n] and of r[n+1]

% r[n]
 n=-3:6;
 u=(n>=0);
 r=n.*u;

 stem(n,r)
 title('Unit ramp sequence r[n]')
 ylim([-.1 6.1])

% r[n+1]
 figure
 n=-3:6;
 u1=(n>=-1);
 r=(n+1).*u1;

 stem(n,r)
 title('Unit ramp sequence r[n+1]')
 ylim([-.1 7.1])


function [ NRI , pval ] = NetReclassificationImprovement( pred_old , pred_new , outcome )
% Net Reclassification Index (NRI)
% Compare classification of an old vs new classification technique
% Usage:
%   [ NRI , pval ] = NetReclassificationImprovement( pred_old , pred_new , outcome )
%
% http://www.epibiostat.ucsf.edu/courses/RoadmapK12/SIGS/Pencina.pdf
% Statist. Med. (in press) (www.interscience.wiley.com) DOI: 10.1002/sim.2929
% Evaluating the added predictive ability of a new marker: From area under the ROC curve to reclassi�cation and beyond
% Michael J. Pencina Ralph B. D'agostino Sr Ralph B. D'Agostino Jr and Ramachandran S. Vasan
%
% (c) Louis Mayaud, 2011 (louis.mayaud@gmail.com)
% Please reference :
% Mayaud, Louis, et al. "Dynamic Data During Hypotensive Episode Improves
%        Mortality Predictions Among Patients With Sepsis and Hypotension*."
%        Critical care medicine 41.4 (2013): 954-962.

% Remove NaNs
Idx = (isnan(pred_old) & isnan(pred_new));
pred_old(Idx) = [];
pred_new(Idx) = [];

% Need to define the following probabilities
PEventUp = sum(outcome==1 & pred_old==0 & pred_new==1)/sum(outcome==1);
PEventDown = sum(outcome==1 & pred_old==1 & pred_new==0)/sum(outcome==1);
PNoneventUp= sum(outcome==0 & pred_old==0 & pred_new==1)/sum(outcome==0);
PNoneventDown= sum(outcome==0 & pred_old==1 & pred_new==0)/sum(outcome==0);

NRI = (PEventUp - PEventDown) - (PNoneventUp - PNoneventDown) ;
z = abs(NRI)/sqrt( (PEventUp + PEventDown)/sum(outcome==1) + (PNoneventUp + PNoneventDown)/sum(outcome==0)  );
[~,pval] = ztest(z,0,1);

% Implemented from Mc Nemar 1947, as detailed in
% Biometrics, 66, 1185-1191, 2010 Dec.
% Westfall et al.
% N01 = sum(pred_old==0 & pred_new==1);
% N10 = sum(pred_old==1 & pred_new==0);
% Nd = N01 + N10 ;
%
% pval = binocdf(N01,Nd,0.5);


function [f, label] = gsp_jtv_fa(G,shift)
%GSP_JTV_FA Frequency axis for the joint time vertex framework
%   Usage:  f = gsp_jtv_fa(G);
%
%   Input parameters:
%       G       : Time-vertex graph structure
%       shift   : Boolean value: 1 to apply fftshift on the frequency vector, 0 otherwise
%   Ouput parameters:
%       f       : Frequency axis (row vector)
%

% Author: Nathanael Perraudin, Francesco Grassi
% Date  : September 2016

if nargin<2
    shift = 0;
end

if ~gsp_check_jtv(G)
    error('GSP_JTV_FA needs the time dimension. Use GSP_JTV_GRAPH');
end

if isempty(G.jtv.NFFT)
    NFFT = G.jtv.T;
else
    NFFT = G.jtv.NFFT;
end

if shift
    f = fftshift(gsp_cfa( NFFT,G.jtv.fs )');
else
    f = gsp_cfa( NFFT,G.jtv.fs )';
end

label = '\omega';

end


function pitchwatch(x,Ts)
% Plot the pitch keys.
% pitchwatch(x,[Ts])
%
% :: Syntax
%    The array x is the input signal and Ts is the (optional) sampling period.
%    Example on use: [x,Fs] = wavread('Hum.wav');
%                    pitchwatch(x,1/Fs);
%
% :: Information
%    Make your own wav-files with the Windows Sound Recorder. Choose the attributes
%    PCM 8000Hz, 16bit, Mono when saving the wav-file. For more information on pitch
%    keys, go to http://www.bookrags.com/wiki/Piano_key_frequencies.

% Set parameters.
if nargin < 2, Ts = 1/8000; end

% Set constants.
xlen = length(x);
wlen = 512;
wlag = 128;

% Zero-pad signal.
xpad = wlag-rem(xlen-wlen-1,wlag)-1;
x    = [x(:); zeros(xpad,1)];
L    = (xlen+xpad-wlen)/wlag+1;
L    = L-wlen/wlag/2;

% Calculate the AMDF and AMDF-fractional pitch periods.
for k1 = 1:L
   k2 = (k1-1)*wlag;
   for k3 = 1:wlen/2-1, c(k3) = sum(abs(x(k2+(1:wlen))-x(k2+k3+(1:wlen)))); end

   n = findpeaks(-c);
   if ~isempty(n), n(find(c(n) > mean(c(n))-sqrt(var(c(n))))) = []; end
   if ~isempty(n), t0 = n(1);
   else,           t0 = []; end

   if ~isempty(t0)
      u1    = x(k2+t0+(1:wlen))-x(k2+(1:wlen));
      u2    = x(k2+t0+(1:wlen))-x(k2+t0+1+(1:wlen));
      t1    = sum(u1.*u2)/sum(u2.*u2);
      y(k1) = 1/Ts/(t0+t1);
   else
      y(k1) = NaN;
   end
end

% Plot pitch keys.
keys = {'C','C#','D','D#','E','F','F#','G','G#','A','A#','B'};
key0 = 12*log2(y/440)+57;
u1   = max(0,min(floor(key0)));
u2   = min(96,max(ceil(key0)));
key1 = mod([u1:u2],12)+1;
key2 = floor([u1:u2]/12);
for k = [1:u2-u1+1], tick(k) = strcat(keys(key1(k)),num2str(key2(k))); end
figure, plot((0.5+[0:L-1])*wlag*Ts,key0,'b.');
set(gca,'FontSize',8,'XLim',[0,L*wlag]*Ts,'YLim',[u1,u2],'YTick',[u1:u2],'YTickLabel',tick);
xlabel('Time'); ylabel('Key');

% FUNCTIONS

function n = findpeaks(x)

n    = find(diff(diff(x) > 0) < 0);
u    = find(x(n+1) > x(n));
n(u) = n(u)+1;


function c = r8mat_add ( m, n, alpha, a, beta, b )

%*****************************************************************************80
%
%% R8MAT_ADD computes C = alpha * A + beta * B for R8MAT's.
%
%  Discussion:
%
%    An R8MAT is an array of R8 values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real ALPHA, the multiplier for A.
%
%    Input, real A(M,N), the first matrix.
%
%    Input, real BETA, the multiplier for A.
%
%    Input, real B(M,N), the second matrix.
%
%    Output, real C(M,N), the sum of alpha*A+beta*B.
%
  c(1:m,1:n) = alpha * a(1:m,1:n) + beta * b(1:m,1:n);

  return
end


function [Q, V, policy, mean_discrepancy] = mdp_MK_learning(P, R, discount, N)

% mdp_Q_learning   Evaluation of the matrix Q, using the Q learning algorithm
%
% Arguments
% -------------------------------------------------------------------------
% Let S = number of states, A = number of actions
%   P(SxSxA)  = transition matrix
%              P could be an array with 3 dimensions or
%              a cell array (1xA), each cell containing a sparse matrix (SxS)
%   R(SxSxA) or (SxA) = reward matrix
%              R could be an array with 3 dimensions (SxSxA) or
%              a cell array (1xA), each cell containing a sparse matrix (SxS) or
%              a 2D array(SxA) possibly sparse
%   discount  = discount rate in ]0; 1[
%   N(optional) = number of iterations to execute, default value: 10000.
%                 It is an integer greater than the default value.
% Evaluation --------------------------------------------------------------
%   Q(SxA) = learned Q matrix
%   V(S)   = learned value function.
%   policy(S) = learned optimal policy.
%   mean_discrepancy(N/100) = vector of V discrepancy mean over 100 iterations
%             Then the length of this vector for the default value of N is 100.

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    * Redistributions of source code must retain the above copyright notice,
%      this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
% OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
% OF THE POSSIBILITY OF SUCH DAMAGE.

% check of arguments
if (discount <= 0 || discount >= 1)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: Discount rate must be in ]0,1[')
    disp('--------------------------------------------------------')
elseif (nargin >= 4) && (N < 10000)
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: N must be upper than 10000')
    disp('--------------------------------------------------------')
else

    % initialization of optional arguments
    if (nargin < 4); N=10000; end;

    % Find number of states and actions
    if iscell(P)
        S = size(P{1},1);
        A = length(P);
    else
        S = size(P,1);
        A = size(R,2);
    end;

    % Initialisations
    Q = zeros(S,A);
    dQ = zeros(S,A);
    mean_discrepancy = [];
    discrepancy = [];

    % Initial state choice
    s = randi([1,S]);
            prob=[NaN NaN];
            while nansum(prob)==0
             s = randi([1,S]);
            [s1, act, prob]=find(P(:,s,:));  %which actions are possible in that state?
            end

    h = waitbar(0,'Initializing waitbar...');

    for n=1:N

        % Reinitialisation of trajectories every 100 transitions
        if (mod(n,100)==0);
            prob=[NaN NaN];
            while nansum(prob)==0
             s = randi([1,S]);
            [s1, act, prob]=find(P(:,s,:));  %which actions are possible in that state?
            end
            waitbar(n/N,h,n/N*100);
        end;

        % Action choice : greedy with increasing probability
        % probability 1-(1/log(n+2)) can be changed

        pn = rand(1);
%         if (pn < (1-(1/log(n+2))))
%           [~,a] = max(Q(s,:));
%         else
          a = act(randi([1,numel(act)]));
%         end;

        % Simulating next state s_new and reward associated to <s,s_new,a>
        p_s_new = rand(1);
        p = 0;
        s_new = 0;
        while ((p < p_s_new) && (s_new < S))
            s_new = s_new+1;
            if iscell(P)
                p = p + P{a}(s,s_new);
            else
                p = p + P(s,s_new,a);
            end;
        end;
        if iscell(R)
            r = R{a}(s,s_new);
        elseif ndims(R) == 3
            r = R(s,s_new,a);
        else
            r = R(s,a);
        end;

        % Updating the value of Q
        % Decaying update coefficient (1/sqrt(n+2)) can be changed
        delta = r + discount*max(Q(s_new,:)) - Q(s,a);
        dQ = (1/sqrt(n+2))*delta;
        Q(s,a) = Q(s,a) + dQ;

        % Current state is updated
        s = s_new;

        % Computing and saving maximal values of the Q variation
        discrepancy(mod(n,100)+1) = abs(dQ);

        % Computing means all over maximal Q variations values
        if (length(discrepancy) == 100)
           mean_discrepancy = [ mean_discrepancy mean(discrepancy)];
           discrepancy = [];
        end;

    end;

    %compute the value function and the policy
    [V, policy] = max(Q,[],2);
close(h);
end;


% See how well partial Kalman filter updates work

seed = 0;
rand('state', seed);
randn('state', seed);
nlandmarks = 6;
T = 12;

[A,B,C,Q,R,Qbig,Rbig,init_x,init_V,robot_block,landmark_block,...
	  true_landmark_pos, true_robot_pos, true_data_assoc, ...
	  obs_rel_pos, ctrl_signal] = mk_linear_slam(...
	      'nlandmarks', nlandmarks, 'T', T, 'ctrl', 'leftright', 'data-assoc', 'cycle');

% exact
[xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);

% approx
%k = nlandmarks-1; % exact
k = 3;
ndx = {};
for t=1:T
  landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
  tmp = [landmark_block(:, landmarks) robot_block'];
  ndx{t} = tmp(:);
end

[xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
			 'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
		       'ndx', ndx);

nrows = 10;
stepsize = T/(2*nrows);
ts = 1:stepsize:T;

if 1 % plot

clim = [0 max(max(Va(:,:,end)))];

figure(2)
if 0
  imagesc(Ve(1:2:end,1:2:end, T))
  clim = get(gca,'clim');
else
  i = 1;
  for t=ts(:)'
    subplot(nrows,2,i)
    i = i + 1;
    imagesc(Ve(1:2:end,1:2:end, t))
    set(gca, 'clim', clim)
    colorbar
  end
end
suptitle('exact')

figure(3)
if 0
  imagesc(Va(1:2:end,1:2:end, T))
  set(gca,'clim', clim)
else
  i = 1;
  for t=ts(:)'
    subplot(nrows,2,i)
    i = i+1;
    imagesc(Va(1:2:end,1:2:end, t))
    set(gca, 'clim', clim)
    colorbar
  end
end
suptitle('approx')

figure(4)
i = 1;
for t=ts(:)'
  subplot(nrows,2,i)
  i = i+1;
  Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
  imagesc(Vd)
  set(gca, 'clim', clim)
  colorbar
end
suptitle('diff')

end % all plot

for t=1:T
  %err(t)=rms(xa(:,t), xe(:,t));
  err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
end
figure(5);plot(err)
title('rms mean pos')

for t=1:T
  i = 1:2*nlandmarks;
  denom = Ve(i,i,t) + (Ve(i,i,t)==0);
  Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
  Verr(t) = max(Vd(:));
end
figure(6); plot(Verr)
title('max relative Verr')


% Small technical example.
% Shows how to retrieve the details on a rectangular grid, after a multi-
% resolution decomposition has been performed.
%
disp('Small technical example.');
disp('Shows how to retrieve the details on a rectangular grid, after a multi-');
disp('resolution decomposition has been performed.');
disp('FOR MORE INFORMATION:  help retrieveR');
disp(' ');
disp('See also the report http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04178D.pdf');
disp('Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>');
disp(' (C) 1998-2006 Stichting CWI, Amsterdam, The Netherlands');
disp(' ');
%---PARAMETERS-----------------------------------------------------------------
% How to execute, set parameters
N = 6;                   %  maximum level (even number) in lifting scheme
filtername = 'Neville4';
%
%---INSERT YOUR IMAGE HERE-----------------------------------------------------
if exist('imread','file') == 2
  Orig = double(imread('zenithgray.TIF','tiff'));
else
  load zenithgray; Orig = zenithgray; clear zenithgray;
end
%
disp([' Dimensions of original      ' int2str( size(Orig) )]);
%---DECOMPOSITION--------------------------------------------------------------
disp([' Filter type is ' filtername]);
[C,S] = QLiftDec2(Orig,N,filtername);
%
%---RETRIEVE APPROXIMATION-----------------------------------------------------
A = retrieveR(N, 'a', C, S);
sizeA = size(A);
%
%---RETRIEVE DETAIL AT EVEN LEVEL----------------------------------------------
level = N-2;
disp([' Level ' int2str(level)]);
D = retrieveR(level, 'd', C, S);
%
disp(['The dimensions of the detail function read as follows ' int2str(size(D))]);


%% BVAR tutorial: Bayesian Dynamic Factor model
% Author:   Filippo Ferroni and  Fabio Canova
% Date:     09/14/2021

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Estimation of a  static and  dynamic  factor  model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear; close all;  clc;

addpath ../../cmintools/
addpath ../../bvartools/

rng('default');
rng(999);

% generate factors/observables

% Setting the parameters
% AR1 factors
Phi    = [0.75 0.0;0.1 0.8];
% Cov factors innovation
Sigma  = [1 -0.2; -0.2 2];
Q      =  chol(Sigma,'lower');
% factor loadings
Lambda = [1 0;
          0.3 1;
          0.5 -1.5;
          -1.1 -0.5;
          1.5 1.5;
      5*randn(45,size(Phi,1))];
% persistence of idisyncratic errors
rho    = 0;
% standard deviation of idisyncratic errors
sig    = 1;

% preallocating memory
% sample length
T      = 301;
f = zeros(T,size(Phi,1));
y = zeros(T,size(Lambda,1));
e = zeros(T,size(Lambda,1));
i = zeros(T,size(Phi,1));

for t = 1 : T-1
    i(t+1,:) = randn(size(f,2),1)';
    f(t+1,:) = (Phi*f(t,:)' + Q*i(t+1,:)')';
    e(t+1,:) = (rho*e(t,:)' + sig*randn(size(Lambda,1),1))';
    y(t+1,:) = (Lambda*f(t+1,:)' + e(t+1,:)')';
end
f(1,:) = [];
y(1,:) = [];

subplot(2,1,1)
plot(y)
subplot(2,1,2)
plot(f)

% true IRFs
hor = 24;
ir_true=zeros(size(y,2), hor,size(f,2));
true    = iresponse(Phi,eye(size(f,2)),hor,Q);
Lam_    = repmat(Lambda,1,1,size(f,2));
if exist('pagemtimes','builtin') == 5
    ir_true = pagemtimes(Lam_,true);
else
    for ff = 1 : size(f,2)
        ir_true(:,:,ff) = Lambda * true(:,:,ff);
    end
end
pause;

%% principal components

nfac   = 2;
transf = 0;

[~,pc,~,egv,~] = pc_T(y,nfac,transf);

% scree plot
figure,
plot(egv(1:10),'b','Linewidth',2);
grid on
title('Eigenvalues')
% STR_RECAP = [ dirname '/screeplot'];
% saveas(gcf,STR_RECAP,'fig');
% savefigure_pdf([STR_RECAP '.pdf']);

%% static factor model

% 2 static factors
nfac = size(f,2);
lags = 0;
% priors
options.priors.F.Lambda.mean = 0;
options.priors.F.Lambda.cov  = 6;
% estimation command
[BSFM] = bdfm_(y,lags,nfac,options);
% consider a subset of draws
%index = 5000:20:size(BSFM.Phi_draws,3);
index = 1:1:size(BSFM.Phi_draws,3);
% plot estimated and true factors
figure,
for gg=1:nfac
    subplot(nfac,1,gg)
    % draws of factors
    plot(squeeze(BSFM.f_draws(:,gg,index)),'Color',[0.7 0.7 0.7])
    hold on
    % true factor
    f_mean = mean(BSFM.f_draws(:,gg,index),3);
    sign_ = sign(corr(f_mean,f(:,gg)));
    plot(sign_ * f(:,gg),'b','Linewidth',2)
    hold on
    plot(f_mean,'k','Linewidth',2)
end
% save plot
dirname  = 'factor_plt';
mkdir(dirname)
STR_RECAP = [ dirname '/sfm'];
saveas(gcf,STR_RECAP,'fig');
if strcmp(version('-release'),'2022b') == 0
	savefigure_pdf([STR_RECAP '.pdf']);
end
pause;

%% dynamic factor model
clear options
nfac   = size(f,2);
lags   = round(size(Phi,2)/nfac);

% Priors options
% factors priors
options.priors.F.Phi.mean    = Phi;
options.priors.F.Phi.cov     = 10 * eye(size(Phi,1));
options.priors.F.Sigma.scale = Sigma;
options.priors.F.Sigma.df    = 4;
options.priors.F.Lambda.mean = 0;
options.priors.F.Lambda.cov  = 6;
% else Jeffrey prior on PhiSigma: options.priors.name = 'Jeffrey';
% idyosincratic error priors
options.priors.G.Sigma.scale = sig;
options.priors.G.Sigma.df    = 4;
% IRF options
% IV identification
options.proxy    = i(lags+2:end,1);
% Sign identification
options.signs{1} = 'y(4,1:3,1)<0'; %
options.signs{2} = 'y(5,1:3,1)>0'; %
% estimation command
[BDFM] = bdfm_(y,lags,nfac,options);
% consider a subset of draws
% index = 5000:20:size(BDFM.Phi_draws,3);
index = 1:1:size(BDFM.Phi_draws,3);
% plot estimated and true factors
figure,
for gg=1:nfac
    subplot(nfac,1,gg)
    plot(squeeze(BDFM.f_draws(:,gg,index)),'Color',[0.7 0.7 0.7])
    hold on
    f_mean = mean(BDFM.f_draws(:,gg,index),3);
    sign_ = sign(corr(f_mean,f(:,gg)));
    % true factor
    plot(sign_ * f(:,gg),'b','Linewidth',2)
    hold on
    plot(f_mean,'k','Linewidth',2)
end
STR_RECAP = [ dirname '/dfm'];
saveas(gcf,STR_RECAP,'fig');
if strcmp(version('-release'),'2022b') == 0
	savefigure_pdf([STR_RECAP '.pdf']);
end

%%

Phi_m = mean(BDFM.Phi_draws(:,:,index),3);
Sig_m = mean(BDFM.Sigma_draws(:,:,index),3);
Ptt  = lyapunov_symm(Phi_m, Sig_m);
f_m  = mean(BDFM.f_draws(:,:,index),3);
figure,
for gg=1:nfac
    subplot(nfac,1,gg)
    plot([f_m(:,gg)-2*Ptt(gg,gg) f_m(:,gg)+2*Ptt(gg,gg)],'Color',[0.7 0.7 0.7],'Linewidth',2)
    hold on
    plot(f(:,gg),'b','Linewidth',2)
    hold on
    plot(f_m(:,gg),'k','Linewidth',2)
end

Phi0 = Phi';
jj = 0;
figure('Name','Phi')
for gg=1: nfac
    for hh =1 : nfac
        jj = jj+1;
        subplot(nfac,nfac,jj)
        histogram(squeeze(BDFM.Phi_draws(gg,hh,index)),40)
        hold on
        plot([Phi0(gg,hh) Phi0(gg,hh)],[0 50],'r','Linewidth',2)
        xlim([-1 1])
    end
end
%

figure('Name','Sigma')
Sigma0 = Q*Q';
jj = 0;
for gg=1:2
    for hh =1 :2
        jj = jj+1;
        subplot(nfac,nfac,jj)
        histogram(squeeze(BDFM.Sigma_draws(gg,hh,index)),40)
        hold on
        plot([Sigma0(gg,hh) Sigma0(gg,hh)],[0 50],'r','Linewidth',2)
    end
end
%
% figure('Name','Lambda')
% Lambda0 = Lambda;
% jj = 0;
% for gg=1:10
%     for hh =1 :2
%         jj = jj+1;
%         subplot(5,4,jj)
%         histogram(squeeze(BDFM.lambda_draws(gg,hh,index)),40)
%         hold on
%         plot([Lambda0(gg,hh) Lambda0(gg,hh)],[0 30],'r','Linewidth',2)
%     end
% end
%
% figure('Name','sigma_g')
% sig0 = sig*ones(size(Lambda,1),1);
% for jj=1:10
%     subplot(3,4,jj)
%     histogram(squeeze(BDFM.sigma_draws(jj,index)),40)
%     hold on
%     plot([sig0(jj) sig0(jj)],[0 30],'r','Linewidth',2)
% end

%% Plot IRFs

index_var          = [1 2 4 5 15 20];
index_sho          = 1;

% some options:
% add the true IRF
options.add_irfs   = ir_true(index_var,:,index_sho);
% additional 90% HPD set
options.conf_sig_2 = 0.9;
% additional 90% HPD set
options.nplots = [2 3];
% variables names for the plots
options.varnames      = {'Var1','Var2','Var4','Var5','Var15','Var20'};
% name of the directory where the figure is saved
options.saveas_dir    = './factor_plt';
% name of the figure to save
options.saveas_strng  = 'sign';
% sign restricted IRF
irfs_to_plot       = BDFM.irsign_draws(index_var,:,index_sho,:);
plot_irfs_(irfs_to_plot,options)

% IV IRF
irfs_to_plot_iv     = BDFM.irproxy_draws(index_var,:,index_sho,:);
% name of the figure to save
options.saveas_strng  = 'iv';
plot_irfs_(irfs_to_plot_iv,options)


function [warp,L,LnInv,bendE] = tpsGetWarp( lambda, xsS, ysS, xsD, ysD )
% Given two sets of corresponding points, calculates warp between them.
%
% Uses booksteins PAMI89 method.  Can then apply warp to a new set of
% points (tpsInterpolate), or even an image (tpsInterpolateIm).
%  "Principal Warps: Thin-Plate Splines and the Decomposition of
%  Deformations".  Bookstein.  PAMI 1989.
%
% USAGE
%  [warp,L,LnInv,bendE] = tpsGetWarp( lambda, xsS, ysS, xsD, ysD )
%
% INPUTS
%  lambda      - rigidity of warp (inf means warp becomes affine)
%  xsS, ysS    - [1xn] correspondence points from source image
%  xsD, ysD    - [1xn] correspondence points from destination image
%
% OUTPUTS
%  warp        - bookstein warping parameters
%  L, LnInv    - see bookstein
%  bendE       - bending energy
%
% EXAMPLE - 1
%  xsS=[0 -1 0 1];  ysS=[1 0 -1 0];  xsD=xsS;  ysD=[3/4 1/4 -5/4 1/4];
%  warp = tpsGetWarp( 0, xsS, ysS, xsD, ysD );
%  [gxs, gys] = meshgrid(-1.25:.25:1.25,-1.25:.25:1.25);
%  tpsInterpolate( warp, gxs, gys, 1 );
%
% EXAMPLE - 2
%  xsS = [3.6929 6.5827 6.7756 4.8189 5.6969];
%  ysS = [10.3819 8.8386 12.0866 11.2047 10.0748];
%  xsD = [3.9724 6.6969 6.5394 5.4016 5.7756];
%  ysD = [6.5354 4.1181 7.2362 6.4528 5.1142];
%  warp = tpsGetWarp( 0, xsS, ysS, xsD, ysD );
%  [gxs, gys] = meshgrid(3.5:.25:7, 8.5:.25: 12.5);
%  tpsInterpolate( warp, gxs, gys, 1 );
%
% See also TPSINTERPOLATE, TPSINTERPOLATEIM, TPSRANDOM
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

dim = size( xsS );
if( all(size(xsS)~=dim) || all(size(ysS)~=dim) || all(size(xsD)~=dim))
  error( 'argument sizes do not match' );
end

% get L
n = size(xsS,2);
deltaXs = xsS'*ones(1,n) - ones(n,1) * xsS;
deltaYs = ysS'*ones(1,n) - ones(n,1) * ysS;
Rsq = (deltaXs .* deltaXs + deltaYs .* deltaYs);
Rsq = Rsq+eye(n); K = Rsq .* log( Rsq ); K( isnan(K) )=0;
K = K + lambda * eye( n );
P = [ ones(n,1), xsS', ysS' ];
L = [ K, P; P', zeros(3,3) ];
LInv = L^(-1);
LnInv = LInv(1:n,1:n);

% recover W's
wx = LInv * [xsD 0 0 0]';
affinex = wx(n+1:n+3);
wx = wx(1:n);
wy = LInv * [ysD 0 0 0]';
affiney = wy(n+1:n+3);
wy = wy(1:n);

% record warp
warp.wx = wx; warp.affinex = affinex;
warp.wy = wy; warp.affiney = affiney;
warp.xsS = xsS; warp.ysS = ysS;
warp.xsD = xsD; warp.ysD = ysD;

% get bending energy (without regularization)
w = [wx'; wy'];
K = K - lambda * eye( n );
bendE = trace(w*K*w')/2;


seed = 1;
rand('state', seed);
randn('state', seed);

N = 4;
dag = zeros(N,N);
C = 1; S = 2; R = 3; W = 4;
dag(C,[R S]) = 1;
dag(R,W) = 1;
dag(S,W)=1;

false = 1; true = 2;
ns = 2*ones(1,N); % binary nodes

bnet = mk_bnet(dag, ns);
if 0
  bnet.CPD{C} = tabular_CPD(bnet, C, [0.5 0.5]);
  bnet.CPD{R} = tabular_CPD(bnet, R, [0.8 0.2 0.2 0.8]);
  bnet.CPD{S} = tabular_CPD(bnet, S, [0.5 0.9 0.5 0.1]);
  bnet.CPD{W} = tabular_CPD(bnet, W, [1 0.1 0.1 0.01 0 0.9 0.9 0.99]);
else
  for i=1:N, bnet.CPD{i} = tabular_CPD(bnet, i); end
end

evidence = cell(1,N);
onodes = [1 3];
data = sample_bnet(bnet);
evidence(onodes) = data(onodes);

clear engine;
engine{1} = belprop_inf_engine(bnet);
engine{2} = jtree_inf_engine(bnet);
engine{3} = global_joint_inf_engine(bnet);
engine{4} = var_elim_inf_engine(bnet);
E = length(engine);

clear mpe;
for e=1:E
  mpe{e} = find_mpe(engine{e}, evidence);
end
for e=2:E
  assert(isequal(mpe{1}, mpe{e}))
end


% Copyright (C) Daphne Koller, Stanford University, 2012

function [MEU OptimalDecisionRule] = OptimizeMEU( I )

  % Inputs: An influence diagram I with a single decision node and a single utility node.
  %         I.RandomFactors = list of factors for each random variable.  These are CPDs, with
  %              the child variable = D.var(1)
  %         I.DecisionFactors = factor for the decision node.
  %         I.UtilityFactors = list of factors representing conditional utilities.
  % Return value: the maximum expected utility of I and an optimal decision rule
  % (represented again as a factor) that yields that expected utility.

  % We assume I has a single decision node.
  % You may assume that there is a unique optimal decision.
  D = I.DecisionFactors(1);
  DF = CalculateExpectedUtilityFactor(I);
  MEU = 0;
  D.val = D.val*0;
  ca=D.card(1);
  l = length(D.val)/ca;
  for i = 1:l
	  start = i*ca-ca+1;
  endd = i*ca;
  [x,y] = max(DF.val(start:endd));
	MEU+=x;
	D.val(start+y-1) = 1;
end
OptimalDecisionRule = D;
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %
  % YOUR CODE HERE...
  %
  % Some other information that might be useful for some implementations
  % (note that there are multiple ways to implement this):
  % 1.  It is probably easiest to think of two cases - D has parents and D
  %     has no parents.
  % 2.  You may find the Matlab/Octave function setdiff useful.
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end


function l = gammaPriorLogProb(prior, x)

% GAMMAPRIORLOGPROB Log probability of Gamma prior.

% PRIOR

% Compute log prior
D = length(x);
l = D*prior.a*log(prior.b)-D*gammaln(prior.a)+(prior.a-1)*sum(log(x))-prior.b*sum(x);


function b = r8po_inverse ( n, r )

%*****************************************************************************80
%
%% R8PO_INVERSE computes the inverse of a matrix factored by R8PO_FA.
%
%  Discussion:
%
%    The R8PO storage format is appropriate for a symmetric positive definite
%    matrix and its inverse.  (The Cholesky factor of a R8PO matrix is an
%    upper triangular matrix, so it will be in R8GE storage format.)
%
%    Only the diagonal and upper triangle of the square array are used.
%    This same storage scheme is used when the matrix is factored by
%    R8PO_FA, or inverted by R8PO_INVERSE.  For clarity, the lower triangle
%    is set to zero.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 February 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real R(N,N), the Cholesky factor, in R8GE storage, returned by R8PO_FA.
%
%    Output, real B(N,N), the inverse matrix, in R8PO storage.
%
  b(1:n,1:n) = r(1:n,1:n);
%
%  Compute Inverse ( R ).
%
  for k = 1 : n

    b(k,k) = 1.0E+00 / b(k,k);
    b(1:k-1,k) = -b(1:k-1,k) * b(k,k);

    for j = k + 1 : n
      t = b(k,j);
      b(k,j) = 0.0E+00;
      b(1:k,j) = b(1:k,j) + t * b(1:k,k);
    end

  end
%
%  Compute Inverse ( R ) * ( Inverse ( R ) )'.
%
  for j = 1 : n

    for k = 1 : j - 1
      t = b(k,j);
      b(1:k,k) = b(1:k,k) + t * b(1:k,j);
    end

    b(1:j,j) = b(1:j,j) * b(j,j);

  end

  return
end


% Tow Thomas Bandpass Filter - Worst-Case Analysis (RSS, EVA, FMCA)
% File: c:\M_files\short_updates\TowThomaswca.m
% updated 11/16/06
clear;clc;
K=1e3;uF=1e-6; % Unit suffixes
% Component values
R1=200*K;R2=10*K;R3=20*K;R4=10*K;R5=20*K;R6=20*K;
C1=0.0159*uF;C2=C1;
%
Nom=[R1 R2 R3 R4 R5 R6 C1 C2]; % Place components in vector X
%
Tr=0.02;Tc=0.1; % 2% resistors, 10% capacitors
%
% Tolerance array T; minus in row 1, plus in row 2
% Column order follows component vector X
%
T=[-Tr -Tr -Tr -Tr -Tr -Tr -Tc -Tc;
   Tr Tr Tr Tr Tr Tr Tc Tc];
%
% Linear frequency; in Hz
%
BF=700;LF=1300;NP=301; % Number of points
F=linspace(BF,LF,NP);
%
Nc=length(Nom); % Nc = number of components
%
% Nominal output
%
Mr=1+(T(2,:)+T(1,:))/2; % For asymmetric tolerances if any
Nav=Nom.*Mr; % Average value of components; = Nom if symmetric tolerances
Tv=(T(2,:)-T(1,:))./(2*Mr); % Tv used for RSS
%
[A,B,D,E,I]=ttbpf(Nav); % call nominal arrays
%
% Arrays are constant and real, and they are calculated only once,
% not at each frequency as in admittance matrix analyses.
% This reduces run times considerably.
%
for i=1:NP;s=2*pi*F(i)*j;Vn(i)=abs(D*((s*I-A)\B)+E);end; % Nominal output
%
Nf=2^Nc;Vm1=zeros(Nf,NP); % Used in FMCA
k=1:Nf;RB=dec2bin(k-1); % Used in FMCA
%
% Sensitivities
%
dpf=0.0001; % derivative perturbation factor
Q=1+dpf;R=1-dpf;
for i=1:NP
   Qx=ones(1,Nc);Rx=Qx; % reset Q & R
   s=2*pi*F(i)*j;
   for p=1:Nc  % start sensitivity loop; p = component counter
      Qx(p)=Q;Rx(p)=R;
      if p > 1;Qx(p-1)=1;Rx(p-1)=1;end % reset previous
      %
      [A,B,D,E,I]=ttbpf(Nom.*Qx); % arrays perturbated forward
      Vr=abs(D*inv(s*I-A)*B+E); % output perturbated forward
      %
      [A,B,D,E,I]=ttbpf(Nom.*Rx); % arrays perturbated backward
      Vb=abs(D*inv(s*I-A)*B+E); % output perturbated backward
      %
      Sens(i,p)=(Vr-Vb)/(2*Vn(i)*dpf); % centered difference approximation
 % Ref:  Numerical Methods for Engineers, 3rd ed.
 % S.C. Chapra & R.P. Canale, McGraw-Hill, 1998, p.93
 %
 % EVA (Extreme Value Analysis)
 %
      if Sens(i,p) > 0
         Hi(p)=1+T(2,p);Lo(p)=1+T(1,p); % component tolerance for WC High
      else
         Hi(p)=1+T(1,p);Lo(p)=1+T(2,p); % component tolerance for WC Low
      end
   end % close p sensitivity loop
%
% Get extreme value low (EVL) and extreme value high (EVH)
%
	[A,B,D,E,I]=ttbpf(Nav.*Lo);
   VL(i)=abs(D*((s*I-A)\B)+E);
   %
	[A,B,D,E,I]=ttbpf(Nav.*Hi);
   VH(i)=abs(D*((s*I-A)\B)+E);
%
% RSS (Root-Sum-Square)
%
   STn=norm(Sens(i,:).*Tv);
   Vrss1(i)=Vn(i)*(1-STn);Vrss2(i)=Vn(i)*(1+STn);
%
% FMCA
%
% Binary array RB contains binary numbers Nc bits wide from 0 to Nf.
% Used for all possible tolerance combinations.
%
   for k=1:Nf  % start FMCA loop
      for p=1:Nc
         if RB(k,p)=='0'
            Tf(p)=1+T(1,p); % low tolerance if a logic '0'
         else
            Tf(p)=1+T(2,p); % high tolerance if a logic '1'
         end
      end % close FMCA tolerance loop
      [A,B,D,E,I]=ttbpf(Nav.*Tf);
      Vm1(k,i)=abs(D*((s*I-A)\B)+E);
   end % close FMCA loop
end % close major i loop
Vmax1=max(Vm1);Vmin1=min(Vm1);
%
% Plot sensitivities
% Number of sensitivities to be plotted = Nc
% Adjust 'g=plot(...' statements accordingly.
% All should be plotted to observe any bipolarity (non-monotonicity).
h=plot(F,Sens(:,1),'k',F,Sens(:,2),'r',F,Sens(:,3),'g',F,Sens(:,4),'b');
set(h,'LineWidth',2);grid on
axis auto
ylabel('%/%');xlabel('Freq (Hz)');
title('Sensitivities')
hold off
legend('R1','R2','R3','R4',0);
figure
%
h=plot(F,Sens(:,5),'k',F,Sens(:,6),'r',F,Sens(:,7),'g',F,Sens(:,8),'b');
set(h,'LineWidth',2);
axis auto;grid on
ylabel('%/%');xlabel('Freq (Hz)');
title('Sensitivities')
legend('R5','R6','C1','C2',0);
figure
%
% RSS
h=plot(F,Vn,'k--',F,Vrss1,'b',F,Vrss2,'r');
set(h,'LineWidth',2);grid on
axis auto
ylabel('Volts');xlabel('Freq (Hz)');
title('RSS')
legend('Nom','RSS Lo','RSS Hi',0);
figure
%
% EVA
h=plot(F,Vn,'k--',F,VH,'r',F,VL,'b');
set(h,'LineWidth',2);grid on
axis auto
xlabel('Freq(Hz)');ylabel('Volts');
title('EVA')
legend('Nom','EVA Hi','EVA Lo',0);
figure
%
% FMCA
h=plot(F,Vn,'k--',F,Vmax1,'r',F,Vmin1,'b');
set(h,'LineWidth',2);grid on
axis auto
xlabel('Freq(Hz)');ylabel('Volts');
title('FMCA');
legend('Nom','FMCA Hi','FMCA Lo',0);
%
% See comments concerning RSS, EVA, & FMCA at end of mfbpfwca.m


function x = gaussRnd(mu, Sigma, n)
% Generate samples from a Gaussian distribution.
% Input:
%   mu: d x 1 mean vector
%   Sigma: d x d covariance matrix
%   n: number of samples
% Outpet:
%   x: d x n generated sample x~Gauss(mu,Sigma)
% Written by Mo Chen (sth4nth@gmail.com).
if nargin == 2
    n = 1;
end
[V,err] = chol(Sigma);
if err ~= 0
    error('ERROR: sigma must be a symmetric positive definite matrix.');
end
x = V'*randn(size(V,1),n)+repmat(mu,1,n);


function f=ref_iedgtii(c,g,a,M)
%REF_IEDGTII   Inverse Reference Even DGT type II
%   Usage  f=ref_edgt(c,g,a,M);
%
%   If a is even, then the input window must be odd-centered of length 2L.
%
%   If a is odd, then the input window must be even-centered of length 2L.
%

L2=size(g,1);
W=size(c,2);
L=L2/2;

b=L/M;
N=L/a;

F=zeros(L,M*N);

l=(0:L-1).';

lsecond=(2*L-1:-1:L).';

gnew=circshift(g,floor(a/2));
for n=0:N-1
  for m=0:M-1
    gshift=circshift(gnew,n*a);

    F(:,M*n+m+1)=exp(2*pi*i*m*(l+.5)/M).*gshift(l+1) + ...
	exp(-2*pi*i*m*(l+.5)/M).*gshift(lsecond+1);

  end;
end;

f=F*c;


function loglik = computeEMLoglik(xInit,PInit,xSmooth,y,F,H,Q,R,B,u)
% COMPUTEEMLOGLIK Compute EM loglikelihood p(y_{1:t},x_{1:t} | theta), where
% theta = {F,H,Q,R,B}.
%
% INPUTS:   xInit       The (xDim x 1) initial state mean for time 1
%           PInit       The (xDim x xDim) initial state covariance for time 1
%           xSmooth     The (xDim x N) matrix of smoothed estimates for time 1:N
%           y           The (yDim x N) matrix of measurements for time 1:N
%           F           The (xDim x xDim) state transition matrix
%           H           The (yDim x xDim) measurement matrix
%           Q           The (xDim x xDim) process noise covariance
%           R           The (yDim x yDim) measurement noise covariance
%           B           The (xDim x uDim) control input gain matrix
%           u           The (uDim x N) control input matrix for time 1:N
%
% OUTPUTS:  loglik   A boolean stating if EM has converged
%
% [1] A. W. Blocker, An EM algorithm for the estimation of affine state-space systems with or without known inputs, (2008)
%
% January 2018 Lyudmil Vladimirov, University of Liverpool.

    [xDim, N] = size(xSmooth);
    yDim = size(y,1);

    loglik = -sum((y-H*xSmooth).^2)/(2*R) - N/2*log(abs(R))...
        -1/2*sum((xSmooth(2:N)-F*xSmooth(1:N-1)-B*u(2:N)).^2)/(Q) - (N-1)/2*log(abs(Q))...
        -sum((xSmooth(1)-xInit).^2)/(2*PInit) - 0.5*log(abs(PInit)) - N*log(2*pi);

%     loglik = -.5*(y-H*xSmooth)/R*(y-H*xSmooth)' - N/2*log(abs(R))...
%         -1/2*(xSmooth(:,2:N)-F*xSmooth(1:N-1)-B*u(2:N))/Q - (N-1)/2*log(abs(Q))...
%         -sum((xSmooth(1)-xInit).^2)/(2*PInit) - 0.5*log(abs(PInit)) - N*log(2*pi);

%     loglik = cell(1,3);
%     loglik{1} = -.5*(xSmooth(:,1) - xInit)/PInit*(xSmooth(:,1) - xInit)' ...
%                   -.5*log(abs(PInit)) - .5*(N-1)*log(abs(Q)) - .5*N*log(abs(R)) ...
%                   -.5*N*(xDim+yDim)*log(2*pi);
%     loglik{2} = 0;
%     loglik{3} = 0;
%     for k = 1:N
%         if(k>1)
%             loglik{2} = loglik{2} + .5*(xSmooth(:,k) ...
%                           - F*xSmooth(:,k-1) - B*u(:,k))/Q*(xSmooth(:,k) - F*xSmooth(:,k-1) - B*u(:,k))';
%         end
%         loglik{3} = loglik{3} + .5*(y(:,k) - H*xSmooth(:,k))/R*(y(:,k) - H*xSmooth(:,k))';
%     end
%     loglik = loglik{1} - loglik{2} - loglik{3};
end


function [interp1_fs, interp2_fs] = get_interp_fourier(sz, params)

% Compute the Fourier series of the interpolation function (b_d in the
% paper). The interpolation method is set using params.interpolation_method:
% - 'ideal' performs ideal reconstruction, which corresponds to a periodic
%   summation of a sinc function b_d in the spatial domain.
% - 'bicubic' uses a bicubic kernel b_d in the spatial domain.

switch lower(params.interpolation_method)
    case 'none'
        % Do nothing
        interp1_fs = ones(sz(1),1);
        interp2_fs = ones(1,sz(2));
    case 'ideal'
        % Ideal reconstruction (flat in the frequency domain)
        interp1_fs = ones(sz(1),1) / sz(1);
        interp2_fs = ones(1,sz(2)) / sz(2);
    case 'bicubic'
        % Take the truncated fourier series from the cubic spline
        a = params.interpolation_bicubic_a;
        interp1_fs = real(1/sz(1) * cubic_spline_fourier((-(sz(1)-1)/2:(sz(1)-1)/2)'/sz(1), a));
        interp2_fs = real(1/sz(2) * cubic_spline_fourier((-(sz(2)-1)/2:(sz(2)-1)/2)/sz(2), a));
    otherwise
        error('Unknown dft interpolation method');
end

if params.interpolation_centering
    % Center the feature grids by shifting the interpolated features
    % Multiply Fourier coeff with e^(-i*pi*k/N)
    interp1_fs = interp1_fs .* exp(-1i*pi / sz(1) * (-(sz(1)-1)/2:(sz(1)-1)/2)');
    interp2_fs = interp2_fs .* exp(-1i*pi / sz(2) * (-(sz(2)-1)/2:(sz(2)-1)/2));
end
if params.interpolation_windowing
    % Window the Fourier series of the interpolation basis function
    win1 = hann(sz(1)+2);
    win2 = hann(sz(2)+2);
    interp1_fs = interp1_fs .* win1(2:end-1);
    interp2_fs = interp2_fs .* win2(2:end-1)';
end

interp1_fs = single(interp1_fs);
interp2_fs = single(interp2_fs);


classdef diskfun < separableApprox
%DISKFUN   DISKFUN class for representing functions on the unit disk.
%
%   Class for approximating smooth functions defined on the unit disk. The
%   functions should be smooth. Diskfun objects can be constructed in a
%   variety of ways:
%
%    1. Y = diskfun(F), where F is a function handle in Cartesian
%       coordinates (x,y), e.g., @(x,y) x.*y + cos(x).
%    2. Y = diskfun(F, 'polar'), where F is a function handle in polar
%       coordinates (theta,r). Here, theta is the angular variable satisfying
%       -pi <= theta <= pi, and r is the radial variable satisfying 0 <= r < 1,
%       e.g., @(theta,r) cos(r.*sin(theta))
%    3. Y = diskfun(F), where F is a matrix of numbers.
%
% If F is a function handle then it should allow for vectorized evaluations.
%
% If F is a matrix, F = (f_ij), the numbers fij are used as function values
% at the tensor Fourier-Chebyshev points on the rectangle [-pi,pi]x[0,1].
%
% DISKFUN(F, k) returns a rank k approximation to F.
%
% DISKFUN(F, [m n]) returns a representation of F using a degree m
% Chebyshev approximation of F in the radial direction and a degree n
% trigonometric approximation in the angular direction. The result is
% compressed in low rank form and the rank k is still determined adaptively
% (satisfying k<=min(m,n)+1).
%
% The DISKFUN software system is based on:
%
% H. Wilber, A. Townsend, and G. Wright, Computing with functions in
% spherical and polar geometries II: The disk, SIAM. J. Sci. Comput., 39-4
% (2017), C238-C262.
%
% See also CHEBFUN2, SPHEREFUN, DISKFUNV.

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

% TODO: Include documentation of fixed eps construction

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

        function f = diskfun(varargin)
            % The main diskfun constructor!

            % Return an empty DISKFUN:
            if ( (nargin == 0) || isempty(varargin{1}) )
                f.domain = [-pi pi 0 1];
                return
            end
            % Call the constructor, all the work is done here:
            f = constructor(f, varargin{:});
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% HIDDEN METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false, Hidden = true )
        % Make f have BMC-II symmetry.
        f = projectOntoBMCII(f);

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% PUBLIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

        % The main bulk of the DISKFUN constructor:
        g = constructor(g, op, dom, varargin);

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )

        % Fast Poisson & Helmholtz solvers:
        u = poisson( f, bc, m, n );
        u = helmholtz( f, k, bc, m, n);

        % Converts a function in polar coordinates to one in Cartesian
        % coordinates on the disk
        fdf = pol2cartf(f, r, th);

        % Disk harmonics
        Y = harmonic(l,m,type);

        % Convert coeffs to a diskfun
        f = coeffs2diskfun(CFS);

        varargout = coeffs2vals(U, varargin);
        varargout = vals2coeffs(U, varargin);
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Private Static methods implemented by DISKFUN class.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = private, Static = true )

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS PROPERTIES
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties (Access = public)
        % DOMAIN: default is [-pi,pi] x [0,1] which corresponds to using
        % polar coordinates.
        % Doubled-up disk will have a domain of [-pi,pi] x [-1,1].
        idxPlus
        idxMinus
        nonZeroPoles = 0;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Private constant properties
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties ( Constant )
        % TODO: Add support for this constant here.
        %alpha = 50;  % Growth factor control.
    end

end


function [uxref, xref] = xsection(qmethod,xy,x_gal,yref,fig)
%xsection   plots/explores solution on horizontal cross-section
%   [uxref, xref] = xsection(qmethod,xy,x_gal,yref,fig);
%   input
%          qmethod    mixed method
%          xy         velocity nodal coordinate vector
%          x_gal   solution vector
%          yref       y-location of grid line
%          fig         figure number
%
%   IFISS function: DJS; 19 September 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
nvtx=length(xy(:,1));
kk=find(xy(:,2)==yref);
if isempty(kk), error('location yref is not a grid-line!'), end
uxref=x_gal(kk);  xref=xy(kk,1);
figure(fig)
plot(xref,uxref,'-k'), axis('square'), title('x-section of flow')
%%
%% compute volume of flow using appropriate quadrature
nny=length(xref); hy=xref(2)-xref(1);
if qmethod >1,
%% Simpson's rule
ww=ones(nny,1); ww(2:2:nny-1)=4; ww(3:2:nny-1)=2;
total=(hy/3)*sum(ww.*uxref);
else
%% Trapezium rule
ww=ones(nny,1); ww(2:2:nny-1)=2; ww(3:2:nny-1)=2;
total=(hy/2)*sum(ww.*uxref);
end
fprintf('\narea of x-section is %10.6e \n',total)
return


%DEMO_MODELASSESMENT1 Demonstration for model assessment with WAIC,
% DIC, number of effective parameters and ten-fold cross validation
%
%  Description
%    We will consider the regression problem in demo_regression1.
%    The analysis is conducted with full Gaussian process, and FIC
%    and PIC sparse approximations. The performance of these models
%    are compared by evaluating ten-fold cross validation,
%    leave-one-out cross-validation, WAIC, DIC and the effective
%    number of parameters. The inference will be conducted using
%    maximum a posterior (MAP) estimate for the parameters, via
%    full Markov chain Monte Carlo (MCMC) and with an integration
%    approximation (IA) for the parameters.
%
%    This demo is organised in three parts:
%     1) data analysis with full GP model
%     2) data analysis with FIC approximation
%     3) data analysis with PIC approximation
%
%  See also DEMO_REGRESSION1, DEMO_REGRESSION_SPARSE1
%
% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2010-2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%========================================================
% PART 1 data analysis with full GP model
%========================================================
disp('Full GP model with Gaussian noise model')

% Load the data
S = which('demo_regression1');
L = strrep(S,'demo_regression1.m','demodata/dat.1');
data=load(L);
x = [data(:,1) data(:,2)];
y = data(:,3);
[n, nin] = size(x);

DIC=repmat(NaN,1,9);DIC2=repmat(NaN,1,9);DIC_latent=repmat(NaN,1,9);
p_eff=repmat(NaN,1,9);p_eff2=repmat(NaN,1,9);p_eff_latent=repmat(NaN,1,9);p_eff_latent2=repmat(NaN,1,9);

% ---------------------------
% --- Construct the model ---
gpcf = gpcf_sexp('lengthScale', [1 1], 'magnSigma2', 0.2^2);
lik = lik_gaussian('sigma2', 0.2^2);
gp = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-9);

% -----------------------------
% --- Conduct the inference ---
%
% We will make the inference first by finding a maximum a posterior estimate
% for the parameters via gradient based optimization. After this we will
% perform an extensive Markov chain Monte Carlo sampling for the parameters.
%
disp('MAP estimate for the parameters')

% --- MAP estimate ---
%     (see gp_optim for more details)

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters, DIC and WAIC with focus on
% latent variables.
models{1} = 'full_MAP';
p_eff_latent = gp_peff(gp, x, y);
% For easier comparison to other methods, compute mean log
% predictive density (mlpd) instead of deviance (-2n*mlpd)
[DIC_latent, p_eff_latent2] = gp_dic(gp, x, y);
WAICV(1) = gp_waic(gp,x,y);
WAICG(1) = gp_waic(gp,x,y, 'method', 'G');

% Evaluate the 10-fold cross-validation results.
disp('MAP estimate for the parameters - k-fold-CV')
cvres =  gp_kfcv(gp, x, y, 'display', 'fold');
mlpd_cv(1) = cvres.mlpd_cv;
rmse_cv(1) = cvres.mrmse_cv;

disp('MAP estimate for the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp, x, y);
mlpd_loo(1) = mean(lpy);
rmse_loo(1) = sqrt(mean((y-Ef).^2));

% --- MCMC approach ---
disp('MCMC integration over the parameters')

% Do the sampling (this takes about 1 minute)
[rfull,g,opt] = gp_mc(gp, x, y, 'nsamples', 220, 'display', 20);

% After sampling delete the burn-in and thin the sample chain
rfull = thin(rfull, 21, 2);

% Evaluate the effective number of parameters, DIC and WAIC.
models{2} = 'full_MCMC';
% For easier comparison to other methods, compute mean log
% predictive density (mlpd) instead of deviance (-2n*mlpd)
[DIC(2), p_eff(2)] =  gp_dic(rfull, x, y, 'focus', 'hyper');
[DIC2(2), p_eff2(2)] =  gp_dic(rfull, x, y);
WAICV(2) = gp_waic(rfull,x,y);
WAICG(2) = gp_waic(rfull,x,y, 'method', 'G');

% Evaluate the 10-fold cross validation results.
%
% We reduce the number of samples so that the sampling takes less time.
% 50 is too small sample size, though, and for reliable results the 10-CV
% should be run with larger sample size.
disp('MCMC integration over the parameters - k-fold-CV')
opt.nsamples= 50;
cvres =  gp_kfcv(gp, x, y, 'inf_method', 'MCMC', 'opt', opt, 'rstream', 1, 'display', 'fold');
mlpd_cv(2) = cvres.mlpd_cv;
rmse_cv(2) = cvres.mrmse_cv;

disp('MCMC integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(rfull, x, y);
mlpd_loo(2) = mean(lpy);
rmse_loo(2) = sqrt(mean((y-Ef).^2));

% --- Integration approximation approach ---
disp('Grid integration over the parameters')

gp_array = gp_ia(gp, x, y, 'int_method', 'grid');

models{3} = 'full_IA';
% For easier comparison to other methods, compute mean log
% predictive density (mlpd) instead of deviance (-2n*mlpd)
[DIC(3), p_eff(3)] =  gp_dic(gp_array, x, y, 'focus', 'hyper');
[DIC2(3), p_eff2(3)] =  gp_dic(gp_array, x, y);
WAICV(3) = gp_waic(gp_array,x,y);
WAICG(3) = gp_waic(gp_array,x,y, 'method', 'G');

% Then the 10 fold cross-validation.
disp('Grid integration over the parameters - k-fold-CV')
clear opt
opt.int_method = 'grid';
cvres = gp_kfcv(gp, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(3) = cvres.mlpd_cv;
rmse_cv(3) = cvres.mrmse_cv;

disp('Grid integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp_array, x, y);
mlpd_loo(3) = mean(lpy);
rmse_loo(3) = sqrt(mean((y-Ef).^2));

%========================================================
% PART 2 data analysis with FIC GP
%========================================================
disp('GP with FIC sparse approximation')

% ---------------------------
% --- Construct the model ---

% Here we conduct the same analysis as in part 1, but this time we
% use FIC approximation

% Initialize the inducing inputs in a regular grid over the input space
[u1,u2]=meshgrid(linspace(-1.8,1.8,6),linspace(-1.8,1.8,6));
X_u = [u1(:) u2(:)];

% Create the FIC GP structure
gp_fic = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6, 'X_u', X_u)

% -----------------------------
% --- Conduct the inference ---

% --- MAP estimate using scaled conjugate gradient algorithm ---
disp('MAP estimate for the parameters')

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp_fic=gp_optim(gp_fic,x,y,'opt',opt,'optimf',@fminlbfgs);

% Evaluate the effective number of parameters and DIC with focus on
% latent variables.
models{4} = 'FIC_MAP';
p_eff_latent(4) = gp_peff(gp_fic, x, y);
[DIC_latent(4), p_eff_latent2(4)] = gp_dic(gp_fic, x, y);
WAICV(4) = gp_waic(gp_fic,x,y);
WAICG(4) = gp_waic(gp_fic,x,y, 'method', 'G');

% Evaluate the 10-fold cross validation results.
disp('MAP estimate for the parameters - k-fold-CV')
cvres = gp_kfcv(gp_fic, x, y, 'display', 'fold');
mlpd_cv(4) = cvres.mlpd_cv;
rmse_cv(4) = cvres.mrmse_cv;

disp('MAP estimate for the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp_fic, x, y);
mlpd_loo(4) = mean(lpy);
rmse_loo(4) = sqrt(mean((y-Ef).^2));

% --- MCMC approach ---
% (the inducing inputs are fixed)
disp('MCMC integration over the parameters')

% Do the sampling (this takes about 1 minute)
rfic = gp_mc(gp_fic, x, y, 'nsamples', 220, 'display', 20);

% After sampling we delete the burn-in and thin the sample chain
rfic = thin(rfic, 21, 2);

% Evaluate the effective number of parameters, DIC and WAIC. Note that
% the effective number of parameters as a second output, but here
% we use explicitly the gp_peff function
models{5} = 'FIC_MCMC';
[DIC(5), p_eff(5)] =  gp_dic(rfic, x, y, 'focus', 'hyper');
[DIC2(5), p_eff2(5)] =  gp_dic(rfic, x, y);
WAICV(5) = gp_waic(rfic,x,y);
WAICG(5) = gp_waic(rfic,x,y, 'method', 'G');

% We reduce the number of samples so that the sampling takes less time.
% 50 is too small sample size, though, and for reliable results the 10-CV
% should be run with larger sample size. We also set the save option to 0.
clear opt
opt.nsamples= 50; opt.display=20;
disp('MCMC integration over the parameters - k-fold-CV')
cvres = gp_kfcv(gp_fic, x, y, 'inf_method', 'MCMC', 'opt', opt, 'display', 'fold');
mlpd_cv(5) = cvres.mlpd_cv;
rmse_cv(5) = cvres.mrmse_cv;

disp('MCMC integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(rfic, x, y);
mlpd_loo(5) = mean(lpy);
rmse_loo(5) = sqrt(mean((y-Ef).^2));

% --- Integration approximation approach ---
disp('Grid integration over the parameters')
gpfic_array = gp_ia(gp_fic, x, y, 'int_method', 'grid');

models{6} = 'FIC_IA';
[DIC(6), p_eff(6)] =  gp_dic(gpfic_array, x, y, 'focus', 'hyper');
[DIC2(6), p_eff2(6)] =  gp_dic(gpfic_array, x, y);
WAICV(6) = gp_waic(gpfic_array,x,y);
WAICG(6) = gp_waic(gpfic_array,x,y, 'method', 'G');

% Then the 10 fold cross-validation.
disp('Grid integration over the parameters - k-fold-CV')
clear opt
opt.int_method = 'grid';
cvres = gp_kfcv(gp_fic, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(6) = cvres.mlpd_cv;
rmse_cv(6) = cvres.mrmse_cv;

disp('Grid integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gpfic_array, x, y);
mlpd_loo(6) = mean(lpy);
rmse_loo(6) = sqrt(mean((y-Ef).^2));

%========================================================
% PART 3 data analysis with PIC approximation
%========================================================
disp('GP with PIC sparse approximation')

[u1,u2]=meshgrid(linspace(-1.8,1.8,6),linspace(-1.8,1.8,6));
X_u = [u1(:) u2(:)];

% Initialize test points
[p1,p2]=meshgrid(-1.8:0.1:1.8,-1.8:0.1:1.8);
p=[p1(:) p2(:)];

% set the data points into clusters. Here we construct two cell arrays.
%  trindex  contains the block index vectors for training data. That is
%           x(trindex{i},:) and y(trindex{i},:) belong to the i'th block.
b1 = [-1.7 -0.8 0.1 1 1.9];
mask = zeros(size(x,1),size(x,1));
trindex={};
for i1=1:4
  for i2=1:4
    ind = 1:size(x,1);
    ind = ind(: , b1(i1)<=x(ind',1) & x(ind',1) < b1(i1+1));
    ind = ind(: , b1(i2)<=x(ind',2) & x(ind',2) < b1(i2+1));
    trindex{4*(i1-1)+i2} = ind';
  end
end

% Create the PIC GP structure and set the inducing inputs and block indexes
gpcf = gpcf_sexp('lengthScale', [1 1], 'magnSigma2', 0.2^2);
lik = lik_gaussian('sigma2', 0.2^2);

gp_pic = gp_set('type', 'PIC', 'lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6, 'X_u', X_u);
gp_pic = gp_set(gp_pic, 'tr_index', trindex);

% -----------------------------
% --- Conduct the inference ---

% --- MAP estimate using scaled conjugate gradient algorithm ---
disp('MAP estimate for the parameters')

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the BFGS quasi-Newton method
gp_pic=gp_optim(gp_pic,x,y,'opt',opt,'optimf',@fminlbfgs);

models{7} = 'PIC_MAP';
p_eff_latent(7) = gp_peff(gp_pic, x, y);
[DIC_latent(7), p_eff_latent2(7)] = gp_dic(gp_pic, x, y);
WAICV(7) = gp_waic(gp_pic, x, y);
WAICG(7) = gp_waic(gp_pic, x, y, 'method', 'G');

% Evaluate the 10-fold cross validation results.
disp('MAP estimate for the parameters - k-fold-CV')
cvres = gp_kfcv(gp_pic, x, y, 'display', 'fold');
mlpd_cv(7) = cvres.mlpd_cv;
rmse_cv(7) = cvres.mrmse_cv;

disp('MAP estimate for the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gp_pic, x, y);
mlpd_loo(7) = mean(lpy);
rmse_loo(7) = sqrt(mean((y-Ef).^2));

% --- MCMC approach ---
disp('MCMC integration over the parameters')

% Do the sampling (this takes about 1 minute)
rpic = gp_mc(gp_pic, x, y, 'nsamples', 220, 'display', 20);

% After sampling we delete the burn-in and thin the sample chain
rpic = rmfield(rpic, 'tr_index');
rpic = thin(rpic, 21, 2);
rpic.tr_index = trindex;

% Evaluate the effective number of parameters and DIC. Note that
% the effective number of parameters as a second output, but here
% we use explicitly the gp_peff function
models{8} = 'PIC_MCMC';
[DIC(8), p_eff(8)] =  gp_dic(rpic, x, y, 'focus', 'hyper');
[DIC2(8), p_eff2(8)] =  gp_dic(rpic, x, y);
WAICV(8) = gp_waic(rpic, x, y);
WAICG(8) = gp_waic(rpic, x, y, 'method', 'G');

% We reduce the number of samples so that the sampling takes less time.
% 50 is too small sample size, though, and for reliable results the 10-CV
% should be run with larger sample size. We also set the save option to 0.
clear opt
opt.nsamples= 50; opt.display=20;
disp('MCMC integration over the parameters - k-fold-CV')
cvres = gp_kfcv(gp_pic, x, y, 'inf_method', 'MCMC', 'opt', opt, 'display', 'fold');
mlpd_cv(8) = cvres.mlpd_cv;
rmse_cv(8) = cvres.mrmse_cv;

disp('MCMC integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(rpic, x, y);
mlpd_loo(8) = mean(lpy);
rmse_loo(8) = sqrt(mean((y-Ef).^2));

% --- Integration approximation approach ---
disp('Grid integration over the parameters')

gppic_array = gp_ia(gp_pic, x, y, 'int_method', 'grid');

models{9} = 'PIC_IA';
[DIC(9), p_eff(9)] =  gp_dic(gppic_array, x, y, 'focus', 'hyper');
[DIC2(9), p_eff2(9)] =  gp_dic(gppic_array, x, y);
WAICV(9) = gp_waic(gppic_array, x, y);
WAICG(9) = gp_waic(gppic_array, x, y, 'method', 'G');

% Then the 10 fold cross-validation.
disp('Grid integration over the parameters - k-fold-CV')
clear opt
opt.int_method = 'grid';
cvres = gp_kfcv(gp_pic, x, y, 'inf_method', 'IA', 'opt', opt, 'display', 'fold');
mlpd_cv(9) = cvres.mlpd_cv;
rmse_cv(9) = cvres.mrmse_cv;

disp('Grid integration over the parameters - LOO-CV')
[Ef,Varf,lpy] =  gp_loopred(gppic_array, x, y);
mlpd_loo(9) = mean(lpy);
rmse_loo(9) = sqrt(mean((y-Ef).^2));

%========================================================
% PART 4 Print the results
%========================================================
disp('Summary of the results')
S = '      ';
for i = 1:length(models)
    S = [S '  ' models{i}];
end

S = sprintf([S '\n CV-mlpd  %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], mlpd_cv);
S = sprintf([S '\n CV-rmse  %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], rmse_cv);
S = sprintf([S '\n LOO-mlpd %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], mlpd_loo);
S = sprintf([S '\n LOO-rmse %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], rmse_loo);
S = sprintf([S '\n ']);
S = sprintf([S '\n WAIC_V   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], WAICV);
S = sprintf([S '\n WAIC_G   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], WAICG);
S = sprintf([S '\n ']);
S = sprintf([S '\n DIC_h    %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], DIC);
S = sprintf([S '\n DIC_a    %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], DIC2);
S = sprintf([S '\n DIC_l    %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], DIC_latent);
S = sprintf([S '\n peff_h   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff);
S = sprintf([S '\n peff_a   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff2);
S = sprintf([S '\n peff_l   %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff_latent);
S = sprintf([S '\n peff_l2  %5.2f     %5.2f     %5.2f     %5.2f    %5.2f     %5.2f    %5.2f    %5.2f    %5.2f'], p_eff_latent2);
S = sprintf([S '\n ']);
S = sprintf([S '\n ']);
S = sprintf([S '\n The notation is as follows:']);
S = sprintf([S '\n CV-mlpd  = mean log predictive density from the 10-fold CV. ']);
S = sprintf([S '\n CV-rmse  = root mean squared error from the 10-fold LOO-CV. ']);
S = sprintf([S '\n LOO-mlpd = mean log predictive density from the LOO-CV. ']);
S = sprintf([S '\n LOO-rmse = root mean squared error from the 10-fold CV. ']);
S = sprintf([S '\n WAIC_V   = WAIC via variance method ']);
S = sprintf([S '\n WAIC_G   = WAIC via Gibbs training utility method ']);
S = sprintf([S '\n DIC_h    = DIC with focus on parameters. ']);
S = sprintf([S '\n DIC_a    = DIC with focus on parameters and latent variables (all). ']);
S = sprintf([S '\n DIC_l    = DIC with focus on latent variables. ']);
S = sprintf([S '\n peff_h   = effective number of parameters (latent variables marginalized). ']);
S = sprintf([S '\n peff_a   = effective number of parameters and latent variables. ']);
S = sprintf([S '\n peff_l   = effective number of latent variables evaluated with gp_peff. ']);
S = sprintf([S '\n peff_l2  = effective number of latent variables evaluated with gp_dic. ']);
S = sprintf([S '\n '])


function [X S Y dist] = OptSpace(M_E,r,niter,tol)
% An algorithm for Matrix Reconstruction from a partially revealed set.
% See "Matrix Completion from a Few Entries"(http://arxiv.org/pdf/0901.3150) for details
% Usage :
% [X S Y dist] = OptSpace(A,r,niter,tol);
% [X S Y dist] = OptSpace(A);
%
% INPUT :
% A     :  The partially revealed matrix.
%          Sparse matrix with zeroes at the unrevealed indices.
%
% r     :  The rank to be used for reconstruction. Use [] to guess the rank.
% niter :  The max. no. of iterations. Use [] to use default (50).
% tol   :  Stop iterations if norm( (XSY' - M_E).*E , 'fro' )/sqrt(|E|) < tol, where
%        - E_{ij} = 1 if M_{ij} is revealed and zero otherwise,
%        - |E| is the size of the revealed set.
%        - Use [] to use the default (1e-6)
%
%
% OUTPUT :
% X      : A size(A,1)xr matrix
% S      : An rxr matrix
% Y      : A size(A,2)xr matrix
% such that M_hat = X*S*Y'
% dist   : A vector containing norm( (XSY' - M_E).*E , 'fro' )/sqrt(|E|) at each
%          successive iteration
%
% Date : 21st April, 2009
% COPYRIGHT 2009 Raghunandan H. Keshavan, Andrea Montanari, Sewoong Oh

if(nargin==1)

	M_E = sparse(M_E);
	[n m] = size(M_E);
	E = spones(M_E);
	eps = nnz(E)/sqrt(m*n) ;

	tol = 1e-6;

	fprintf(1,'Rank not specified. Trying to guess ...\n');
	r = guessRank(M_E) ;
	fprintf(1,'Using Rank : %d\n',r);

	m0 = 10000 ;
	rho = 0;

	niter = 50;
elseif(nargin==4)

	M_E = sparse(M_E);
	[n m] = size(M_E);
	E = spones(M_E);
	eps = nnz(E)/sqrt(m*n) ;

	if( length(tol) == 0 )
		tol = 1e-6;
	end

	if( length(r) == 0 )

		fprintf(1,'Rank not specified. Trying to guess ...\n');
		r = guessRank(M_E) ;
		fprintf(1,'Using Rank : %d\n',r);
	end

	m0 = 10000 ;
	rho = 0;

	if( length(niter) == 0 )
		niter = 50 ;
	end
else
	fprintf(1,'Improper arguments (See "help OptSpace")\n');
	fprintf(1,'Usage :\n[X S Y dist] = OptSpace(A,r,niter,tol) \n') ;
	fprintf(1,'[X S Y dist] = OptSpace(A)\n');
	return;
end

rescal_param = sqrt( nnz(E) * r / norm(M_E,'fro')^2 ) ;
M_E = M_E * rescal_param ;

fprintf(1,'Trimming ...\n');
% Trimming

M_Et = M_E ;
d = sum(E);
d_=mean(full(d));
for col=1:m
    if ( sum(E(:,col))>2*d_ )
        list = find( E(:,col) > 0 );
        p = randperm(length(list));
        M_Et( list( p(ceil(2*d_):end) ) , col ) = 0;
    end
end

d = sum(E');
d_= mean(full(d));
for row=1:n
    if ( sum(E(row,:))>2*d_ )
        list = find( E(row,:) > 0 );
        p = randperm(length(list));
        M_Et(row,list( p(ceil(2*d_):end) ) ) = 0;
    end
end

fprintf(1,'Sparse SVD ...\n');
% Sparse SVD
[X0 S0 Y0] = svds(M_Et,r) ;

clear M_Et;

% Initial Guess
X0 = X0*sqrt(n) ; Y0 = Y0*sqrt(m) ;
S0 = S0 / eps ;

fprintf(1,'Iteration\tFit Error\n');

% Gradient Descent
X = X0;Y=Y0;
S = getoptS(X,Y,M_E,E);

dist(1) = norm( (M_E - X*S*Y').*E ,'fro')/sqrt(nnz(E) )  ;
fprintf(1,'0\t\t%e\n',dist(1) ) ;

for i = 1:niter

% Compute the Gradient
	[W Z] = gradF_t(X,Y,S,M_E,E,m0,rho);

% Line search for the optimum jump length
	t = getoptT(X,W,Y,Z,S,M_E,E,m0,rho) ;
	X = X + t*W;Y = Y + t*Z;S = getoptS(X,Y,M_E,E) ;

% Compute the distortion
	dist(i+1) = norm( (M_E - X*S*Y').*E,'fro' )/sqrt(nnz(E));
	fprintf(1,'%d\t\t%e\n',i,dist(i+1) ) ;
	if( dist(i+1) < tol )
		break ;
	end
end

S = S /rescal_param ;

% Function to Guess the Rank of the input Matrix
function r = guessRank(M_E);
	[n m] = size(M_E);
	epsilon = nnz(M_E)/sqrt(m*n);
    S0 = svds(M_E,100) ;

    S1=S0(1:end-1)-S0(2:end);
    S1_ = S1./mean(S1(end-10:end));
    r1=0;
    lam=0.05;
    while(r1<=0)
        for idx=1:length(S1_)
            cost(idx) = lam*max(S1_(idx:end)) + idx;
        end
        [v2 i2] = min(cost);
        r1 = max(i2-1);
        lam=lam+0.05;
    end

	clear cost;
    for idx=1:length(S0)-1
        cost(idx) = (S0(idx+1)+sqrt(idx*epsilon)*S0(1)/epsilon  )/S0(idx);
    end
    [v2 i2] = min(cost);
    r2 = max(i2);

	r = max([r1 r2]);

% * * * * * * * * * * * * * * * * * * * *
% Function to compute the distortion
function out = F_t(X,Y,S,M_E,E,m0,rho)
[n r] = size(X) ;

out1 = sum( sum( ( (X*S*Y' - M_E).*E ).^2 ) )/2 ;

out2 =  rho*G(Y,m0,r) ;
out3 =  rho*G(X,m0,r) ;
out = out1+out2+out3 ;

function out = G(X,m0,r)

z = sum(X.^2,2)/(2*m0*r) ;
y = exp( (z-1).^2 ) - 1 ;
y( find(z < 1) ) = 0 ;
out = sum(y) ;
% * * * * * * * * * * * * * * * * * * * *

% Function to compute the gradient
function [W Z] = gradF_t(X,Y,S,M_E,E,m0,rho)
[n r] = size(X);
[m r] = size(Y);

XS = X*S ;
YS = Y*S' ;
XSY = XS*Y' ;

Qx = X'* ( (M_E - XSY).*E )*YS /n;
Qy = Y'* ( (M_E - XSY).*E )'*XS /m;

W = ( (XSY - M_E).*E )*YS + X*Qx + rho*Gp(X,m0,r);
Z = ( (XSY - M_E).*E )'*XS + Y*Qy + rho*Gp(Y,m0,r);

function out = Gp(X,m0,r)
z = sum(X.^2,2) /(2*m0*r) ;
z = 2*exp( (z-1).^2 ).*(z-1) ;
z( find(z<0) ) = 0;

out = X.*repmat(z,1,r) / (m0*r) ;
% * * * * * * * * * * * * * * * * * * * *

% * * * * * * * * * * * * * * * * * * * *
% Function to find Sopt given X, Y
function out = getoptS(X,Y,M_E,E)

[n r] = size(X);
C = X' * ( M_E ) * Y ; C = C(:) ;

for i = 1:r
        for j = 1:r
                ind = (j-1)*r + i ;
                temp = X' * (  (X(:,i) * Y(:,j)').*E ) * Y ;
                A(:,ind) = temp(:) ;
        end
end

S = A\C ;
out = reshape(S,r,r) ;
% * * * * * * * * * * * * * * * * * * * *

% * * * * * * * * * * * * * * * * * * * *
% Function to perform line search
function out = getoptT(X,W,Y,Z,S,M_E,E,m0,rho)
norm2WZ = norm(W,'fro')^2 + norm(Z,'fro')^2;
f(1) = F_t(X, Y,S,M_E,E,m0,rho) ;

t = -1e-1 ;
for i = 1:20
        f(i+1) = F_t(X+t*W,Y+t*Z,S,M_E,E,m0,rho) ;

        if( f(i+1) - f(1) <= .5*(t)*norm2WZ )
            out = t ;
            return;
        end
        t = t/2 ;
end
out = t ;