function H = get_shelve_lagrange(f,H,FlagSub,fSub,FlagAliasing,fAliasing,Bandwidth_in_Oct)
%GET_SHELVE_LAGRANGE Lagrange interpolation towards shelving filter
%
%   Usage: H = get_shelve_lagrange(f,H,FlagSub,fSub,FlagAliasing, ...
%                                  fAliasing,Bandwidth_in_Oct)
%
%   Input parameters:
%       f                -  frequency vector in Hz, typical 0 to half
%                           sampling frequency, equidistant sampling is
%                           assumed (i.e. DFT frequencies) but not required
%       H                -  complex spectrum (mag/phase) at specified frequencies
%                           with meaningful slope, typically used for +3dB/oct.
%       FlagSub          -  use low-shelf part, 0 or 1
%       fSub             -  cut-frequency in Hz for low-shelf,
%                           check e.g. fLow=100Hz in [Fig2, Sch13]
%       FlagAliasing     -  use high-sehlf part, 0 or 1
%       fAliasing        -  cut-frequency in Hz for high-shelf,
%                           check e.g. fAliasing=2kHz in [Fig2, Sch13]
%       Bandwidth_in_Oct -  interpolation bandwidth, i.e. shelf knee range
%                           in octaves, allowed: 0.5, 1, 2, 3, 4
%
%   Output parameters:
%       H                -  return complex spectrum at specified frequencies
%                           after low-/ and high-shelf interpolation              -
%
%   GET_SHELVE_LAGRANGE(f,H,FlagSub,fSub,FlagAliasing,fAliasing,Bandwidth_in_Oct)
%   does an Lagrange interplation to get a shelving filter. This function is used
%   in wfs_iir_prefilter(conf).
%
%   Note 1: there is a analytical expression for arbitrary Bandwidth_in_Oct
%   which is not implemented yet
%   Note 2: the Offset_dB specified for Bandwidth_in_Oct work only for the
%   3dB/oct. slope, other driving functions may have other slopes and
%   different interpolation offsets may be required
%
%   See also: wfs_iir_prefilter
%
%   References:
%       Schultz, Erbes, Spors, Weinzierl (2013) - "Derivation of IIR prefilters
%       for soundfield synthesis using linear secondary source distributions",
%       International Conference on Acoustics (AIA-DAGA), p.2372-2375,
%       http://pub.dega-akustik.de/AIA_DAGA_2013/data/articles/000604.pdf

%*****************************************************************************
% 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 *
% Revision: 07/02/2013 frank.schultz@uni-rostock.de initial development      *
%*****************************************************************************
    Hphase = unwrap(angle(H));  %save original phase
    H = 20*log10(abs(H));       %interpolation in dB

    if Bandwidth_in_Oct==4
        Offset_dB = 1.5053;
    elseif Bandwidth_in_Oct==3
        Offset_dB = 1.1286;
    elseif Bandwidth_in_Oct==2
        Offset_dB = 0.7528;
    elseif Bandwidth_in_Oct==1
        Offset_dB = 0.3766;
    elseif Bandwidth_in_Oct==0.5
        Offset_dB = 0.1857;
    else
        disp('error, Bandwidth_in_Oct must be 1,2,3 or 4')
    end

    if FlagSub
        %Sub-Bass limitation:
        tmp=(find(f<=fSub)); fSub_i = tmp(end);
        H_Sub = H*0+H(fSub_i);
        H_Sub(1) = H_Sub(2);
    end

    if FlagAliasing
        % Aliasing frequency limitation:
        tmp=(find(f<=fAliasing)); falias_i = tmp(end);
        H_Alias = H*0+H(falias_i);
        H_Alias(1) = H_Alias(2);
    end

    if FlagSub
        % Lagrange interpolation for SUB:
        fl = f(fSub_i)*2^(-Bandwidth_in_Oct/2);
        fh = f(fSub_i)*2^(+Bandwidth_in_Oct/2);
        tmp=(find(f<=fl)); fl_i = tmp(end);
        tmp=(find(f<=fh)); fh_i = tmp(end);

        P1x = log10(f(fl_i));
        P1y = H_Sub(fl_i);
        P2x = log10(f(fSub_i));
        P2y = H_Sub(fSub_i)+Offset_dB;
        P3x = log10(f(fh_i));
        P3y = H(fh_i);

        % Lagrange interpolation tmp variables:
        ipol1=P1y/((P1x-P2x)*(P1x-P3x));
        ipol2=P2y/((P2x-P1x)*(P2x-P3x));
        ipol3=P3y/((P3x-P1x)*(P3x-P2x));

        for fi=1:fl_i-1
        H(fi) = H_Sub(fi);
        end

        for fi=fl_i:fh_i
            H(fi)=          (ipol1*(log10(f(fi))-P2x)*(log10(f(fi))-P3x))+...
                            (ipol2*(log10(f(fi))-P1x)*(log10(f(fi))-P3x))+...
                            (ipol3*(log10(f(fi))-P1x)*(log10(f(fi))-P2x));
        end
    end

    if FlagAliasing
        % Lagrange interpolation for ALIASING:
        fl = f(falias_i)*2^(-Bandwidth_in_Oct/2);
        fh = f(falias_i)*2^(+Bandwidth_in_Oct/2);
        tmp=(find(f<=fl)); fl_i = tmp(end);
        tmp=(find(f<=fh)); fh_i = tmp(end);

        P1x = log10(f(fl_i));
        P1y = H(fl_i);
        P2x = log10(f(falias_i));
        P2y = H_Alias(falias_i)-Offset_dB;
        P3x = log10(f(fh_i));
        P3y = H_Alias(fh_i);

        % Lagrange interpolation tmp variables:
        ipol1=P1y/((P1x-P2x)*(P1x-P3x));
        ipol2=P2y/((P2x-P1x)*(P2x-P3x));
        ipol3=P3y/((P3x-P1x)*(P3x-P2x));

        for fi=fl_i:fh_i
            H(fi)=      (ipol1*(log10(f(fi))-P2x)*(log10(f(fi))-P3x))+...
                        (ipol2*(log10(f(fi))-P1x)*(log10(f(fi))-P3x))+...
                        (ipol3*(log10(f(fi))-P1x)*(log10(f(fi))-P2x));
        end

        for fi=fh_i+1:length(H)
            H(fi) = H_Alias(fi);
        end
    end

    %apply original phase and delog
    H = 10.^(H/20).*exp(1i*Hphase);
    H(1) = abs(H(2));
    %we ignore the special treatment for DC and fs/2 as would be usual for
    %DFT data, since we need a 'classical' frequency response
end


 function diff_im = anisodiff2D(im, num_iter, delta_t, kappa, option)
%ANISODIFF2D Conventional anisotropic diffusion
%   DIFF_IM = ANISODIFF2D(IM, NUM_ITER, DELTA_T, KAPPA, OPTION) perfoms
%   conventional anisotropic diffusion (Perona & Malik) upon a gray scale
%   image. A 2D network structure of 8 neighboring nodes is considered for
%   diffusion conduction.
%
%       ARGUMENT DESCRIPTION:
%               IM       - gray scale image (MxN).
%               NUM_ITER - number of iterations.
%               DELTA_T  - integration constant (0 <= delta_t <= 1/7).
%                          Usually, due to numerical stability this
%                          parameter is set to its maximum value.
%               KAPPA    - gradient modulus threshold that controls the conduction.
%               OPTION   - conduction coefficient functions proposed by Perona & Malik:
%                          1 - c(x,y,t) = exp(-(nablaI/kappa).^2),
%                              privileges high-contrast edges over low-contrast ones.
%                          2 - c(x,y,t) = 1./(1 + (nablaI/kappa).^2),
%                              privileges wide regions over smaller ones.
%
%       OUTPUT DESCRIPTION:
%                DIFF_IM - (diffused) image with the largest scale-space parameter.
%
%   Example
%   -------------
%   s = phantom(512) + randn(512);
%   num_iter = 15;
%   delta_t = 1/7;
%   kappa = 30;
%   option = 2;
%   ad = anisodiff2D(s,num_iter,delta_t,kappa,option);
%   figure, subplot 121, imshow(s,[]), subplot 122, imshow(ad,[])
%
% See also anisodiff1D, anisodiff3D.

% References:
%   P. Perona and J. Malik.
%   Scale-Space and Edge Detection Using Anisotropic Diffusion.
%   IEEE Transactions on Pattern Analysis and Machine Intelligence,
%   12(7):629-639, July 1990.
%
%   G. Grieg, O. Kubler, R. Kikinis, and F. A. Jolesz.
%   Nonlinear Anisotropic Filtering of MRI Data.
%   IEEE Transactions on Medical Imaging,
%   11(2):221-232, June 1992.
%
%   MATLAB implementation based on Peter Kovesi's anisodiff(.):
%   P. D. Kovesi. MATLAB and Octave Functions for Computer Vision and Image Processing.
%   School of Computer Science & Software Engineering,
%   The University of Western Australia. Available from:
%   <http://www.csse.uwa.edu.au/~pk/research/matlabfns/>.
%
% Credits:
% Daniel Simoes Lopes
% ICIST
% Instituto Superior Tecnico - Universidade Tecnica de Lisboa
% danlopes (at) civil ist utl pt
% http://www.civil.ist.utl.pt/~danlopes
%
% May 2007 original version.

% Convert input image to double.
im = double(im);

% PDE (partial differential equation) initial condition.
diff_im = im;

% Center pixel distances.
dx = 1;
dy = 1;
dd = sqrt(2);

% 2D convolution masks - finite differences.
hN = [0 1 0; 0 -1 0; 0 0 0];
hS = [0 0 0; 0 -1 0; 0 1 0];
hE = [0 0 0; 0 -1 1; 0 0 0];
hW = [0 0 0; 1 -1 0; 0 0 0];
hNE = [0 0 1; 0 -1 0; 0 0 0];
hSE = [0 0 0; 0 -1 0; 0 0 1];
hSW = [0 0 0; 0 -1 0; 1 0 0];
hNW = [1 0 0; 0 -1 0; 0 0 0];

% Anisotropic diffusion.
for t = 1:num_iter

        % Finite differences. [imfilter(.,.,'conv') can be replaced by conv2(.,.,'same')]
        nablaN = imfilter(diff_im,hN,'conv');
        nablaS = imfilter(diff_im,hS,'conv');
        nablaW = imfilter(diff_im,hW,'conv');
        nablaE = imfilter(diff_im,hE,'conv');
        nablaNE = imfilter(diff_im,hNE,'conv');
        nablaSE = imfilter(diff_im,hSE,'conv');
        nablaSW = imfilter(diff_im,hSW,'conv');
        nablaNW = imfilter(diff_im,hNW,'conv');

        % Diffusion function.
        if option == 1
            cN = exp(-(nablaN/kappa).^2);
            cS = exp(-(nablaS/kappa).^2);
            cW = exp(-(nablaW/kappa).^2);
            cE = exp(-(nablaE/kappa).^2);
            cNE = exp(-(nablaNE/kappa).^2);
            cSE = exp(-(nablaSE/kappa).^2);
            cSW = exp(-(nablaSW/kappa).^2);
            cNW = exp(-(nablaNW/kappa).^2);
        elseif option == 2
            cN = 1./(1 + (nablaN/kappa).^2);
            cS = 1./(1 + (nablaS/kappa).^2);
            cW = 1./(1 + (nablaW/kappa).^2);
            cE = 1./(1 + (nablaE/kappa).^2);
            cNE = 1./(1 + (nablaNE/kappa).^2);
            cSE = 1./(1 + (nablaSE/kappa).^2);
            cSW = 1./(1 + (nablaSW/kappa).^2);
            cNW = 1./(1 + (nablaNW/kappa).^2);
        end

        % Discrete PDE solution.
        diff_im = diff_im + ...
                  delta_t*(...
                  (1/(dy^2))*cN.*nablaN + (1/(dy^2))*cS.*nablaS + ...
                  (1/(dx^2))*cW.*nablaW + (1/(dx^2))*cE.*nablaE + ...
                  (1/(dd^2))*cNE.*nablaNE + (1/(dd^2))*cSE.*nablaSE + ...
                  (1/(dd^2))*cSW.*nablaSW + (1/(dd^2))*cNW.*nablaNW );

        % Iteration warning.
        %fprintf('\rIteration %d\n',t);
end


function sig = MakeSignal(Name,n)
% MakeSignal -- Make artificial signal
%  Usage
%    sig = MakeSignal(Name,n)
%  Inputs
%    Name   string: 'HeaviSine', 'Bumps', 'Blocks',
%            'Doppler', 'Ramp', 'Cusp', 'Sing', 'HiSine',
%            'LoSine', 'LinChirp', 'TwoChirp', 'QuadChirp',
%            'MishMash', 'WernerSorrows' (Heisenberg),
%            'Leopold' (Kronecker), 'Piece-Regular' (Piece-Wise Smooth),
%	     'Riemann','HypChirps','LinChirps', 'Chirps', 'Gabor'
%	     'sineoneoverx','Cusp2','SmoothCusp','Gaussian'
%	     'Piece-Polynomial' (Piece-Wise 3rd degree polynomial)
%    n      desired signal length
%  Outputs
%    sig    1-d signal
%
%  References
%    Various articles of D.L. Donoho and I.M. Johnstone
%
   if nargin > 1,
	t = (1:n) ./n;
   end
	if strcmp(Name,'HeaviSine'),
	    sig = 4.*sin(4*pi.*t);
	    sig = sig - sign(t - .3) - sign(.72 - t);
	elseif strcmp(Name,'Bumps'),
	    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
	    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
	    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
	    sig = zeros(size(t));
	    for j =1:length(pos)
	       sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
	    end
	elseif strcmp(Name,'Blocks'),
	    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
	    hgt = [4 (-5) 3 (-4) 5 (-4.2) 2.1 4.3  (-3.1) 2.1 (-4.2)];
	    sig = zeros(size(t));
	    for j=1:length(pos)
	        sig = sig + (1 + sign(t-pos(j))).*(hgt(j)/2) ;
	    end
	elseif strcmp(Name,'Doppler'),
	    sig = sqrt(t.*(1-t)).*sin((2*pi*1.05) ./(t+.05));
	elseif strcmp(Name,'Ramp'),
	    sig = t - (t >= .37);
	elseif strcmp(Name,'Cusp'),
	    sig = sqrt(abs(t - .37));
	elseif strcmp(Name,'Sing'),
	    k = floor(n * .37);
	    sig = 1 ./abs(t - (k+.5)/n);
	elseif strcmp(Name,'HiSine'),
	    sig = sin( pi * (n * .6902) .* t);
	elseif strcmp(Name,'LoSine'),
	    sig = sin( pi * (n * .3333) .* t);
	elseif strcmp(Name,'LinChirp'),
	    sig = sin(pi .* t .* ((n .* .500) .* t));
	elseif strcmp(Name,'TwoChirp'),
	    sig = sin(pi .* t .* (n .* t)) + sin((pi/3) .* t .* (n .* t));
	elseif strcmp(Name,'QuadChirp'),
	    sig = sin( (pi/3) .* t .* (n .* t.^2));
	elseif strcmp(Name,'MishMash'),  % QuadChirp + LinChirp + HiSine
	    sig = sin( (pi/3) .* t .* (n .* t.^2)) ;
	    sig = sig +  sin( pi * (n * .6902) .* t);
	    sig = sig +  sin(pi .* t .* (n .* .125 .* t));
	elseif strcmp(Name,'WernerSorrows'),
	    sig = sin( pi .* t .* (n/2 .* t.^2)) ;
	    sig = sig +  sin( pi * (n * .6902) .* t);
	    sig = sig +  sin(pi .* t .* (n .* t));
	    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
	    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
	    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
	    for j =1:length(pos)
	       sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
	    end
	elseif strcmp(Name,'Leopold'),
	    sig = (t == floor(.37 * n)/n);  % Kronecker
	elseif strcmp(Name,'Riemann'),
		sqn = round(sqrt(n));
	    sig = t .* 0;  % Riemann's Non-differentiable Function
		sig((1:sqn).^2) = 1. ./ (1:sqn);
		sig = real(ifft(sig));
	elseif strcmp(Name,'HypChirps'), % Hyperbolic Chirps of Mallat's book
		alpha	= 15*n*pi/1024;
		beta    = 5*n*pi/1024;
		t  	= (1.001:1:n+.001)./n;
		f1      = zeros(1,n);
		f2      = zeros(1,n);
		f1  	= sin(alpha./(.8-t)).*(0.1<t).*(t<0.68);
		f2  	= sin(beta./(.8-t)).*(0.1<t).*(t<0.75);
		M  	= round(0.65*n);
		P 	= floor(M/4);
		enveloppe = ones(1,M); % the rising cutoff function
            enveloppe(1:P) = (1+sin(-pi/2+((1:P)-ones(1,P))./(P-1)*pi))/2;
            enveloppe(M-P+1:M) = reverse(enveloppe(1:P));
  		env 	= zeros(1,n);
  		env(ceil(n/10):M+ceil(n/10)-1) = enveloppe(1:M);
		sig     = (f1+f2).*env;
	elseif strcmp(Name,'LinChirps'), % Linear Chirps of Mallat's book
		b 	= 100*n*pi/1024;
		a 	= 250*n*pi/1024;
		t 	= (1:n)./n;
		A1 	= sqrt((t-1/n).*(1-t));
		sig	= A1.*(cos((a*(t).^2)) + cos((b*t+a*(t).^2)));
	elseif strcmp(Name,'Chirps'), % Mixture of Chirps of Mallat's book
		t 	= (1:n)./n.*10.*pi;
  		f1 	= cos(t.^2*n/1024);
		a 	= 30*n/1024;
  		t 	= (1:n)./n.*pi;
  		f2 	= cos(a.*(t.^3));
  		f2 	= reverse(f2);
		ix 	= (-n:n)./n.*20;
 		g 	= exp(-ix.^2*4*n/1024);
		i1 	= (n/2+1:n/2+n);
		i2 	= (n/8+1:n/8+n);
		j  	= (1:n)/n;
    		f3 	= g(i1).*cos(50.*pi.*j*n/1024);
		f4 	= g(i2).*cos(350.*pi.*j*n/1024);
		sig 	= f1+f2+f3+f4;
   	 enveloppe = ones(1,n); % the rising cutoff function
  	 enveloppe(1:n/8) = (1+sin(-pi/2+((1:n/8)-ones(1,n/8))./(n/8-1)*pi))/2;
  	 enveloppe(7*n/8+1:n) = reverse(enveloppe(1:n/8));
 		sig 	= sig.*enveloppe;
        elseif strcmp(Name,'Gabor'), % two modulated Gabor functions in
				     % Mallat's book
		N = 512;
   		t = (-N:N)*5/N;
        	j = (1:N)./N;
		g = exp(-t.^2*20);
		i1 = (2*N/4+1:2*N/4+N);
		i2 = (N/4+1:N/4+N);
		sig1 = 3*g(i1).*exp(i*N/16.*pi.*j);
		sig2 = 3*g(i2).*exp(i*N/4.*pi.*j);
   		sig = sig1+sig2;
	elseif strcmp(Name,'sineoneoverx'), % sin(1/x) in Mallat's book
		N = 1024;
		i1 = (-N+1:N);
		i1(N) = 1/100;
		i1 = i1./(N-1);
		sig = sin(1.5./(i1));
		sig = sig(513:1536);
	elseif strcmp(Name,'Cusp2'),
		N = 64;
		i1 = (1:N)./N;
		x = (1-sqrt(i1)) + i1/2 -.5;
		M = 8*N;
		sig = zeros(1,M);
		sig(M-1.5.*N+1:M-.5*N) = x;
		sig(M-2.5*N+2:M-1.5.*N+1) = reverse(x);
		sig(3*N+1:3*N + N) = .5*ones(1,N);
	elseif strcmp(Name,'SmoothCusp'),
		sig = MakeSignal('Cusp2');
		N = 64;
		M = 8*N;
		t = (1:M)/M;
		sigma = 0.01;
		g = exp(-.5.*(abs(t-.5)./sigma).^2)./sigma./sqrt(2*pi);
		g = fftshift(g);
		sig2 = iconv(g',sig)'/M;
   	elseif strcmp(Name,'Piece-Regular'),
		sig1=-15*MakeSignal('Bumps',n);
		t = (1:fix(n/12)) ./fix(n/12);
		sig2=-exp(4*t);
		t = (1:fix(n/7)) ./fix(n/7);
		sig5=exp(4*t)-exp(4);
		t = (1:fix(n/3)) ./fix(n/3);
		sigma=6/40;
		sig6=-70*exp(-((t-1/2).*(t-1/2))/(2*sigma^2));
		sig(1:fix(n/7))= sig6(1:fix(n/7));
		sig((fix(n/7)+1):fix(n/5))=0.5*sig6((fix(n/7)+1):fix(n/5));
		sig((fix(n/5)+1):fix(n/3))=sig6((fix(n/5)+1):fix(n/3));
		sig((fix(n/3)+1):fix(n/2))=sig1((fix(n/3)+1):fix(n/2));
		sig((fix(n/2)+1):(fix(n/2)+fix(n/12)))=sig2;
		sig((fix(n/2)+2*fix(n/12)):-1:(fix(n/2)+fix(n/12)+1))=sig2;
sig(fix(n/2)+2*fix(n/12)+fix(n/20)+1:(fix(n/2)+2*fix(n/12)+3*fix(n/20)))=...
-ones(1,fix(n/2)+2*fix(n/12)+3*fix(n/20)-fix(n/2)-2*fix(n/12)-fix(n/20))*25;
		k=fix(n/2)+2*fix(n/12)+3*fix(n/20);
		sig((k+1):(k+fix(n/7)))=sig5;
		diff=n-5*fix(n/5);
		sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
		% zero-mean
		bias=sum(sig)/n;
		sig=bias-sig;
   	elseif strcmp(Name,'Piece-Polynomial'),
		t = (1:fix(n/5)) ./fix(n/5);
		sig1=20*(t.^3+t.^2+4);
		sig3=40*(2.*t.^3+t) + 100;
		sig2=10.*t.^3 + 45;
		sig4=16*t.^2+8.*t+16;
		sig5=20*(t+4);
		sig6(1:fix(n/10))=ones(1,fix(n/10));
		sig6=sig6*20;
		sig(1:fix(n/5))=sig1;
		sig(2*fix(n/5):-1:(fix(n/5)+1))=sig2;
		sig((2*fix(n/5)+1):3*fix(n/5))=sig3;
		sig((3*fix(n/5)+1):4*fix(n/5))=sig4;
		sig((4*fix(n/5)+1):5*fix(n/5))=sig5(fix(n/5):-1:1);
		diff=n-5*fix(n/5);
		sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
		%sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=-ones(1,fix(n/10))*20;
		sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=ones(1,fix(n/10))*10;
		sig((n-fix(n/10)+1):(n+fix(n/20)-fix(n/10)))=ones(1,fix(n/20))*150;
		% zero-mean
		bias=sum(sig)/n;
		sig=sig-bias;
   	elseif strcmp(Name,'Gaussian'),
		sig=GWN(n,beta);
		g=zeros(1,n);
		lim=alpha*n;
		mult=pi/(2*alpha*n);
		g(1:lim)=(cos(mult*(1:lim))).^2;
		g((n/2+1):n)=g((n/2):-1:1);
		g = rnshift(g,n/2);
		g=g/norm(g);
		sig=iconv(g,sig);
       else
	    disp(sprintf('MakeSignal: I don*t recognize <<%s>>',Name))
	    disp('Allowable Names are:')
	       disp('HeaviSine'),
	       disp('Bumps'),
	       disp('Blocks'),
	       disp('Doppler'),
	       disp('Ramp'),
	       disp('Cusp'),
	       disp('Crease'),
	       disp('Sing'),
	       disp('HiSine'),
	       disp('LoSine'),
	       disp('LinChirp'),
	       disp('TwoChirp'),
	       disp('QuadChirp'),
	       disp('MishMash'),
	       disp('WernerSorrows'),
	       disp('Leopold'),
	       disp('Sing'),
	       disp('HiSine'),
	       disp('LoSine'),
	       disp('LinChirp'),
	       disp('TwoChirp'),
	       disp('QuadChirp'),
	       disp('MishMash'),
	       disp('WernerSorrows'),
	       disp('Leopold'),
	       disp('Riemann'),
	       disp('HypChirps'),
	       disp('LinChirps'),
	       disp('Chirps'),
	       disp('sineoneoverx'),
	       disp('Cusp2'),
	       disp('SmoothCusp'),
	       disp('Gabor'),
	       disp('Piece-Regular');
	       disp('Piece-Polynomial');
	       disp('Gaussian');
	end

%
% Originally made by David L. Donoho.
% Function has been enhanced.

%
%  Part of Wavelab Version 850
%  Built Tue Jan  3 13:20:39 EST 2006
%  This is Copyrighted Material
%  For Copying permissions see COPYING.m
%  Comments? e-mail wavelab@stat.stanford.edu


%bilinearFit
% Fits data to bilinear function
%
% Author: Jonathan Karr, jkarr@stanford.edu
% Affiliation: Covert Lab, Department of Bioengineering, Stanford University
% Last Updated: 1/11/2012
function [a, b, cutX, gof] = bilinearFit(x, y)

tf = ~isnan(x) & ~isnan(y);
x = x(tf);
y = y(tf);

[~, order] = sort(x);
x = x(order);
y = y(order);

a = NaN(numel(x), 2);
b = NaN(numel(x), 2);
sse = NaN(numel(x), 2);
rsquare = NaN(numel(x), 2);
adjrsquare = NaN(numel(x), 2);
for i = 2:numel(x)-2
    [fitobject1, gof1] = fit(x(1:i), y(1:i), 'poly1');
    [fitobject2, gof2] = fit(x(i+1:end), y(i+1:end), 'poly1');
    a(i, :) = [fitobject1.p2 fitobject2.p2];
    b(i, :) = [fitobject1.p1 fitobject2.p1];
    sse(i, :) = [gof1.sse gof2.sse];
    rsquare(i, :) = [gof1.rsquare gof2.rsquare];
    adjrsquare(i, :) = [gof1.adjrsquare gof2.adjrsquare];
end

[~, cutIdx] = nanmin(sum(sse, 2));
cutX = mean(x(cutIdx:cutIdx+1));

a = a(cutIdx, :);
b = b(cutIdx, :);
sse = sse(cutIdx, :);
rsquare = rsquare(cutIdx, :);
adjrsquare = adjrsquare(cutIdx, :);

sse0 = sum(sse);
rsquare0 = 1 - sse0 / sum((y-mean(y)).^2);
n = numel(x);
p = 3;
adjrsquare0 = 1 - (1 - rsquare0) * (n-1) / (n-p-1);

gof = struct(...
    'sse', [sse0 sse], ...
    'rsquare', [rsquare0 rsquare], ...
    'adjrsquare', [adjrsquare0 adjrsquare]);


function wavelenvsth
% _____________________________________________________
% This function gets the nomograph which shows  the relationship which exists
% between the wavelength, wave period and water depth using the dispersion
% equation.
%
% 0. Syntax:
% >> wavelenvsth
%
% 1. Inputs:
%     None.
%
% 2. Outputs:
%     Nomograph.
%
% 3. Example:
%  >> run(wavelenvsth)
%
%  4. Notes:
%     - It's only necessary to run the function.
%     - In the nomograph, it is activated the data cursor mode in order to
%       get the wavelength and waver depth in function of the water period.
%     - Wave depths in meters.
%     - Wave length in meters.
%
% 5. Referents:
%      Darlymple, R.G. and Dean R.A. (1999). Water Wave Mechanics
%              for Engineers and Scientist. Advanced Series on Ocean Engineering, Vol. 2
%              World Scientific. Singapure.
%      Le Mehuate, Bernard. (1976). An introduction to hidrodynamics and water waves.
%              Springer-Verlag. USA.
%
% Gabriel Ruiz.
% Jun-2006
% UNAM
%_______________________________________________________________________

%%%%%%%%%%  M A I N      F U N C T I O N   %%%%%%%%%%
% clear all; clc;
 h = 1:1:130;
    g = 9.81;
        T = 4.5;
            dh = h';
        m=1;
    fg =figure('Menubar', 'none', 'Name', 'Wavelength vs Period and Depth', 'NumberTitle', 'off',...
                    'Position' , [ 6 37 1011 697 ] , 'Color' , [ 0.87 0.87 0.87 ]);

    while T ~= 17

        for i=1:130
                    con = 1;
                        l(con) = 0;
                    l(con+1) = 1.56 * T ^ 2;

                    while abs( l(con+1) - l(con) ) > 0.0001,
                                l(con+2) = ( ( 9.81 * T ^ 2 ) / ( 2 * pi ) ) * tanh( ( 2 * pi * h(i) ) / l(con+1) );
                                con = con + 1;
                    end

                    L(i) =  l(con);
                    k = ( 2 * pi ) / L(i);
        end

        Ls{1,m} = L';
            hold on
                fgh =plot(dh,Ls{1,m});
                    xlabel('Water Depth (h), meters');
                        ylabel('Wave Length (L), meters');
                    xlim( [ 1 130 ] )
                set(fgh,'Color',rand(1,3));
            set(gca,'Box','on');
        drt = title('Wavelength vs Period and Depth', 'HorizontalAlignment' , 'center' , 'FontWeight', 'bold');
            set(gca, 'XGrid', 'off', 'XMinorTick', 'on' , 'YGrid' , 'off' , 'YMinorTick' , 'on', 'Fontsize', 8 );
                m = m+1;
                    T = 0.5+T;
    end

    T= 4.5:0.5:17;
        hj =length(T);
    textos2 = 'T = ';

    for i = 1 : hj
            stringer{i,1} = num2str(T(1,i));
            textos{i,1} = horzcat(textos2,stringer{i,1},' s');
    end
        asdk = legend(textos, 'Location', 'EastOutside');
            set(asdk, 'FontSize', 6);
                clc;
            datacursormode on;
        dcm_obj = datacursormode(fg);
       fundat = str2func('camdatos');
    set(dcm_obj, 'DisplayStyle', 'Window' , 'UpdateFcn' , fundat)
%%%%%%%%%%%%    E N D     M A I N     F U N C T I O N %%%%%%%%%%%%%

function [txtdcm,pos] = camdatos(empt,event_obj)    %% Subfunction 1
pos = get(event_obj,'Position');
txtdcm = {['Water Depth: ',num2str(pos(1))] , ['Wave Length: ',num2str(pos(2))]};

% End of Subfunction 1


%[2016]-"Jaya: A simple and new optimization algorithm for solving
%constrained and unconstrained optimization problems"

% (9/12/2020)

function JA = jJayaAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial (26)
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
  end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Best
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Pre
Xnew = zeros(N,dim);

curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Iteration
while t <= max_Iter
  % Identify best & worst in population
  [~, idxB] = min(fit);
  Xbest     = X(idxB,:);
  [~, idxW] = max(fit);
  Xworst    = X(idxW,:);
  % Start
  for i = 1:N
    for d = 1:dim
      % Random numbers
      r1 = rand();
      r2 = rand();
      % Update (1)
      Xnew(i,d) = X(i,d) + r1 * (Xbest(d) - abs(X(i,d))) - ...
        r2 * (Xworst(d) - abs(X(i,d)));
    end
    % Boundary
    XB = Xnew(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    Xnew(i,:) = XB;
  end
  % Fitness
  for i = 1:N
    Fnew = fun(feat,label,(Xnew(i,:) > thres),opts);
    % Greedy selection
    if Fnew < fit(i)
      fit(i) = Fnew;
      X(i,:) = Xnew(i,:);
    end
    % Best
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = X(i,:);
    end
  end
  % Save
  curve(t) = fitG;
  fprintf('\nIteration %d Best (JA)= %f',t,curve(t))
  t = t + 1;
end
% Select features based on selected index
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
JA.sf = Sf;
JA.ff = sFeat;
JA.nf = length(Sf);
JA.c  = curve;
JA.f  = feat;
JA.l  = label;
end


function [X, A, B, t, varargout] = VBMC(P, Y, options)
% Variational Bayesian Low Rank Matrix Completion
% Author (a.k.a. person to blame): S. Derin Babacan
%
% Last updated: January 31, 2012
% Usage:
% [X, A, B, t, varargout] = VSBL_MAT(P, Y, options)
% Solves for X = AB' in
% Y = P(X) + N = P(AB') + N  where X, A, B are low rank,
% N is dense Gaussian noise, and P is a binary sampling matrix.
%
% If you're using this code, please acknowledge
% Reference:
%    S. D. Babacan, M. Luessi, R. Molina, and A. K. Katsaggelos,
%    "Sparse Bayesian Methods for Low-Rank Matrix Estimation,"
%    IEEE Transactions on Signal Processing, 2012.
%
% This code is not optimized for speed. It implements the full variational
% Bayesian inference described in the paper, without any manipulation of
% the matrices. These are described in the paper above but not implemented
% in this version of the code. These manipulations might lead to significant
% decrease in running times depending on the size of matrix Y.
%
% -----------------------------------------------------------------------
%                                INPUTS
%   P               : binary (0-1) sampling matrix same size as Y
%   Y               : input matrix
%   options         : All options are *optional*. The default values are set
%                     automatically.
%
%   verbose       : output the progress? (0/1) default: 0.
%   init          : Initialization method.
%                   'rand': initialize A and B with random matrices
%                   'ml'  : Apply SVD to Y and initialize A and B using its factors.
%                   (default)
%   a_gamma0
%   b_gamma0      : hyperparameter values for gamma. default: 1e-6
%
%
%   a_beta0
%   b_beta0      : hyperparameter values for beta. default: 0
%
%   inf_flag      : Flag for inference of components of E. default: 1
%                    1: Standard
%                    2: fixed point MacKay
%   MAXITER       : max number of iterations. default: 100
%   UPDATE_BETA   : update noise variance? (0/1). default: 1
%   UPDATE_BETA_START : iteration number to start updating the noise variance
%   DIM_RED       : prune irrelevant dimensions during iterations? (0/1).  default: 1
%   DIMRED_THR    : threshold to remove columns from A and B. Used only if
%                   DIM_RED is 1.  default: 1e4
%   initial_rank  : The initial rank of A and B to start. Default is full
%                   rank, calculated from the SVD of Y. A smaller value is
%                   helpful if the data dimensions is large.
%   X_true,E_true : Original X and E matrices for simulations.
%                   If supplied and verbose=1,
%                   the errors are reported at each iteration
% -----------------------------------------------------------------------
%                                OUTPUTS
% X               : low rank component
% A, B            : low rank factors of X = AB'
% optional outputs (see the paper for the full definitions):
%   varargout{1} = gamma    --- hyperparameters for A and B
%   varargout{2} = beta     --- noise inverse variance
%   varargout{3} = it       --- number of iterations
%   varargout{4} = t        --- running time
%   varargout{5} = Sigma_A  --- covariance matrix of A
%   varargout{6} = Sigma_B  --- covariance matrix of B
% -----------------------------------------------------------------------
%
% Copyright (c): S. Derin Babacan, 2011
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License <http://www.gnu.org/licenses/> for more details.
%

%% Check input variables,
if ~exist('options','var')
    options = populate_vars([]);
else
    options = populate_vars(options);
end

verbose      = options.verbose;
a_gamma0     = options.a_gamma0;
b_gamma0     = options.b_gamma0;
a_beta0      = options.a_beta0;
b_beta0      = options.b_beta0;
MAXITER      = options.MAXITER;
inf_flag     = options.inf_flag;
UPDATE_BETA  = options.UPDATE_BETA;
DIMRED       = options.DIMRED;

if DIMRED,
    if isfield(options, 'DIMRED_THR')
        DIMRED_THR = options.DIMRED_THR;
    else
        DIMRED_THR = 1e4;
    end
end

UPDATE_BETA_START = options.UPDATE_BETA_START;

% For synthetic simulations
if isfield(options, 'X_true')
    X_true = options.X_true;
end

%% Initialization
[m n] = size(Y);
mn = m*n;
pmn = length(find(P == 1));
p = pmn/mn;

Y2sum = sum(Y(:).^2);
scale2 = Y2sum / (mn);
scale = sqrt(scale2);

% Initialize A and B
switch options.init,

    % Maximum likelihood
    case 'ml'
        [U, S, V] = svd(Y, 'econ');

        if strcmp(options.initial_rank, 'auto')
            r = min([m,n]);
        else
            r = options.initial_rank;
        end

        A = U(:,1:r)*(S(1:r,1:r)).^(0.5);
        B = (S(1:r,1:r)).^(0.5)*V(:,1:r)'; B = B';

        Sigma_A = repmat( scale*eye(r,r), [1 1 m] );
        Sigma_B = repmat( scale*eye(r,r), [1 1 n] );

        gammas = 1./diag(S.^2);
        gammas = gammas(1:r);
        gammas = ones(r,1)*scale;

        ssscale2 = 1;
        beta = ssscale2./scale2;

        if UPDATE_BETA == 0 & isfield(options, 'beta')
            beta = options.beta;
        end

    % Random initialization
    case 'rand'

        if strcmp(options.initial_rank, 'auto')
            r = min([m,n]);
        else
            r = options.initial_rank;
        end

        A = randn(m,r) * sqrt(scale);
        B = randn(n,r) * sqrt(scale);
        gammas = scale*ones(r,1);

        Sigma_A = repmat( scale*eye(r,r), [1 1 m] );
        Sigma_B = repmat( scale*eye(r,r), [1 1 n] );

        if UPDATE_BETA == 0 & isfield(options, 'beta')
            beta = options.beta;
        else
            beta = 1./scale2;
        end

end

X = A*B';

%% Iterations
tic
for it=1:MAXITER,

    old_X = X;

    Aw = diag(gammas);

    %% A step
    diagsA = 0;
    for i=1:m, %iterate over rows

        observed = find(P(i,:));
        Bi = B(observed,:);
        Sigma_A(:,:,i) = (beta*Bi'*Bi + beta*sum(Sigma_B(:,:,observed),3) + Aw)^(-1);
        A(i,:) = beta*Y(i,observed)*Bi*Sigma_A(:,:,i);
        diagsA = diagsA + diag( Sigma_A(:,:,i) );

    end

    %% B step
    diagsB = 0;
    for j=1:n, %Iterate over cols

        observed = find(P(:,j));
        Aj = A(observed,:);
        Sigma_B(:,:,j) = (beta*Aj'*Aj + beta*sum(Sigma_A(:,:,observed),3) + Aw)^(-1);
        B(j,:) = beta*Y(observed,j)'*Aj*Sigma_B(:,:,j);
        diagsB = diagsB + diag( Sigma_B(:,:,j) );

    end

    %% update X
    X = A*B';

    %% estimate gammas
    % Choose inference method (fixed point Mackay or standard)
    if inf_flag == 1, %Standard
        gammas = (m + n + a_gamma0)./( diag(B'*B) + diag(sum(Sigma_B,3)) + diag(A'*A)+ diag(sum(Sigma_A,3))+ b_gamma0);
    elseif inf_flag == 2, %Mackay
        gammas = (m + n + a_gamma0 - gammas.*( diag(sum(Sigma_A,3)) + diag(sum(Sigma_B,3)) ))./( diag(A'*A) + diag(B'*B)+b_gamma0);
    end

    %% estimate beta
    if  UPDATE_BETA & it>=UPDATE_BETA_START,

        err = sum(sum( abs(Y - P.*X).^2 ) );

        for l = 1: m
            observed = find(P(l, :));
            err = err + trace(B(observed,:)'*B(observed,:)*Sigma_A(:, :, l)) ...
                + trace(A(l, :)'*A(l, :) *sum(Sigma_B(:, :, observed), 3)) ...
                + trace( Sigma_A(:, :, l)*sum(Sigma_B(:, :, observed), 3));

        end

        beta = (pmn + a_beta0)/(err+b_beta0);
    end

    %% Prune irrelevant dimensions?
    if DIMRED,
        MAX_GAMMA = min(gammas) * DIMRED_THR;

        if sum(find(gammas > MAX_GAMMA)),

            indices = find(gammas <= MAX_GAMMA);

            A = A(:,indices);
            B = B(:,indices);
            gammas = gammas(indices);

            Sigma_A = Sigma_A(indices,indices,:);
            Sigma_B = Sigma_B(indices,indices,:);

            [m r] = size(A);
            [n r] = size(B);
        end

    end

    %% Check convergence and display progress
    conv = sum(sum( ( old_X - X ).^2 ) ) / mn;

    if verbose,

        if exist('X_true','var')
            % For synthetic simulations
            err = sum(sum( ( X - X_true ).^2 ) ) / sum(sum( ( X_true ).^2 ) );
            rms = sqrt( sum(sum( ( X - X_true ).^2 ) )/mn );
            relrec = norm(X_true-X,'fro')/norm(X_true,'fro');

            fprintf('it %d: Error = %g, beta = %g, conv = %g, r = %d\n', it, relrec, beta, conv, r);

        else

            fprintf('it %d: beta = %g, conv = %g, r = %d\n', it, beta, conv, r);

        end
    end

    % Check for convergence (do at least 5 iterations)
    if it > 5 & conv < options.thr
        break;
    end

end
t = toc;

varargout{1} = gammas;
varargout{2} = beta;
varargout{3} = it;
varargout{4} = t;
varargout{5} = Sigma_A;
varargout{6} = Sigma_B;

%% Function to populate options.
function [options] = populate_vars(options)

if isempty(options)
    options.init         = 'ml';
    options.verbose      = 1;
    options.a_gamma0     = 1e-6;
    options.b_gamma0     = 1e-6;
    options.a_beta0      = 0;
    options.b_beta0      = 0;
    options.MAXITER      = 100;
    options.DIMRED       = 1;
    options.UPDATE_BETA  = 1;
    options.mode         = 'VB';
    options.thr          = 1e-7;
    options.initial_rank = 'auto';
    options.inf_flag     = 1;
    options.UPDATE_BETA_START = 1;
else

    if ~isfield(options, 'init')
        options.init = 'ml';
    end

    if ~strcmp(options.init,'ml') | ~strcmp(options.init,'rand')
        options.init = 'ml';
    end

    if ~isfield(options, 'verbose')
        options.verbose   = 1;
    end

    if ~isfield(options, 'a_gamma0')
        options.a_gamma0   = 1e-6;
    end

    if ~isfield(options, 'b_gamma0')
        options.b_gamma0   = 1e-6;
    end

    if ~isfield(options, 'a_beta0')
        options.a_beta0   = 0;
    end

    if ~isfield(options, 'b_beta0')
        options.b_beta0   = 0;
    end

    if ~isfield(options, 'MAXITER')
        options.MAXITER   = 100;
    end

    if ~isfield(options, 'DIMRED')
        options.DIMRED   = 1;
    end

    if ~isfield(options, 'inf_flag')
        options.inf_flag   = 1;
    end

    if options.inf_flag ~= 1 & options.inf_flag ~= 2,
        options.inf_flag = 1; % Standard inference
    end

    if ~isfield(options, 'UPDATE_BETA')
        options.UPDATE_BETA = 1;
    end

    if ~isfield(options, 'UPDATE_BETA_START')
        options.UPDATE_BETA_START = 1;
    end

    if ~isfield(options, 'mode')
        options.mode = 'VB';
    end

    if ~isfield(options, 'thr')
        options.thr = 1e-7;
    end

    if ~isfield(options, 'initial_rank')
        options.initial_rank = 'auto';
    end

end


function Y = multiplyOneRegion( psfMatData, X );
%
%           Y = multiplyOneRegion( psfMatData, X );
%
%  This function computes the multiplication of a point spread
%  function matrix times an image vector; that is,
%
%       Y = psfMatrix * X
%
%  Here we assume the dimension of the PSF is essentially the same
%  as the dimension of the image.
%
%  Input:
%   psfMatData  -  complex array containing the matrix data, usually
%                  computed from onePsfMatrix.m
%            X  -  array containing the image to which the psf matrix
%                  is to be multiplied
%
%  Output:
%          Y  -  contains the result after multiplication.
%

%  J. Nagy  1/6/02

%
%  First we determine if any padding is needed.
%
Xpad = padarray(X, size(psfMatData) - size(X), 'post');

%
%  Now we perform the multiplications.
%  Note that this should work for 2D and 3D images.
%

Y = ifftn( psfMatData .* fftn( Xpad ) );

[nx, ny, nz] = size( X );
Y = real( Y(1:nx, 1:ny, 1:nz) );


function [w, g_sqr] = rmsprop(w, g_sqr, grad, opts, lr)
%RMSPROP
%   Example RMSProp solver, for use with CNN_TRAIN and CNN_TRAIN_DAG.
%
%   Set the initial learning rate for RMSProp in the options for
%   CNN_TRAIN and CNN_TRAIN_DAG. Note that a learning rate that works for
%   SGD may be inappropriate for RMSProp; the default is 0.001.
%
%   If called without any input argument, returns the default options
%   structure.
%
%   Solver options: (opts.train.solverOpts)
%
%   `epsilon`:: 1e-8
%      Small additive constant to regularize variance estimate.
%
%   `rho`:: 0.99
%      Moving average window for variance update, between 0 and 1 (larger
%      values result in slower/more stable updating).

% Copyright (C) 2016 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if nargin == 0 % Return the default solver options
  w = struct('epsilon', 1e-8, 'rho', 0.99);
  return;
end

if isempty(g_sqr)
  g_sqr = 0 ;
end

g_sqr = g_sqr * opts.rho + grad.^2 * (1 - opts.rho) ;

w = w - lr * grad ./ (sqrt(g_sqr) + opts.epsilon) ;


function [tUs, odrIdx, TXmean, Wgt]  = MPCA(TX,gndTX,testQ,maxK)
% MPCA: Multilinear Principle Component Analysis
%
% %[Prototype]%
% function [tUs, odrIdx, TXmean, Wgt]  = MPCA(TX,gndTX,testQ,maxK)
%
% %[Author Notes]%
% Author: Haiping LU
% Email : hplu@ieee.org   or   eehplu@gmail.com
% Affiliation: Department of Electrical and Computer Engineering
%              University of Toronto
% Release date: June 24, 2008 (Version 1.1)
% Please email me if you have any problem, question or suggestion
%
% %[Algorithm]%:
% This function implements the Multilinear Principal Component Analysis
% (MPCA) algorithm presented in the follwing paper:
%    Haiping Lu, K.N. Plataniotis, and A.N. Venetsanopoulos,
%    "MPCA: Multilinear Principal Component Analysis of Tensor Objects",
%    IEEE Transactions on Neural Networks,
%    Vol. 19, No. 1, Page: 18-39, January 2008.
% Please reference this paper when reporting work done using this code.
%
% %[Toolbox needed]%:
% This function needs the tensor toolbox available at
% http://csmr.ca.sandia.gov/~tgkolda/TensorToolbox/
%
% %[Syntax]%: [tUs, odrIdx, TXmean, Wgt]  = MPCA(TX,gndTX,testQ,maxK)
%
% %[Inputs]%:
%    TX: the input training data in tensorial representation, the last mode
%        is the sample mode. For Nth-order tensor data, TX is of
%        (N+1)th-order with the (N+1)-mode to be the sample mode.
%        E.g., 30x20x10x100 for 100 samples of size 30x20x10
%        If your training data is too big, resulting in the "out of memory"
%        error, you could work around this problem by reading samples one
%        by one from the harddisk, or you could email me for help.
%
%    gndTX: the ground truth class labels (1,2,3,...) for the training data
%           E.g., a 100x1 vector if there are 100 samples
%           If the class label is not available (unsupervised learning),
%           please set gndTX=-1;
%
%    testQ: the percentage of variation kept in each mode, suggested value
%           is 97, and you can try other values, e.g., from 95 to 100, to
%           see whether better performance can be obtained.
%
%    maxK: the maximum number of iterations, suggested value is 1, and you
%          can try a larger value if computational time is not a concern.
%
% %[Outputs]%:
%    tUs: the multilinear projection, consiting of N
%         projection matrices, one for each mode
%
%    odrIdx: the ordering index of projected features in decreasing
%            variance (if unsupervised) or discriminality (if supervised)
%            for vectorizing the projected tensorial features
%
%    TXmean: the mean of the input training samples TX
%
%    Wgt: the weight tensor for use in modified distance measures. Please
%         refer to Section IV.B and IV.C of the paper.
%
% %[Supported tensor order]%
% This function supports N=2,3,4, for other order N, please modify the
% codes accordingly or email hplu@ieee.org or eehplu@gmail.com for help
%
% %[Examples]%
%%%%%%%%%%%%%%%%%%%%%%%%%%Example on 2D face data%%%%%%%%%%%%%%%%%%%%%%%%%%
%       load FERETC80A45%each sample is a second-order tensor of size 32x32
%       N=ndims(fea2D)-1;%Order of the tensor sample
%       Is=size(fea2D);%32x32x320
%       numSpl=Is(3);%There are 320 face samples
%       testQ=97;%Keep 97% variation in each mode
%       maxK=1;%One iteration only
%       [tUs, odrIdx, TXmean, Wgt] = MPCA(fea2D,gnd,testQ,maxK);
%       fea2Dctr=fea2D-repmat(TXmean,[ones(1,N), numSpl]);%Centering
%       newfea=ttm(tensor(fea2Dctr),tUs,1:N);%MPCA projection
%       %Vectorization of the tensorial feature
%       newfeaDim=size(newfea,1)*size(newfea,2);
%       newfea=reshape(newfea.data,newfeaDim,numSpl)';%Note: Transposed
%       selfea=newfea(:,odrIdx(1:P));%Select the first "P" sorted features
%       %"P" is the dimension of the final feature vector to be fed into a
%       %standard classifier (e.g., nearest neighbor classifier), you may
%       %need to test different values of P for best performance
%       Wgt=reshape(Wgt,newfeaDim,1);%Vectorizing weight tensor
%       Wgt=Wgt(odrIdx);%Select the weights accordingly
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%Example on 3D gait data%%%%%%%%%%%%%%%%%%%%%%%%%%
%       load USF17Gal %each sample is a third-order tensor of size 32x22x10
%       N=ndims(fea3D)-1;%Order of the tensor sample
%       Is=size(fea3D);%32x22x10x731
%       numSpl=Is(4);%There are 731 gait samples
%       testQ=97;%Keep 97% variation in each mode
%       maxK=1;%One iteration only
%       [tUs, odrIdx, TXmean, Wgt] = MPCA(fea3D,gnd,testQ,maxK);
%       fea3Dctr=fea3D-repmat(TXmean,[ones(1,N), numSpl]);%Centering
%       newfea = ttm(tensor(fea3Dctr),tUs,1:N);%MPCA projection
%       %Vectorization of the tensorial feature
%       newfeaDim=size(newfea,1)*size(newfea,2)*size(newfea,3);
%       newfea=reshape(newfea.data,newfeaDim,numSpl)';%Note: Transposed
%       selfea=newfea(:,odrIdx(1:P));%Select the first "P" sorted features
%       %"P" is the dimension of the final feature vector to be fed into a
%       %standard classifier (e.g., nearest neighbor classifier), you may
%       %need to test different values of P for best performance
%       Wgt=reshape(Wgt,newfeaDim,1);%Vectorizing weight tensor
%       Wgt=Wgt(odrIdx);%Select the weights accordingly
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %[Notes]%:
% A. Developed using Matlab R2006a
% B. Revision history:
%       Version 1.0 released on March 1, 2008
%       Version 1.1 released on June 24, 2008
%           ---Example usage on 2D data is included
%       Version 1.2 released on March 12, 2011
%           ---No change to this code, minor change in MPCALDA.m
%           ---Inclusion of a survey paper for in-depth analysis
%           ---Inclusion of a BibTeX file with MPCA extensions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%TX: (N+1)-dimensional tensor of Tensor Sample Dimension x NumSamples
N=ndims(TX)-1;%The order of samples.
IsTX=size(TX);
Is=IsTX(1:N);%The dimensions of the tensor
numSpl=IsTX(N+1);%Number of samples

%%%%%%%%%%%%%Zero-Mean%%%%%%%%%%
TXmean=mean(TX,N+1);%The mean
TX=TX-repmat(TXmean,[ones(1,N), numSpl]);%Centering

%The full projection for initialization
Qs=ones(N,1)*testQ;
Us=cell(N,1);
tUs=cell(N,1);
Lmds=cell(N,1);
for n=1:N
    In=Is(n);Phi=zeros(In,In);
    for m=1:numSpl
        switch N
            case 2
                Xm=TX(:,:,m);
            case 3
                Xm=TX(:,:,:,m);
            case 4
                Xm=TX(:,:,:,:,m);
            otherwise
                error('Order N not supported. Please modify the code here or email hplu@ieee.org for help.')
        end
        tX=tensor(Xm);
        tXn=tenmat(tX,n);
        Xn=tXn.data;
        Phi=Phi+Xn*Xn';
    end
    [Un,Lmdn]=eig(Phi);
    Lmd=diag(Lmdn);
    [stLmd,stIdx]=sort(Lmd,'descend');
    Us{n}=Un(:,stIdx);
    tUs{n}=Us{n}';
    Lmds{n}=Lmd(stIdx);
end

%Cumulative distribution of eigenvalues
cums=cell(N,1);
for n=1:N
    In=length(Lmds{n});
    cumLmds=zeros(In,1);
    Lmd=Lmds{n};
    cumLmds(1)=Lmd(1);
    for in=2:In
        cumLmds(in)=cumLmds(in-1)+Lmd(in);
    end
    cumLmds=cumLmds./sum(Lmd);
    cums{n}=cumLmds;
end

%MPCA Iterations
if maxK>0
    tPs=cell(N,1);
    pUs=cell(N,1);
    %%%%%%%%%%%%%Determine Rn, the dimension of projected space%%%%
    for n=1:N
        cum=cums{n};
        idxs=find(cum>=Qs(n)/100);
        Ps(n)=idxs(1);
        tUn=tUs{n};
        tPn=tUn(1:Ps(n),:);
        tPs{n}=tPn;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for iK=1:maxK
        for n=1:N
            In=Is(n);
            Phi=double(zeros(In,In));
            for m=1:numSpl
                switch N
                    case 2
                        Xm=TX(:,:,m);
                    case 3
                        Xm=TX(:,:,:,m);
                    case 4
                        Xm=TX(:,:,:,:,m);
                    otherwise
                        error('Order N not supported. Please modify the code here or email hplu@ieee.org for help.')
                end
                tX=ttm(tensor(Xm),tPs,-n);
                tXn=tenmat(tX,n);
                Xn=tXn.data;
                Phi=Phi+Xn*Xn';
            end
            Pn=Ps(n);
            Phi=double(Phi);
            if Pn<In
                option=struct('disp',0);
                [pUs{n},pLmdn]=eigs(Phi,Pn,'lm',option);
                pLmds{n}=diag(pLmdn);
            else
                [pUn,pLmdn]=eig(Phi);
                pLmd=diag(pLmdn);
                [stLmd,stIdx]=sort(pLmd,'descend');
                pUs{n}=pUn(:,stIdx(1:Pn));
                pLmds{n}=pLmd(stIdx(1:Pn));
            end
            tPs{n}=pUs{n}';
        end
    end
    Us=pUs;
    tUs=tPs;
    Lmds=pLmds;
    Is=Ps;
else
    if testQ<100
        error('At least one iteration is needed');
    end
end

%Calculate the weight tensor Wgt
Wgt=zeros(Is);
switch N
    case 2
        for i1=1:Is(1)
            for i2=1:Is(2)
                Wgt(i1,i2)=sqrt(Lmds{1}(i1)*Lmds{2}(i2));
            end
        end
    case 3
        for i1=1:Is(1)
            for i2=1:Is(2)
                for i3=1:Is(3)
                    Wgt(i1,i2,i3)=sqrt(Lmds{1}(i1)*Lmds{2}(i2)*Lmds{3}(i3));
                end
            end
        end
    case 4
        for i1=1:Is(1)
            for i2=1:Is(2)
                for i3=1:Is(3)
                    for i4=1:Is(4)
                        Wgt(i1,i2,i3,i4)=sqrt(Lmds{1}(i1)*Lmds{2}(i2)*Lmds{3}(i3)*Lmds{4}(i4));
                    end
                end
            end
        end
    otherwise
        error('Order N not supported. Please modify the code here or email hplu@ieee.org for help.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Yps=ttm(tensor(TX),tUs,1:N);%MPCA projections of samples TX
vecDim=1;
for n=1:N, vecDim=vecDim*Is(n); end
vecYps=reshape(Yps.data,vecDim,numSpl); %vectorization of Yps
%%%%%%%%%%%%%%Now vecYps contains the feature vectors for training data

if max(gndTX)<0%%%%%%%%%%%%%%%%%%%%%%%%Sort by Variance%%%%%%%%%%%%%%%%%%%%
    TVars=diag(vecYps*vecYps');
    [stTVars,odrIdx]=sort(TVars,'descend');
else%%%%%%%%%%%%%%%Sort according to Fisher's discriminality%%%%%%%%%%%%%%%
    classLabel = unique(gndTX);
    nClass = length(classLabel);%Number of classes
    ClsIdxs=cell(nClass);
    Ns=zeros(nClass,1);
    for i=1:nClass
        ClsIdxs{i}=find(gndTX==classLabel(i));
        Ns(i)=length(ClsIdxs{i});
    end
    Ymean=mean(vecYps,2);
    TSW=zeros(vecDim,1);
    TSB=zeros(vecDim,1);
    for i=1:nClass
        clsYp=vecYps(:,ClsIdxs{i});
        clsMean=mean(clsYp,2);
        FtrDiff=clsYp-repmat(clsMean,1,Ns(i));
        TSW=TSW+sum(FtrDiff.*FtrDiff,2);
        meanDiff=clsMean-Ymean;
        TSB=TSB+Ns(i)*meanDiff.*meanDiff;
    end
    FisherRatio=TSB./TSW;
    [stRatio,odrIdx]=sort(FisherRatio,'descend');
end


function v = norm( F )
%NORM   Frobenius norm of a CHEBFUN2V.
%   V = NORM(F) returns the Frobenius norm of the two/three components, i.e.
%       V = sqrt(norm(F1).^2 + norm(F2).^2),
%   or
%       V = sqrt(norm(F1).^2 + norm(F2).^2 + norm(F3).^2) .

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

% Empty check:
if ( isempty( F ) )
    v = [];
    return
end

nF = F.nComponents;
v = 0;
for jj = 1:nF
    v = v + sum( svd( F.components{jj} ).^2 );
%     v = v + sum2( power( F.components{jj}, 2 ) );
end
v = sqrt(v);

end


From smalone@engin.umich.edu Wed Jul 15 13:37:52 1998
To: "Jeffrey A. Fessler" <fessler@eecs.umich.edu>
Subject: Re: pwls_sor

Prof. Fessler,

I'm sending along two of my pwls_sor routines.  Both compute one iteration
of the routine.  One uses precomputed diag[G'*W*G].  The other does not.
I could combine the two, if you want, using the nargin command to
determine which one to use.  But I'm assuming your mostly interested in
the routine that uses the precomputed vector.  Let me know if you need any
changes to the code.

Brendhan

----pwls_sor with precomputed diag[G'*W*G]---

function [x, r] = pwls_sor(x, r, G, W, R, y, beta, GTWG)

%    --	PWLS+SOR Iterative Routine --
%	Copyright Brendhan Givens, University of Michigan, July 9, 1998
%
%	Details of this algorithm are described in
%	"Penalized Weighted Least-Squares Image Reconstruction
%	for Positron Emission Tomography", Jeffrey A. Fessler,
%	IEEE Transactions on Medical Imaging, Vol. 13, No. 2,
%	June 1994.
%
%	[x, r] = pwls_sor(x, r, G, W, R, y, beta, GTWG)
%
%	pwls_sor.m computes one iteration of the overdetermined symmetric
%	positive definite linear system (G'*W*G + beta*R)x = G'*W*y using
%	a sequential minimization procedure with a non-negativity
%	constraint.  Input matrices are assumed to be in sparse format.
%	Input residual vector must be initialized as r = y - G*x.  The
%	diag[G'*W*G] is precomputed for faster iterations.
%
%	input	x	REAL approximation vector
%		r	REAL residual vector
%		G	REAL FULL RANK matrix (sparse)
%		W	REAL diagonal weight matrix (sparse)
%		R	REAL SYMMETRIC penalty matrix (sparse)
%		y	REAL projection data
%		beta	REAL scalar
%		GTWG	REAL SYMMETRIC POSITIVE INDEFINITE vector
%			where elements gtwg(j) = G(:,j)'*W*G(:,j)
%
%	output	x	REAL NON-NEGATIVE approximation vector
%		r	REAL residual vector

[m,n] = size(G);			% initialization
w = diag(W);

for j = 1:n				% begin iteration
   d = ((G(:,j).*w)'*r - beta*R(:,j)'*x)/(GTWG(j) + beta*R(j,j));
   x_old = x(j);
   x(j) = max(0, x(j) + d);		% update with non-negativity
constraint
   r = r + (x_old - x(j))*G(:,j);	% update residual
end

----pwls_sor without precompute----

function [x, r] = pwls_sor(x, r, G, W, R, y, beta)

%    --	PWLS+SOR Iterative Routine --
%	Brendhan Givens, University of Michigan, July 9, 1998
%
%	Details of this algorithm are described in
%	"Penalized Weighted Least-Squares Image Reconstruction
%	for Positron Emission Tomography", Jeffrey A. Fessler,
%	IEEE Transactions on Medical Imaging, Vol. 13, No. 2,
%	June 1994.
%
%	[x, r] = pwls_sor(x, r, G, W, R, y, beta)
%
%	pwls_sor.m computes one iteration of the overdetermined symmetric
%	positive definite linear system (G'*W*G + beta*R)x = G'*W*y using
%	a sequential minimization procedure with a non-negativity
%	constraint.  Input matrices are assumed to be in sparse format.
%	Input residual vector must be initialized as r = y - G*x.
%
%	input	x	REAL approximation vector
%		r	REAL residual vector
%		G	REAL FULL RANK matrix (sparse)
%		W	REAL diagonal weight matrix (sparse)
%		R	REAL SYMMETRIC penalty matrix (sparse)
%		y	REAL projection data
%		beta	REAL scalar
%
%	output	x	REAL NON-NEGATIVE approximation vector
%		r	REAL residual vector

[m,n] = size(G);			% initialization
w = diag(W);

for j = 1:n				% begin iteration
   p = (G(:,j).*w);
   d = (p'*r - beta*R(:,j)'*x)/(p'*G(:,j) + beta*R(j,j));
   x_old = x(j);
   x(j) = max(0, x(j) + d);		% update with non-negativity
constraint
   r = r + (x_old - x(j))*G(:,j);	% update residual
end

On Wed, 15 Jul 1998, Jeffrey A. Fessler wrote:

>
> please email me your pwls_sor.m routine - the one that is just
> the update without the loop wrapper liked we discussed.  there is
> a nuc. eng. student who wants to use it.
> in fact, i think i'll put a collection of such routines on my web page!
> (with your name on this one of course).
> jf
>


function [h,g,a,info] = wfilt_oddevenb(N)
%WFILT_ODDEVENB  Kingsbury's symmetric odd filters
%
%   Usage: [h,g,a] = wfilt_oddevenb(N);
%
%   `[h,g,a]=wfilt_oddevenb(N)` with $N \in {1}$ returns Kingsbury's
%   odd filters.
%
%   Examples:
%   ---------
%   :::
%     figure(1);
%     wfiltinfo('ana:oddevenb1');
%
%     figure(2);
%     wfiltinfo('syn:oddevenb1');
%
%   References: king02

% AUTHOR: Zdenek Prusa

info.istight = 0;

a = [2;2];

switch(N)
 case 1
    % Example 1. from the reference. Symmetric near-orthogonal
    garr = [
             0              0
             0             -7.062639508928571e-05
             0              0
            -0.0017578125   1.341901506696429e-03
             0             -1.883370535714286e-03
             0.022265625   -7.156808035714285e-03
            -0.046875       2.385602678571428e-02
            -0.0482421875   5.564313616071428e-02
             0.2968750     -5.168805803571428e-02
             0.55546875    -2.997576032366072e-01
             0.2968750      5.594308035714286e-01
            -0.0482421875  -2.997576032366072e-01
            -0.046875      -5.168805803571428e-02
             0.022265625    5.564313616071428e-02
             0              2.385602678571428e-02
            -0.0017578125  -7.156808035714285e-03
             0             -1.883370535714286e-03
             0              1.341901506696429e-03
             0              0
             0             -7.062639508928571e-05
    ];

    % This scaling is not in the reference paper, but it is here to be
    % consistent
    garr = garr*sqrt(2);
    %garr = setnorm(garr,'energy');

    offset = -10;

  otherwise
        error('%s: No such filters.',upper(mfilename));
end

    %garr = [garr(:,3:4),garr(:,1:2)];
    modrange = (-1).^((0:size(garr,1)-1) + offset+1).';
    modrange2 = (-1).^((0:size(garr,1)-1) + offset).';

    harr =       [garr(:,2).*modrange2,...
                  garr(:,1).*modrange,...
                  ];

% In the biorthogonal case, the filters do not get time reversed
garr = flipud(garr);

htmp=mat2cell(harr,size(harr,1),ones(1,size(harr,2)));
h = cellfun(@(hEl)struct('h',hEl,'offset',offset),htmp(1:2),...
                   'UniformOutput',0);

gtmp=mat2cell(garr,size(garr,1),ones(1,size(garr,2)));

g = cellfun(@(gEl)struct('h',gEl,'offset',offset),gtmp(1:2),...
                   'UniformOutput',0);


function quad = lattice ( dim_num, m, z, f )

%*****************************************************************************80
%
%% LATTICE applies a lattice integration rule.
%
%  Discussion:
%
%    Because this is a standard lattice rule, it is really only suited
%    for functions which are periodic, of period 1, in both X and Y.
%
%    For a suitable F, and a given value of M (the number of lattice points),
%    the performance of the routine is affected by the choice of the
%    generator vector Z.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994,
%    ISBN: 0198534728,
%    LC: QA311.S56
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer M, the order (number of points) of the rule.
%
%    Input, integer Z(DIM_NUM), the generator vector.  Typically, the elements
%    of Z satisfy 1 <= Z(1:DIM_NUM) < M, and are relatively prime to M.
%    This is easy to guarantee if M is itself a prime number.
%
%    Input, external real F, the name of the user-supplied routine
%    which evaluates the function, of the form:
%    function f ( dim_num, x )
%    integer dim_num
%    real f
%    real x(dim_num)
%    f = ...
%
%    Output, real QUAD, the estimated integral.
%
  quad = 0.0;

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

  quad = quad / m;

  return
end


function ct = hct(crit,y,z,tt)
%HCT Compute Harrel's C for survival model at several time points
%
%  Description
%    CT = HCT(CRIT,Y,Z,TT) Compute Harrel's C statistic estimate at
%    times TT using criteria vector CRIT (where larger value means
%    larger risk of incidence), observed time vector Y and event
%    indicator vector Z (0=event, 1=censored)
%
%  Reference
%    L. E. Chambless, C. P. Cummiskey, and G. Cui (2011). Several
%    methods to assess improvement in risk prediction models:
%    Extension to survival analysis. Statistics in Medicine
%    30(1):22-38.
%

% Copyright (C) 2012 Ernesto Ulloa, 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.

ip=inputParser;
ip.addRequired('crit',@(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('y',@(x) ~isempty(x) && isreal(x))
ip.addRequired('z', @(x) ~isempty(x) && isreal(x))
ip.addRequired('tt', @(x) ~isempty(x) && isreal(x))

ip.parse(crit,y,z,tt)

if size(y,2) ~= size(z,2)
  error('y and z dimensions must match')
end

for i=1:size(tt,2)
  comp=bsxfun(@and,bsxfun(@and,bsxfun(@lt,y(:,i),y(:,i)'),y(:,i)<=tt(i)),z(:,end)==0);
  conc=bsxfun(@gt,crit(:,i),crit(:,i)').*comp;
  ct(i,1)=sum(conc(:))./sum(comp(:));
end

end


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha)

%project_points.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)
%
%Projects a 3D structure onto the image plane.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion coefficients (radial and tangential) (4x1 vector)
%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2.
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
%
%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion
%
%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*(xx + alpha*yy) + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 7,
   alpha = 0;
   if nargin < 6,
      k = zeros(5,1);
      if nargin < 5,
         c = zeros(2,1);
         if nargin < 4,
            f = ones(2,1);
            if nargin < 3,
               T = zeros(3,1);
               if nargin < 2,
                  om = zeros(3,1);
                  if nargin < 1,
                     error('Need at least a 3D structure to project (in project_points.m)');
                     return;
                  end;
               end;
            end;
         end;
      end;
   end;
end;

[m,n] = size(X);

[Y,dYdom,dYdT] = rigid_motion(X,om,T);

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

dxdom = zeros(2*n,3);
dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

dxdT = zeros(2*n,3);
dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);

r4 = r2.^2;

dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;

r6 = r2.^3;

dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom;
dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;

dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom;
dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT;
dcdistdk = [ r2' r4' zeros(n,2) r6'];

xd1 = x .* (ones(2,1)*cdist);

dxd1dom = zeros(2*n,3);
dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
dxd1dom = dxd1dom + coeff.* dxdom;

dxd1dT = zeros(2*n,3);
dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
dxd1dT = dxd1dT + coeff.* dxdT;

dxd1dk = zeros(2*n,5);
dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

delta_x = [k(3)*a1 + k(4)*a2 ;
   k(3) * a3 + k(4)*a1];

%ddelta_xdx = zeros(2*n,2*n);
aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

ddelta_xdom = zeros(2*n,3);
ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);

ddelta_xdT = zeros(2*n,3);
ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);

ddelta_xdk = zeros(2*n,5);
ddelta_xdk(1:2:end,3) = a1';
ddelta_xdk(1:2:end,4) = a2';
ddelta_xdk(2:2:end,3) = a3';
ddelta_xdk(2:2:end,4) = a1';

xd2 = xd1 + delta_x;

dxd2dom = dxd1dom + ddelta_xdom ;
dxd2dT = dxd1dT + ddelta_xdT;
dxd2dk = dxd1dk + ddelta_xdk ;

% Add Skew:

xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];

% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha

dxd3dom = zeros(2*n,3);
dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
dxd3dT = zeros(2*n,3);
dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
dxd3dk = zeros(2*n,5);
dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
dxd3dalpha = zeros(2*n,1);
dxd3dalpha(1:2:2*n,:) = xd2(2,:)';

% Pixel coordinates:
if length(f)>1,
    xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
    coeff = reshape(f*ones(1,n),2*n,1);
    dxpdom = (coeff*ones(1,3)) .* dxd3dom;
    dxpdT = (coeff*ones(1,3)) .* dxd3dT;
    dxpdk = (coeff*ones(1,5)) .* dxd3dk;
    dxpdalpha = (coeff) .* dxd3dalpha;
    dxpdf = zeros(2*n,2);
    dxpdf(1:2:end,1) = xd3(1,:)';
    dxpdf(2:2:end,2) = xd3(2,:)';
else
    xp = f * xd3 + c*ones(1,n);
    dxpdom = f  * dxd3dom;
    dxpdT = f * dxd3dT;
    dxpdk = f  * dxd3dk;
    dxpdalpha = f .* dxd3dalpha;
    dxpdf = xd3(:);
end;

dxpdc = zeros(2*n,2);
dxpdc(1:2:end,1) = ones(n,1);
dxpdc(2:2:end,2) = ones(n,1);

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(5,1);
alpha = 0.01*randn(1,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);

% Test on om: OK

dom = 0.000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points2(X,om2,T,f,c,k,alpha);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: OK!!

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points2(X,om,T2,f,c,k,alpha);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points2(X,om,T,f2,c,k,alpha);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points2(X,om,T,f,c2,k,alpha);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.001 * norm(k)*randn(5,1);
k2 = k + dk;

[x2] = project_points2(X,om,T,f,c,k2,alpha);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on alpha: OK!!

dalpha = 0.001 * norm(k)*randn(1,1);
alpha2 = alpha + dalpha;

[x2] = project_points2(X,om,T,f,c,k,alpha2);

x_pred = x + reshape(dxdalpha * dalpha,2,n);

norm(x2-x)/norm(x2 - x_pred)


function figure_num = elman_contour ( figure_num )

%*****************************************************************************80
%
%% ELMAN_CONTOUR displays a contour plot of a 2D stochastic diffusivity function.
%
%  Discussion:
%
%    The diffusivity function is compute by DIFFUSIVITY_2D_ELMAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 July 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Howard Elman, Darran Furnaval,
%    Solving the stochastic steady-state diffusion problem using multigrid,
%    IMA Journal on Numerical Analysis,
%    Volume 27, Number 4, 2007, pages 675-688.
%
%  Parameters:
%
%    Input/output, integer FIGURE_NUM, the current number of figures.
%
  if ( nargin < 1 )
    figure_num = 0;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELMAN_CONTOUR\n' );
  fprintf ( 1, '  Display contour or surface plots of the stochastic\n' );
  fprintf ( 1, '  diffusivity function defined by DIFFUSIVITY_2D_ELMAN.\n' );
%
%  Initialize the normal random number generator.
%
  seed = -1;
  [ ~, ~ ] = r8vec_normal_01 ( -1, seed );
%
%  Specify the X and Y evaluation points.
%
  nx = 51;
  a = 1.0;
  xvec = linspace ( -a, +a, nx );
  yvec = linspace ( -a, +a, nx );

  [ xmat, ymat ] = meshgrid ( xvec, yvec );
%
%  Sample OMEGA.
%
  m_1d = 5;
  msq = m_1d * m_1d;
  seed = 123456789;
  if ( seed == 0 )
    omega = randn ( msq, 1 );
  else
    [ omega, seed ] = r8vec_normal_01 ( msq, seed );
  end
%
%  Compute the diffusivity field.
%
  cl = 0.1;
  dc0 = 10.0;
  dc = diffusivity_2d_elman ( a, cl, dc0, m_1d, omega, nx, nx, xmat, ymat );
%
%  Make a surface plot.
%
  figure_num = figure_num + 1;
  figure ( figure_num );
  surf ( ymat, xmat, dc, 'EdgeColor', 'interp' )
  xlabel ( 'Y' )
  ylabel ( 'X' )
  zlabel ( 'DC(X,Y)' )
  title ( 'ELMAN Stochastic diffusivity function' )

  filename = 'elman_contour.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot file stored as "%s".\n', filename );

  return
end


function m = r8mat_shortest_path ( n, m )

%*****************************************************************************80
%
%% R8MAT_SHORTEST_PATH computes the shortest distance between all pairs of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Floyd,
%    Algorithm 97, Shortest Path,
%    Communications of the ACM,
%    Volume 5, Number 6, June 1962, page 345.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input/output, real M(N,N).
%    On input, M(I,J) contains the length of the direct link between
%    nodes I and J, or Inf if there is no direct link.
%    On output, M(I,J) contains the distance between nodes I and J,
%    that is, the length of the shortest path between them.  If there
%    is no such path, then M(I,J) will remain Inf.
%
  for i = 1 : n
    for j = 1 : n
      if ( m(j,i) < Inf )
        for k = 1 : n
          if ( m(i,k) < Inf )
            s = m(j,i) + m(i,k);
            if ( s < m(j,k) )
              m(j,k) = s;
            end
          end
        end
      end
    end
  end

  return
end


function [ vX, paramLambda ] = SolveBp( mA, vB, paramEpsilon )
% ----------------------------------------------------------------------------------------------- %
%[ vX ] = SolveBpAdmm( mA, vB, paramLambda )
% Solve Basis Pursuit (Q1Eps) problem using ADMM.
% Input:
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramEpsilon      -   Parameter Epsilon.
%                           Sets the threshold for the Least squares error
%                           of the solution.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: [0, inf).
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           Sets the balance between L1 minimization and
%                           Least Squares minimization.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: [0, inf).
% References
%   1.  A
% Remarks:
%   1.  A
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     03/04/2018
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

SOLVER_METHOD_ADMM  = 1; %<! ADMM
SOLVER_METHOD_PPGM  = 2; %<! PRoximal Gradient Method
SOLVER_METHOD_CD    = 3; %<! Coordinate Descent

solverMethod = SOLVER_METHOD_CD;

switch(solverMethod)
    case(SOLVER_METHOD_ADMM)
        hOptFun = @(paramLambda) (0.5 * sum(((mA * SolveBpAdmm(mA, vB, paramLambda)) - vB) .^ 2)) - paramEpsilon;
    case(SOLVER_METHOD_PPGM)
        hOptFun = @(paramLambda) (0.5 * sum(((mA * SolveLsL1ProxAccel(mA, vB, paramLambda, 200)) - vB) .^ 2)) - paramEpsilon;
    case(SOLVER_METHOD_CD)
        hOptFun = @(paramLambda) (0.5 * sum(((mA * SolveLsL1Cd(mA, vB, paramLambda, 200)) - vB) .^ 2)) - paramEpsilon;
end

sSolverOptions = optimset('Display', 'off');

paramLambda = fzero(hOptFun, [0.00001, 1000], sSolverOptions);

vX = SolveBpAdmm(mA, vB, paramLambda);

end


function varths = Get_Varths_swing(zetas,nms,Cont_dt)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

zetas2     = zetas.^2; zetas3 = zetas.*zetas2; zetas4 = zetas.*zetas3;

varths = zeros(1,4);
gamms = zeros(1,3);

gamms(1) = -1/24 +zetas4(1)/8 -zetas3(1)/3 +zetas2(1)/4;
gamms(2) = 5/12 - zetas4(1)/4 +zetas3(1)/3 +zetas2(1)/2 -zetas(1);
gamms(3) = (1 + zetas4(1))/8 - zetas2(1)/4;
varths(1) = Cont_dt(nms(1)-1)*gamms(1) + Cont_dt(nms(1))*gamms(2) + Cont_dt(nms(1)+1)*gamms(3);

gamms(1)  = 1/12 - zetas4(1)/8 +(zetas3(1) - zetas2(1))/2;
gamms(2)  = 5/6  +zetas4(1)/4 - 2*zetas3(1)/3;
gamms(3)  = 1/12 -zetas4(1)/8 + zetas3(1)/6;
varths(2) = Cont_dt(nms(1))*gamms(1) + Cont_dt(nms(1)+1)*gamms(2) + Cont_dt(nms(1)+2)*gamms(3);

gamms(1)  = (1 - zetas4(2))/8 +zetas3(2)/3 - zetas2(2)/4;
gamms(2)  = 5/12 + zetas4(2)/4 - zetas3(2)/3 - zetas2(2)/2 + zetas(2);
gamms(3)  = 1/12 - (1 + zetas4(2))/8 + zetas2(2)/4;
varths(3) = Cont_dt(nms(2)-1)*gamms(1) + Cont_dt(nms(2))*gamms(2) + Cont_dt(nms(2)+1)*gamms(3);

gamms(1)  = zetas4(2)/8 - zetas3(2)/2 + zetas2(2)/2;
gamms(2)  = -zetas4(2)/4 + 2*zetas3(2)/3;
gamms(3)  = zetas4(2)/8 - zetas3(2)/6;
varths(4) = Cont_dt(nms(2))*gamms(1) + Cont_dt(nms(2)+1)*gamms(2) + Cont_dt(nms(2)+2)*gamms(3);

end


function legendre_polynomial_test10 ( p )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL_TEST10 tests PN_PAIR_PRODUCT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the maximum degree of the polynomial
%    factors.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_TEST10\n' );
  fprintf ( 1, '  Compute a pair product table for Pn(n,x):\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Tij = integral ( -1 <= x <= +1 ) Pn(i,x) Pn(j,x) dx\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Pn(n,x) polynomials are orthonormal,\n' );
  fprintf ( 1, '  so T should be the identity matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Maximum degree P = %d\n', p );

  table = pn_pair_product ( p );

  r8mat_print ( p + 1, p + 1, table, '  Pair product table:' );

  return
end


function [X,rho,eta] = prrgmres(A,L,N,b,k)
%PRRGMRES Preconditioned RRGMRES algorithm for square inconsistent systems
%
% [X,rho,eta] = rrgmres(A,L,N,b,k)
%
% PRRGMRES applies smoothing-norm preconditioning to the RRGMRES method,
% which is a variant of RRGMRES for square linear systems A x = b, with
% starting vector A*b (instead of b as in GMRES).  This function returns
% all k iterates, stored as columns of the matrix X.  The solution norm
% and residual norm are returned in eta and rho, resp.
%
% The preconditioner uses two matrices: the matrix L that defines the
% smoothing norm, and the matrix N whose columns span the null space
% of L.  It is assumed that L is p-times-n with p < n.
%
% For symmetric matrices, use the function pmr2 instead.

% Reference: P. C. Hansen and T. K. Jensen, "Smoothing-norm preconditioning
% for regularizing minimum-residual methods", SIAM J. Matrix Anal. Appl.
% 29 (2006), 1-14.

% Per Christian Hansen, IMM, September 21, 2007.

% Check input arguments.
if (k < 1), error('Number of steps k must be positive'), end
[m,n] = size(A);
p = size(L,1);
if (m ~= n), error('A must be square'), end
if nargin < 5, error('Too few input arguments'), end

% Allocate space.
X = zeros(n,k); % Matrix of solutions.
V = zeros(p,k); % Orthonormal vectors spanning Krylov subspace
h = zeros(k,1); % New column of Hessenberg matrix
Q = zeros(k+1); % H = Q*T, Q orthogonal
T = zeros(k);   %          T upper triangular
W = zeros(p,k); % W = V*inv(T)
if (nargout>1), rho = zeros(k,1); end
if (nargout>2), eta = zeros(k,1); end

% Initialization for working with pseudoinverses of L.
[Q0,R0] = qr(A*N,0);  % Compate QR factorization of A*N.
T0 = N'*Q0;
[QL,RL] = qr(L',0);   % Compact QR factgorization of L'.
TN = pinit(N,A);      % Prepare for A-weighted pseudoinverse computations.
bb = RL\( QL'*( b - Q0*(T0\(N'*b)) ) );

% Initialize variables.
v1     = A*( QL*(RL'\bb) );
v2     = Q0*( T0\(N'*v1) );
r      = RL\( QL'*(v1-v2) );
alpha  = norm(r);
V(:,1) = r/alpha;  % Initial vector of Krylov subspace is A*b.
Q(1,1) = 1;
x0     = N*( R0\(Q0'*b) );
xi     = zeros(p,1);
beta   = V(:,1)'*bb;

% Begin iterations.
for i=1:k

  v1 = A*( QL*(RL'\V(:,i)) );
  v2 = Q0*( T0\(N'*v1) );
  r  = RL\( QL'*(v1-v2) );

  % Modified Gram-Schmidt on the new vector.
  for j=1:k
    h(j) = V(:,j)'*r;
    r = r - V(:,j)*h(j);
  end
  alpha = norm(r);

  % Store new Arnoldi vector and update projected rhs.
  V(:,i+1) = r/alpha;
  beta = [beta;V(:,i+1)'*bb];

  % Apply previous rotations to h.
  T(1:i,i) = Q(1:i,1:i)'*h(1:i);

  % Compute Givens rotation parameters.
  rc = T(i,i);
  if alpha == 0
    c = 1; s = 0;
  elseif abs(alpha) > abs(rc)
    tau = -rc/alpha;
    s = 1 / sqrt(1 + abs(tau)^2);
    c = s*tau;
  else
    tau = -alpha/rc;
    c = 1 / sqrt(1 + abs(tau)^2);
    s = c*tau;
  end

  % Apply givens rotations.
  T(i,i) = c'*rc - s'*alpha;
  Q(1:i,[i,i+1]) = Q(1:i,i)*[c s];
  Q(i+1,[i,i+1]) = [-s c];

  if abs(T(i,i)) <= eps
    disp('Hession matrix is (numerically) singular')
  end

  % Update W = V*inv(T);
  W(:,i) = (V(:,i) - W(:,1:i-1)*T(1:i-1,i))/T(i,i);

  % Update solution.
  xi = xi + (Q(1:i+1,i)'*beta)*W(:,i);

  % Update output variables.
  X(:,i) = lsolve(L,xi,N,TN) + x0;
  if nargout>1, rho(i) = norm(A*X(:,i)-b); end
  if nargout>2, eta(i) = norm(xi); end

end


function op = smooth_logdet(q,C)
% SMOOTH_LOGDET   The -log( det( X ) ) function.
%   (Note the minus sign)
%   FUNC = SMOOTH_LOGDET( q ) returns a function handle that
%   provides a TFOCS-compatible implementation of the funciton
%       -q*log( det( X ) )
%
%   FUNC = SMOOTH_LOGDET( q, C ) represents
%       -q*log( det( X ) ) + < C, X >, where C is symmetric/Hermitian
%
%   X must be symmetric/Hermitian and positive definite,
%   and q must be a positive real number (if not provided,
%   the default value is q = 1).
%
%   N.B. it is the user's responsibility to ensure
%   that X is Hermitian and pos. def., since
%   automatically checking is expensive.
%
%   This function is differentiable, but the gradient
%   is not Lipschitz on the domain X > 0
%
%   This function does support proximity operations, and so
%   it may be used as a nonsmooth function.
%   However, the input must be symmetric positive definite
%   (and if C is used, then it must also be > tC )

% SRB: have not yet tested this.
% SRB: I think we CAN compute the proximity operator to logdet.
%       Will implement this in prox_logdet
if nargin < 1, q = 1; end
if nargin < 2, C = []; end
if ~isreal(q) || q <= 0
    error('First argument must be real and positive');
end

%op = @smooth_logdet_impl;
if isempty(C)
    op = @(varargin)smooth_logdet_impl( q, varargin{:} );
else
    op = @(varargin)smooth_logdet_impl_C( q, C, varargin{:} );
end

function [ v, g ] = smooth_logdet_impl( q, x, t )
if size(x,1) ~= size(x,2)
    error('smooth_logdet: input must be a square matrix');
end
switch nargin
    case 2
        % the function is being used in a "smooth" fashion
        %v = -log(det(x));
        v = -2*q*sum(log(diag(chol(x))));  % chol() takes half the time as det()
                                 % and it is easier to avoid overflow errors
                                 % since we sum the logs.
                                 % Also, chol() will warn if not pos. def.
        if nargout > 1
            g = -q*inv(x);
            % it would be nice to make g a function handle
            % that calculates g(y) = -x\y
        end

    case 3
        % the function is being used in a "nonsmooth" fashion
        % i.e. return g = argmin_g  -q*log(det(g)) + 1/(2t)||g-x||^2
        x = full(x+x')/2;  % March 2015, project it to be symmetric
        [V,D]   = safe_eig(x);
        d       = diag(D);
        % This is OK: input need not be pos def
        %if any(d<=0),
            %v   = Inf;
            %g   = nan(size(x));
            %return;
%%             error('log_det requires a positive definite point');
        %end
        l       = ( d + sqrt( d.^2 + 4*t*q ) )/2;
        g       = V*diag(l)*V';
        v       = -q*sum(log(l));
    otherwise
        error('Wrong number of arguments');
end

function [ v, g ] = smooth_logdet_impl_C( q, C, x, t )
if size(x,1) ~= size(x,2)
    error('smooth_logdet: input must be a square matrix');
end
if size(C,1) ~= size(C,2)
    error('smooth_logdet: input must be a square matrix');
end
switch nargin
    case 3
        % the function is being used in a "smooth" fashion
        %v = -log(det(x));
        v = -2*q*sum(log(diag(chol(x))));  % chol() takes half the time as det()
                                 % and it is easier to avoid overflow errors
                                 % since we sum the logs.
                                 % Also, chol() will warn if not pos. def.
        v = v + tfocs_dot( C, x );
        if nargout > 1
            g = -q*inv(x) + C;
            % it would be nice to make g a function handle
            % that calculates g(y) = -x\y
        end

    case 4
        % the function is being used in a "nonsmooth" fashion
        % i.e. return g = argmin_g  -q*log(det(g)) + 1/(2t)||g-x||^2
        x       = x - t*C;
        x = full(x+x')/2;  % March 2015, project it to be symmetric
        [V,D]   = safe_eig(x);
        d       = diag(D);
        % This is OK: input need not be pos def
        l       = ( d + sqrt( d.^2 + 4*t*q ) )/2;
        g       = V*diag(l)*V';
        v       = -q*sum(log(l));
        v       = v + tfocs_dot( C, g );
    otherwise
        error('Wrong number of arguments');
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [y, Sigma_y] = GMR(Priors, Mu, Sigma, x, in, out)
%
% Gaussian Mixture Regression.
% This source code is the implementation of the algorithms described in
% Section 2.4, p.38 of the book "Robot Programming by Demonstration: A
% Probabilistic Approach".
%
% Author:	Sylvain Calinon, 2009
%			http://programming-by-demonstration.org
%
% This function performs Gaussian Mixture Regression (GMR), using the
% parameters of a Gaussian Mixture Model (GMM). Given partial input data,
% the algorithm computes the expected distribution for the resulting
% dimensions. By providing temporal values as inputs, it thus outputs a
% smooth generalized version of the data encoded in GMM, and associated
% constraints expressed by covariance matrices.
%
% Inputs -----------------------------------------------------------------
%   o Priors:  1 x K array representing the prior probabilities of the K GMM
%              components.
%   o Mu:      D x K array representing the centers of the K GMM components.
%   o Sigma:   D x D x K array representing the covariance matrices of the
%              K GMM components.
%   o x:       P x N array representing N datapoints of P dimensions.
%   o in:      1 x P array representing the dimensions to consider as
%              inputs.
%   o out:     1 x Q array representing the dimensions to consider as
%              outputs (D=P+Q).
% Outputs ----------------------------------------------------------------
%   o y:       Q x N array representing the retrieved N datapoints of
%              Q dimensions, i.e. expected means.
%   o Sigma_y: Q x Q x N array representing the N expected covariance
%              matrices retrieved.
%
% This source code is given for free! However, I would be grateful if you refer
% to the book (or corresponding article) in any academic publication that uses
% this code or part of it. Here are the corresponding BibTex references:
%
% @book{Calinon09book,
%   author="S. Calinon",
%   title="Robot Programming by Demonstration: A Probabilistic Approach",
%   publisher="EPFL/CRC Press",
%   year="2009",
%   note="EPFL Press ISBN 978-2-940222-31-5, CRC Press ISBN 978-1-4398-0867-2"
% }
%
% @article{Calinon07,
%   title="On Learning, Representing and Generalizing a Task in a Humanoid Robot",
%   author="S. Calinon and F. Guenter and A. Billard",
%   journal="IEEE Transactions on Systems, Man and Cybernetics, Part B",
%   year="2007",
%   volume="37",
%   number="2",
%   pages="286--298",
% }

nbData = size(x,2);
nbVar = size(Mu,1);
nbStates = size(Sigma,3);

%% Fast matrix computation (see the commented code for a version involving
%% one-by-one computation, which is easier to understand).
%%
%% Compute the influence of each GMM component, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nbStates
  Pxi(:,i) = Priors(i).* gaussPDF(x, Mu(in,i), Sigma(in,in,i));
end
beta = Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates);
%% Compute expected means y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
  y_tmp(:,:,j) = repmat(Mu(out,j),1,nbData) + Sigma(out,in,j)*inv(Sigma(in,in,j)) * (x-repmat(Mu(in,j),1,nbData));
end
beta_tmp = reshape(beta,[1 size(beta)]);
y_tmp2 = repmat(beta_tmp,[length(out) 1 1]) .* y_tmp;
y = sum(y_tmp2,3);
%% Compute expected covariance matrices Sigma_y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
  Sigma_y_tmp(:,:,1,j) = Sigma(out,out,j) - (Sigma(out,in,j)*inv(Sigma(in,in,j))*Sigma(in,out,j));
end
beta_tmp = reshape(beta,[1 1 size(beta)]);
Sigma_y_tmp2 = repmat(beta_tmp.*beta_tmp, [length(out) length(out) 1 1]) .* repmat(Sigma_y_tmp,[1 1 nbData 1]);
Sigma_y = sum(Sigma_y_tmp2,4);

% %% Slow one-by-one computation (better suited to understand the algorithm)
% %%
% %% Compute the influence of each GMM component, given input x
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for i=1:nbStates
%   Pxi(:,i) = gaussPDF(x, Mu(in,i), Sigma(in,in,i));
% end
% beta = (Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates))';
% %% Compute expected output distribution, given input x
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% y = zeros(length(out), nbData);
% Sigma_y = zeros(length(out), length(out), nbData);
% for i=1:nbData
%   % Compute expected means y, given input x
%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   for j=1:nbStates
%     yj_tmp = Mu(out,j) + Sigma(out,in,j)*inv(Sigma(in,in,j)) * (x(:,i)-Mu(in,j));
%     y(:,i) = y(:,i) + beta(j,i).*yj_tmp;
%   end
%   % Compute expected covariance matrices Sigma_y, given input x
%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   for j=1:nbStates
%     Sigmaj_y_tmp = Sigma(out,out,j) - (Sigma(out,in,j)*inv(Sigma(in,in,j))*Sigma(in,out,j));
%     Sigma_y(:,:,i) = Sigma_y(:,:,i) + beta(j,i)^2.* Sigmaj_y_tmp;
%   end
% end


function [Y,Error] = Sammon(X,d)
% Sammon mapping on X
% Usage: [Y,Error] = sammon(X,D)

% References:
% Sammon, John W. Jr., "A Nolinear Mapping for Data Structure Analysis",
% IEEE Transactions on Computers, vol. C-18, no. 5, pp 401-409, May 1969.

    %% Default setting
    maxHalves = 20;
    maxIter   = 500;
    theta     = 1e-9;

    %% Obtain distance matrix
    Dist = pdist2(X,X);

    %% Remaining inialisation
    N     = size(X,1);
    scale = 0.5/sum(Dist(:));
    Dist  = Dist + eye(N);
    Dinv  = 1./Dist;

    %% Initialized by PCA, map to d dimension
    [UU,DD] = svd(X);
    Y       = UU(:,1:d)*DD(1:d,1:d);
    %% Randomly initialize Y
    % Y      = randn(N,d);

    % Obtain error
    oneMatrix = ones(N,d);
    dist      = pdist2(Y,Y) + eye(N);
    dinv      = 1./dist;
    delta     = Dist - dist;
    Error     = sum(sum((delta.^2).*Dinv));

    %% Optimize process
    for i = 1 : maxIter
        % Compute gradient, Hessian and search direction
        delta    = dinv - Dinv;
        deltaOne = delta*oneMatrix;
        g        = delta*Y - Y.*deltaOne;
        dinv3    = dinv.^3;
        Y2       = Y.^2;
        H        = dinv3*Y2 - deltaOne - 2*Y.*(dinv3*Y) + Y2.*(dinv3*oneMatrix);
        s        = -g(:)./abs(H(:));
        yOld     = Y;

        % Use step-halving procedure to ensure progress id made
        for j = 1 : maxHalves
            Y(:)   = yOld(:) + s;
            dist   = pdist2(Y,Y) + eye(N);
            dinv   = 1./dist;
            delta  = Dist - dist;
            newError = sum(sum((delta.^2).*Dinv));
            if newError < Error
                break;
            else
                s = 0.5*s;
            end
        end

        % Evaluate termination criterion
        if abs((Error-newError)/Error) < theta
            break;
        end

        % Update error
        Error = newError;
    end
    Error = Error * scale;
end


function [SDR,ISR,SIR,SAR]=bss_mcrit(s_true,e_spat,e_interf,e_artif)

% BSS_MCRIT Computation of some separation performance criteria given the
% decomposition of an estimated source image into four components
% representing respectively the true source image, spatial (or filtering)
% distortion, interference and artifacts.
%
% [SDR,ISR,SIR,SAR]=bss_mcrit(s_true,e_spat,e_interf,e_artif)
%
% Inputs:
% s_true: I x T matrix containing the true source image (one row per channel)
% e_spat: I x T matrix containing the spatial (or filtering) distortion component
% e_interf: I x T matrix containing the interference component
% e_artif: I x T matrix containing the artifacts component
%
% Outputs:
% SDR: Signal to Distortion Ratio
% ISR: source Image to Spatial distortion Ratio
% SIR: Source to Interference Ratio
% SAR: Sources to Artifacts Ratio

%%% Errors %%%
if nargin<4, error('Not enough input arguments.'); end
[It,Tt]=size(s_true);
[Is,Ts]=size(e_spat);
[Ii,Ti]=size(e_interf);
[Ia,Ta]=size(e_artif);
if ~((It==Is)&&(It==Ii)&&(It==Ia)), error('All the components must have the same number of channels.'); end
if ~((Tt==Ts)&&(Tt==Ti)&&(Tt==Ta)), error('All the components must have the same duration.'); end

%%% Energy ratios %%%
% SDR
SDR=10*log10(sum(sum(s_true.^2))/sum(sum((e_spat+e_interf+e_artif).^2)));
% ISR
ISR=10*log10(sum(sum(s_true.^2))/sum(sum(e_spat.^2)));
% SIR
SIR=10*log10(sum(sum((s_true+e_spat).^2))/sum(sum(e_interf.^2)));
% SAR
SAR=10*log10(sum(sum((s_true+e_spat+e_interf).^2))/sum(sum(e_artif.^2)));

return;


function [ ux, uy, uz ] = LKPR3D( image1, image2, r, numlevels, iterations, sigma )
%This function estimates deformation between two subsequent 3-D images using
%Lucas-Kanade optical flow equation with pyramidal approach.
%
%   Description :
%
%   -image1, image2 : two subsequent images or frames.
%   -r : redius of neighbourhood, default value is 2.
%   -numlevels : the number of levels in pyramid, default value is 2.
%   -iterations : number of iterations in refinement, default value is 1.
%   -sigma : standard deviation of Gaussian function, default value is 0.5.
%
%   Reference:
%   Lucas, B. D., Kanade, T., 1981. An iterative image registration
%   technique with an application to stereo vision. In: Proceedings of the
%   7th international joint conference on Artificial intelligence - Volume 2.
%   Morgan Kaufmann Publishers Inc., San Francisco, CA, USA, pp. 674-679.
%
%   Author: Mohammad Mustafa
%   By courtesy of The University of Nottingham and Mirada Medical Limited,
%   Oxford, UK
%
%   Published under a Creative Commons Attribution-Non-Commercial-Share Alike
%   3.0 Unported Licence http://creativecommons.org/licenses/by-nc-sa/3.0/
%
%   June 2012

% Default parameters
if nargin==5
    sigma=0.5;
elseif nargin==4
    iterations=1; sigma=0.5;
elseif nargin==3
    numlevels=2; iterations=1; sigma=0.5;
elseif nargin==2
    r=2; numlevels=2; iterations=1; sigma=0.5;
end

currentImage1=image1; currentImage2=image2;

% s contains sizes of each pyramid levels
s=zeros(1,3,numlevels);
s(:,:,1)=(size(image1));

% Each of pyramid levels will have resized image
pyrIm1=zeros(size(image1,1),size(image1,2),size(image1,3),numlevels);
pyrIm2=pyrIm1;
pyrIm1(:,:,:,1)=currentImage1;
pyrIm2(:,:,:,1)=currentImage2;

% Building pyramid by downsampling
for i=2:numlevels
    currentImage1 = sampling3D(currentImage1,1);
    currentImage2 = sampling3D(currentImage2,1);
    % Adjusting the size
    pyrIm1(1:size(currentImage1,1),1:size(currentImage1,2),...
            1:size(currentImage1,3),i)=currentImage1;
    pyrIm2(1:size(currentImage2,1),1:size(currentImage2,2),...
            1:size(currentImage2,3),i)=currentImage2;
    s(:,:,i)=size(currentImage1);
end

% Base operation
currentImage1=pyrIm1(1:s(1,1,numlevels),1:s(1,2,numlevels),...
                        1:s(1,3,numlevels),numlevels);
currentImage2=pyrIm2(1:s(1,1,numlevels),1:s(1,2,numlevels),...
                        1:s(1,3,numlevels),numlevels);

[ux,uy,uz]=LKW3D(currentImage1,currentImage2,r,sigma);

% Refining flow vectors
if iterations>0
    for i=1:iterations
    [ux,uy,uz]=refinedLK3D(ux,uy,uz,currentImage1,currentImage2,r,sigma);
    end
end

% Operations at higher levels of pyramids

for i=(numlevels-1):-1:1
    % Size and magnitudes of flow vectors are upsampled
    temp=2 * sampling3D(ux,2);
    uxInitial=temp(1:s(1,1,i),1:s(1,2,i),1:s(1,3,i));
    temp=2 * sampling3D(uy,2);
    uyInitial=temp(1:s(1,1,i),1:s(1,2,i),1:s(1,3,i));
    temp=2 * sampling3D(uz,2);
    uzInitial=temp(1:s(1,1,i),1:s(1,2,i),1:s(1,3,i));

    currentImage1=pyrIm1(1:s(1,1,i),1:s(1,2,i),1:s(1,3,i),i);
    currentImage2=pyrIm2(1:s(1,1,i),1:s(1,2,i),1:s(1,3,i),i);

    [ux,uy,uz]=refinedLK3D(uxInitial,uyInitial,uzInitial,...
                            currentImage1,currentImage2,r,sigma);
    if iterations>0
        for j=1:iterations
            [ux,uy,uz]=refinedLK3D(ux,uy,uz,currentImage1,currentImage2,2,sigma);
        end
    end
end

end


function [v, V_p, V_k, V_d] = pinHoleDepth(p,k,d)

% PINHOLEDEPTH Pin hole projection with distance measurement
%   [v, V_p, V_k, V_d] = PINHOLEDEPTH(l, k, d) projects the Eucliden point
%   l into a pinhole+depth camera. k is the intrinsic vector of the
%   subjacent pinhole model, and d the distortion vector.
%
%   See also PINHOLE, INVPINHOLEDEPTH.

%   Copyright 2015-     Joan Sola @ IRI-UPC-CSIC.

if nargout == 1
    [v(1:2,:),v(3,:)] = pinHole(p,k,d);
else
    [u,s,U_p,U_k,U_d] = pinHole(p,k,d);
    S_p = [0 0 1]; % s = p(3,1) --> ds/dp = [0 0 1];
    v = [u;s];
    V_p = [U_p ; S_p];
    V_k = [U_k ; zeros(1, length(k))];
    V_d = [U_d ; zeros(1, length(d))];
end

return

%%
syms x y z u0 v0 au av real
p = [x;y;z];
k = [u0;v0;au;av];
d = [];

[v, V_p, V_k, V_d] = pinHoleDepth(p,k,d);

simplify(V_p - jacobian(v,p))
simplify(V_k - jacobian(v,k))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE