% test for front propagation on 3D meshes
%
%   Copyright (c) 2007 Gabriel Peyre

path(path, 'toolbox/');
path(path, '../toolbox_graph_data/');
path(path, '../toolbox_graph_data/off/');

rep = ['results/geodesic-mesh/'];
if not(exist(rep))
    mkdir(rep);
end

disp('Loading mesh.');
if not(exist('name'))
    name = 'bunny';
    name = 'elephant-50kv';
    name = 'david50kf';
    name = 'david_head';
    name = 'hand';
end
options.name = name;
[vertex,faces] = read_mesh(name);
clf;
plot_mesh(vertex, faces,options);
saveas(gcf, [rep name '-mesh.png'], 'png');

nverts = max(size(vertex));
if not(exist('nstart'))
    nstart = 1;
end
start_points = [1 round(nverts/2)];
start_points = floor(rand(nstart,1)*nverts)+1;
start_points = start_points(:);
options.end_points = [];
% enforce a nice starting position for the first point
switch name
    case 'bunny'
        start_points(1) = 8900;
    case 'elephant-50kv'
        start_points(1) = 24575;
    case 'david50kf'
        start_points(1) = 20361;
    case 'david_head'
        start_points(1) = 18080;
    case 'hand'
        start_points(1) = 29719;
end

disp('Performing propagation.');
[D,S,Q] = perform_fast_marching_mesh(vertex, faces, start_points, options);

% compute geodesics
npaths = max(20,4*nstart);
% npaths = 1;
[tmp,I] = sort( D(:) ); I = I(end:-1:1); I = I(1:round(nverts*1));
end_points = floor( rand(npaths,1)*(length(I)-1) )+1;
end_points = I(end_points);
% [tmp,I] = sort( D(:) ); end_points(1) = I(end);

options.v2v = compute_vertex_ring(faces);
options.e2f = compute_edge_face_ring(faces);

disp('Extracting geodesics');
options.method = 'discrete';
options.method = 'continuous';
paths = compute_geodesic_mesh(D,vertex,faces, end_points, options);

options.colorfx = 'equalize';
plot_fast_marching_mesh(vertex,faces, D, paths, options);
saveas(gcf, [rep name '-geodesics-' num2str(nstart) '.png'], 'png');


samplen=1000;%Total number of initial states
InitialMat=zeros(4,samplen);%Store initialized x, y, theta, v into each row by sequence
for i=1:samplen
rr=0;
while rr<=1
xini1=(-10+rand(1)*(20));%-10~10
yini1=(-10+rand(1)*(20));%-10~10
rr = (xini1)^2+(yini1)^2;%Initialize location states which are inside safe region
end
thetaini1=(-10+rand(1)*(20));%-10~10
vini1=(-10+rand(1)*(20));%-10~10
InitialMat(1,i)=xini1;
InitialMat(2,i)=yini1;
InitialMat(3,i)=thetaini1;
InitialMat(4,i)=vini1;
end
save('InitialStateData','InitialMat');


function y = sinh(x)
%SINH         Implements  sinh(x)  for intervals
%
%   y = sinh(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 12/04/05     S.M. Rump  extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,sinh(full(sx)),m,n);
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if x.complex
    y = ( exp(x) - exp(-x) ) / 2;
    setround(rndold)
    return
  end

  y = x;

  xinf = x.inf(:);
  xsup = x.sup(:);

  IndexInfPos = ( xinf>=0 );
  len1 = sum(IndexInfPos);
  IndexSupNeg = ( xsup<=0 );
  len2 = sum(IndexSupNeg);

  Y = sinhpos( [ xinf(IndexInfPos) ; -xsup(IndexSupNeg) ] , -1 );
  y.inf(IndexInfPos) = Y(1:len1);
  y.sup(IndexSupNeg) = -Y( len1+1 : end );

  IndexInfNeg = ~IndexInfPos;
  len1 = sum(IndexInfNeg);
  IndexSupPos = ~IndexSupNeg;
  len2 = sum(IndexSupPos);

  Y = sinhpos( [ -xinf(IndexInfNeg) ; xsup(IndexSupPos) ] , 1 );
  y.inf(IndexInfNeg) = -Y(1:len1);
  y.sup(IndexSupPos) = Y( len1+1 : len1+len2 );

  setround(rndold)

function y = sinhpos(x,rnd)
% rigorous sinh(x) for nonnegative double vector x with
% rounding corresponding to rnd

  INTLAB_STDFCTS_SINH = getappdata(0,'INTLAB_STDFCTS_SINH');

  y = x;

  % small input
  index = ( x<8 );
  if any(index)
    setround(0)
    xx = x(index);
    [f,e] = log2(xx);
    bits = 14 + min(e,0);
    xs = pow2( floor(f.*2.^bits) , e-bits );  % max. 14 bits of mantissa,
                                              % no bit below 2^-14
    d = xx - xs;                              % 0 <= d < 2^-14*x < 2^-11
    sinhxs = sinh(xs);                        % round to nearest

    % bounds for exp(xs)
    INTLAB_STDFCTS_EXP = getappdata(0,'INTLAB_STDFCTS_EXP');
    setround(-1)
    expinf = exp(xs)*(1-INTLAB_STDFCTS_EXP.EPS);
    setround(1)
    expsup = exp(xs)*(1+INTLAB_STDFCTS_EXP.EPS);
    expinf(xs==0) = 1;                    % gives accuracy for small x
    expsup(xs==0) = 1;

    % use sinh(xx) = sinh(xs+d) = sinh(xs)*cosh(d) + cosh(xs)*sinh(d)
    setround(rnd)
    corr = INTLAB_STDFCTS_SINH.EPS;
    if rnd==-1
      % coshxs <= cosh(xs)
      coshxs = 0.5*( expinf + 1./expsup );  % rounded downwards
      dd = d.*d;
      % 0 <= err(sinhd) <= sinh(d)*d^4/4! < 6e-8*d^3/6
      % 0 <= err(coshd) <= sinh(d)*d^5/5! < 4.8e-8*d^4/24
      y(index) = sinhxs + ...
          ( ( sinhxs .* ( -corr + (1-corr)*dd/2.*( 1 + dd/12 ) ) + ...
              coshxs.*d.*dd/6 ) + ...
            coshxs.*d ...
          );
    else
      % coshxs >= cosh(xs)
      coshxs = 0.5*( expsup + 1./expinf );  % correctly rounded according to rnd
      dd = d.*d;
      y(index) = sinhxs + ...
          ( ( sinhxs .* ( corr + (1+corr)*dd/2.*( 1 + dd/12.*( 1+4.8e-8 ) ) ) + ...
              coshxs.*d.*dd/6.*( 1+6e-8 ) ) + ...
            coshxs.*d ...
          );
    end
  end

  % medium input
  index = ( ~index ) & ( x<709 );         % 8 <= x < 709
  if any(index)
    xx = x(index);
    exprnd = exp_rnd(xx,rnd);
    setround(rnd)
    y(index) = 0.5 * ( exprnd + (-1)./exprnd );
  end

  % large input
  index = ( x>=709 );
  if any(index)
    INTLAB_STDFCTS_E = getappdata(0,'INTLAB_STDFCTS_E');
    exprnd = exp_rnd( x(index)-1 , rnd )/2;
    setround(rnd)
    if rnd==-1
      y(index) = exprnd .* INTLAB_STDFCTS_E.INF;
    else
      y(index) = exprnd .* INTLAB_STDFCTS_E.SUP;
    end
  end


function [M_mean,n]=meanfilt3(M,IND,k,v)

%Creating spherical mask environment with radius k
[kI,kJ,kK]=cart2im(k,k,k,v);
kI=round(kI); kJ=round(kJ); kK=round(kK);
[MASK_J,MASK_I,MASK_K]=meshgrid(-kJ:kJ,-kI:kI,-kK:kK);
% [MASK_X,MASK_Y,MASK_Z]=im2cart(MASK_I,MASK_J,MASK_K,v);
MASK_X=MASK_J.*v(2); MASK_Y=MASK_I.*v(1); MASK_Z=MASK_K.*v(3);

R=sqrt(MASK_X.^2 + MASK_Y.^2 + MASK_Z.^2);

Lv=R<=k;
MASK_I=MASK_I(Lv); MASK_J=MASK_J(Lv); MASK_K=MASK_K(Lv);

%Getting mask indices
[IND_mask]=maskfind(M,IND(:),MASK_I(:),MASK_J(:),MASK_K(:));
L_valid=IND_mask>0;
INT_valid=M(IND_mask(L_valid));
INT_mask=nan(size(IND_mask));
INT_mask(L_valid)=INT_valid;

%Calculating median, ignoring NaN's
M_mean=gnanmean(INT_mask,2);

%Calculating number of elements used in median calculation
n=sum(~isnan(INT_mask),2);

%         %Creating spherical mask environment
%         k=k+iseven(k);
%         k_offset=round(k/2)-1;
%         [MASK_J,MASK_I,MASK_K]=meshgrid(-k_offset:k_offset);
%         R=sqrt(MASK_J.^2 + MASK_I.^2 + MASK_K.^2);
%         Lv=R<=k_offset;
%         MASK_I=MASK_I(Lv); MASK_J=MASK_J(Lv); MASK_K=MASK_K(Lv);
%
%         %Getting mask indices
%         [IND_mask]=maskfind(M,IND(:),MASK_I(:),MASK_J(:),MASK_K(:));
%         L_valid=IND_mask>0;
%         INT_valid=M(IND_mask(L_valid));
%         INT_mask=nan(size(IND_mask));
%         INT_mask(L_valid)=INT_valid;
%
%         %Calculating median, ignoring NaN's
%         M_median=nanmedian(INT_mask,2);
%
%         %Calculating number of elements used in median calculation
%         n=sum(~isnan(INT_mask),2);
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [Q,c,f,x,info] = quaddecomp(p,z)
%QUADDECOMP Internal function to decompose quadratic expression

[n,m]=size(p);
info = 0;

% Is it a scalar
if (n*m==1)
    % Involved in polynomial expression
    [mt,variabletype] = yalmip('monomtable');
    x_lin = getvariables(p);
    x_var = find(any(mt(x_lin,:),1));
    if nargin==2
        x_var = union(x_var,depends(z));
    end
    x = recover(x_var);
    if all(variabletype(x_lin) ==0)% is(p,'linear')
        n = length(x);
        Q = spalloc(n,n,0);
        fc = getbase(p);
        f = fc(1);
        if nargin==2
            vars = getvariables(p);
            c = zeros(length(x),1);
            for i = 1:length(vars)
                c(find(vars(i)==x_var)) = fc(1+i);
            end
        else
            c = fc(2:end);c=c(:);
        end
        return
    end
    variabletype = variabletype(x_lin);
    if all(variabletype<=2)

        base = getbase(p);
        if nnz(base(1))==0
            f = 0;
            base = base(2:end);
        else
            f = base(1);
            base = base(2:end);
        end
        mt = mt(x_lin,x_var);
        quads   = find (variabletype == 2);
        bilins  = find (variabletype == 1);
        linears  = find (variabletype == 0);
        [varsC,aux1,aux2] = find(mt(linears,:)');
        [varsQ,aux1,aux2] = find(mt(quads,:)');
        [varsB,aux1,aux2] = find(mt(bilins,:)');
        if isempty(varsQ)
            varsQ = [];
        end
        if isempty(varsB)
            varsB = [];
        end
        if isempty(varsC)
            varsC = [];
        end
        c = sparse(varsC,1,base(linears),length(x),1);
        ii = [varsQ ; varsB(1:2:end) ; varsB(2:2:end)];
        jj = [varsQ ; varsB(2:2:end) ; varsB(1:2:end)];
        kk = [base(quads)  base(bilins)/2  base(bilins)/2];
        Q = sparse(ii,jj,kk,length(x),length(x));
    else
        if nargout==5
            info = 1;
            Q = [];
            c = [];
            f = [];
            x = [];
        else
            error('Function is not quadratic');
        end
    end

else
    if nargout==5
        info = 1;
        Q = [];
        c = [];
        f = [];
        x = [];
    else
        error('Function is not scalar');
    end
end


function [h]=arrows(x,y,w,fac,color)
% function [h]=arrows(x,y,w,fac,color)
%    Draws arrows with their tails at each point corresponding
%	to identical indices in the matrices x,y.  The matrices u
%	and v are the components of the vector to be represented.
%	fac is the scaling factor
%
%  Geometry of arrowheads (choosing HEADA and HEADL):
%    If the arrow is defined by the points A B C B D where A is the base of
%  the arrow, B is the head, and C and D are the corners of the arrowhead, then
%  HEADA is the angle BAC (or BAD), and HEADL is the ratio of distances AC/AB.
%

% revision 3/20/97 to use nans for line breaks
% much more efficient and returns only a single handle

HEADA=10*pi/180; HEADL=.75;
z=x(:)+i*y(:);

if nargin < 5,color='red';end
if nargin < 4,help arrows,end
w=w(:)*fac;
r=w*HEADL; wr1=r*exp(+i*HEADA); wr2=r*exp(-i*HEADA);
wplot=ones(length(z),6);
wplot(:,1)=z;
wplot(:,[2,4])=(z+w)*ones(1,2);
wplot(:,3)=z+wr1; wplot(:,5)=z+wr2;
wplot(:,6)=z*nan;
wplot=wplot.';
wplot=wplot(:);
%z=eps*ones(size(wplot));
%h=line(real(wplot),imag(wplot),z,'color',color);
h=line(real(wplot),imag(wplot),'color',color);
set(h(1),'userdata',fac);


function [mg] = kg2mg(kg)
% Convert mass from kilograms to milligrams.
% Chad Greene 2012
mg = kg*1000000;


function month_cal_gregorian ( y, m )

%*****************************************************************************80
%
%% MONTH_CAL_GREGORIAN prints a Gregorian month calendar.
%
%  Format:
%
%    GREGORIAN CALENDAR
%        APRIL 1997 AD
%
%    Su  M Tu  W Th  F Sa
%           1  2  3  4  5
%     6  7  8  9 10 11 12
%    13 14 15 16 17 18 19
%    20 21 22 23 24 25 26
%    27 28 29 30
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer  Y, M, the YM date.
%

%
%  Make local copies of the input.
%
  m2 = m;
  y2 = y;
%
%  Check the month and year.  After this call, month is
%  guaranteed to be between 1 and 12.
%
  [ y2, m2, ierror ] = ym_check_gregorian ( y2, m2 );

  if ( ierror ~= 0 )
    return
  end
%
%  Find the day of the week for Y M 1.
%
  d = 1;
  f = 0.0;

  w = ymdf_to_weekday_gregorian ( y2, m2, d, f );
%
%  Find the appropriate label for the first box in the calendar.
%
  iday = 2 - w;
%
%  Print out a heading.
%
  s1 = month_to_month_name_common ( m2 );
  s2 = y_to_s_gregorian ( y2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Gregorian Calendar\n' );
  fprintf ( 1, '%s %s\n', s1, s2 );
  fprintf ( 1, '\n' );
%
%  Get the days of the week.
%
  fprintf ( 1, ' ' );
  for w = 1 : 7
    lab = weekday_to_name_common2 ( w );
    fprintf ( 1, '%3s', lab );
  end
  fprintf ( 1, '\n' );
%
%  Print out a line of day numbers.
%
  while ( iday <= month_length_gregorian ( y2, m2 ) )

    fprintf ( 1, ' ' );

    for w = 1 : 7

      if ( iday < 1 )
        fprintf ( 1, '   ' );
      elseif ( month_length_gregorian ( y2, m2 ) < iday )
        fprintf ( 1, '   ' );
      else
        fprintf ( 1, ' %2d', iday );
      end

      iday = iday + 1;

    end

    fprintf ( 1, '\n' );

  end

  return
end


%FIGURE_sliding_distance

%%%% EXPERIMENT %%%%
%
% For a slendar rod toppling from rest, plot how the slip distance varies
% as a function of coefficient of friction.
%
%

clc; clear;

setup.Tspan = [0,10];  %only used for timeout of the simulation
setup.tol = 1e-12;   %Accuracy of the intergation method
setup.dataFreq = 750;   %How much data to return?
setup.solver = @ode45;   %Tell the simulation to use a still solver.

%%%% HACK %%%%
% If the perturbation is too small, then numerical errors start to creep
% into the results. If perturbation is larger than the critical angle of
% the smallest non-zero coefficient of friction, then it will cause a
% direct error by violating the initial phase assumption.
setup.perturbation = 1e-2;
%%%% DONE %%%%

MoI = logspace(-3,0,250);
Mu = [0,0.05,0.1,0.5,1,5,10,inf];
P.m = 1;
P.g = 1;
P.L = 1;

%%%% Run big experiment %%%%
Data = runSlipExperiment(setup,MoI,Mu);

%%%% Plotting %%%%
N_mu = length(Data);
xBnd = [min(Data(1).moi), max(Data(1).moi)];
Mu = zeros(N_mu,1);
style = {'k--','b--','r--','m--','g--',...
    'k-','b-','r-','m-','g-'...
    'k:','b:','r:','m:','g:'};
LINEWIDTH = 3;
FontSize.Title = 16;
FontSize.label = 12;
names = cell(1,N_mu);
for i=1:N_mu
    Mu(i) = Data(i).mu;
    names{i} = num2str(['u = ' num2str(Mu(i))]);
end

%%%% Distance %%%%
H_dist = figure(100); clf; hold on;
set(H_dist,'Name','SlipDist','NumberTitle','off')
IDX = false(2,length(Mu));
for i=1:length(Data)
    sty = style{mod(i-1,length(style))+1};
    subplot(2,1,1); hold on;
    idx = Data(i).pos~=0;  IDX(1,i) = sum(idx)~=0;
    semilogx(Data(i).moi(idx),Data(i).pos(idx),sty,'LineWidth',LINEWIDTH);
    title('Backwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Slip Distance','FontSize',FontSize.label)
    set(gca,'Xscale','log')
    subplot(2,1,2); hold on;
    idx = Data(i).neg~=0;  IDX(2,i) = sum(idx)~=0;
    semilogx(Data(i).moi(idx),Data(i).neg(idx),sty,'LineWidth',LINEWIDTH);
    title('Forwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Slip Distance','FontSize',FontSize.label)
    set(gca,'Xscale','log')
end
%subplot(2,1,1); legend(names(IDX(1,:)),'Location','NorthEast');
% extents = [xBnd,-1,0];  axis(extents);
subplot(2,1,2); legend(names(IDX(2,:)),'Location','SouthWest');
% extents = [xBnd,0,0.14];

%%%% Critical Angle %%%%
H_angle = figure(101); clf; hold on;
set(H_angle,'Name','CriticalAngle','NumberTitle','off')
IDX = false(2,length(Mu));
for i=1:length(Data)
    sty = style{mod(i-1,length(style))+1};

    idx1 = Data(i).pos~=0;  IDX(1,i) = sum(idx1)~=0;
    subplot(2,1,1); hold on
    semilogx(Data(i).moi(idx1),-Data(i).angle(idx1)*180/pi,sty,'LineWidth',LINEWIDTH);
    title('Backwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    set(gca,'Xscale','log')

    idx2 = Data(i).neg~=0; IDX(2,i) = sum(idx2)~=0;
    idx2a = idx2 & idx1;   %In these special cases, it slips backwards and then forwards. Only plot the FIRST critical angle.
    idx2b = idx2 & ~idx1;
    subplot(2,1,2); hold on;
%     semilogx(Data(i).moi(idx2a),-Data(i).angle(idx2a)*180/pi,sty,'LineWidth',LINEWIDTH);
    semilogx(Data(i).moi(idx2b),-Data(i).angle(idx2b)*180/pi,sty,'LineWidth',LINEWIDTH);
    title('Forwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    set(gca,'Xscale','log')
end

subplot(2,1,1); legend(names(IDX(1,:)),'Location','NorthEast');
% extents = [xBnd,0,45];  axis(extents);
subplot(2,1,2); legend(names(IDX(2,:)),'Location','NorthWest');
% extents = [xBnd,30,60];

%%%% SAVE %%%%
save('DATA_Slip_vs_Mu.mat','Data');
save2pdf('../WriteUp/Figures/Slip_vs_Mu__Distance.pdf',H_dist,600);
save2pdf('../WriteUp/Figures/Slip_vs_Mu__Angle.pdf',H_angle,600);


% Fig. 9.8  Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
 n=[1 1];
 d = [1 0 0];
 rlocus(n,d)
 axis([-6 2 -3 3])
 hold on
 r=roots([1 1 1]);
 plot(r,'*')
  z=0:.1:.9;
 wn= 1:6;
 sgrid(z, wn)
 hold off


function [X, info] = IRhybrid_flsqr(A, b, varargin)
%IRhybrid_flsqr Hybrid version of FLSQR algorithm [...]
%
% options  = IRhybrid_flsqr('defaults')
% [X,info] = IRhybrid_flsqr(A,b)
% [X,info] = IRhybrid_flsqr(A,b,K)
% [X,info] = IRhybrid_flsqr(A,b,options)
% [X,info] = IRhybrid_flsqr(A,b,K,options)
%
% IRhybrid_flsqr is a hybrid iterative regularization method used for
% solving large-scale, ill-posed inverse problems of the form:
%               b = A*x + noise .
% The method combines FLSQR iteration (iterative regularization method)
% with a Tikhonov regularization method to stabilize the semiconvergence
% behavior that is characteristic of many iterative solvers applied to
% ill-posed problems.
%
% With 'defaults' as input returns the default options.  Otherwise outputs
% the iterates specified in K, using max(K) as MaxIter, and using all other
% default options.  With options as input: uses the user-specified options
% and all the other default options.
%
%
% Inputs:
%  A : either (a) a full or sparse matrix
%             (b) a matrix object that performs the matrix*vector operation
%             (c) user-defined function handle
%  b : right-hand side vector
%  K : (optional) integer vector that specifies which iterates are returned
%      in X; the maximum number of iterations is assumed to be max(K)
%      [ positive integer | vector of positive components ]
%  options : structure with the following fields (optional)
%      x0         - initial guess for the iterations; default = zero vector
%                   [ array | {'none'} ]
%      x_true     - true solution; allows us to returns error norms with
%                   respect to x_true at each iteration
%                   [ array | {'none'} ]
%      RegParam   - a value or a method to find the regularization
%                   parameter for the projected problems:
%                   [  non-neg. scalar | {'wgcv'} | 'gcv' | 'modgcv' |...
%                     'discrep' | 'discrepit' | 'optimal']
%                   This also determines which stopping rule is used
%                   If 'gcv', 'wgcv' or 'modgcv' is chosen, the iteration is
%                     stopped when the GCV function minimum stabilizes or
%                     increases within a certain window of iterations (see
%                     'stopGCV', 'FlatTol' and 'MinTol').
%                   If 'discrep' is chosen, and NoiseLevel is provided,
%                     then the discrepancy principle is used as stopping
%                     (see 'NoiseLevel' and 'eta').
%                   If 'discrepit' is chosen, and NoiseLevel is provided,
%                     then the discrepancy principle is used to set the
%                     parameter at each iteration, and the stabilization of
%                     successive parameters is used as stopping criterion
%                     (see 'NoiseLevel', 'eta' and 'regPflatTol').
%                   If 'optimal' is chosen, and x_true is provided,
%                     no stopping criterion is considered
%      stopGCV    - stopping criterion for the iterations when GCV is used
%                   [ 'GCVvalues' | {'resflat'} ]
%      resflatTol - tolerace for the stabilization of the residual
%                   (to be used if stopGCV is 'resflat')
%                   [ {0.05} | non-negative scalar ]
%      regPflatTol- tolerance for the stabilization of
%                   successive regularization parameters
%                   (to be used as stopping criterion)
%                   [ {0.9} | non-negative scalar ]
%      GCVflatTol - tolerance for detecting flatness (stabilization)
%                   in the GCV function as a stopping criterion
%                   [ {10^-6} | non-negative scalar ]
%      GCVminTol  - window of iterations - if the GCV minimum continues
%                   to increase over this window, then the iterations are
%                   stopped:
%                   [ {3} | positive integer ]
%      GCVweight  - weight to be used if RegParam is 'wgcv'
%                   [ 'adapt' | non-negative scalar ]
%      NoiseLevel - norm of noise in rhs divided by norm of rhs (must be
%                   assigned if RegParam is 'discrep')
%                   [ {'none'} | nonnegative scalar ]
%      eta        - safety factor for the discrepancy principle
%                   [ {1.01} | scalar greater than (and close to) 1 ]
%      RegParam0  - regularization parameter used in the first  projected
%                   problem (needed if RegParam is 'discrep')
%                   [ {1} | positive scalar ]
%      MaxIter    - maximum number of iterations
%                   [ {'none'} | positive integer ]
%      DecompOut  - returns the Golub-Kahan decomposition to the user
%                   [ 'on' | {'off'} ]
%      IterBar    - shows the progress of the iterations
%                   [ {'on'} | 'off' ]
%      NoStop     - specifies whether the iterations should proceed
%                   after a stopping criterion is satisfied
%                   [ 'on' | {'off'} ]
%   SparsityTrans - sparsity transform for the solution
%                   [ {'none'} | 'dwt' ]
%      wname      - discrete wavelet transform name (meaningful if
%                   SpartistyTrans is 'dwt')
%                   [ {'db1'} ]
%      wlevels    - discrete wavelet transform level (meaningful if
%                   SpartistyTrans is 'dwt')
%                   [ {2} | positive integer]
%   hybridvariant - kind of hybrid method to be implemented
%                   [ {'I'} | 'R' ]
%            tolX - tolerance for the weights (the modulus of the weights
%                   cannot be below this threshold
%                   [ {10^-10} | non-negative scalar ]
% Note: the options structure can be created using the function IRset.
%
% Outputs:
%   X : computed solutions, stored column-wise (at the iterations listed in K)
%   info: structure with the following fields:
%      its      - number of the last computed iteration
%      saved_iterations - iteration numbers of iterates stored in X
%      StopFlag - string that describes the output/stopping condition:
%                   * Flat GCV curve
%                   * Minimum of GCV function (within window of MinTol its)
%                   * Performed max number of iterations
%                   * Discrepancy principle satisfied
%                   * Breakdown of the Golub-Kahan bidiagonalization algorithm
%      StopReg  - structure with the following fields:
%                   * X: solution satisfying the stopping criterion
%                   * It: iteration satisfying the stopping criterion
%                   * RegP: regularization parameter at the iteration satisfying
%                     the stopping crierion
%                   * Xnrm: norm of the solution satisfying satisfying the
%                     stopping criterion
%                   * Rnrm: relative residual norm at the iteration
%                     satisfying the stopping criterion
%                   * Enrm: relative error norm at the iteration
%                     satisfying the stopping criterion (requires x_true)
%      Xnrm     - solution norms at each iteration
%      Rnrm     - relative residual norms at each iteration
%      Enrm     - relative error norms at each iteration (requires x_true)
%      RegP     - sequence of the regularization parameters
%      GCValues - GCV function evaluated at minimum point at each
%                 iteration (if RegParam is 'gcv', 'wgcv', 'modgcv')
%      V        - Golub-Kahan bidiagonalization basis vectors for the solution
%      U        - Golub-Kahan bidiagonalization basis vectors
%      B        - lower bidiagonal matrix computed by Golub-Kahan bidiagonalization
%
% See also: IRcgls, IRhybrid_fgmres, IRhybrid_gmres, IRhybrid_flsqr, IRget, IRset

% Julianne Chung, Virginia Tech
% Silvia Gazzola, University of Bath
% June, 2018.

% Initialization
defaultopt = struct('x0', 'none', 'MaxIter', 100 ,...
    'x_true', 'none', 'NoStop','off', 'IterBar', 'on',...
    'RegParam','wgcv',...
    'SparsityTrans', 'none', 'wname', 'db1', 'wlevels', 2,...
    'qnorm', 1, 'weight0', 'none',...
    'hybridvariant', 'I', 'tolX', 10^-10,...
    'GCVweight', 'adapt',...
    'GCVflatTol', 10^-6, 'GCVminTol', 3,...
    'stopGCV', 'GCVvalues', 'resflatTol', 0.05, 'regPflatTol', 0.9,...
    'NoiseLevel', 'none', 'eta', 1.01, 'RegParam0', 1, 'DecompOut', 'off');

if nargin == 0
    error('Not enough input arguments')
elseif nargin == 1
    % If input is 'defaults,' return the default options in X
    if nargout <= 1 && isequal(A,'defaults')
        X = defaultopt;
        return;
    else
        error('Not enough input arguments')
    end
end

defaultopt.restart = 'off';
defaultopt.verbosity = 'on';

% Check for acceptable number of optional input arguments
switch length(varargin)
    case 0
        K = []; options = [];
    case 1
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = [];
        else
            % no matter the order of appearance
            K = []; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = varargin{2};
        else
            % again, no matter the order of appearance
            K = varargin{2}; options = varargin{1};
        end
        if isfield(options, 'MaxIter') && ~isempty(options.MaxIter) && (~isempty(K) && options.MaxIter ~= max(K))
            warning('The value of MaxIter is discarded; the maximum value in K is taken as MaxIter')
        end
    otherwise
        error('Too many input parameters')
end

if isempty(options)
    options = defaultopt;
end

options = IRset(defaultopt, options);

MaxIter    = IRget(options, 'MaxIter',    [], 'fast');
RegParam   = IRget(options, 'RegParam',   [], 'fast');
x_true     = IRget(options, 'x_true',     [], 'fast');
NoStop     = IRget(options, 'NoStop',     [], 'fast');
IterBar    = IRget(options, 'IterBar',    [], 'fast');
omega      = IRget(options, 'GCVweight',  [], 'fast');
stopGCV    = IRget(options, 'stopGCV',    [], 'fast');
resdegflat = IRget(options, 'resflatTol', [], 'fast');
degflat    = IRget(options, 'GCVflatTol', [], 'fast');
regPflat   = IRget(options, 'regPflatTol', [], 'fast');
mintol     = IRget(options, 'GCVminTol',  [], 'fast');
NoiseLevel = IRget(options, 'NoiseLevel', [], 'fast');
eta        = IRget(options, 'eta',        [], 'fast');
RegParamk  = IRget(options, 'RegParam0',  [], 'fast');
restart    = IRget(options, 'restart',    [], 'fast');
verbose    = IRget(options, 'verbosity',  [], 'fast');
DecompOut  = IRget(options, 'DecompOut', [], 'fast');
tolX       = IRget(options, 'tolX',      [], 'fast');
q          = IRget(options, 'qnorm', [], 'fast');
SparsityTrans = IRget(options, 'SparsityTrans', [], 'fast');
hybridvariant = IRget(options, 'hybridvariant', [], 'fast');

verbose = strcmp(verbose, 'on');

adaptWGCV = strcmp(RegParam, {'wgcv'}) && strcmp(omega, {'adapt'});

% setting K
if isempty(K)
    K = MaxIter;
end
% sorting the iterations (in case they are shuffled in input)
K = K(:); K = sort(K,'ascend'); K = unique(K);
if ~((isreal(K) && (all(K > 0)) && all(K == floor(K))))
    error('K must be a vector of positive real integers')
end
if K(end) ~= MaxIter
    MaxIter = K(end);
end
% note that there is no control on K, as it does not go through IRset

if (strcmp(RegParam,'discrep') || strcmp(RegParam,'discrepit')) && ischar(NoiseLevel)
    error('The noise level (NoiseLevel) must be assigned')
end

if strcmp(RegParam,'optimal') && ischar(x_true)
    error('The true solution (x_true) must be assigned')
end

StopIt = MaxIter;

restart = strcmp(restart, 'on');
if restart
    ktotcount  = IRget(options, 'ktotcount', [],'fast');
    TotIterMax = IRget(options, 'TotIterMax',[],'fast');
    if strcmp(TotIterMax, 'none') || TotIterMax < MaxIter
        TotIterMax = MaxIter;
    end
    if strcmp(ktotcount, 'none')
        error('the total iteration counter must be assigned')
    end
    Ktot = IRget(options, 'Ktot', [], 'fast');
    % no checks on Ktot, it should be given from IRrestart
end

d = Atransp_times_vec(A, b);
n = length(d);
m = length(b);

if strcmp(SparsityTrans, 'none')
    Trans = speye(n);
elseif strcmp(SparsityTrans, 'dwt')
    wname   = IRget(options, 'wname',   [], 'fast');
    wlevels = IRget(options, 'wlevels', [], 'fast');
    if wlevels > 1/2*(log2(n))
        error('The assigned wavelet levels are too high. Make sure that wlevels <= 1/2*(log2(n))')
    end
    Trans = FreqMatrix('dwt', [sqrt(n) sqrt(n)], wname, wlevels);
end

% setting x0

x0 = IRget(options, 'x0', [], 'fast');

if strcmp(x0, 'none')
    x0 = zeros(n,1);
    r = b(:);
    precX = ones(n,1);
else
    try
        x0 = Trans*x0;
    catch
        error('Check the length of x0')
    end
    if max(abs(x0(:))) == 0
        r = b;
        x0 = zeros(n,1);
        precX = ones(n,1);
    else
        x0 = Trans'*x0;
        Ax0 = A_times_vec(A, x0);
        r = b(:) - Ax0;
        precX = x0;
        precX(abs(precX) < tolX) = eps;
        precX = (precX).^((2-q)/2);
    end
end

x = x0; % useful in case we have an immediate breakdown of the algorithm
beta = norm(r(:));
nrmb = norm(b(:));

% means no true solution
notrue = strcmp(x_true,'none');
% means we do not want to stop when the stopping criterion is satisfied
NoStop = strcmp(NoStop,'on');

% assessing if we want inner Tikhonov regularization
if strcmp(RegParam,'off')
    RegParam = 0;
end

Rfactor = strcmpi(hybridvariant, 'R');

% Declare matrices.
X                = zeros(n,length(K));
Xnrm             = zeros(max(K),1);
Rnrm             = zeros(max(K),1);
RegParamVect     = zeros(max(K),1);
M = zeros(max(K)+1,max(K));
T = zeros(max(K)+1);
Z = zeros(n, max(K));
V = zeros(n, max(K)+1);
U = zeros(m, max(K)+1);
rhs              = zeros(max(K)+1,1); % projected right-hand side
if restart
    saved_iterations = zeros(1, length(Ktot));
else
    saved_iterations = zeros(1, length(K));
end
GCV   = zeros(max(K),1);
Omega = zeros(max(K), 1);
warningGCV          = 0;
if notrue
    errornorms = false;
else
    errornorms = true;
    Enrm       = zeros(max(K),1);
    nrmtrue = norm(x_true(:));
    BestReg.RegP = [];
    BestReg.It = [];
    BestReg.X =[];
    BestReg.Enrm = [];
    BestReg.Xnrm = [];
    BestReg.Rnrm = [];
    BestEnrm = 1e10;
end
% Main Code Begins Here
u = r;
U(:,1) = u/beta; % no matter the preconditioning (because it's on the right)
rhs(1) = beta;
% Iterate
noIterBar = strcmp(IterBar,{'off'});
if ~noIterBar
  h_wait = waitbar(0, 'Running iterations, please wait ...');
end
j = 0;
for k=1:MaxIter
    if ~noIterBar
        waitbar(k/MaxIter, h_wait)
    end
    if restart, ktotcount = ktotcount + 1; end
    v = Atransp_times_vec(A, U(:,k)); v = v(:);
    v = Trans*v;
    for i = 1:k-1
        T(i,k)=V(:,i)'*v;
        v = v - T(i,k)*V(:,i);
    end
    T(k,k) = norm(v);
    v = v / T(k,k);
    %
    z = precX.*v;
    u = Trans'*z;
    u = A_times_vec(A, u); u = u(:);  %%%%
    for i = 1:k
        M(i,k) = U(:,i)'*u;
        u = u - M(i,k)*U(:,i);
    end
    M(k+1,k) = norm(u);
    u = u / M(k+1,k);
    U(:,k+1) = u;
    V(:,k) = v;
    Z(:,k) = z;

    rhsk = rhs(1:k+1); % current projected rhs

    if abs(T(k,k)) <= eps || abs(M(k+1,k)) <= eps
        if verbose
            disp('Flexible Golub-Kahan algorithm breaks down')
        end
        M = M(1:k+1,1:k);
        T = T(1:k,1:k);
        Z = Z(:,1:k);
        V = V(:,1:k);
        U = U(:,1:k+1);
        X(:,j+1) = x;
        X = X(:,1:j+1);
        if restart
            saved_iterations(j+1) = ktotcount-1;
        else
            saved_iterations(j+1) = k-1;
        end
        saved_iterations = saved_iterations(1:j+1);
        if k>1
            Xnrm    = Xnrm(1:k-1);
            Rnrm    = Rnrm(1:k-1);
            RegParamVect    = RegParamVect(1:k-1);
            if errornorms, Enrm = Enrm(1:k-1); end
        end
        % stop because the bidiagonal matrix is (numerically) singular
        % No chioce: even if NoStop is 'on'...we simpy cannot compute the solution, anymore
        if StopIt == MaxIter
            StopFlag = 'Breakdown of the Golub-Kahan algorithm';
            StopReg.X = x;
            StopReg.It = k-1;
            StopReg.RegP = RegParamk;
            StopReg.Xnrm = Xnrm(k-1);
            StopReg.Rnrm = Rnrm(k-1);
            if errornorms, StopReg.Enrm = Enrm(k-1); end
        end
        break
    end
    Mk = M(1:k+1,1:k);
    [Uk, Sk, Vk] = svd(Mk);
    if k==1
        Sk = Sk(1,1);
    else
        Sk = diag(Sk);
    end
    rhskhat = Uk'*rhsk;
    flsqr_res = abs(rhskhat(k+1))/nrmb;

    if Rfactor
        % update the Householder-QR factorization of Lk
        if k == 1
            [ZUk, ZRk] = householderQR(Z(:,1:k));
        else
            [ZUk, ZRk] = upd_householderQR(Z(:,1:k-1),...
            Z(:,k), ZUk, ZRk);
        end
        ZRksq = ZRk(1:k,1:k);
        [Uk, Vk, ~, Ck, Sk] = gsvd(Mk, ZRksq);
        rhskhat = Uk'*rhsk;
        if k==1
            gammak = Ck(1)/Sk(1);
        else
            gammak = sqrt(diag(Ck'*Ck)./diag(Sk'*Sk));
        end
    else
        ZRksq = eye(k);
    end

    % if tik
        if isscalar(RegParam)
            RegParamk = RegParam;
            RegParamVect(k) = RegParamk;
        elseif strcmp(RegParam,'discrep')
            if k==1
                RegParamVect(k) = RegParamk;
            end
        elseif strcmp(RegParam, 'discrepit')
            if flsqr_res > eta*NoiseLevel
                RegParamk = 0;
                RegParamVect(k) = RegParamk;
            else
                RegParamk = fzero(@(l)discrfcn(l, Mk, ZRksq, rhsk, nrmb, eta*NoiseLevel), [0, 1e10]);
                RegParamVect(k) = RegParamk;
            end
        elseif strcmp(RegParam, 'optimal')
            optfun = @(l)TikOptParam(l, Mk, ZRksq, rhsk, x0, Z(:,1:k), Trans, x_true);
            RegParamk = fmincon(optfun,0,[],[],[],[],0,.1);
            RegParamVect(k) = RegParamk;
        elseif strcmp(RegParam,'wgcv')
            if k>1
            if ~Rfactor
                if adaptWGCV
                    %Use the adaptive, weighted GCV method
                    Omega(k) = min(1, findomega(rhskhat, Sk));
                    omega = mean(Omega(1:k));
                end
                RegParamk = fminbnd('TikGCV', 0, Sk(1), [], rhskhat, Sk, omega);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', Sk, nrmb, m, n);
            else
                if adaptWGCV
                    %Use the adaptive, weighted GCV method
                    Omega(k) = min(1, findomega(rhskhat, gammak));
                    omega = mean(Omega(1:k));
                end
                RegParamk = fminbnd('TikGCV', 0, gammak(k), [], rhskhat, gammak, omega);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', gammak, nrmb, m, n);
            end
            RegParamVect(k) = RegParamk;
            GCV(k) = GCValk;
            else
            RegParamk = 0; GCValk = 0;
            RegParamVect(k) = RegParamk; GCV(k) = GCValk;
            end
        elseif strcmp(RegParam,'gcv')
            if ~Rfactor
                RegParamk = fminbnd('TikGCV', 0, Sk(1), [], rhskhat, Sk);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', Sk, nrmb, m, n);
            else
                RegParamk = fminbnd('TikGCV', 0, gammak(k), [], rhskhat, gammak);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', gammak, nrmb, m, n);
            end
            RegParamVect(k) = RegParamk;
            GCV(k) = GCValk;
        elseif strcmp(RegParam,'modgcv')
            if ~Rfactor
                RegParamk = fminbnd('TikGCV', 0, Sk(1), [], rhskhat, Sk, m);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', Sk, nrmb, m, n);
            else
                RegParamk = fminbnd('TikGCV', 0, gammak(k), [], rhskhat, gammak, m);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', gammak, nrmb, m, n);
            end
            RegParamVect(k) = RegParamk;
            GCV(k) = GCValk;
        else
            error('Invalid parameter choice method')
        end
        if ~Rfactor
            Dk = Sk.^2 + RegParamk^2;
            % Dk = Sk.^2 + RegParamk;
            rhskhat = Sk .* rhskhat(1:k);
            yhat = rhskhat(1:k)./Dk;
            y = Vk * yhat;
%             MZk = [Mk; RegParamk*eye(k)];
%             rhsZk = [rhsk; zeros(k,1)];
%             y = MZk\rhsZk;
        else
            MZk = [Mk; RegParamk*ZRksq(1:k,:)];
            % MZk = [Mk; sqrt(RegParamk)*ZRksq(1:k,:)];
            rhsZk = [rhsk; zeros(k,1)];
            y = MZk\rhsZk;
        end
        Rnrm(k) = norm(rhsk - Mk*y)/nrmb;
        d = Z(:,1:k)*y;
        x = x0 + d;
        precX = abs(x);
        precX(precX < tolX) = eps;
        precX = precX.^((2-q)/2);
        x = Trans'*x;
        % Compute norms
        Xnrm(k) = norm(x(:));
        if errornorms
            Enrm(k) = norm(x_true(:) - x(:))/nrmtrue;
            if Enrm(k)<BestEnrm
                BestReg.RegP = RegParamk;
                BestReg.It = k;
                BestReg.X = x;
                BestEnrm = Enrm(k);
                BestReg.Enrm = BestEnrm;
                BestReg.Xnrm = Xnrm(k);
                BestReg.Rnrm = Rnrm(k);
            end
        end
        AlreadySaved = 0;
        if any(k==K)
            j = j+1;
            X(:,j) = x;
            saved_iterations(j) = k;
            % this is used to save the last iteration, in the case
            % K = MaxIterIn
            % (when performing restarts, and the inner stopping criterion is not satisfied)
            if restart, saved_iterations(j) = ktotcount; end
            AlreadySaved = 1;
        end
        if restart
            if any(ktotcount == Ktot) && ~ AlreadySaved
                j = j+1;
                X(:,j) = x;
                saved_iterations(j) = ktotcount;
                AlreadySaved = 1;
            end
            if ktotcount == TotIterMax
                if ~ AlreadySaved
                    j = j+1;
                    saved_iterations(j) = ktotcount;
                    X(:,j) = x;
                end
                StopIt = k;
                StopReg.X = x;
                StopReg.It = k;
                StopReg.RegP = RegParamk;
                StopReg.Xnrm = Xnrm(k);
                StopReg.Rnrm = Rnrm(k);
                if errornorms
                    Enrm = Enrm(1:k);
                    StopReg.Enrm = Enrm(k);
                end
                Xnrm    = Xnrm(1:k);
                Rnrm    = Rnrm(1:k);
                RegParamVect    = RegParamVect(1:k);
                M = M(1:k+1,1:k);
                T = T(1:k,1:k);
                Z = Z(:,1:k);
                V = V(:,1:k);
                U = U(:,1:k+1);
                X = X(:,1:j);
                saved_iterations = saved_iterations(1:j);
                if verbose
                    disp('reached maximum number of iterations')
                end
                StopFlag = 'reached maximum number of iterations';
                break
            end
        end
        % update parameters, check stopping criteria
        if isscalar(RegParam)
        % Purely iterative method case.
        if strcmp(NoiseLevel, 'none')
            if k>1
            if abs((Rnrm(k)-Rnrm(k-1)))/Rnrm(k-1) < resdegflat && ...
                Rnrm(k) == min(Rnrm(1:k)) && StopIt == MaxIter
                if verbose
                    disp('The stopping criterion for flsqr is satisfied')
                end
                % Stop because the residual stabilizes.
                StopFlag = 'The residual norm stabilizes';
                if ~AlreadySaved && ~NoStop
                    j = j+1;
                    X(:,j) = x;
                    if restart
                        saved_iterations(j) = ktotcount;
                    else
                        saved_iterations(j) = k;
                    end
                    AlreadySaved = 1;
                end
                StopIt = k;
                StopReg.RegP = RegParamk;
                StopReg.It = k;
                StopReg.X = x;
                if errornorms, StopReg.Enrm = Enrm(k); end
                if ~ NoStop
                    Xnrm    = Xnrm(1:k);
                    Rnrm    = Rnrm(1:k);
                    RegParamVect = RegParamVect(1:k);
                    M = M(1:k+1,1:k);
                    T = T(1:k,1:k);
                    Z = Z(:,1:k);
                    V = V(:,1:k);
                    U = U(:,1:k+1);
                    if errornorms, Enrm = Enrm(1:k); end
                    X = X(:,1:j);
                    saved_iterations = saved_iterations(1:j);
                    break
                end
            end
            end
        else
            if Rnrm(k) < eta*NoiseLevel
            % Stopping criterion.
            if StopIt == MaxIter
                if verbose
                    disp('The discrepancy principle is satisfied')
                end
                StopFlag = 'The discrepancy principle satisfied';
                if ~AlreadySaved && ~NoStop
                    j = j+1;
                    X(:,j) = x;
                    if restart
                        saved_iterations(j) = ktotcount;
                    else
                        saved_iterations(j) = k;
                    end
                    AlreadySaved = 1;
                end
                StopIt = k;
                StopReg.RegP = RegParamk;
                StopReg.It = k;
                StopReg.X = x;
                if errornorms, StopReg.Enrm = Enrm(k); end
                if ~ NoStop
                    Xnrm    = Xnrm(1:k);
                    Rnrm    = Rnrm(1:k);
                    RegParamVect    = RegParamVect(1:k);
                    M = M(1:k+1,1:k);
                    T = T(1:k,1:k);
                    Z = Z(:,1:k);
                    V = V(:,1:k);
                    U = U(:,1:k+1);
                    if errornorms, Enrm = Enrm(1:k); end
                    X = X(:,1:j);
                    saved_iterations = saved_iterations(1:j);
                    % Stop because the discrepancy principle is satisfied.
                    break
                end
            end
            end
        end
        elseif strcmp(RegParam,'discrep')
            if Rnrm(k) < eta*NoiseLevel
                % stopping criterion
                if StopIt == MaxIter % the method has not stopped, yet
                    if verbose
                        disp('The discrepancy principle is satisfied')
                    end
                    StopFlag = 'discrepancy principle (secant update method) satisfied';
                    if ~AlreadySaved && ~NoStop
                        j = j+1;
                        X(:,j) = x;
                        if restart
                            saved_iterations(j) = ktotcount;
                        else
                            saved_iterations(j) = k;
                        end
                        AlreadySaved = 1;
                    end
                    StopIt = k;
                    StopReg.X = x;
                    StopReg.It = k;
                    StopReg.RegP = RegParamk;
                    StopReg.Xnrm = Xnrm(k);
                    StopReg.Rnrm = Rnrm(k);
                    if errornorms, StopReg.Enrm = Enrm(k); end
                    if ~ NoStop
                        Xnrm    = Xnrm(1:k);
                        Rnrm    = Rnrm(1:k);
                        RegParamVect    = RegParamVect(1:k);
                        M = M(1:k+1,1:k);
                        T = T(1:k,1:k);
                        Z = Z(:,1:k);
                        V = V(:,1:k);
                        U = U(:,1:k+1);
                        if errornorms, Enrm = Enrm(1:k); end
                        X = X(:,1:j);
                        saved_iterations = saved_iterations(1:j);
                        % stop because the discrepancy principle is satisfied
                        break
                    else
                        RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*(RegParamk^2);
                        % RegParamk = RegParamk^2;
                        % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))^2*RegParamk;
                        % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*RegParamk;
                        RegParamk = sqrt(RegParamk);
                        if k~=MaxIter, RegParamVect(k+1) = RegParamk; end
                    end
                else
                    RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*(RegParamk^2);
                    % RegParamk = RegParamk^2;
                    % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*RegParamk;
                    RegParamk = sqrt(RegParamk);
                    if k~=MaxIter, RegParamVect(k+1) = RegParamk; end
                end
            else
                RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*(RegParamk^2);
                % RegParamk = RegParamk^2;
                % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*RegParamk;
                RegParamk = sqrt(RegParamk);
                if k~=MaxIter, RegParamVect(k+1) = RegParamk; end
            end
        elseif strcmp(RegParam,'discrepit')
            if k>2
                % stopping criterion
                if StopIt == MaxIter % the method has not stopped, yet
                    if abs(RegParamVect(k)-RegParamVect(k-1))/RegParamVect(k-1) < regPflat && abs(RegParamVect(k-1)-RegParamVect(k-2))/RegParamVect(k-2)<regPflat
                        if verbose
                            disp('The stopping criterion for the discrepancy principle is satisfied')
                        end
                        StopFlag = 'discrepancy principle (stopping criterion) satisfied';
                        if ~AlreadySaved && ~NoStop
                            j = j+1;
                            X(:,j) = x;
                            if restart
                                saved_iterations(j) = ktotcount;
                            else
                                saved_iterations(j) = k;
                            end
                            AlreadySaved = 1;
                        end
                        StopIt = k;
                        StopReg.X = x;
                        StopReg.It = k;
                        StopReg.RegP = RegParamk;
                        StopReg.Xnrm = Xnrm(k);
                        StopReg.Rnrm = Rnrm(k);
                        if errornorms, StopReg.Enrm = Enrm(k); end
                        if ~ NoStop
                            Xnrm    = Xnrm(1:k);
                            Rnrm    = Rnrm(1:k);
                            RegParamVect    = RegParamVect(1:k);
                            M = M(1:k+1,1:k);
                            T = T(1:k,1:k);
                            Z = Z(:,1:k);
                            V = V(:,1:k);
                            U = U(:,1:k+1);
                            if errornorms, Enrm = Enrm(1:k); end
                            X = X(:,1:j);
                            saved_iterations = saved_iterations(1:j);
                            % stop because the discrepancy principle is satisfied
                            break
                        end
                    end
                end
            end
        elseif strcmp(RegParam,'wgcv') || strcmp(RegParam,'gcv') || strcmp(RegParam,'modgcv')
            % check the stopping criterion (all the possibilities)
            if k > 1
            if strcmpi(stopGCV, 'GCVvalues')
                if StopIt == MaxIter % the method has not stopped, yet
                if abs((GCV(k)-GCV(k-1)))/GCV(2) < degflat && StopIt == MaxIter
                % the method has not stopped, yet
                        if verbose
                            disp('The stopping criterion for GCV principle is satisfied')
                        end
                        % stop because the GCV curve is too flat
                        StopFlag = 'GCV curve too flat';
                        if ~AlreadySaved && ~ NoStop
                            j = j+1;
                            X(:,j) = x;
                            if restart
                                saved_iterations(j) = ktotcount;
                            else
                                saved_iterations(j) = k;
                            end
                            AlreadySaved = 1;
                        end
                        StopIt = k;
                        StopReg.X = x;
                        StopReg.It = k;
                        StopReg.RegP = RegParamk;
                        StopReg.Xnrm = Xnrm(k);
                        StopReg.Rnrm = Rnrm(k);
                        if errornorms, StopReg.Enrm = Enrm(k); end
                        if ~ NoStop
                            Xnrm    = Xnrm(1:k);
                            Rnrm    = Rnrm(1:k);
                            RegParamVect    = RegParamVect(1:k);
                            M = M(1:k+1,1:k);
                            T = T(1:k,1:k);
                            Z = Z(:,1:k);
                            V = V(:,1:k);
                            U = U(:,1:k+1);
                            if errornorms, Enrm = Enrm(1:k); end
                            X = X(:,1:j);
                            saved_iterations = saved_iterations(1:j);
                            % stop because the GCV stopping criterion is satisfied
                            break
                        end
                elseif GCV(k-1) < GCV(k) && ~ warningGCV && StopIt == MaxIter % Potential minimum reached.
                    warningGCV = 1;
                    % Save data just in case.
                    x_save = x;
                    k_save = k; % for computing the GCV stopping criterion
                    j_save = j;
                    AlreadySaved_save = AlreadySaved;
                    RegParamk_save = RegParamk;
                    if restart, ktotcount_save = ktotcount; end
                elseif warningGCV && k > min(k_save + mintol, MaxIter) && StopIt == MaxIter % Passed window
                    if GCV(k_save) < GCV(k_save+1:min(k_save + mintol, MaxIter))
                        if verbose
                            disp('The stopping criterion for GCV principle is satisfied')
                        end
                        StopFlag = 'increasing GCV minima';
                        StopIt = k_save;
                        StopReg.It = k_save;
                        StopReg.X = x_save;
                        StopReg.RegP = RegParamk_save;
                        StopReg.Xnrm = Xnrm(k_save);
                        StopReg.Rnrm = Rnrm(k_save);
                        if errornorms
                            StopReg.Enrm = Enrm(k_save);
                        end
                        if ~ NoStop
                            j = j_save;
                            saved_iterations = saved_iterations(1:j);
                            X = X(:,1:j);
                            if ~AlreadySaved_save
                                j = j+1;
                                X(:,j) = x_save;
                                if restart
                                    saved_iterations(j) = ktotcount_save;
                                else
                                    saved_iterations(j) = k_save;
                                end
                            end
                            Xnrm    = Xnrm(1:k_save);
                            Rnrm    = Rnrm(1:k_save);
                            RegParamVect    = RegParamVect(1:k_save);
                            M = M(1:k_save+1,1:k_save);
                            T = T(1:k_save,1:k_save);
                            Z = Z(:,1:k_save);
                            V = V(:,1:k_save);
                            U = U(:,1:k_save+1);
                            if errornorms
                                Enrm = Enrm(1:k_save);
                                if BestReg.It > k_save
                                    [BestReg.Enrm, BestReg.It] = min(Enrm);
                                    BestReg.RegP = RegParamVect(BestReg.It);
                                    BestReg.Xnrm = Xnrm(BestReg.It);
                                    BestReg.Rnrm = Rnrm(BestReg.It);
                                    % recompute the best solution again
                                    ktemp = BestReg.It;
                                    Mtemp = M(1:ktemp+1,1:ktemp);
                                    Ztemp = Z(:,1:ktemp);
                                    rhsk = rhs(1:ktemp+1);
                                    [Uk, Sk, Vk] = svd(Mtemp);
                                    if ktemp==1
                                        Sk = Sk(1,1);
                                    else
                                        Sk = diag(Sk);
                                    end
                                    rhskhat = Uk'*rhsk;
                                    if Rfactor
                                        [~, ZRksq] = qr(Ztemp,0);
                                    end
                                    if ~Rfactor
                                        Dk = Sk.^2 + RegParamk^2;
                                        rhskhat = Sk .* rhskhat(1:ktemp);
                                        yhat = rhskhat(1:ktemp)./Dk;
                                        y = Vk * yhat;
                                    else
                                        MLk = [Mtemp; RegParamk*ZRksq];
                                        rhsLk = [rhsk; zeros(ktemp,1)];
                                        y = MLk\rhsLk;
                                    end
                                    dtemp = Ztemp*y;
%                                     if precond, dtemp = P_solve(L, dtemp); end
                                    xtemp = x0 + dtemp;
                                    BestReg.X = xtemp;
                                end
                            end
                            X = X(:,1:j);
                            saved_iterations = saved_iterations(1:j);
                            if restart, ktotcount = ktotcount_save; end
                            % stop because the GCV stopping criterion is satisfied
                            break
                        end
                    else
                        warningGCV = 0;
                    end
                end
            elseif strcmpi(stopGCV, 'resflat')
                if abs((Rnrm(k)-Rnrm(k-1)))/Rnrm(k-1) < resdegflat && ...
                    Rnrm(k) == min(Rnrm(1:k)) && StopIt == MaxIter
                    if verbose
                        disp('The stopping criterion for GCV principle is satisfied')
                    end
                    % stop because discrepancy (i.e., residual for the
                    % regularized problem) stabilizes
                    StopFlag = 'the residual norm stabilizes';
                    if ~AlreadySaved && ~NoStop
                        j = j+1;
                        X(:,j) = x;
                        if restart
                            saved_iterations(j) = ktotcount;
                        else
                            saved_iterations(j) = k;
                        end
                        AlreadySaved = 1;
                    end
                    StopIt = k;
                    StopReg.X = x;
                    StopReg.It = k;
                    StopReg.RegP = RegParamk;
                    StopReg.Xnrm = Xnrm(k);
                    StopReg.Rnrm = Rnrm(k);
                    if errornorms, StopReg.Enrm = Enrm(k); end
                    if ~ NoStop
                        Xnrm    = Xnrm(1:k);
                        Rnrm    = Rnrm(1:k);
                        RegParamVect    = RegParamVect(1:k);
                        M = M(1:k+1,1:k);
                        T = T(1:k,1:k);
                        Z = Z(:,1:k);
                        V = V(:,1:k);
                        U = U(:,1:k+1);
                        if errornorms, Enrm = Enrm(1:k); end
                        X = X(:,1:j);
                        saved_iterations = saved_iterations(1:j);
                        break
                    end
                end
                end
            end
            end
        end
end
if k == MaxIter
    if StopIt == MaxIter
        % Stop because max number of iterations reached
        if verbose
            disp('Reached maximum number of iterations')
        end
        StopFlag = 'reached maximum number of iterations';
        if ~AlreadySaved
            j = j+1;
            X(:,j) = x;
            if restart
                saved_iterations(j) = ktotcount;
            else
                saved_iterations(j) = k;
            end
        end
        StopReg.X = x;
        StopReg.It = k;
        StopReg.RegP = RegParamk;
        StopReg.Xnrm = Xnrm(k);
        StopReg.Rnrm = Rnrm(k);
        if errornorms, StopReg.Enrm = Enrm(k); end
        Xnrm    = Xnrm(1:k);
        Rnrm    = Rnrm(1:k);
        RegParamVect    = RegParamVect(1:k);
        M = M(1:k+1,1:k);
        T = T(1:k,1:k);
        Z = Z(:,1:k);
        V = V(:,1:k);
        U = U(:,1:k+1);
        if errornorms, Enrm = Enrm(1:k); end
        X = X(:,1:j);
        saved_iterations = saved_iterations(1:j);
    end
end
if ~noIterBar, close(h_wait), end
if nargout==2
  if NoStop
      info.its = k;
  else
      info.its = StopIt;
  end
  info.saved_iterations = saved_iterations(1:j);
  info.StopReg = StopReg;
  info.StopFlag = StopFlag;
  if errornorms
    info.Enrm = Enrm;
    info.BestReg = BestReg;
  end
  info.Xnrm = Xnrm;
  info.Rnrm = Rnrm;
  info.RegP = RegParamVect;
  if strcmp(RegParam,'wgcv') || strcmp(RegParam,'gcv') || strcmp(RegParam,'modgcv')
    info.GCValues = GCV(1:k);
  end
  if strcmp(DecompOut,'on')
      info.V = V(:,1:k);
      info.U = U(:,1:k+1);
      info.Z = Z(:,1:k);
      info.T = T(1:k,1:k);
      info.M = M(1:k+1,1:k);
  end
  if restart
      info.ktotcount = ktotcount;
  end
end

%% ---------------SUBFUNCTIONS ---------------------------------------
function [U,R] = householderQR(L)
%
%  [U,R] = householderQR(L)
%  This function computes the Householder-QR factorization of L
%  (a "projected" regularization matrix), that will be used to define a
%  "projected" regularization matrix R to employ within the LSQR iterates.
%

[m, n] = size(L);
R = L;
U = zeros(m, n);
for k = 1:n
    x = L(k:m,k);
    e = zeros(length(x),1); e(1) = 1;
    u = sign(x(1))*norm(x(:))*e + x;
    u = u./norm(u(:));
    R(k:m, k:n) = R(k:m, k:n) -2*u*(u'*R(k:m, k:n));
    U(k:m,k) = u;
end

function [U,R] = upd_householderQR(L,ll,U,R)
%
% [U,R] = upd_householderQR(L, ll, U, R)
% This function updates the Householder-QR factorization of [L, ll].
%
% Input:
%   L  - matrix whose QR factorization is defined by U and R
%   ll - column appended to L, i.e., [L, ll]
%    U - matrix defining the orthogonal matrix Q, such that L = QR
%    R - upper triangular factor of L = QR
%

[m,n] = size(L);
Unew = zeros(m, n+1);
Unew(:,1:n) = U;
w = ll;
for i = 1:n
    u = U(i:m,i);
    w(i:m) = w(i:m) - 2*u*(u'*w(i:m));
end
v = w(1:n); x = w(n+1:m);
e = zeros(length(x),1); e(1) = 1;
u = sign(x(1))*norm(x(:))*e + x;
u = u./norm(u(:));
x = x -2*u*(u'*x);
Unew(n+1:m,n+1)=u;
U = Unew;
rr = [v; x];
R = [R, rr];

function omega = findomega(bhat, s)
%
%   omega = findomega(bhat, s)
%
%  This function computes a value for the omega parameter used in wGCV.
%
%  The method: Assume the 'optimal' regularization parameter to be the
%  smallest singular value.  Then we take the derivative of the GCV
%  function with respect to alpha, evaluate it at alpha_opt, set the
%  derivative equal to zero and then solve for omega.
%
%  Input:   bhat -  vector U'*b, where U = left singular vectors
%              s -  vector containing the singular values
%
%  Output:     omega - computed value for the omega parameter.

%
%   First assume the 'optimal' regularization parameter to be the smallest
%   singular value.
%

%
% Compute the needed elements for the function.
%
m = length(bhat);
n = length(s);

t0 = sum(abs(bhat(n+1:m)).^2);
alpha = s(end);
s2 = abs(s) .^ 2;
alpha2 = alpha^2;

tt = 1 ./ (s2 + alpha2);

t1 = sum(s2 .* tt);
t2 = abs(bhat(1:n).*alpha.*s) .^2;
t3 = sum(t2 .* abs((tt.^3)));

t4 = sum((s.*tt) .^2);
t5 = sum((abs(alpha2*bhat(1:n).*tt)).^2);

v1 = abs(bhat(1:n).*s).^2;
v2 = sum(v1.* abs((tt.^3)));

%
% Now compute omega.
%
omega = (m*alpha2*v2)/(t1*t3 + t4*(t5 + t0));


%%
% \documentclass[12pt]{article}
%
% \title{ODEbox: A Toolbox for Ordinary Differential Equations\\
% Example 1}
%
% \author{Matthew Harker and Paul O'Leary\\
% Institute for Automation\\
% University of Leoben\\
% A-8700 Leoben,
% Austria\\
% URL: automation.unileoben.ac.at\\
% \\
% Original: January 9, 2013\\
% $\copyright$ 2013\\
% \\
% Last Modified: \today}
%%
%\section{Initial Value Problem}
%
% This file demonstrated the solution of initial value problems using the
% DOPBox and ODEBox toolbaxes. The example being considered here is:
%
% \begin{equation}
%    \ddot{y} + 6\,\dot{y} + 9 y = 0
%    \hspace{5mm}
%    \text{with}
%    \hspace{5mm}
%    y(0) = 10
%   \hspace{5mm}
%   \text{and}
%   \hspace{5mm}
%   \dot{y}(0) = -75
% \end{equation}
%
% The analytical solution to this problem is:
%
% \begin{equation}
%   y(x) = 10 \, e^{-3\,x} - 45 \,x {e^{-3\,x}}.
% \end{equation}
%
% This example has been taken from~\cite{adams}.
%
% This M-file demonstrated the new approach to solving this problem and
% compared the result with a classical runga-kutta solution as provided by
% MATLAB.
%
%%
% \section{Prepare the workspace}
%
close all;
clear all;
setUpGraphics(12) ;
%%
% Define the equation and solution as a strings for documentation
%
Eq = '$$\ddot{y} - 6\,\dot{y} - 9 y = 0$$, with, $$y(0) = 10$$ and $$\dot{y}(1) = -75$$' ;
Sol = '$$y(x) = 10 \, e^{-3\,x} - 45 \,x {e^{-3\,x}}$$';
%
%------------------------------------------------------------------
%%
% \section{Define the Matrix Linear Differential Operator}
%
% Define the number of points used in the solution
%
noPts = 85 ;
%
% Define the interval in which the problem is to be solved
%
xMin = 0 ;
xMax = 3 ;
%
% Use a nonlinear node placement. This ensures a higher density of nodes
% where the solution has a highed derivative.
%
x = linspace(0,1,noPts)';
x = 5 * x.^2;
%
% Setup the differentiating matrix, with support length 13.
%
ls = 13;
D = dopDiffLocal( x, ls, ls );
%
% Define the matrix C of constraints, i.e. the initial conditions
%
C = zeros(noPts,2) ;
%
C(1,1) = 1 ;
C(:,2) = D(1,:)' ;
%
d = [ 10 ; -75]  ;
%
% Compute the linear differential operator.
%
L = D*D + 6*D + 9*eye(noPts) ;
%%
%\section{Compute the Solutions}
%
% Solve the algebraic system of equations as a least squares problem. This
% is the actual computation of the solution of the differential equation.
%
y = odeLSE( L, [] , C, d );
%%
% For comparison solve using the MATLAB ode45 method.
% Runge-Kutta Solution:
%
[xm,Y] = ode45(@eulerODE01,[xMin xMax],d');
%
ym = Y(:,1);
%%
% Compute the Analytic Solution as an inline to compare the results
%
f = inline('10*exp(-3*t) - 45*t.*exp(-3*t)') ;
%
ya = f(x);
%%
% \section{Plot the Results}
%
% for plot purposes solve beyond the ends
%
xp = linspace(xMin-1/4,xMax+1/4,1000) ;
yp = f(xp);
%
% Present the solutions
%
setUpGraphics(12)
FigureSize=[1 1 10 6];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.13 0.15 0.86 0.82];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig1 = figure;
plot( xp, yp, 'k' ) ;
hold on
plot( x, y, 'ko', 'MarkerFaceColor', 'w' ) ;
%
plot( xm, ym,'kv');
legend('Analytical','New','Runge-Kutta','Location','NorthEast');
xlabel('$$x$$');
ylabel('$$y$$');
grid on;
range = axis;
plot( range(1:2), [0,0],'k');
axis([-0.5,3.5,-5,15]);
plot( x, -4.5*ones(size(x)), 'k.');
%
%\caption{Comparison of the analytical solution, the new numberical
% method and the Runga-Kutta solution.}
%%
%
setUpGraphics(12)
FigureSize=[1 1 10 6];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.13 0.13 0.86 0.82];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig2 = figure;
subplot(2,1,1)
plot( x, y - ya, 'k', 'MarkerFaceColor', 'w' ) ;
hold on
plot( x, y - ya, 'k.', 'MarkerFaceColor', 'w' ) ;
range = axis;
plot( range(1:2), [0,0],'k');
ylabel('$$e_{L}$$');
grid on;
%title(Sol);
%
subplot(2,1,2)
plot( xm, ym - f(xm), 'k', 'MarkerFaceColor', 'w' ) ;
hold on
plot( xm, ym - f(xm), 'k.', 'MarkerFaceColor', 'w' ) ;
hold on;
range = axis;
plot( range(1:2), [0,0],'k');
xlabel('$$x$$');
ylabel('$$e_{RK}$$');
grid on;
%
% \caption{Comparison the residual error for the new method (top)  (Bottom)and
% for the Runga-Kutta method. Note the residual is orders of magnitude smaller for the new method.}
%
%%
%\section{Save the figures to disk.}
%
fileType = 'eps';
printFigure( fig1, 'ivpExp3Sol', fileType);
printFigure( fig2, 'ivpExp3Err', fileType);
%
%% Define the Bibliography
%
% \bibliographystyle{IEEETran}
% \bibliography{odebib}


function [price, opt, L] = PROJ_GMDB_DCA_Fast(proj_params, S_0, gmdb_params, r, q, modelInput)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for DCA-Style Garuanteed Minimum Withdraw Benefit (GMWB) using PROJ method
%        This version is based on a dollar cost average style investment account (see reference below)
%
% Terminal Payoff:  Payoff(tau) = L*exp(g*tau) + (Gam(tau) - L*exp(g*tau))^+
%                      Gam(tau) = S_M * sum_{m=0}^M(alpha*gamma / S_m)
%                          tau  = time of death (discrete periods)
%                            M  = number of periods until time of death (each period length dt)
%
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
%
% Author: Justin Lars Kirkby
% References: 1) Equity-Linked  Guaranteed Minimum Death Benefits with Dollar Cost Averaging, J.L.Kirkby & D.Nguyen, 2021
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% r   = interest rate (e.g. 0.05)
% q   = dividend yield (e.g. 0.05)
% M   = number of subintervals of [0,T] (total of M+1 monitoring points in time grid, including S_0)
% gmdb_params = container of GMDB contract params, see below
% modelInput =  model inputs, see below
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% proj_params = numerical params
%   proj_params.N = number of basis elements, e.g. N = 2^10
%   proj_params.L1 = gridwidth param, e.g. L1 = 8
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ------------------
% GMDB Contract Params
% ------------------
L = gmdb_params.L;   % Guarantee Level: Set L = -1 to use ATMF value for L
alpha = gmdb_params.alpha;  % Period premium payment, paid every dt time units
gamma = gmdb_params.gamma;  % Proportion of investment retained by policyholder (fee is 1-gamma)
contract_type = gmdb_params.contract_type;  % Contract type: 1 = GMDB, 2 = GMDB-RS (Ratchet strike)
p = gmdb_params.death_prob;  % death probability distribution, must be consistent with dt
g = gmdb_params.g;

% ------------------
% Model Inputs
% ------------------
dt = modelInput.dt;  % Time increment, premiums paid / underlying S is monitored every dt
phiR = modelInput.rnCHF;  % Risk neutral CHF for time period dt

call = 1;
ER = 0;

Z = gen_func(-r, dt, p);
if g == 0
    Zrg = Z;
else
    Zrg = gen_func(-(r-g), dt, p);
end

if L == -1
    MF = gen_func(r - q - g, dt, p);
    Zg = gen_func(-g, dt, p);
    L = alpha * gamma * (exp((r-q)*dt)*MF - Zg) / (exp((r-q)*dt) - 1);
end

Mmax = length(p); Tmax = Mmax*dt;
pr_alpha = getTruncationAlpha(Tmax, proj_params.L1, modelInput, modelInput.model);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = proj_params.N;
dx = 2*pr_alpha/(N-1); a = 1/dx;
A    = 32*a^4;
C_aN = A/N;

%%% SHIFTS
x1    = zeros(1,Mmax);

if contract_type == 1
    strikes = S_0*L*exp(g*dt*(1:Mmax)) ./ (alpha*gamma*(2:Mmax + 1));
else
    strikes = S_0*ones(1,Mmax);
end

for m=1:Mmax
    if m == 1
        x1(m) = ER;
    else
        x1(m) = ER + log(1+exp(x1(m-1)));  %%BENHAMOU SHIFT
    end
    %x1(m) = log(m) + .5*(m+1)*ER;    %% LOWER BOUND SHIFT derived in APROJ paper
end

Nm   = floor(a*(x1-ER));
x1   = ER + (1-N/2)*dx + Nm*dx;
NNM  = N + Nm(Mmax-1);   %Number of columns of PSI

%%% Now check that we wont fall off the grid later
for m=1:Mmax
    ystar = log((m+1)*strikes(m)/S_0 -1);
    nbar  = floor((ystar-x1(m))*a+1);
    % fprintf('%.0f\n', nbar);
    if nbar + 1 > N
        proj_params.L1 = proj_params.L1*1.25;
        [price, opt, L] = PROJ_GMDB_DCA_Fast(proj_params, S_0, gmdb_params, r, q, modelInput);
        return;
    end
end

dxi   = 2*pi*a/N;
xi    = dxi*(1:(N-1))';
PhiR = [1; phiR(xi)];

b0    = 1208/2520; b1 = 1191/2520; b2 = 120/2520; b3 = 1/2520;
zeta  = (sin(xi/(2*a))./xi).^4./(b0 + b1*cos(xi/a) +b2*cos(2*xi/a) +b3*cos(3*xi/a));
AA = 1/A;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PSI Matrix: 5-Point GAUSSIAN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PSI = make_PSI(N,NNM,x1(1),dx,dxi);

%%%%%%%%%%%%%%
% STEP 1) Value the European!!!
%%%%%%%%%%%%%%
beta  = [AA; zeta.*PhiR(2:N).*exp(-1i*x1(1)*xi)];   %grand(end)=.5*grand(end);
beta  = real(fft(beta));

s = european_price(zeta,PhiR,xi,dx, r, q, dt, strikes(1), S_0, ER, N, a, call);
s = p(1) * 2 * s;

%%%%%%%%%%%%%%
% STEP 2) Value the rest
%%%%%%%%%%%%%%

PhiR  = C_aN*PhiR;
beta  = PSI(:,1:N)*beta.*PhiR;  %Nm(1)=0

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

%%%%% Loop to find PSI_M
for n=2:Mmax
    opt_v = intermediate_asian_price(N, dx, dt, xi, zeta, beta, x1(n), n, r, q, strikes(n), S_0);
    % fprintf('%.12f \n',opt_v);

    beta(2:N) = zeta.*beta(2:N).*exp(-1i*x1(n)*xi); beta(1) = AA;
    beta      = real(fft(beta));
    if n < Mmax
        beta      = PSI(:,Nm(n)+1:Nm(n)+N)*beta.*PhiR;
    end

    s = s + p(n) * (n + 1) * opt_v;
end

opt = s * alpha * gamma / S_0;
price = L*Zrg - alpha * (exp(r*dt) - Z) / (exp(r*dt) - 1) + opt;

end

function PSI = make_PSI(N,NNM,x_1,dx,dxi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PSI Matrix: 5-Point GAUSSIAN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PSI     = zeros(N,NNM);    %The first row will remain ones
PSI(1,:) = ones(1,NNM);

%%%% Sample
Neta  = 5*(NNM) + 15;   %sample size
Neta5 = (NNM) + 3;
g2    = sqrt(5-2*sqrt(10/7))/6;
g3    = sqrt(5+2*sqrt(10/7))/6;
v1    = .5*128/225;
v2    = .5*(322+13*sqrt(70))/900;
v3    = .5*(322 - 13*sqrt(70))/900;

thet                 = zeros(1,Neta);   %sample initialized
thet(5*(1:Neta5)-2)  = x_1 -1.5*dx + dx*(0:Neta5-1);
thet(5*(1:Neta5)-4)  = x_1 -1.5*dx + dx*(0:Neta5-1) - dx*g3;
thet(5*(1:Neta5)-3)  = x_1 -1.5*dx + dx*(0:Neta5-1) - dx*g2;
thet(5*(1:Neta5)-1)  = x_1 -1.5*dx + dx*(0:Neta5-1) + dx*g2;
thet(5*(1:Neta5))    = x_1 -1.5*dx + dx*(0:Neta5-1) + dx*g3;

%%%% Weights
sig      = [-1.5-g3, -1.5-g2, -1.5, -1.5+g2, -1.5+g3, -.5-g3, -.5-g2, -.5, -.5+g2, -.5+g3,];
sig(1:5) = (sig(1:5) + 2).^3/6;
sig(6:10) = 2/3 - .5*(sig(6:10)).^3 - (sig(6:10)).^2;

sig([1 5 6 10]) = v3*sig([1 5 6 10]);
sig([2 4 7 9]) = v2*sig([2 4 7 9]);
sig([3 8]) = v1*sig([3 8]);

%%%% Fill Matrix
zz  = exp(1i*dxi*log(1+exp(thet)));
thet   = zz;

for j=2:N-1
    PSI(j,:) =  sig(1)*(thet(1:5:Neta-19) + thet(20:5:Neta)) ...
              + sig(2)*(thet(2:5:Neta-18) + thet(19:5:Neta-1)) ...
              + sig(3)*(thet(3:5:Neta-17)  + thet(18:5:Neta-2)) ...
              + sig(4)*(thet(4:5:Neta-16)  + thet( 17:5:Neta-3)) ...
              + sig(5)*(thet(5:5:Neta-15)  + thet( 16:5:Neta-4)) ...
              + sig(6)*(thet(6:5:Neta-14)  + thet( 15:5:Neta-5)) ...
              + sig(7)*(thet(7:5:Neta-13)  + thet( 14:5:Neta-6)) ...
              + sig(8)*(thet(8:5:Neta-12)  + thet( 13:5:Neta-7)) ...
              + sig(9)*(thet(9:5:Neta-11)  + thet( 12:5:Neta-8)) ...
              + sig(10)*(thet(10:5:Neta-10)  + thet( 11:5:Neta-9));

    thet = thet.*zz;
end
end

function Val = intermediate_asian_price(N, dx, dt, xi, zeta, chf, x_1, M, r, q, W, S_0)
call = 1;
a = 1/dx;
A    = 32*a^4;
AA = 1/A;
C_aN = A/N;
T = M*dt;

%%%%% FINAL VALUE
ystar = log((M+1)*W/S_0 -1);
nbar  = floor((ystar-x_1)*a+1);
C     = S_0/(M+1);
D     = W - C;
x_1   = ystar- (nbar-1)*dx;

beta(2:N) = zeta.*chf(2:N).*exp(-1i*x_1*xi); beta(1)=AA;
beta      = real(fft(beta)).';

Cc1 = C*( exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 7*exp(-dx)/27 )/20;

Cc2 = C*.05*(28/27 + exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 14*exp(-dx)/27 ...
              + 121/54*exp(-.75*dx) + 23/18*exp(-.5*dx) + 235/54*exp(-.25*dx));

Cc3 = C*( (28 + 7*exp(-dx))/3 ...
              + ( 14*exp(dx) + exp(-7/4*dx) + 242*cosh(.75*dx) + 470*cosh(.25*dx))/12 ...
              +.25*(exp(-1.5*dx) + 9*exp(-1.25*dx) + 46*cosh(.5*dx)))/90;

Cc4 = C*( 14/3*(2+cosh(dx)) ...
              + .5*(cosh(1.5*dx) + 9*cosh(1.25*dx) +23*cosh(.5*dx))...
              +  1/6*(cosh(7/4*dx) + 121*cosh(.75*dx) +235*cosh(.25*dx)))/90;

G           = zeros(nbar+1,1);
E           = exp(ystar-(nbar-1)*dx+dx*(0:nbar));

G(nbar+1)   = D/24    - Cc1*E(nbar+1);
G(nbar)     = .5*D    - Cc2*E(nbar);
G(nbar-1)   = 23*D/24 - Cc3*E(nbar-1);
G(1:nbar-2) = D       - Cc4*E(1:nbar-2);

Val = C_aN*exp(-r*T)*sum(beta(1:nbar+1).*G);
if call==1  %Call Option, use Put-Call-Parity
    if r - q == 0
        mult = M + 1;
    else
        mult = (exp((r-q)*T*(1+1/M))-1)/(exp((r-q)*dt)-1);
    end
    Val = Val + C*exp(-r*T)*mult - W*exp(-r*T);
end
Val = max(0, Val);

end

function price = european_price(zeta,PhiR,xi,dx, r, q, T, W, S_0, c1, N, a, call)

W = 2*W - S_0;  % we shift the strike then divide by 2 at the end, this is asian with M=2

lws = log(W/S_0);
lam = c1 -(N/2 -1)*dx;
nbar = floor(a*(lws-lam)+1);
if nbar>=N
    nbar = N-1;
end
xmin = lws - (nbar-1)*dx;

Cons = 32*a^4;

beta  = [1/Cons; zeta.*PhiR(2:N).*exp(-1i*xmin*xi)];   %grand(end)=.5*grand(end);
beta  = real(fft(beta)).';

G = zeros(1,nbar +1);
G(nbar +1) = W*(1/24 - 1/20*exp(dx)*(exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 7*exp(-dx)/27));

G(nbar )   =  W*(.5 -.05*(28/27 + exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 14*exp(-dx)/27 ...
            + 121/54*exp(-.75*dx) + 23/18*exp(-.5*dx) + 235/54*exp(-.25*dx)));

G(nbar -1) = W*( 23/24 - exp(-dx)/90*( (28 + 7*exp(-dx))/3 ...
            + ( 14*exp(dx) + exp(-7/4*dx) + 242*cosh(.75*dx) + 470*cosh(.25*dx))/12 ...
            +.25*(exp(-1.5*dx) + 9*exp(-1.25*dx) + 46*cosh(.5*dx))) );

G(1: nbar -2) = W - S_0*exp(xmin +dx*(0:nbar-3))/90*( 14/3*(2+cosh(dx)) ...
                + .5*(cosh(1.5*dx) + 9*cosh(1.25*dx) +23*cosh(.5*dx))...
                +  1/6*(cosh(7/4*dx) + 121*cosh(.75*dx) +235*cosh(.25*dx)));

if call == 1  % Use put-call parity
    price = Cons*exp(-r*T)/N*G*(beta(1:length(G))') + S_0*exp(-q*T) - W*exp(-r*T);
else
    price = Cons*exp(-r*T)/N*G*(beta(1:length(G))');
end

price = 0.5*max(price, 0);  % Protect against deep out of money case

end


function [T, DISTR, df, Ybar, S] = symNormalTest2(TEST_TYPE, Ybar1, S1, n1, Ybar2, S2, n2, COV_TYPE, COV_DIFF)

% Two-sample tests for symmetric matrices.
%
%   [T, DISTR, df, Ybar, S] = symNormalTest2(TEST_TYPE, Ybar1, S1, n1, Ybar2, [S2], [n2], [COV_TYPE], [COV_DIFF])
%
% Input:
%   TEST_TYPE   Controls the type of test (assuming equal variances
%               between the two groups):
%                   'full': H0: both groups have the same mean tensors.
%                   'val' : H0: both groups have the same eigenvalues,
%                           with possibly different unknown eigenvectors.
%                   'vec' : H0: both groups have the same eigenvectors,
%                           with common unknown eigenvalues.
%   Ybar1, Ybar2  pxpx[sz] arrays of mean matrices for each group
%   S1, S2        qxqx[sz] arrays of covariance matrices for each group, q=p(p+1)/2
%                      (default S2 = 0)
%   n1, n2        Number of subjects in each group (default n2 = 1).
%   COV_TYPE      Type of covariance: 'spherical', 'orth-inv' or 'full' (default).
%   COV_DIFF      Assume different covariances: 1 (yes - default) or 0 (no)
% Output:
%   T           [sz] array of test statistics
%   DISTR       'f' or 'gamma'
%   df          2x1 degrees of freedom of F, or 2x[sz] parameters of Gamma (nu/2, 2a)
%   Ybar        pxpx[sz] array of pooled mean matrices
%   S           qxqx[sz] array of pooled covariance matrices
%
% E.g.:
%   [Ybar1, S1, n1] = symNormalStats(Y(:,:,g1));
%   [Ybar2, S2, n2] = symNormalStats(Y(:,:,g2));
%   [T, DISTR, df] = symNormalTest2('vec', Ybar1, S1, n1, Ybar2, S2, n2);
%
% Copyright by Armin Schwartzman, 2009

% HISTORY:
%   2008.12.30 ASH (armins@hsph.harvard.edu) wrote it.
%

% Check inputs
if ~exist('S2'),
    S2 = 0;
end
if ~exist('n2'),
    n2 = 1;
end
if (size(Ybar1,1) ~= size(Ybar1,2) | size(S1,1) ~= size(S1,2) | ...
    size(Ybar2,1) ~= size(Ybar2,2) | size(S2,1) ~= size(S2,2)),
    error('Wrong input format');
end
if (size(Ybar1,1) ~= size(Ybar2,1) | size(S1,1) ~= size(S2,1)),
    error('Wrong input format');
end
if ~exist('COV_TYPE'), COV_TYPE = 'full'; end
if (~strmatch(COV_TYPE,'spherical') & ~strmatch(COV_TYPE,'rot-inv') & ~strmatch(COV_TYPE,'full')),
    error('Only spherical, rot-inv and full covariance types supported.')
end
if ~exist('COV_DIFF'), COV_DIFF = 1; end

% Constants
n = n1 + n2;
p = size(Ybar1, 1);
q = size(S1, 1);
if (q ~= p*(p+1)/2),
    error('Wrong input format');
end

% Pooled mean and covariance
Ybar = (n1*Ybar1 + n2*Ybar2)/n;
S = ((n1-1)*S1 + (n2-1)*S2)/(n-2);

% Test type
switch TEST_TYPE,
    case 'full',
        d = permute(vecd(Ybar1 - Ybar2), [1 ndims(Ybar1)+1 2:ndims(Ybar1)]);
        switch COV_TYPE,
        case 'spherical',
            T = n1*n2/n * ndfun('mult', permute(d, [2 1 3:ndims(d)]), d);
            DISTR = 'f';
            if COV_DIFF,
                error('Full test, different spherical cov., not implemented.')
            end
            s2 = S(1,1,:);
            df = [q; q*(n-2)];
            T = df(2)/df(1) * T./(q*(n-2)*s2);
        case 'orth-inv',
            error('Full test, orth-inv cov., not implemented.')
        case 'full',
            if COV_DIFF,
                S = S1/n1 + S2/n2;
                Sinv = ndfun('inv', S);
                T = ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv, d));
                Sinv1 = ndfun('mult', Sinv, ndfun('mult', S1/n1, Sinv));
                T1 = ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv1, d));
                Sinv2 = ndfun('mult', Sinv, ndfun('mult', S2/n2, Sinv));
                T2 = ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv2, d));
                m = 1./((T1./T).^2/(n1-1) + (T2./T).^2/(n2-1));
                df(2,:) = m-q+1; df(1,:) = q;
                DISTR = 'f';
                T = (m-q+1)./(q*m) .* T;
                df = shiftdim(df);
            else
                Sinv = ndfun('inv', S);
                T = n1*n2/n * ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv, d));
                df = [q; n-q-1];
                DISTR = 'f';
                T = df(2)./df(1) .* T./(n-2);
            end
        end

    case 'val',
        [V1,L1] = ndSymEig(Ybar1); % [V1,L1] = ndfunm('eig', Ybar1);
        [V2,L2] = ndSymEig(Ybar2); % [V2,L2] = ndfunm('eig', Ybar2);
        T = n1*n2/n * sum(sum((L1 - L2).^2, 1), 2);
        switch COV_TYPE,
        case 'spherical',
            DISTR = 'f';
            if COV_DIFF,
                error('Eigval test, different spherical cov., not implemented.')
            end
            s2 = S(1,1,:);
            df = [p; q*(n-2)];
            T = df(2)/df(1) * T./(q*(n-2)*s2);
        case 'orth-inv',
            error('Full test, orth-inv cov., not implemented.')
        case 'full',
            Omega = 0;
            for i=1:p,
                W = cat(1, v(i,i,V1), -v(i,i,V2));
                Omega = Omega + ndfun('mult', W, permute(W, [2 1 3:ndims(W)]));
            end
            Omega = (n1*n2)/n * Omega;
            SS = zeros(size(Omega));  % (2q)x(2q)x[]
            if COV_DIFF,
                SS(1:q,1:q,:) = S1(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S2(1:q,1:q,:)/n2;
            else
                SS(1:q,1:q,:) = S(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S(1:q,1:q,:)/n2;
            end
            [a,nu] = chi2approx(SS, Omega);
            DISTR = 'gamma';
            df = cat(1, nu/2, 2*a);  % shape parameter, scale parameter
        end

    case 'vec',
        [V1,L1] = ndSymEig(Ybar1); % [V1,L1] = ndfunm('eig', Ybar1);
        [V2,L2] = ndSymEig(Ybar2); % [V2,L2] = ndfunm('eig', Ybar2);
        T = 2*n1*n2/n * (sum(sum(L1.*L2, 1), 2) - sum(sum(Ybar1.*Ybar2, 1), 2));
        switch COV_TYPE,
        case 'spherical',
            DISTR = 'f';
            if COV_DIFF,
                error('Eigvec test, different spherical cov., not implemented.')
            end
            s2 = S(1,1,:);
            df = [q-p; q*(n-2)];
            T = df(2)/df(1) * T./(q*(n-2)*s2);
        case 'orth-inv',
            error('Eigvec test, orth-inv cov., not implemented.')
        case 'full',
            Omega = 0;
            for i=1:p,
                for j=1:p,
                    W = omega(i,j,n1,n2,V1,V2);
                    Omega = Omega + ndfun('mult', W, permute(W, [2 1 3:ndims(W)]));
                end
            end
            Omega = (n1*n2)/n * Omega;
            SS = zeros(size(Omega));  % (2q)x(2q)x[]
            if COV_DIFF,
                SS(1:q,1:q,:) = S1(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S2(1:q,1:q,:)/n2;
            else
                SS(1:q,1:q,:) = S(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S(1:q,1:q,:)/n2;
            end
            [a,nu] = chi2approx(SS, Omega);
            DISTR = 'gamma';
            df = cat(1, nu/2, 2*a);  % shape parameter, scale parameter
        end
end

% Adjust output
T = shiftdim(T, 2);

end

%------------------------------------------------------------------------
% Auxiliary functions

function v = v(i,j,U)
    Ui = zeros(size(U));
    Ui(:,1,:) = U(:,i,:);
    Uj = zeros(size(U));
    Uj(1,:,:) = U(:,j,:);
    v = ndfun('mult', Ui, Uj);
    v = vecd((v + permute(v, [2 1 3:ndims(v)]))/2);
    v = permute(v, [1 ndims(v)+1 2:ndims(v)]);
end

function W = omega(i,j,n1,n2,U1,U2)
    sz = size(U1);
    p = sz(1);
    v1 = v(j, i, permute(U1, [2 1 3:ndims(U1)]))/2;
    v2 = v(j, i, permute(U2, [2 1 3:ndims(U1)]))/2;
    b = zeros([p 1 sz(3:end)]);
    b(1:p,:) = (n1*v2(1:p,:) + n2*v1(1:p,:))/(n1+n2);
    Eji = zeros(size(U1)); Eji(j,i,:) = 1/2;  Eji(i,j,:) = 1/2;
    Eji = permute(vecd(Eji), [1 ndims(Eji)+1 2:ndims(Eji)]);
    w1 = Eji - ndfun('mult', B(U1), b);
    w2 = Eji - ndfun('mult', B(U2), b);
    W = cat(1, w1, -w2);
end

function B = B(U)
    sz = size(U);
    p = sz(1);
    q = p*(p+1)/2;
    B = zeros([q p sz(3:end)]);
    for i=1:p,
        vv = v(i,i,U);
        B(:,i,:) = vv(1:q,1,:);
    end
end

function [a,nu] = chi2approx(S,Q)
    SQ = ndfun('mult',S,Q);
    k1 = ndfunm('trace',SQ);
    k2 = ndfunm('trace',ndfun('mult',SQ,SQ));
    a = k2./k1; nu = k1.^2./k2;
end

%------------------------------------------------------------------------
% Debugging
% M = zeros(3);
% S = eye(6);
% Y = symNormalRnd(M, S, [100 4]);
% [Ybar1, S1, n1] = symNormalStats(Y(:,:,1:40,:), 'full');
% [Ybar2, S2, n2] = symNormalStats(Y(:,:,41:100,:), 'full');
% [T, DISTR, df, Ybar, S] = symNormalTest2('val', Ybar1, S1, n1, Ybar2, S2, n2)


function [f] = elec_fit_sphere_optim(r, X, Y, Z, xo, yo, zo)

% elec_fit_sphere_optim - Optimization for elec_fit_sphere.m
%
% Called from elec_fit_sphere.m
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:55 $

% Licence:  GNU GPL, no implied or express warranties
% History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% with center (Xo,Yo,Zo) and radius r, the equation of a sphere is:
%
% r^2 = (x-xo)^2  +  (y-yo)^2  +  (z-zo)^2
%
% This function below creates a scalar value to
% return to the fminsearch function in elec_fit_sphere.

S = (X-xo).^2  +  (Y-yo).^2  +  (Z-zo).^2  -  r^2;

f = sum( S.^2 );


function owens_test01 ( )

%*****************************************************************************80
%
%% TEST01 demonstrates the use of T.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01:\n' );
  fprintf ( 1, '  T evaluates Owen''s T function.\n' );
  fprintf ( 1, '  Compare to tabulated values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          H            A        ' );
  fprintf ( 1, 'T                         T\n' );
  fprintf ( 1, '                                ' );
  fprintf ( 1, '(Tabulated)               (T)                     DIFF\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, h, a, t1 ] = owen_values ( n_data );

    if ( n_data == 0 )
      break
    end

    t2 = t ( h, a );

    fprintf ( 1, '  %12.8f  %12.8f  %24.16e  %24.16e  %10.4e\n', ...
    h, a, t1, t2, abs ( t1 - t2 ) );

  end

  return
end


function p = pscale(p,alpha,var)
%PSCALE       scale of argument by alpha
%
%For a polynomial p and scalar alpha, scaling with repect to "var".
%
%  call:     q = pscale(p,alpha,var)
%  result:   q{x} = p{alpha*x}    for x denoting the dependent variable (or var).
%
%For univariate polynomial p the parameter "var" is optional (default: dependent variable).
%
%If p does not depend on var, function is executed, possibly with warning, or an error message
%  is given depending on setting of 'AccessVariable', see polynominit
%
%Execution with error bounds in case p or alpha of type intval
%

% written  07/17/02     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if size(p.e,2)<=1                               % univariate polynomial
    if ( nargin==3 ) & ~isequal(var,p.v)          % polynomial not depending on specified variable
      INTLAB_POLYNOM_ACCESS_VARIABLE = getappdata(0,'INTLAB_POLYNOM_ACCESS_VARIABLE');
      switch INTLAB_POLYNOM_ACCESS_VARIABLE
        case 1, warning('polynomial does not depend on specified variable')
        case 2, error('polynomial does not depend on specified variable')
      end
      if rndold
        setround(rndold)
      end
      return
    end
    n = p.e;                                      % degree of polynomial
    p.c = ( alpha.^(n:-1:0) ) .* p.c ;
  else                                            % multivariate polynomial
    if ~ischar(var)                               % dependent variable must be string
      error('dependent variable must be string')
    end
    index = find(strcmp(p.v,var));                % index of dependent variable
    if isempty(index)                             % polynomial not depending on specified variable
      INTLAB_POLYNOM_ACCESS_VARIABLE = getappdata(0,'INTLAB_POLYNOM_ACCESS_VARIABLE');
      switch INTLAB_POLYNOM_ACCESS_VARIABLE
        case 1, warning('polynomial does not depend on specified variable')
        case 2, error('polynomial does not depend on specified variable')
      end
      if rndold
        setround(rndold)
      end
      return
    end
    p.c = ( alpha.^p.e(:,index) ) .* p.c;
  end

  if rndold
    setround(rndold)
  end


function tec_write ( tec_file_name, dim_num, node_num, element_num, ...
  element_order, node_data_num, node_coord, element_node, node_data )

%*****************************************************************************80
%
%% TEC_WRITE writes finite element data to a TEC file.
%
%  Discussion:
%
%    This program writes the node, element and data files that define
%    a finite element geometry and data based on that geometry:
%    * a set of nodes,
%    * a set of elements based on those nodes,
%    * a set of data values associated with each node.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string TEC_FILE_NAME, the name of the TEC file.
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer NODE_DATA_NUM, the number of data items per node.
%
%    Input, real NODE_COORD(DIM_NUM,NODE_NUM), the coordinates of nodes.
%
%    Input, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM);
%    the global index of local node I in element J.
%
%    Input, real NODE_DATA(NODE_DATA_NUM,NODE_NUM), the data values associated
%    with each node.
%
  tec_file_unit = fopen ( tec_file_name, 'wt' );
%
%  Write the title.
%
  fprintf ( tec_file_unit, 'TITLE = "%s"\n', tec_file_name );
%
%  Write the variable names.
%
  fprintf ( tec_file_unit, 'VARIABLES = ' );

  name = 'X';
  for dim = 1 : dim_num
    if ( dim == 1 )
      fprintf ( tec_file_unit, '"%s"', name );
    else
      fprintf ( tec_file_unit, ', "%s"', name );
    end
    name = s_inc ( name );
  end

  name = 'data_001';
  for dim = 1 : node_data_num
    fprintf ( tec_file_unit, ', "%s"', name );
    name = file_name_inc ( name );
  end

  fprintf ( tec_file_unit, '\n' );
%
%  Write the ZONE record.
%
  if ( dim_num == 2 & element_order == 3 )
    zonetype = 'FETRIANGLE';
  elseif ( dim_num == 2 & element_order == 4 )
    zonetype = 'FEQUADRILATERAL';
  elseif ( dim_num == 3 & element_order == 4 )
    zonetype = 'FETETRAHEDRON';
  elseif ( dim_num == 3 & element_order == 8 )
    zonetype = 'FEBRICK';
  else
    zonetype = 'FEUNKNOWN';
  end

  fprintf ( tec_file_unit, 'ZONE N = %d, E = %d, ', node_num, element_num );
  fprintf ( tec_file_unit, 'DATAPACKING = POINT, ZONETYPE = %s\n', ...
    zonetype );
%
%  Write the node coordinates and node data.
%
  for node = 1 : node_num
    for dim = 1 : dim_num
      fprintf ( tec_file_unit, '  %10f', node_coord(dim,node) );
    end
    for data = 1 : node_data_num
      fprintf ( tec_file_unit, '  %10f', node_data(data,node) );
    end
    fprintf ( tec_file_unit, '\n' );
  end
%
%  Write the element-node connectivity.
%
  for element = 1 : element_num
    for order = 1 : element_order
      fprintf ( tec_file_unit, '  %6d', element_node(order,element) );
    end
    fprintf ( tec_file_unit, '\n' );
  end

  fclose ( tec_file_unit );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEC_WRITE wrote all data to "%s".\n', ...
    tec_file_name );

  return
end


function testFMinSearchNew
% Test function to show two things:
%
% - how the newly-modified fminsearchbnd works
% - how to use output functions and plot functions in fminsearch or fminsearchbnd (or other Matlab
% optimization routines) - heretofore documentation and examples for these have been sparse.
%
% note: use the fminsearchbnd that was modified 2007-Nov-29 by Ken Purchase, which handles plot and
% output functions properly.
%

    % set up the function that returns a sim structure (the simulation function)
    optFn = @(x, varargin) 100*(x(2)-x(1)^2)^2 + (1-x(1))^2;

    x0 = [-1.2 1];
    mins = [-2 0]; %[2 -inf];
    maxs = [Inf Inf]; %[inf 3];

    % I'm going to pass in a useless extra parameter, just to show that fminsearchbnd and fminsearch
    % pass this back thru to your optimization function.  Your function could make use if this if
    % you watned.
    extraParams = 1;

    % Set up optimization options - you can leave any of these blank and fminsearch will use
    % defaults.
    searchOptions = struct(...
        'Display','none',...
        'MaxIter','200*numberOfVariables',...
        'MaxFunEvals','200*numberOfVariables',...
        'TolX',1e-6,...
        'TolFun',1e-6, ...
        'FunValCheck','off',...
        'OutputFcn', @firstOutputFunction,...
        'PlotFcns',@firstPlotFunction);
    % NOTE: you could add several output or plot functions by incluing a cell array of function
    % handles, such as {@firstOutputFunction, @secondOutputFunction}

    % Run the optimization:
    [outX,fval,exitflag,output] = fminsearchbnd(optFn, x0, mins, maxs, searchOptions, extraParams);

    % Finally, re-run the best case thru the simulation function and display result:
    outX
    finalValue = optFn(outX, extraParams)

    % Define output and print functions.  These functions are nexted WITHIN the overall routine so they
    % have access to variables in the above code if needed.
    %

    function stop = firstOutputFunction(xOutputfcn, optimValues, state, varargin)
        % create an output function for the fMinSearch
        %
        % inputs:
        % 1) xOutputfcn = the current x values
        % 2) optimValues - structure having:
        %         optimValues.iteration = iter;  % iteration number
        %         optimValues.funccount = numf;  % number of function eval's so far
        %         optimValues.fval = f;          % value of the function at current iter.
        %         optimValues.procedure = how;   % how is fminsearch current method (expand, contract, etc)
        % 3) State = 'iter','init' or 'done'     % where we are in the fminsearch algorithm.
        % 4) varargin is passed thru fminsearch to the user function and can be anything.
        %

        stop = false;

        % NOTE: this makes a bit of a messy display, but shows what you can do with an output
        % function.  You can get much of the same information using the fminsearch input option
        % 'Display', 'iter'
        disp(sprintf('Iteration: %d,  Evals: %d,  Current Min Value: %d', ...
            optimValues.iteration, optimValues.funccount, optimValues.fval));
        disp(['Best x so far: [' sprintf('%g ', xOutputfcn) ']']);

        % you could place plotting code here if you didn't want the automatic figure handling of the
        % plot functions.

        % you can also modify the value of 'stop' here to true if you want fminseach to terminate
        % based on any criteria you'd put here.
    end

    function stop = firstPlotFunction(xOutputfcn, optimValues, state, varargin)
        % create an print function for the fMinSearch
        %
        % NOTE: The plot functions do their own management of the plot and axes - if you want to
        % plot on your own figure or axes, just do the plotting in the output function, and leave
        % the plot function blank.
        %
        % One thing the plot function DOES have it that it installs STOP and PAUSE buttons on the
        % plot that allow you to interrupt the optimization to go in and see what's going on, and
        % then resume, or stop the iteration and still have it exit normally (and report output
        % values, etc).
        %
        % inputs:
        % 1) xOutputfcn = the current x values
        % 2) optimValues - structure having:
        %         optimValues.iteration = iter;  % iteration number
        %         optimValues.funccount = numf;  % number of function eval's so far
        %         optimValues.fval = f;          % value of the function at current iter.
        %         optimValues.procedure = how;   % how is fminsearch current method (expand, contract, etc)
        % 3) State = 'iter','init' or 'done'     % where we are in the fminsearch algorithm.
        % 4) varargin is passed thru fminsearch to the user function and can be anything.
        %

        stop = false;

        hold on;
        % this is fun - it simply plots the optimization variable (inverse figure of merit) as it
        % goes along, so you can see it improving, or stop the iterations if it stagnates.
        rectangle('Position', ...
            [(optimValues.iteration - 0.45) optimValues.fval, 0.9, 0.5*optimValues.fval]);
        set(gca, 'YScale', 'log');

        % when you run this, try pressing the 'stop' or 'pause' buttons on the plot.

        % you can add any code here that you desire.

    end

end % end of test code