classdef IMMOEA_F2 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = (1+5*repmat(2:obj.D,size(X,1),1)/obj.D).*X(:,2:obj.D) - repmat(X(:,1),1,obj.D-1);
            g = 1 + 9*mean(t.^2,2);
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-(PopObj(:,1)./g).^2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^2;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function [SD] = Demodulator(RxIn, PN, MF, Walsh);
%
% DEMODULATOR			This function performs demodulation of the forward
%                                       Channel packet, based on RAKE Receiver
% 						Block Diagram
%						Input Signal -> [Matched Filter] -> [Sampler] -> [RAKE Receiver] -> [Walsh] -> [DeSpreading]
%
% 						Inputs: RxIn - input signal (I/Q) analoge
%                                    PN - PN sequence (used for De-spreading)
%                                    MF - matched filter taps
%                                    Walsh - Used row of Walsh matrix for  recovering
%
% 						Outputs: SD - Soft Decisions of RAKE receiver
%

global R
N = length(RxIn)/R;

%--------------- Matched Rx Filter (Analog) --------------------
L = length(MF);
L_2 = floor(L/2);
rr = conv(flipud(conj(MF)), RxIn);
rr = rr(L_2+1: end - L_2);

%----------- Rx Symbols Sampling ------------
% R = 1.2288 Mcps
Rx = sign(real(rr(1:R:end))) + j*sign(imag(rr(1:R:end)));

%----------- RAKE Receiver ------------
Rx = reshape(Rx, 64, N/64); 				% -------- column oriented

%-------- Walsh recovering ---------
Walsh = ones(N/64, 1)*sign(Walsh'-1/2);	%--- row oriented Walsh
PN = reshape(PN, 64, N/64)'; 					%--- conjugated row oriented PN sequence
PN = PN.*Walsh;									%--- Walsh Orthogonalization

%---------- Despreading (Correlate and Sum)
% Input Rate = 1.2288 Mpbs, Output Rate = 19.2 KBps
SD= PN*Rx;
SD= real(diag(SD));  % Find Soft Decisions (on main diagonal)


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR:  SSD versus discretization width (midpoint quadrature rule)
%
% - load data (setup2DHNSPData)
% - interpolate on various grids (splineInter)
% - compute SSD and plot it
%==============================================================================

clear; help(mfilename);

setup2DHNSPData; clf; h = []; Q = [];
imgModel('reset','imgModel','splineInter');
[T,R] = imgModel('coefficients',dataT,dataR,omega,'out',0);
for j=1:10,
  m    = 2^j*[1,1];
  h(j) = prod((omega(2:2:end)-omega(1:2:end))./m);
  xc   = getCellCenteredGrid(omega,m);
  res  = imgModel(T,omega,xc) - imgModel(R,omega,xc);
  psi  = 0.5*h(j)*res'*res;
  Q(j) = psi;
end;
figure(1); clf; p1=semilogx(h/h(1),Q+eps,'kx',h/h(1),Q,'k-');
%==============================================================================


function [block] = spm_vb_gamma(Y,block)
% Variational Bayes for GLMAR model - Update gamma and get w_dev, wk_mean
% FORMAT [block] = spm_vb_gamma(Y,block)
%
% Y      - [T x N] time series
% block  - data structure (see spm_vb_glmar)
%__________________________________________________________________________
% Copyright (C) 2005-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny and Nelson Trujillo-Barreto
% $Id: spm_vb_gamma.m 6079 2014-06-30 18:25:37Z spm $

if block.verbose
    disp('Updating gamma');
end

N  = block.N;
k  = block.k;
k  = block.k;

Bk = kron(diag(block.mean_alpha),block.Dw);
B  = block.Hw*Bk*block.Hw';

for n=1:N
    % Block matrices Bnn [k x k] and Bni [k x k*(N-1)]
    subblock_n           = [(n-1)*k+1:n*k];
    Bnn                  = B(subblock_n,subblock_n);
    % Equation 17 in paper VB2
    for j=1:k
        block.gamma(j,n) = 1-block.w_cov{n}(j,j)*Bnn(j,j);
        block.b(j,n)     = Bnn(j,j);
    end
    % Record Standard Deviation of parameter estimates
    % to be used in Taylor series approximation to posterior
    block.w_dev(:,n)     = sqrt(diag(block.w_cov{n}));
end
block.gamma_tot          = sum(block.gamma,2);
block.wk_mean            = reshape(block.w_mean,k,N);


function [Index,MAX] = CpuGroup(numberOfGroups,xPrime,numberOfVariables)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    varsPerGroup = floor(numberOfVariables/numberOfGroups);
    if varsPerGroup == 1
        Index = linspace(1,numberOfVariables,numberOfVariables);
        MAX   = numberOfVariables;
    else
        B      = ones(1,varsPerGroup*numberOfGroups);
        remain = ones(1,(numberOfVariables-varsPerGroup*numberOfGroups))*(numberOfGroups+1);
        R      = reshape(B,varsPerGroup,numberOfGroups);
        k      = linspace(1,numberOfGroups,numberOfGroups);
        index  = R.*repmat(k,varsPerGroup,1);
        index  = reshape(index,1,varsPerGroup*numberOfGroups);
        INDEX  = [index remain];
        [~,I]  = sort(xPrime);
        Index(I) = INDEX;
        if(mod(numberOfVariables,numberOfGroups)==0)
            MAX = numberOfGroups;
        else
            MAX = numberOfGroups + 1;
        end
    end
end


function err = getHcurlerror3NE2(node,elem,curlE,Eh,markedElem)
%% GETHCURLERROR3NE2 Hcurl norm of approximation error for the quadratic (1st type) Nedelect element in 3-D.
%
% err = getHcurlerror3NE1(node,elem,curlE,Eh,markedElem);
%
% NOTE: it is identical to getHcurlerror3NE since the added basis has no
% contribution to the curl part.
%
% Example
%
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     maxIt = 4;
%     HcurlErr = zeros(maxIt,1);
%     N = zeros(maxIt,1);
%     for k = 1:maxIt
%         [node,elem] = uniformbisect3(node,elem);
%         [elem2dof,dofSign,edge] = dof3edge(elem);
%         pde = Maxwelldata2;
%         uI = edgeinterpolate1(pde.exactu,node,edge);
%         HcurlErr(k) = getHcurlerror3NE1(node,elem,pde.curlu,uI);
%         N(k) = length(uI);
%     end
%     r = showrate(N,HcurlErr,1,'b-+');
%     legend('||u-u_I||_{curl}',['N^{' num2str(r) '}'],'LOCATION','Best');
%
% See also getHcurlerror3NE1, getHcurlerror3NE2, getL2error3NE
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Sort elem to ascend ordering
elem = sort(elem,2);

%% Construct Data Structure
[Dlambda,volume] = gradbasis3(node,elem);
% elem2dof
[elem2edge,edge] = dof3edge(elem);
% elem2face = dof3RT0(elem);
[elem2face,face] = dof3face(elem);
NT = size(elem,1);  NE = size(edge,1); NF = size(face,1);
elem2dof = [elem2edge elem2edge+NE elem2face+2*NE elem2face+2*NE+NF];
% local indices
locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % phi
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % psi
               3 2 4; 3 1 4; 2 1 4; 2 1 3; ...
               4 2 3; 4 1 3; 4 1 2; 3 1 2]; % chi

%% compute H1 error element-wise using quadrature rule with order quadOrder
err = zeros(NT,1);
[lambda,w] = quadpts3(4);
nQuad = size(lambda,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);
    if isnumeric(curlE) % a constant vector
        curlEp = repmat(curlE,NT,1);
    else % function handel
        curlEp = curlE(pxy);
    end
    curlEhp = zeros(NT,3);
    % compute Ehp at quadrature points
    for k = 1:20
        k1 = locBasesIdx(k,1);
        k2 = locBasesIdx(k,2);
        k3 = locBasesIdx(k,3);
        if k<=6
            % curl phi = 2*Dlambda_i cross Dlambda_j;
            curlBasis_k = 2*cross(Dlambda(:,:,k1),Dlambda(:,:,k2),2);
        elseif k<=12
            % curl psi = 0;
            curlBasis_k = 0;
        else % chi = lambda_{i1}phi_{i2,i3}
            % curl chi =  Dlambda_{i1}cross phi_{i2,i3} + lambda_{i1}curl phi_{i2,i3}
            curlBasis_k = cross(Dlambda(:,:,k1),lambda(p,k2)*Dlambda(:,:,k3) ...
                                   -lambda(p,k3)*Dlambda(:,:,k2),2) ...
                        + lambda(p,k1)*2*cross(Dlambda(:,:,k2),Dlambda(:,:,k3),2);
        end
        curlEhp = curlEhp + repmat(Eh(elem2dof(:,k)),1,3).*curlBasis_k;
    end
    err = err + w(p)*volume.*sum((curlEp - curlEhp).^2,2);
end
% modify the error
err(isnan(err)) = 0; % remove the singular part
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % L2 error on some marked region
end
err = sqrt(sum(err));
%% TODO write more M-lint


function [stan,rtfr,hat] = tfrrstan(x,t,N,G,h,trace);
%TFRRSTAN Reassigned Stankovic distribution.
%	[TFR,RTFR,HAT] = TFRRSTAN(X,T,N,H,TRACE)
%	computes the Stankovic distribution and its reassigned version.
%
%	X     : analysed signal.
%	T     : the time instant(s)      (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%       G     : frequency averaging window
%	h     : stft window              (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR,  : time-frequency representation and its reassigned
%	RTFR    version. When called without output arguments,
%	        TFRRSTAN runs TFRQVIEW.
%	HAT   : Complex matrix of the reassignment vectors.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); t=1:2:128;
%	 G=tftb_window(9,'hanning'); h=tftb_window(61,'hanning'); tfrrstan(sig,t,128,G,h,1);
%
%	See also  all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, August, September 1997.
%  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 2 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, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (nargin <= 2),
 N=xrow;
end;

hlength=floor(N/4);
hlength=hlength+1-rem(hlength,2);

if (nargin == 1),
 t=1:xrow; G=[0.25; 0.5; 0.25]; h = tftb_window(hlength); trace=0;
elseif (nargin == 2)|(nargin == 3),
 G=[0.25; 0.5; 0.25]; h = tftb_window(hlength); trace=0;
elseif (nargin == 4),
 h = tftb_window(hlength); trace = 0;
elseif (nargin == 5),
 trace = 0;
end;

[Grow,Gcol]=size(G); LG=(Grow-1)/2;
if (Gcol~=1)|(rem(Grow,2)==0),
 error('G must be a smoothing window with odd length');
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol~=1),
 error('X must have only one column');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2;
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

if (tcol==1),
 Dt=1;
else
 Deltat=t(2:tcol)-t(1:tcol-1);
 Mini=min(Deltat); Maxi=max(Deltat);
 if (Mini~=Maxi),
  error('The time instants must be regularly sampled.');
 else
  Dt=Mini;
 end;
 clear Deltat Mini Maxi;
end;

stan = zeros(N,tcol);
rtfr = zeros(N,tcol);
hat  = zeros(N,tcol);

if trace, disp('Stankovic distribution (with reassignement)'); end;
Ex=mean(abs(x(min(t):max(t))).^2); Threshold=1.0e-3*Ex;
Dh=dwindow(h); Th=h.*[-Lh:Lh]';
for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 ti= t(icol);
 tau=-min([round(N/2)-1,Lh,ti-1]):min([round(N/2)-1,Lh,xrow-ti]);
 indices= rem(N+tau,N)+1;
 tfr= zeros(N,3);
 tfr(indices,1)=x(ti+tau).*conj( h(Lh+1-tau));
 tfr(indices,2)=x(ti+tau).*conj(Th(Lh+1-tau));
 tfr(indices,3)=x(ti+tau).*conj(Dh(Lh+1-tau));
 tfr=fft(tfr);
 stan(:,icol)=G(LG+1) * abs(tfr(:,1)).^2;

 for jcol=1:N,
  stanTh=G(LG+1)*tfr(jcol,1)*conj(tfr(jcol,2));
  stanDh=G(LG+1)*tfr(jcol,1)*conj(tfr(jcol,3));
  for kstan=1:min(N/2-1,LG),
   stanbefore=rem(rem(jcol-kstan-1,N)+N,N)+1;
   stanafter =rem(rem(jcol+kstan-1,N)+N,N)+1;
   stan(jcol,icol)= stan(jcol,icol) ...
                  + G(LG+1-kstan)*tfr(stanbefore,1)*conj(tfr(stanafter ,1)) ...
                  + G(LG+1+kstan)*tfr(stanafter ,1)*conj(tfr(stanbefore,1));
   stanTh= stanTh + G(LG+1-kstan)*tfr(stanbefore,1)*conj(tfr(stanafter ,2)) ...
                  + G(LG+1+kstan)*tfr(stanafter ,1)*conj(tfr(stanbefore,2));
   stanDh= stanDh + G(LG+1-kstan)*tfr(stanbefore,1)*conj(tfr(stanafter ,3)) ...
                  + G(LG+1+kstan)*tfr(stanafter ,1)*conj(tfr(stanbefore,3));
  end;
  stan(jcol,icol)=real(stan(jcol,icol));
  if abs(stan(jcol,icol))>Threshold,
   icolhat = round(icol - real(stanTh/stan(jcol,icol))/Dt);
   jcolhat = round(jcol - N*imag(stanDh/stan(jcol,icol)/(2.0*pi)));

   jcolhat= rem(rem(jcolhat-1,N)+N,N)+1;
   icolhat= min(max(icolhat,1),tcol);

   rtfr(jcolhat,icolhat)=rtfr(jcolhat,icolhat) + stan(jcol,icol) ;
   hat(jcol,icol)= jcolhat + j * icolhat;
   %fprintf('%12.3f %12.3f , %12.3f %12.3f \n',jcol,icol,jcolhat,icolhat);
  else
   rtfr(jcol,icol)=rtfr(jcol,icol) + stan(jcol,icol);
   hat(jcol,icol)= jcol + j * icol;
  end;
 end;

end ;

if trace, fprintf('\n'); end;

if (nargout==0),
 TFTBcontinue=1;
 while (TFTBcontinue==1),
  choice=menu ('Choose the representation:',...
               'stop',...
               'Stankovic distribution',...
               'reassigned Stankovic distribution');
  if (choice==1), TFTBcontinue=0;
  elseif (choice==2),
   tfrqview(stan,x,t,'type1');
  elseif (choice==3),
   tfrqview(rtfr,x,t,'type1');
  end;
 end;
end;


function [c, frec, info] = multidgtrealmp(f,dicts,varargin)
%MULTIDGTREALMP  Matching Pursuit Decomposition with Multi-Gabor Dictionary
%   Usage:  c = multidgtrealmp(f,dicts)
%           c = multidgtrealmp(f,dicts,errdb,maxit)
%           [c,frec,info] = multidgtrealmp(...)
%
%   Input parameters:
%       f        : Input signal
%       dicts    : Dictionaries. Format {g1,a1,M1,g2,a2,M2,...}
%       errdb    : Target normalized approximation error in dB
%       maxit    : Maximum number of iterations.
%   Output parameters:
%       c        : Sparse representation
%       frec     : Reconstructed signal
%       info     : Struct with additional output paramerets
%
%   `multidgtrealmp(f,{g1,a1,M1,g2,a2,M2,...,gW,aW,MW})` returns sparse
%   representation of a signal in `W` Gabor dictionaries using the
%   fast matching pursuit algorithm. `gw` is a Gabor window defined
%   as in |dgt| and |dgtreal|, `aw` is a hop factor, `Mw` is the number of
%   frequency channels. All `aw` and `Mw` must be divisible by `min(a1,...,aW)`.
%   The algorithm will try to reach -40 dB relative approximation error
%   in at most `numel(f)` iterations.
%   The function returns a cell-array with elements storing coefficients
%   for individual Gabor systems such that they can be directly used in
%   |idgtreal|.
%
%   `multidgtrealmp(f,dicts,errdb,maxit)` tries to reach normalized
%   approximation error *errdb* dB in at most *maxit* iterations.
%
%   `[c,frec,info] = multidgtrealmp(...)` in addition returns the
%   aproximation *frec* and a struct `info` with the following fields:
%
%     .iter     Number of iterations done.
%
%     .atoms    Number of atoms selected.
%
%     .relres   Final approximation error. If resets are enabled, this is a
%               vector additionally containing the approximation error at
%               every reset.
%
%     .g        Cell array of numeric windows used in the multi-dictionary
%
%     .a        Array of hop factors for indivitual dictionaries
%
%     .M        Array of numbers of channels for individual dictionaries
%
%     .synthetize  Anonymous function which can be used to synthetize from
%                  the (modified) coefficients as
%                  `frec = sum(info.synthetize(c),dim)`
%                  where `dim=2` if the input *f* was a column vector and
%                  `dim=1` if it was a row vector.
%
%   The normalized approximation error is computed as
%   `errdb=20*log10(norm(f-frec)/norm(f))`.
%
%   The function takes the following optional parameters at the end of
%   the line of input arguments:
%
%     'kenrnthr',kt     Kernel threshold. Must be in range ]0,1].
%                       Default is 1e-4.
%
%     'timeinv'         Use the time invariant phase convention. The
%                       default is 'freqinv'.
%
%     'pedanticsearch'  Be pedantic about the energy of pairs of
%                       conjugated atoms in the selection step.
%                       Disabled by default.
%
%     'algorithm',alg   Algorithm to use. Available:
%                       'mp'(default),'selfprojmp','cyclicmp'
%
%     'reset'           Reset the decomposition until stopping criteria
%                       are met. The reset means a complete synthesis and
%                       re-analysis. Available:
%                       'noreset' (default), 'reset', 'quickreset'
%                       Option 'quickreset' applies a heuristic to stop the
%                       algorithm more quickly when the targer error is
%                       reached.
%
%   Reset conditions can be controlled by the following key-value pairs:
%
%     'resetit',it      Reset the decomposition every `it` iteration.
%                       Must be a positive integer.
%
%     'reseterrdb',err  Reset the decomposition when the error estimate
%                       in dB decreases below `err`. Must be negative.
%                       Default is `10*log10(1.1*kv.kernthr)`.
%
%   The computational routine is only available in C. Use |ltfatmex| to
%   to compile it.
%
%   Algorithms
%   ----------
%
%   By default, the function uses the fast MP using approximate update
%   in the coefficient domain as described in::
%
%   "Z. Prusa, N. Holighaus, P. Balazs: Fast Matching Pursuit with Multi-Gabor Dictionaries"
%
%   The kernel threshold limits the minimum approximation error which
%   can be reached. For example the default threshold 1e-4 in general
%   allows achieving at least -40 dB.
%
%   Note that this algorithm internally computes the error as
%   `errdb = 10*log10(err(k)/norm(f)^2)`, with `err(0) = norm(f)^2` and
%   `err(k) = err(k-1) - abs(cselected(k))^2`. Here, `cselected(k)` is the
%   Gabor coefficient selected at the k-th MP step. This formula is exact
%   only if the kernel threshold equals 0. Otherwise it serves as a cheap
%   estimate. The exact error is only computed when this function is run
%   with resets enabled and only in between resets.
%
%   Examples
%   --------
%
%   The following example shows the decomposition in 3 dictionaries and
%   plots contributions from the individual dictionaries and the residual.:::
%
%       [f,fs] = gspi;
%       [c, frec, info] = multidgtrealmp(f,...
%       {'blackman',128,512,'blackman',512,2048,'blackman',2048,8192});
%       frecd = info.synthetize(c);
%       figure(1);
%       xvals = (0:numel(f)-1)/fs;
%       subplot(4,1,1); plot(xvals,frecd(:,1));ylim([-0.5,0.5]);
%       subplot(4,1,2); plot(xvals,frecd(:,2));ylim([-0.5,0.5]);
%       subplot(4,1,3); plot(xvals,frecd(:,3));ylim([-0.5,0.5]);
%       subplot(4,1,4); plot(xvals,f-frec);ylim([-0.5,0.5]); xlabel('Time (s)');
%
%   See also: dgtreal idgtreal
%
%   References: ltfatnote052 mazh93 stchr10 rerose17

%AUTHOR: Zdenek Prusa

thismfile = upper(mfilename);
complainif_notenoughargs(nargin,2,thismfile);
% Define initial value for flags and key/value pairs.
definput.keyvals.errdb=-40;
definput.keyvals.maxit=[];
definput.keyvals.resetit=0;
definput.keyvals.reseterrdb = 0;
%definput.keyvals.iterstep=[];
definput.keyvals.kernthr = 1e-4;
%definput.flags.print={'quiet','print'};
definput.flags.algversion={'fast','slow'};
definput.flags.algorithm={'mp','selfprojmp','cyclicmp'};
definput.flags.search={'plainsearch','pedanticsearch'};
definput.flags.phaseconv={'freqinv','timeinv'};
definput.flags.reset={'noreset','reset','quickreset'};

[flags,kv]=ltfatarghelper({'errdb','maxit'},definput,varargin);

if exist('comp_multidgtrealmp','file') ~= 3 && flags.do_fast
    error(['%s: MEX/OCT file is missing. Either compile the MEX/OCT ',...
           'interfaces or re-run the function with ''slow'''], thismfile);
end

if flags.do_slow
    error('%s: ''slow'' is not supported yet.',thismfile)
end

if flags.do_reset || flags.do_quickreset % Check reseterrdb or resetit
   if kv.reseterrdb == 0
        if kv.resetit == 0 % Set both to conservative default values depending on kernthr
            kv.reseterrdb = 10*log10(1.1*kv.kernthr);
            kv.resetit = round(1000/sqrt(kv.kernthr));
        else % If only resetit is given, ignore reseterrdb
            kv.reseterrdb = -flintmax;
        end
   elseif kv.resetit == 0 % If only reseterrdb is given, ignore resetit
        kv.resetit = flintmax;
   end
   if kv.reseterrdb >= 0
       error('%s: Reset error tolerance must be negative.',thismfile);
   end
   if kv.resetit < 0 || kv.resetit ~= round(kv.resetit)
       error('%s: Number of iterations before reset must be a positive integer.',thismfile);
   end
end

%% ----- step 1 : Verify f and determine its length -------
% Change f to correct shape.
[f,~,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,[],[],upper(mfilename));

if W>1
    error('%s: Input signal can be single channel only.',upper(mfilename));
end

if kv.errdb > 0
    error('%s: Target error must be lower than 0 dB.',upper(mfilename));
end

if ~(kv.kernthr > 0 && kv.kernthr <= 1)
    error('%s: Kernel threshold must be in range ]0,1].',upper(mfilename));
end

if ~iscell(dicts), error('%s: dicts must be cell',thismfile); end
if rem(numel(dicts),3) ~= 0 || ~all(cellfun(@(x)isscalar(x), dicts([2:3:end,3:3:end])))
    error('%s: bad format of dicts. Check {g1,a1,M1,g2,a2,M2,...,gW,aW,MW}',...
        thismfile);
end

dictno = numel(dicts)/3;
gin = dicts(1:3:end);
a = cell2mat(dicts(2:3:end));
M = cell2mat(dicts(3:3:end));

if any(rem(M,a) ~= 0) || any(M./a<2)
    error(['%s: Only integer oversampling greater than 1 is allowed ',...
           'i.e. M/a must be an integer>=2.'],...
    upper(mfilename));
end

if dictno > 1
    asort  = sort(a);
    Msort  = sort(M);
    if any(rem(asort(2:end),asort(1:end-1)) ~= 0)
        error('%s: all au and av must be divisible by min(au,av)',thismfile);
    end
    if any(rem(Msort(2:end),Msort(1:end-1)) ~= 0)
        error('%s: all Mu and Mv must be divisible by min(Mu,Mv)',thismfile);
    end
end

info.a = a;
info.M = M;
info.iter = 0;
info.relres = [];
fnorm = norm(f);

L = filterbanklength(Ls,[a(:);M(:)]);
if isempty(kv.maxit), kv.maxit = L; end

info.g = cell(dictno,1);
for dIdx = 1:dictno
    info.g{dIdx} = setnorm(gabwin(gin{dIdx},a(dIdx),M(dIdx),L),'2');
end

for dIdx = 1:dictno
    condnum = gabframebounds(info.g{dIdx},a(dIdx),M(dIdx));
    if condnum > 1e3
        error('%s: Dictionary %d is badly conditioned.',dIdx,upper(mfilename));
    end
end

% Initial residuum
fpad = postpad(f,L);

% Determine reset/no reset mode and run MP decomposition
if flags.do_noreset
    [c,info.atoms,info.iter,info.status] = ...
        comp_multidgtrealmp(fpad,info.g,a,M,flags.do_timeinv,...
                            kv.kernthr,kv.errdb,kv.maxit,kv.maxit,...
                            flags.do_pedanticsearch, flags.algorithm );
else
    c = cell(1,dictno);
    for dIdx = 1:dictno
        c{dIdx} = zeros(floor(M(dIdx)/2) + 1, L/a(dIdx),class(f));
    end

    remainingit = kv.maxit;
    currerrdb = kv.errdb;

    info.resets = -1;
    reseterrdb = kv.reseterrdb;

    while remainingit > 0 && currerrdb < 0
        info.resets = info.resets + 1;

        % Adjust number of iterations
        currit = min([remainingit,kv.resetit]);
        if flags.do_quickreset % Adjust reset error condition
            reseterrdb = max([1.2*currerrdb,kv.reseterrdb]);
        end

        [ctmp,info.atoms,info.iter,info.status] = ...
            comp_multidgtrealmp(fpad,info.g,a,M,flags.do_timeinv,...
                                kv.kernthr,reseterrdb,currit,currit,...
                                flags.do_pedanticsearch, flags.algorithm );

        c = cellfun(@(cEl,ctmpEl) cEl + ctmpEl, c(:)',ctmp(:)','UniformOutput', 0);

        remainingit = remainingit - info.iter;

        %The following means that some other stopping criterion has been reached
        if info.status ~=0 && info.status ~=3 && currit ~= info.iter, break; end

        if remainingit > 0
            % Recompute residuum
            frec = sum(cell2mat(cellfun(@(cEl,gEl,aEl,MEl) idgtreal(cEl,gEl,aEl,MEl,flags.phaseconv),...
                   c(:)',info.g(:)',num2cell(a(:))',num2cell(M(:))','UniformOutput',0)),2);

            fpad = postpad(f,L) - frec;
            relres = norm(fpad(:)) / fnorm;
            % Since we are changing the residual, we must adjust the target error
            currerrdb = kv.errdb - 20*log10(relres);
            if currerrdb < 0
                info.relres(info.resets+1) = relres;
            end
            if info.resets >= 1 && info.relres(end) > info.relres(end-1)
                info.status = 6;
                break;
            end
        end
    end

    %Fix the info
    info.atoms = sum(cellfun(@(cEl) numel(find( abs(cEl) > 0 )), c));
    info.iter = kv.maxit - remainingit;
end

if nargout>1
  permutedsize2 = permutedsize; permutedsize2(2) = dictno;
  info.synthetize = @(c) ...
      assert_sigreshape_post(...
      postpad(cell2mat(cellfun(@(cEl,gEl,aEl,MEl) idgtreal(cEl,gEl,aEl,MEl,flags.phaseconv),...
      c(:)',info.g(:)',num2cell(a(:))',num2cell(M(:))','UniformOutput',0)),Ls),...
      dim,permutedsize2,order);
  dim2 = 2;
  if dim == 2, dim2 = 1; end
  frec = sum(info.synthetize(c),dim2);
  if fnorm == 0
    info.relres(end+1) = 0;
  else
    info.relres(end+1) = norm(frec(:)-f(:))/fnorm;
  end

  % Fix the status
  if info.relres(end) <= 10^(kv.errdb/20)
      info.status = 0;
  elseif info.iter == kv.maxit
      info.status = 2;
  end

  status_str = {...
  'Target error reached',...
  'Maximum number of atoms reached',...
  'Maximum number of iterations reached',...
  'Stalled (abs. norm. error estimate became negative)',...
  'Selected coefficient tolerance reached',...
  'All zeros',...
  'Stalled (Residual has increased since last reset. Try to reduce resetit.)'...
  };
  info.message = status_str{1 + info.status};
end


 function fit = de_ftab_fit(sl, fm, varargin)
%function fit = de_ftab_fit(sl, fm, [options])
%|
%| Fit to DE table fm(), suitable for subsequent interpolation / extrapolation.
%| Uses either classic polynomial basis functions,
%| or an exponential model: -log(sum_k p_k exp(-m_k . s))
%|
%| in
%|	sl	{L}		sample locations for each of L materials
%|	fm	[s1 ... sL M]	DE tables f_m(s_1,...,s_L), size [(Ns) M]
%|
%| option
%|	'type'	'poly' | 'exp'	type of fit (default 'poly') todo: change?
%|	'wt'	{M}		fit weighting for each of M energies.
%|				(default depends on 'type'; see code)
%|	'show'	1|0		plot? (default false)
%|	'ntype' ''		options for negative sl values (default '')
%|				'abs' - experimental symmetrizing option
%| options for 'poly'
%|	'order'			polynomial order (default 3)
%|	'maxdegree'		argument for poly_string() (default [])
%| options for 'exp'
%|	'kev'	[Ne 1]		kev *for fitting* (default [10:5:200]')
%|	'mac'	[Ne L]		*for fitting* (this or mtype are required)
%|	'mtype' {L}		material types
%|	'macbar' [M L]		if nonempty, use it to match derivative at s=0.
%|	'wls_simplex_arg' {}	arguments for wls_simplex (default {})
%|
%| out
%|	fit	strum		strum object for fitted f_m(s_1,...,s_L)
%|				fit.coef is [nbasis M] for 'poly'
%|	methods:
%|				(sll denotes stacked array [(Ns) L] or [*Ns L])
%|	.fmfun(sll [() L])	fm function evaluation [() L] -> [() M]
%|	.fgrad(sll)		fm gradient evaluation [() L] -> [() L M]
%|	.fhess(sll)		fm hessian evaluation [() L] -> [() L L M]
%|	.show_sp([en, sp])	plot true spectrum vs fitted spectrum
%|	.show_fm(sl, [fm])	mesh plot of fm and its fit
%|	.show_err(sl, fm)	mesh plot of fit error
%|	.mac_eff		effective mass atten coef based on fit
%|				(valid for 'exp' only)
%|
%| Copyright 2006-3-3, Jeff Fessler, University of Michigan

if nargin == 1 && streq(sl, 'test'), de_ftab_fit_test, return, end
if nargin < 2, ir_usage, end

arg.show = false;
arg.type = '';
arg.mtype = {};
arg.ntype = ''; % how to handle negatives
arg.kev = [10:5:200]';
arg.mac = [];
arg.macbar = [];
arg.wls_simplex_arg = {}; % pass to wls_simplex which passes it to lsqlin
arg.order = 3;
arg.maxdegree = [];
arg.wt = {};
arg.dc = false; % default: exclude dc (constant) from (polynomial) fit
arg = vararg_pair(arg, varargin);
if isempty(arg.type), arg.type = 'poly'; end

LL = length(sl);

if ndims(fm) == LL
	MM = 1;
elseif ndims(fm) == LL + 1
	MM = size(fm,ndims(fm)); % last dimension is number of spectra
else
	error 'invalid fm size'
end

for ll=1:LL % make sure arguments match table dimensions
	if length(sl{ll}) ~= size(fm, ll)
		fail('dim mismatch %d', ll)
	end
end

switch arg.type
case 'poly'
	[fit fmfun fgrad fhess] = de_ftab_fit_poly(sl, fm, MM, arg.wt, ...
		arg.order, arg.maxdegree, arg.dc);

case 'exp'
	fit = de_ftab_fit_exp(sl, fm, MM, arg.wt, ...
		arg.mtype, arg.mac, arg.kev, arg.macbar, arg.wls_simplex_arg);
	fmfun = @de_ftab_fit_exp_eval;
	fgrad = @de_ftab_fit_exp_grad;
	fhess = @de_ftab_fit_exp_hess;
	fit.mac_eff = zeros(MM, LL); % [M L] mac effective
	for mm=1:MM
		fit.mac_eff(mm,:) = fit.mac{mm}' * fit.coef{mm};
	end

otherwise
	fail('unknown fit type %s', arg.type)
end

fit.LL = LL;
fit.MM = MM;
fit.type = arg.type;
fit.ntype = arg.ntype;

meth = {'fmfun', fmfun, '(sll [() L]) -> [() M]'; ...
	'fgrad', fgrad, '(sll [() L]) -> [() L M]'; ...
	'fhess', fhess, '(sll [() L]) -> [() L L M]'; ...
	'show_err', @de_ftab_fit_show_err, '(sl, fm)'; ...
	'show_fm', @de_ftab_fit_show_fm, '(sl, [fm])'; ...
	'show_sp', @de_ftab_fit_show_sp, '([en, sp])'; ...
	};
fit = strum(fit, meth);

if arg.show && im
	fit.show_fm(sl, fm);
end

end % de_ftab_fit()

%
% de_ftab_fit_poly()
%
function [fit, ffun, fgrad, fhess] = ...
	 de_ftab_fit_poly(sl, fm, MM, wt, order, maxdegree, dc)

sll = ndgrid_jf('mat', sl{:});

% for fitting, up weight the no-bone part
% because soft tissue is most prevalant
if isempty(wt)
	wt{1} = 1 ./ (stackpick(sll,2) + 1);
	wt{2} = wt{1};
end

fit.basis_order = order;
fit.basis_maxdegree = maxdegree;
fit.basis_dc = dc;

[fit.basis_func fit.basis_d1 fit.basis_d2] = ...
	ir_poly2_fun(order, 'maxdegree', maxdegree, 'dc', dc);

fit.nbasis = numel(fit.basis_func(0,0));

ss1 = col(stackpick(sll,1));
ss2 = col(stackpick(sll,2));
tmp = fit.basis_func(ss1, ss2); % [*Ns nbasis]
fit.coef = zeros(fit.nbasis, MM);
for mm=1:MM
	fit.coef(:,mm) = (diag_sp(wt{mm}(:)) * tmp) ...
		\ (wt{mm}(:) .* col(fm(:,:,mm)));
end

ffun = @(fit,sl) ...
	reshape([fit.basis_func(sl{1}(:), sl{2}(:)) * fit.coef(:,1); ...
		fit.basis_func(sl{1}(:), sl{2}(:)) * fit.coef(:,2)], ...
		[size(sl{1}) 2]);
ffun = @de_ftab_fit_poly_eval;
fgrad = @() error('not done');
fhess = @() error('not done');

end % de_ftab_fit_poly()

%
% de_ftab_fit_poly_eval()
%
function fm = de_ftab_fit_poly_eval(fit, sll)

% if fit.MM ~= 2
s1 = stackpick(sll,1);
s2 = stackpick(sll,2);
fm = [	fit.basis_func(s1(:), s2(:)) * fit.coef(:,1);
	fit.basis_func(s1(:), s2(:)) * fit.coef(:,2)];
fm = reshape(fm, [size(s1) 2]);

end % de_ftab_fit_poly_eval()

%
% de_ftab_fit_exp()
% todo: more documentation
% todo: use a set of exponents, not "mac"
%
function fit = de_ftab_fit_exp(sl, fm, MM, wt, mtype, mac, kev, macbar, ...
	wls_simplex_arg)

sll = ndgrid_jf('mat', sl{:}); % [(Ns)]

if isempty(mac)
	if isempty(mtype), error 'mac or mtype required', end
	mac = xray_read_atten(mtype, kev); % [Ne L]
else
	if size(mac,1) ~= length(kev), fail 'size mismatch', end
end

LL = length(sl);

if isempty(wt), wt = num2cell(ones(MM,1)); end

Ab = exp(-reshapee(sll, [], LL) * mac'); % [*Ns Ne] "over-complete" basis

fit.kev = cell(1,MM);
fit.mac = cell(1,MM);
fit.coef = cell(1,MM);
for mm=1:MM
	warg = wls_simplex_arg;
	if LL == 1 && ~isempty(macbar) % constrain deriv. at 0
		warg = {wls_simplex_arg{:}, 'inprodv', mac' / macbar(mm,1)};
	end

	if MM == 1 % todo: kludgy
		dat = fm;
	else
		dat = stackpick(fm,mm); % [(Ns)]
	end
	y = exp(-dat);
	Wh = spdiag(sqrt(wt{mm}(:)), 'nowarn');
	% initial coefficients for each candidate energy
	x = wls_simplex(Ab, y(:), Wh, [], warg{:}); % [Ne 1]

	ie = x > 1e-6; % find key energies
	fit.kev{mm} = kev(ie);
	fit.mac{mm} = mac(ie,:); % [Ne L] (now Ne may be smaller than before)

	warg = wls_simplex_arg;
	if LL == 1 && ~isempty(macbar) % constrain derivative at 0
		warg = {wls_simplex_arg{:}, 'inprodv', fit.mac{mm}' / macbar(mm,1)};
	end

	A = exp(-reshapee(sll, [], LL) * fit.mac{mm}'); % [*Ns Ne] final basis
	% final coefficients at key enerties:
	fit.coef{mm} = wls_simplex(A, y(:), Wh, [], warg{:}); % [Ne 1]
end

end % de_ftab_fit_exp()

%
% de_ftab_fit_exp_eval()
% evaluate
% in
%	sll	[(Ns) L]	stackup of s1,s2,...,s_L
% out
%	f	[(Ns) M]	stackup of f1,f2,...,f_M
%
function f = de_ftab_fit_exp_eval(fit, sll)
LL = fit.LL;
if LL == 1
	Ns = size(sll);
	if Ns(end) == 1, Ns = Ns(1:end-1); end
else
	Ns = size(sll); if LL ~= Ns(end), fail 'bug', end; Ns = Ns(1:end-1);
end
sll = reshapee(sll, [], LL); % [*Ns L]
MM = fit.MM;

persistent warned
if ~isvar('warned') || isempty(warned)
	warned = 0;
end

f = zeros(prod(Ns),MM);
for mm=1:MM

	switch fit.ntype
	case '' % standard
		A = exp(-sll * fit.mac{mm}'); % [*Ns Ne]
		tmp = -log(A * fit.coef{mm}); % [*Ns 1]
		f(:,mm) = tmp;

	case 'abs' % trick for negatives
		if LL == 1
			A = exp(-abs(sll) * fit.mac{mm}'); % [*Ns Ne]
			tmp = -log(A * fit.coef{mm}); % [*Ns 1]
			f(:,mm) = -sign(sll) .* log(A * fit.coef{mm}); % [*Ns 1]
%			bad = sll < 0;
%			f(bad,mm) = sll(bad) * fit.mac_eff(mm,1);
		else
			if ~warned
				warned = 1;
				warn 'negative trick not done for L>1'
			end
		end

	otherwise
		fail('bad ntype: %s', fit.ntype)
	end

end
f = reshape(f, [Ns MM]);

end % de_ftab_fit_exp_eval()

%
% de_ftab_fit_exp_grad()
% evaluate gradient of f for each of the given s vectors.
% in
%	sll	[(Ns) L]	stackup of s1,s2,...,s_L
% out
%	g	[(Ns) L M]	stackup of gradients of f(s)
%
function g = de_ftab_fit_exp_grad(fit, sll)
LL = fit.LL;
Ns = size(sll);
if LL == 1
	if Ns(end) == 1, Ns = Ns(1:end-1); end
else
	if LL ~= Ns(end); error 'bug', end
	Ns = Ns(1:end-1);
end
sll = reshape(sll, [], LL); % [*Ns L]
MM = fit.MM;

persistent warned
if ~isvar('warned') || isempty(warned)
	warned = 0;
end

g = zeros(prod(Ns), LL, MM);
for mm=1:fit.MM
	alf = fit.coef{mm}; % [Ne 1]
	mac = fit.mac{mm}; % [Ne L]
	Ne = length(alf); % # exponential terms in the fit

	switch fit.ntype
%	case '' % standard
	case 'abs'
		if any(sll(:) < 0) % trick for negatives
			if LL == 1
				sll = abs(sll);
%				bad = sll < 0;
%				g(bad,1,mm) = fit.mac_eff(mm,1);
			else
				if ~warned
					warned = 1;
					warn 'negative trick not done for L>1'
				end
			end
		end
	end

	A = exp(-sll * mac'); % [*Ns Ne]
	Q = A .* repmat(alf', [prod(Ns) 1]); % [*Ns Ne]
	denom = sum(Q,2); % [*Ns 1]
	Q = Q ./ repmat(denom, [1 Ne]);
	g(:,:,mm) = Q * mac;

end
g = reshape(g, [Ns LL MM]);

end % de_ftab_fit_exp_grad()

%
% de_ftab_fit_exp_hess()
% evaluate hessian of f for each of the given s vectors.
% in
%	sll	[(Ns) L]	stackup of s1,s2,...,s_L
% out
%	h	[(Ns) L L M]	stackup of hessians of f(s)
%
function h = de_ftab_fit_exp_hess(fit, sll)
Ns = size(sll); LL = Ns(end); Ns = Ns(1:end-1);
sll = reshape(sll, [], LL); % [*Ns L]
MM = fit.MM;

persistent warned
if ~isvar('warned') || isempty(warned)
	warned = 0;
end

h = zeros(prod(Ns), LL, LL, MM);
for mm=1:fit.MM
	mac = fit.mac{mm}; % [Ne L]
	alf = fit.coef{mm}; % [Ne 1]
	Ne = length(alf); % # exponential terms in the fit

	switch fit.ntype
%	case '' % standard
	case 'abs'
		if any(sll(:) < 0) % trick for negatives
			if LL == 1
				sll_sign = sign(sll);
				sll = abs(sll);
%				bad = sll < 0;
%				hm(bad,1,1) = 0;
			else
				if ~warned
					warned = 1;
					warn 'negative trick not done for L>1'
				end
			end
		end
	end

	A = exp(-sll * fit.mac{mm}'); % [*Ns Ne]

	Q = A .* repmat(alf', [prod(Ns) 1]); % [*Ns Ne]
	denom = sum(Q,2); % [*Ns 1]
	Q = Q ./ repmat(denom, [1 Ne]);
	g = Q * mac; % [*Ns L] gradient

	% loop over LL because it is smaller than *Ns or Ne
	hm = zeros(prod(Ns), LL, LL);
	for l1=1:LL
		for l2=1:LL
			hm(:,l1,l2) = g(:,l1) .* g(:,l2) ...
				- Q * (mac(:,l1) .* mac(:,l2));
		end
	end

	switch fit.ntype
%	case '' % standard
	case 'abs'
		if any(sll(:) < 0) % trick for negatives
			if LL == 1
				hm(:,1,1) = hm(:,1,1) .* sll_sign;
%				bad = sll < 0;
%				hm(bad,1,1) = 0;
			end
		end
	end

	h(:,:,:,mm) = hm;

end
h = reshape(h, [Ns LL LL MM]);

end % de_ftab_fit_exp_hess()

%
% de_ftab_fit_show_sp()
% compare true spectra to fitted spectra
%
function out = de_ftab_fit_show_sp(fit, en, sp)
if nargin < 1, fail 'de_ftab_fit_show_sp(fit, en, sp)', end

if ~streq(fit.type, 'exp'), printm 'show_sp only done for exp', return, end
if im
	clf, pl = (fit.MM+1)*100 + 10 + 1;
	if nargin > 1
		subplot(pl)
		plot(en, sp * diag(1 ./ max(sp)))
		tmp = num2cell(char([1:fit.MM] + '0'));
		legend(tmp{:})
		axisx(minmax(en))
	end

	if isfield(fit, 'kev')
		for mm=1:fit.MM
			subplot(pl+mm)
			bar(fit.kev{mm}, fit.coef{mm})
			xtick(fit.kev{mm}(fit.coef{mm} > 0))
			axis tight
			if nargin > 1, axisx(minmax(en)), end
		end
	else
		warn 'kev unknown'
	end
end

if nargout, out = []; end

end % de_ftab_fit_show_sp()

%
% de_ftab_fit_show_fm()
% compare fit to sampled fm
%
function out = de_ftab_fit_show_fm(fit, sl, fm, sll)
if nargin < 2, error 'de_ftab_fit_show_fm(fit, sl, fm)', end

do_fm = nargin > 2;
if ~isvar('sll') || isempty(sll)
	sll = ndgrid_jf('mat', sl{:});
end

fh = fit.fmfun(sll);

if ~iscell(sl), fail 'sl must be cell', end
switch length(sl) % LL
case 1
	s1 = sl{1};
	im clf
	arg = {};
	leg = {};
	order = 'bgrcm';
	if do_fm
		for mm=1:fit.MM
			arg = {arg{:}, s1, fm(:,mm), [order(mm) '.']};
			leg = {leg{:}, sprintf('true m=%d', mm)};
		end
	end
	for mm=1:fit.MM
		arg = {arg{:}, sll, fh(:,mm), [order(mm) '-']};
		leg = {leg{:}, sprintf('fit m=%d', mm)};
	end
	plot(arg{:})
	xlabel 's1'
	ylabel 'fm(s1)'
	legend(leg{:}, 4)

case 2
	s1 = sl{1};
	s2 = sl{2};
	smax(1) = max(s1);
	smax(2) = max(s2);
	if do_fm
		fmax = max(fm(:));
	else
		fmax = max(fh(:));
	end

	ax = [0 smax(1) 0 smax(2) 0 fmax];
	cax = [0 fmax];

	im clf
	im('pl', 2, fit.MM)
	for mm=1:fit.MM
		if do_fm
		show(mm, s1, s2, fm(:,:,mm), ax, cax, sprintf('f_%d(s)', mm))
		end
		m0 = mm + fit.MM;
		show(m0, s1, s2, fh(:,:,mm), ax, cax, sprintf('f_%d fit', mm))
	end
%	% text(-60, 10, '[cm^2/g]')

case 3
	s1 = sl{1};
	s2 = sl{2};
	s3 = sl{3};
	smax(1) = max(s1);
	smax(2) = max(s2);
	smax(3) = max(s3);
	if do_fm
		fmax = max(fm(:));
	else
		fmax = max(fh(:));
	end

	ax = [0 smax(2) 0 smax(3) 0 fmax];
	cax = [0 fmax];

	im clf
	im('pl', 2, fit.MM)
	for mm=1:fit.MM
		if do_fm
		show(mm, s2, s3, fm(1,:,:,mm), ax, cax, sprintf('f_%d(s)', mm))
		end
		m0 = mm + fit.MM;
		show(m0, s2, s3, fh(1,:,:,mm), ax, cax, sprintf('f_%d fit', mm))
	end
%	% text(-60, 10, '[cm^2/g]')

otherwise
	fail 'not done'
end

if nargout, out = []; end

end % de_ftab_fit_show_fm()

%
% de_ftab_fit_show_err()
% show fit errors, and relate to HU
% f = mac s, mac(70kev,H2O) = 0.2 cm^2 / g = 1000 HU
% mh = mac * 1000 HU / (0.2 g/cm^2)
% so Df/Dmh = Df/Dmac * Dmac/Dmh = (50 g/cm^2) (0.2 cm^2/g) / 1000 HU = 1/100 HU
% so Dmh = 100 HU * Df for 50 cm of H2O.
%
function out = de_ftab_fit_show_err(fit, sl, fm)
if nargin < 3, error 'de_ftab_fit_show_err(fit, sl, fm)', end

sll = ndgrid_jf('mat', sl{:});

fh = fit.fmfun(sll);
err = fh - fm;
printm('worst model error = %g of %g', max(abs(err(:))), max(fm(:)))
printm('worst model error = %g HU over 50cm H2O', 100*max(abs(err(:))))
for mm=1:fit.MM
	ee = stackpick(err,mm);
	printm('worst error (mm=%d): %g', mm, max(abs(ee(:))))
end

if max(abs(err(:))) == 0
	if nargout, out = []; end
return
end

if 0
	e1 = err(:,:,1);
	e2 = err(:,:,2);
	disp([minmax(e1); minmax(e2)]')
	printm('worst error1 %g', max(col(abs(err(:,1,:)))))
	printm('worst error2 %g', max(col(abs(err(:,2,:)))))
end
%err = abs(err);

switch length(sl) % LL
case 1
	s1 = sl{1};
	arg = {};
	leg = {};
	order = 'bgrcm';
	for mm=1:fit.MM
		arg = {arg{:}, s1, err(:,mm), [order(mm) '-']};
		leg = {leg{:}, sprintf('m=%d', mm)};
	end
	plot(arg{:})
	xlabel 's1'
	ylabel 'error'
	legend(leg{:}, 4)

case 2
	s1 = sl{1};
	s2 = sl{2};

	elim = minmax(err(:))';
	%elim = [-1 1] * 0.01; % +/- 1 HU
	ax = [0 max(s1) 0 max(s2) elim];
	im clf, im('pl', 1, fit.MM)
	for mm=1:fit.MM
		show(mm, s1, s2, err(:,:,mm), ax, elim, sprintf('f_%d error', mm))
	end

case 3
	s1 = sl{1};
	s2 = sl{2};
	s3 = sl{3};

	elim = minmax(err(:))';
	ax = [0 max(s2) 0 max(s3) elim];
	im clf, im('pl', 1, fit.MM)
	for mm=1:fit.MM
		show(mm, s2, s3, err(1,:,:,mm), ax, elim, sprintf('f_%d error', mm))
	end

otherwise
	fail 'not done'
end

if nargout, out = []; end

end % de_ftab_fit_show_err()

%
% show()
%
function show(pl, x, y, f, ax, cax, ti)
f = squeeze(f); % for LL=3 case
if ~im, return, end
im('subplot', pl)
if 1
	mesh(x,y,f')
	colormap hsv, caxis(cax), cbar
	axis(ax)
	xtick, ytick, ztick, zwhite
else
	im(x,y,f), cbar
	xtick, ytick
end
xlabel 's_1', ylabel 's_2', title(ti)

end % show()

%
% de_ftab_fit_test()
%
function de_ftab_fit_test

%stype = 'mono,70,100,160';
%stype = 'ps1';
%stype = 'poly1,60';
stype = 'poly1,80,100,160'; % stress test
xrs = xray_read_spectra(stype);

list.sl{1} = linspace(0, 50, 26); % coarse for fast fitting
list.sl{2} = linspace(0, 30, 31);
list.sl{3} = linspace(0, 2, 11);
list.mtype = {'water', 'bone', 'iodine'};
list.wls_simplex_arg = {{'reg', 1e-16}, {'reg', 1e-9}, {'reg', 1e-4}}; % more reg for LL>1 case

if 0 % look at derivatives
	LL = 2;
	sl = {list.sl{1:LL}};
	mtype = {list.mtype{1:LL}};
	warg = list.wls_simplex_arg{LL};
	mas = xray_read_mac(mtype);
	mac = mas.mac(xrs.en);
	sll = ndgrid_jf('mat', sl{:});
	fm = de_ftab_fm(sll, mac, xrs.Ide);
	fit = de_ftab_fit(sl, fm, 'show', 1, 'type', 'exp', 'mtype', mtype, ...
		'wls_simplex_arg', warg);
	g = fit.fgrad(sll);
end

%for LL=1:3
for LL=2
	sl = {list.sl{1:LL}};
	mtype = {list.mtype{1:LL}};
	warg = list.wls_simplex_arg{LL};

	mas = xray_read_mac(mtype);
	mac = mas.mac(xrs.en);
	if im
		clf, semilogy(xrs.en, mac), legend(mtype{:})
	end
	sll = ndgrid_jf('mat', sl{:});
	fm = de_ftab_fm(sll, mac, xrs.Ide);
%	fit = de_ftab_fit(sl, fm, 'show', 1, 'type', 'poly')
	fit = de_ftab_fit(sl, fm, 'show', 1, 'type', 'exp', 'mtype', mtype, ...
		'wls_simplex_arg', warg);

	fh = fit.fmfun(sll);
	g = fit.fgrad(sll);
	h = fit.fhess(sll);

	if 1 && LL == 2 % test gradient (for yong)
		if 1 % for gradient graphically
			list.slfine{1} = linspace(0, 90, 261); % fine for evaluating
			list.slfine{2} = linspace(0, 90, 511);
		else
			list.slfine{1} = linspace(0, 5, 261); % fine for evaluating
			list.slfine{2} = linspace(0, 3, 511);
		end
%		list.slfine{3} = linspace(0, 2, 201);
		slfine = {list.slfine{1:LL}};
		slf = ndgrid_jf('mat', slfine{:});
		fh = fit.fmfun(slf);
		gr = fit.fgrad(slf); % [261 511 L M]
		if im % examine gradient values graphically
			pl = @(mm) subplot(100 + 10 * fit.MM + mm);
			for mm=1:fit.MM
				pl(mm)
				tmp = gr(:,:,:,mm);
				plot(tmp(:,:,1), tmp(:,:,2), '.')
				titlef('%d kvp', xrs.kvp(mm))
				xlabel 'g1', ylabel 'g2'
				axis([0 max(col(tmp(:,:,1))) 0 max(col(tmp(:,:,2)))])
			end
		return
		end
		i1 = 5;
		i2 = 9;
		d1 = slfine{1}(i1+1) - slfine{1}(i1);
		d2 = slfine{2}(i2+1) - slfine{2}(i2);
		gh(:,1) = (fh(i1+1,i2,:) - fh(i1,i2,:)) / d1;
		gh(:,2) = (fh(i1,i2+1,:) - fh(i1,i2,:)) / d2;
		pr transpose(gh)
		gg = squeeze(gr(i1,i2,:,:));
		pr gg
		pr transpose(fit.mac_eff) % at origin
%		gg - fit.mac_eff'
	return
	end

	if im
		fit.show_sp(xrs.en, xrs.sp);
		prompt
		fit.show_fm(sl, fm);
		prompt
		fit.show_err(sl, fm);
		prompt
	end

end

end % de_ftab_fit_test()


%
%  MODEL REDUCTION BY THE ALGORITHMS LRSRM AND DSPMR. THE GOAL IS TO
%  GENERATE A "NUMERICALLY MINIMAL REALIZATION" OF THE GIVEN SYSTEM
%  AS WELL AS A REDUCED SYSTEM OF RELATIVELY SMALL ORDER.
%
%  This demo program shows how the model reduction routines 'lp_lrsrm'
%  and 'lp_dspmr' work. Also, the use of 'lp_lradi', supplementary
%  routines, and user-supplied functions is demonstrated.

% -----------------------------------------------------------------------
% Generate test problem
% -----------------------------------------------------------------------
%
% As test example, we use an FEM-semidiscretized problem, which leads to
% a generalized system where M (the mass matrix) and N (the negative
% stiffness matrix) are sparse, symmetric, and definite.

load rail821   % load the matrices M N Btilde Ctilde of the generalized
               % system

%load rail3113   % Uncomment this to get an example of larger order.

disp('Problem dimensions:')

n = size(M,1)   % problem order (number of states)
m = size(Btilde,2)   % number of inputs
q = size(Ctilde,1)   % number of outputs

% -----------------------------------------------------------------------
% Initialization/generation of data structures used in user-supplied
% functions and computation of ADI shift parameters
% -----------------------------------------------------------------------
%
% See 'demo_u1', 'demo_u2', 'demo_u3', and 'demo_l1' for more detailed
% comments.

name = 'msns';
[M0,MU,N0,B0,C0,prm,iprm] = msns_pre(M,N,Btilde,Ctilde);  % preprocessing
msns_m_i(M0,MU,N0);   % initialization for multiplication with A0
msns_l_i;   % initialization for solving systems with A0

disp('Parameters for heuristic algorithm which computes ADI parameters:')
l0 = 20   % desired number of distinct shift parameters
kp = 50   % number of steps of Arnoldi process w.r.t. A0
km = 25   % number of steps of Arnoldi process w.r.t. inv(A0)

b0 = ones(n,1);   % This is just one way to choose the Arnoldi start
                  % vector.

p = lp_para(name,[],[],l0,kp,km,b0);   % computation of ADI shift
                                       % parameters

disp('Actual number of ADI shift parameters:');
l = length(p)

disp('ADI shift parameters:');
p

msns_s_i(p)   % initialization for shifted systems of linear equations
              % with A0+p(i)*I  (i = 1,...,l)

% -----------------------------------------------------------------------
% Solution of Lyapunov equations A0*X0+X0*A0' = -B0*B0' and
% A0'*X0+X0*A0 = -C0'*C0
% -----------------------------------------------------------------------

disp('Parameters for stopping criteria in LRCF-ADI iteration:')
max_it = 200   % (large value)
min_res = 0   % (avoided)
with_rs = 'S'   % ("activated")
min_in = 0   % (avoided)

zk = 'Z';
rc = 'C';
Bf = [];
Kf = [];
info = 3;

disp('... solving A0*XB0+XB0*A0'' = - B0*B0''...');
tp = 'B';

figure(1), hold off; clf;   % (lp_lradi will plot residual history.)

[ZB0,flag_B] = lp_lradi(tp,zk,rc,name,Bf,Kf,B0,p,max_it,min_res,...
               with_rs,min_in,info);
                                       % compute ZB0

title('LRCF-ADI for CALE  A_0X_{B0}+X_{B0}A_0^T = -B_0B_0^T')
disp('Termination flag:')
flag_B
disp('Size of ZB0:');
size_ZB0 = size(ZB0)

disp('... solving A0''*XC0+XC0*A0 = - C0''*C0...');
tp = 'C';

figure(2), hold off; clf;   % (lp_lradi will plot residual history.)

[ZC0,flag_C] = lp_lradi(tp,zk,rc,name,Bf,Kf,C0,p,max_it,min_res,...
               with_rs,min_in,info);
                                       % compute ZC0

title('LRCF-ADI for CALE  A_0^T X_{C0} + X_{C0} A_0 = -C_0^TC_0')
disp('Termination flag:')
flag_C
disp('Size of ZC0:');
size_ZC0 = size(ZC0)

% -----------------------------------------------------------------------
% Plot the transfer function of the system for a certain frequency range
% -----------------------------------------------------------------------

disp('... computing transfer function of original system ...');

freq = lp_lgfrq(1e-10,1e10,200);   % generate a set of 200 "frequency
                                   % sampling points" in the interval
                                   % [10^-10,10^+10].
G = lp_trfia(freq,N,Btilde,Ctilde,[],M);   % compute "transfer function
                                           % sample" for these frequency
                                           % points
nrm_G = lp_gnorm(G,m,q);   % compute norms of the "transfer function
                           % sample" for these frequency points

figure(3); hold off; clf;
loglog(freq,nrm_G,'k:');
xlabel('\omega');
ylabel('Magnitude');
t_text = 'dotted: ||G||';
title(t_text);
pause(1)

% -----------------------------------------------------------------------
% Generate reduced systems of high accuracy and possibly high order
% -----------------------------------------------------------------------

disp(' ')
disp('Generate reduced systems of high accuracy and possibly high order')
disp('-----------------------------------------------------------------')

disp('Parameters for model reduction:')
max_ord = []   % (avoided)
tol = 1e-14   % (This criterion determines the reduced order. The very
              % small value is chosen to generate a "numerically minimal
              % realization".)

disp('... computing reduced system by LRSRM ...');
[Ars,Brs,Crs] = lp_lrsrm(name,B0,C0,ZB0,ZC0,max_ord,tol);   % run LRSRM

disp('Reduced order:')
disp(length(Ars))

Grs = lp_trfia(freq,Ars,Brs,Crs,[],[]);   % compute "transfer function
                                          % sample" for reduced system
nrm_dGrs = lp_gnorm(G-Grs,m,q);   % compute norm of DIFFERENCE of
                                  % transfer function samples of original
                                  % and reduced system.
figure(3); hold on
loglog(freq,nrm_dGrs,'r-');
t_text = [t_text, ',    solid: ||G-G_{LRSRM}||'];
title(t_text); pause(1)

disp('... computing reduced system by DSPMR ...');
[Ard,Brd,Crd] = lp_dspmr(name,B0,C0,ZB0,ZC0,max_ord,tol);   % run DSPMR

disp('Reduced order:')
disp(length(Ard))

Grd = lp_trfia(freq,Ard,Brd,Crd,[],[]);   % compute "transfer function
                                          % sample" for reduced system
nrm_dGrd = lp_gnorm(G-Grd,m,q);   % compute norm of DIFFERENCE of
                                  % transfer function samples of original
                                  % and reduced system.
figure(3); hold on
loglog(freq,nrm_dGrd,'b--'); pause(1)
t_text = [t_text, ',    dashed: ||G-G_{DSPMR}||'];
title(t_text); pause(1)

% -----------------------------------------------------------------------
% Generate reduced systems of low order
% -----------------------------------------------------------------------

disp(' ')
disp('Generate reduced systems of low order')
disp('-------------------------------------')

disp('Parameters for model reduction:')
max_ord = 25   % (This criterion determines the reduced order.)
tol = 0    % (avoided)

disp('... computing reduced system by LRSRM ...');
[Ars,Brs,Crs] = lp_lrsrm(name,B0,C0,ZB0,ZC0,max_ord,tol);   % run LRSRM

disp('Reduced order:')
disp(length(Ars))

Grs = lp_trfia(freq,Ars,Brs,Crs,[],[]);   % compute "transfer function
                                          % sample" for reduced system
nrm_dGrs = lp_gnorm(G-Grs,m,q);   % compute norm of DIFFERENCE of
                                  % transfer function samples of original
                                  % and reduced system.
figure(3); hold on
loglog(freq,nrm_dGrs,'r-');

disp('... computing reduced system by DSPMR ...');
[Ard,Brd,Crd] = lp_dspmr(name,B0,C0,ZB0,ZC0,max_ord,tol);   % run DSPMR

disp('Reduced order:')
disp(length(Ard))

Grd = lp_trfia(freq,Ard,Brd,Crd,[],[]);   % compute "transfer function
                                          % sample" for reduced system
nrm_dGrd = lp_gnorm(G-Grd,m,q);   % compute norm of DIFFERENCE of
                                  % transfer function samples of original
                                  % and reduced system.
figure(3); hold on
loglog(freq,nrm_dGrd,'b--');

% -----------------------------------------------------------------------
% Destroy global data structures
% -----------------------------------------------------------------------

msns_m_d;
msns_l_d;
msns_s_d(p);


function [comod] = bz_CrossFreqMod(lfp,phaserange,amprange,varargin)
% [comod] = bz_CrossFreqMod(lfp,phaserange,amprange,flagPlot)
%
%
%This function calculates the modulation index of phase-amplitude between
%phaserange (lower frequencies) to amplitude range (higher frequencies).
%It can really take a long time if you do very small steps of frequency,
%due to wavelet processing each frequency at a time.
%
%%INPUT
%    lfp            a buzcode structure with fields lfp.data,
%                                                   lfp.timestamps
%                                                   lfp.samplingRate
%                   -lfp can also be a [t x 1] timeseries signal. in which
%                   case you need to input 'samplingRate'
%    phaserange     [min:steps:max] array of frequencies to filter for the
%                   phase signal
%    amprange       [min:stepsmax] array of frequencies range for wavelets
%                   for the power signal
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     phaseCh      channel to compute phase. If empty take first channel
%     ampChans     channels to compute amplitude. If empty take first channel
%     makePlot      default true
%    =========================================================================
%
%OUTPUT
%   comod               phase-frequency versus amplitude-frequency
%                       comodulogram matrix in modulation index units
%
%Dependencies
%   bz_Filter
%   bz_WaveSpec
%
%   AntonioFR, 2/2019

%% Parse inputs

p = inputParser;
addParameter(p,'phaseCh',lfp.channels(1),@isnumeric);
addParameter(p,'ampChans',lfp.channels(1),@isnumeric);
addParameter(p,'samplingRate',1250,@isnumeric);
addParameter(p,'makePlot',true,@islogical);

parse(p,varargin{:});
phaseCh = p.Results.phaseCh;
ampChans = p.Results.ampChans;
samplingRate = p.Results.samplingRate;
makePlot = p.Results.makePlot;

%lfp input
if isstruct(lfp)
    data = lfp.data;
    timestamps = lfp.timestamps;
    samplingRate = lfp.samplingRate;
elseif iscell(lfp) %for multiple trials
    celllengths = cellfun(@length,lfp);
    data = vertcat(lfp{:});
elseif isnumeric(lfp)
    data = lfp;
    timestamps = [1:length(lfp)]'./samplingRate;
end

%% Filter LFP for phase
nfreqs = length(amprange);

for bnd = 1:length(phaserange)-1
    filtered_phase(bnd,:) = bz_Filter(lfp,'passband',phaserange(bnd:bnd+1),'filter','fir1','channels',phaseCh);
end

%% Wavelet Transform LFP in intervals
for ch = 1:length(ampChans)
    comod = zeros(length(amprange)-1,length(filtered_phase),length(ampChans));

    lfpCh = lfp; % this should be remove when bz_WaveSpec can take 'channels' input
    lfpCh.data = lfp.data(:,find(lfp.channels == ampChans(ch)));

    for apr = 1:length(amprange)-1
        wavespec_amp = bz_WaveSpec(lfp,'frange',[amprange(apr) amprange(apr+1)],'nfreqs',1);

        wavespec_amp.data = abs(wavespec_amp.data);
        %% Bin phase and power
        numbins = 50;
        phasebins = linspace(-pi,pi,numbins+1);
        phasecenters = phasebins(1:end-1)+(phasebins(2)-phasebins(1));

        for idx = 1:length(filtered_phase)
            [phasedist,~,phaseall] = histcounts(filtered_phase(idx).phase,phasebins);

            phaseAmp = zeros(numbins,1);
            for bb = 1:numbins
                phaseAmp(bb) = mean(wavespec_amp.data(phaseall==bb),1);
            end

            phaseAmp = phaseAmp./sum(phaseAmp,1);
            comod(apr,idx,ch) = sum(phaseAmp.*log(phaseAmp./(ones(numbins,size(phaseAmp,2))/numbins)))/log(numbins);
        end

    end
    ampfreqs = wavespec_amp.freqs;
    clear lfpCh
end

%% Plot
if makePlot

    figure
    for ch = 1:length(ampChans)
        subplot(1,length(ampChans),ch);
        contourf(phaserange(2:end),amprange(2:end),abs(comod(:,:,ch)),20,'LineColor','none');
        colorbar ('SouthOutside'); colormap jet;
        %title(signals)
        if ch > 1
            set(gca,'YTick',[]);
        end
    end

end


function [ xpoly, ypoly, zpoly, avg_resid ] = sv2poly( xpos, ypos, zpos, ...
    times, xvel, yvel, zvel )
%SV2POLY Compute best fit polynomials from 3D state vectors
%
% This function determines a polynomial order that achieves good precision
% without being unreasonably high or overfitting.  One would generally only
% use this function on state vectors from an orbital system, where the
% positions/velocity would reasonably follow a smooth path.
%
% "Optimal" polynomial order depends on both the length of the collect
% around its orbit and the noise in the state vector data.
%
% Written by: Wade Schwartzkopf, NGA/R
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Check arguments
if ~isequal(size(xpos), size(ypos), size(zpos), size(times)) || ...
        (exist('zvel','var') && ...
        ~isequal(size(xvel), size(yvel), size(zvel), size(times)))
    error('SV2POLY:INVALID_INPUT_ARGS','All state vector components must be same size/shape.');
end
% Vectorize inputs to assure column vectors
times = times(:);
xpos = xpos(:);
ypos = ypos(:);
zpos = zpos(:);
if exist('zvel', 'var')
    xvel = xvel(:);
    yvel = yvel(:);
    zvel = zvel(:);
end

% Because of the scale of the values used in orbital state vectors,
% MATLAB's polyfit oftens detects them as badly conditioned, but results
% work out OK anyway for reasonable polynomail orders. We will assure in
% this function that polynomial order is reasonable.
old_state = warning('off','MATLAB:polyfit:RepeatedPointsOrRescale');

%% Method 1.  Naive approach.
% Hardcode polynomial order.  This is usually sufficient.  5th order
% generally works well for most spaceborne SAR collects of typical length
% (< 200 s), often putting evaluation of the polynomials well within 1 mm
% of error.
% polyorder = min(5, numel(times) - 1);

if exist('zvel', 'var')
    %% Method 2.  Cross check with velocity.
    % One could also find the order of polynomial that most accurately
    % describes this position, but use velocity (which is presumably
    % measured independently) as cross-validation so that the data is not
    % being overfit.
    for polyorder = 1:(numel(times)-1)
        xpoly = polyfit(times, xpos, polyorder);
        ypoly = polyfit(times, ypos, polyorder);
        zpoly = polyfit(times, zpos, polyorder);
        error_list(polyorder) = mean(sqrt(sum(([xvel, yvel, zvel] - ...
            [polyval(polyder(xpoly), times), ...
            polyval(polyder(ypoly), times), ...
            polyval(polyder(zpoly), times)]).^2, 2)));
    end
    % Increase order only as long as it results in a "significant" decrease
    % (half) in the error of the velocity.
    polyorder = find(error_list(1:(end-1))<(2*error_list(2:end)), 1, 'first');
    if isempty(polyorder), polyorder = numel(error_list); end  % All orders improve quality
else
    %% Method 3. "Significant" improvement.
    % One could also use the lowest order of polynomial that significantly
    % reduces the error of the position fit.
    for polyorder = 1:(numel(times)-2)  % polyorder of n-1 will always be exact match
        % mu term used for error computation in loop since we are
        % potentially computing high order polynomials, generally well
        % beyond what is required for a good fit, and these will likely be
        % badly conditioned. For our final fit, after the polynomial order
        % is determined, we will assume order and fit is reasonable-- even
        % if MATLAB would have thrown a warning.
        [xpoly, ~, xmu] = polyfit(times, xpos, polyorder);
        [ypoly, ~, ymu] = polyfit(times, ypos, polyorder);
        [zpoly, ~, zmu] = polyfit(times, zpos, polyorder);
        error_list(polyorder) = mean(sqrt(sum(([xpos, ypos, zpos] - ...
            [polyval(xpoly, times, [], xmu), ...
            polyval(ypoly, times, [], ymu), ...
            polyval(zpoly, times, [], zmu)]).^2, 2)));
    end
    % Increasing polynomial order by one must result in error being cut in
    % half in order to be considered "significant".
    polyorder = find(error_list(1:(end-1))<(2*error_list(2:end)), 1, 'first');
    if isempty(polyorder), polyorder = numel(error_list); end  % All orders improve quality
end

% Once optimal polynomial order is determined, do actual polynomial fit
xpoly  = polyfit(times, xpos, polyorder);
ypoly  = polyfit(times, ypos, polyorder);
zpoly  = polyfit(times, zpos, polyorder);

warning(old_state);

% Compute final residuals if requested
if nargout>3
    avg_resid = mean(sqrt(sum(([xpos, ypos, zpos] - ...
        [polyval(xpoly, times), ...
        polyval(ypoly, times), ...
        polyval(zpoly, times)]).^2, 2)));
end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function e = R2e(R)

% R2E  Rotation matrix to Euler angles conversion.
%   R2E(R) returns the euler angles vector [roll;pitch;yaw] corresponding
%   to the orientation of the rotation matrix R. The result is such that
%   E2R(R2E(R)) = R and R2E(E2R(E)) = E.
%
%   See also E2R, FRAME.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

s = whos('R');

if (strcmp(s.class,'sym'))
    roll  = atan(R(3,2)/R(3,3));
    pitch = asin(-R(3,1));
    yaw   = atan(R(2,1)/R(1,1));
else
    roll  = atan2(R(3,2),R(3,3));
    pitch = atan2(-R(3,1), sqrt(R(1,1)^2+R(2,1)^2));
    yaw   = atan2(R(2,1),R(1,1));
end

e = [roll;pitch;yaw];

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

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

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

%   # END GPL LICENSE


function [xn, yn, zn] = findnearest (x, y, z, x0, y0, N)
d = (x0 - x).^2 + (y0 - y).^2;
[md, i] = min (d);
xn1 = x(i); yn1 = y(i); zn1 = z(i);
xn2 = []; yn2 = []; zn2 = [];
if N > 1
    x = [x(1:i-1); x(i+1:end)];
    y = [y(1:i-1); y(i+1:end)];
    z = [z(1:i-1); z(i+1:end)];
    [xn2, yn2, zn2] = findnearest (x, y, z, x0, y0, N-1);
end
xn = [xn1; xn2];
yn = [yn1; yn2];
zn = [zn1; zn2];


function [f1,f2,DialedNum] = DecodedFrequencies(E);

%Computes the energy of the Signal
%Input Variables
%    E    = Energy Matrix of the Filtered Signals
%
% Output Variables
%    f1   = first decoded frequency
%    f2   = second decoded frequency

Fmat = [697 770 852 941 1209 1336 1477 1633];

LowEnergyMatrix = E(1:4);
HighEnergyMatrix = E(5:8);

[e1,e1Index]=max(LowEnergyMatrix);
[e2,e2Index]=max(HighEnergyMatrix);

f1 = Fmat(e1Index);
f2 = Fmat(4+e2Index);

if        f1 == Fmat(1) && f2 == Fmat(5)
   DialedNum = '1';
   elseif f1 == Fmat(1) && f2 == Fmat(6)
   DialedNum = '2';
   elseif f1 == Fmat(1) && f2 == Fmat(7)
   DialedNum = '3';
   elseif f1 == Fmat(1) && f2 == Fmat(8)
   DialedNum = 'A';
   elseif f1 == Fmat(2) && f2 == Fmat(5)
   DialedNum = '4';
   elseif f1 == Fmat(2) && f2 == Fmat(6)
   DialedNum = '5';
   elseif f1 == Fmat(2) && f2 == Fmat(7)
   DialedNum = '6';
   elseif f1 == Fmat(2) && f2 == Fmat(8)
   DialedNum = 'B';
   elseif f1 == Fmat(3) && f2 == Fmat(5)
   DialedNum = '7';
   elseif f1 == Fmat(3) && f2 == Fmat(6)
   DialedNum = '8';
   elseif f1 == Fmat(3) && f2 == Fmat(7)
   DialedNum = '9';
   elseif f1 == Fmat(3) && f2 == Fmat(8)
   DialedNum = 'C';
   elseif f1 == Fmat(4) && f2 == Fmat(5)
   DialedNum = '*';
   elseif f1 == Fmat(4) && f2 == Fmat(6)
   DialedNum = '0';
   elseif f1 == Fmat(4) && f2 == Fmat(7)
   DialedNum = '#';
   elseif f1 == Fmat(4) && f2 == Fmat(8)
   DialedNum = 'D';
else
    DialedNum = 'p';
end


function [tri, bc] = gridDataFast2D(x, y, xi, yi)
% Copyright (c) 2012, Chao Huang
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

% enforce x, y, xi, yi to be column vectors
x = x(:);
y = y(:);
xi = xi(:);
yi = yi(:);

% check if the MATLAB version is new enough to have DelaunayTri
if exist('DelaunayTri') %#ok<EXIST>
    % triangulize the data
    tri = DelaunayTri(x, y);
else
    % call the old version of gridDataFast using tsearch
    [tri, bc] = gridDataFast2D_tsearch(x, y, xi, yi);
    return
end

% catch trinagulation error
if isempty(tri)
    error('Data cannot be triangulated.');
end

% find the nearest triangle and the corresponding Barycentric coordinates
[t, bc] = pointLocation(tri,[xi yi]);

% check points are valid
if any(isnan(t))
    error('Cartesian points must lie within the k-space grid defined by kgrid');
end

tri = tri(t,:);


function r8cc_print_some ( m, n, nz_num, colptr, rowind, a, ilo, jlo, ihi, ...
  jhi, title )

%*****************************************************************************80
%
%% R8CC_PRINT_SOME prints some of a R8CC matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in A.
%
%    Input, integer COLPTR(N+1), points to the first element of each column.
%
%    Input, integer ROWIND(NZ_NUM), contains the row indices of the elements.
%
%    Input, real A(NZ_NUM), the matrix.
%
%    Input, integer ILO, JLO, IHI, JHI, the first row and
%    column, and the last row and column to be printed.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  incx = 5;
%
%  Print the columns of the matrix, in strips of 5.
%
  for j2lo = jlo : incx : jhi

    j2hi = j2lo + incx - 1;
    j2hi = min ( j2hi, n );
    j2hi = min ( j2hi, jhi );

    inc = j2hi + 1 - j2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col: ' );

    for j = j2lo : j2hi
      fprintf ( 1, '%7d       ', j );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row\n' );
    fprintf ( 1, '  ---\n' );
%
%  Determine the range of the rows in this strip.
%
    i2lo = max ( ilo, 1 );
    i2hi = min ( ihi, m );

    for i = i2lo : i2hi

      fprintf ( 1, '%4d  ', i );
%
%  Print out (up to) 5 entries in row I, that lie in the current strip.
%
      for j = j2lo : j2hi

        aij = 0.0;

        for k = colptr(j) : colptr(j+1)-1
          if ( rowind(k) == i )
            aij = a(k);
            break;
          end
        end

        fprintf ( 1, '%12g  ', aij );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function i4vec_aminz_index_test ( )

%*****************************************************************************80
%
%% I4VEC_AMINZ_INDEX_TEST tests I4VEC_AMINZ_INDEX;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_AMINZ_INDEX_TEST\n' );
  fprintf ( 1, '  For an integer vector:\n' );
  fprintf ( 1, '  I4VEC_AMINZ_INDEX: index of minimum nonzero absolute entry;\n' );

  seed = 123456789;

  b = -n;
  c = n;

  [ a, seed ] = i4vec_uniform_ab ( n, b, c, seed );

  i4vec_print ( n, a, '  Input vector:' );

  fprintf ( 1, '\n' );

  ival = i4vec_aminz_index ( n, a );

  fprintf ( 1, '  Minimum abs nonzero index: %d\n', ival );

  return
end


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] = ubah_run(fid, data, tc, opts)
% This file is the run core for the market strategy.
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] = market_run(fid, data, tc, opts)
%
% cum_ret: cumulative wealth achived at the end of a period.
% cumprod_ret: cumulative wealth achieved till the end each period.
% daily_ret: daily return achieved by a strategy.
% daily_portfolio: daily portfolio, achieved by the strategy
%
% data: market sequence vectors
% fid: handle for write log file
% tc: transaction fee rate
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret] ...
%          = market_run(fid, data, 0, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[n, m]=size(data);

% Variables for return, start with uniform weight
cum_ret = 1;
cumprod_ret = ones(n, 1);
daily_ret = ones(n, 1);

% portfolio at the beinning (end) of a period
day_weight = ones(m, 1)/m;  %#ok<*NASGU>
day_weight_o = zeros(m, 1);
daily_portfolio = zeros(n, m);

% print log file head
fprintf(fid, '-------------------------------------\n');
fprintf(fid, 'Parameters [tc: %f].\n', tc);
fprintf(fid, 'day\t Daily Return\t Total return\n');

fprintf(1, '-------------------------------------\n');
if(~opts.quiet_mode)
    fprintf(1, 'Parameters [tc: %f].\n', tc);
    fprintf(1, 'day\t Daily Return\t Total return\n');
end

% Backtests
for t = 1:1:n,

    % Calculate t's portfolio at the beginning of t-th trading day
    if (t >= 2)
        [day_weight] = ubah_kernel(data(1:t-1, :), day_weight_o);
    end

    % Normalize the constraint, always useless
    day_weight = day_weight./sum(day_weight);
    daily_portfolio(t, :) = day_weight';

    % Cal t's return and total return
    daily_ret(t, 1) = (data(t, :)*day_weight)*(1-tc/2*sum(abs(day_weight-day_weight_o)));
    cum_ret = cum_ret * daily_ret(t, 1);
    cumprod_ret(t, 1) = cum_ret;

    % Adjust weight(t, :) for the transaction cost issue
    day_weight_o = day_weight.*data(t, :)'/daily_ret(t, 1);

    % Log information
    fprintf(fid, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
    if (~opts.quiet_mode)
        if (~mod(t, opts.display_interval)),
            fprintf(1, '%d\t%f\t%f\n', t, daily_ret(t, 1), cumprod_ret(t, 1));
        end
    end
end

% Output the cumulative return and log it.
fprintf(fid, 'Market(tc=%.4f), Cumulative return: %.2f\n',tc, cum_ret);
fprintf(fid, '-------------------------------------\n');
fprintf(1, 'Market(tc=%.4f), Cumulative return: %.2f\n',tc, cum_ret);
fprintf(1, '-------------------------------------\n');

end
%%%%%%%%%%%%%%End%%%%%%%%%%%%%%%%%%%%%%


function [rVectECI, vVectECI] = getECIVectFromCR3BPVect(ut, rVectCR3BP, vVectCR3BP, secBodyInfo, celBodyData)

    priBodyInfo = secBodyInfo.getParBodyInfo(celBodyData);
    gmu = priBodyInfo.gm;

    rotMatEci2Cr3bp = getCR3BPRotMat(ut, secBodyInfo, celBodyData);
    zVectCR3BP = rotMatEci2Cr3bp(:,3);

    rotMatCr3bp2Eci = rotMatEci2Cr3bp;
    rVectECI = rotMatCr3bp2Eci * rVectCR3BP;

    secPeriod = computePeriod(secBodyInfo.sma, gmu);
    rotRateRadSec = 2*pi/secPeriod;
    omegaRI = zVectCR3BP * rotRateRadSec;

    vVectECI = rotMatCr3bp2Eci*(vVectCR3BP + crossARH(omegaRI, rVectCR3BP));
end


function OUT = multiscaleRetinex(I, method, varargin)

if nargin == 0
%     I = imload; %imread(imgetfile); %imread('office_1.jpg');
%     J = multiscaleRetinex(I, 'MSRCR');
%     ezFig I J
%     return;

    I = imload; %imread(imgetfile); %('office_1.jpg');
    method = Popupmenu({'MSRCR','MSR','SSR'});
    J = ImCtrl(@multiscaleRetinex, I, method);
    ezFig I J
end

if nargin == 1
   method = 'MSRCR';
end

I = im2double(I).*255+1; % +1 to avoid Inf
SSR = @SSRlog; % @SSR; %

% TODO: gray or RGB

switch (method)
    case {'SSR' 'single scale retinex'}, f = SSR;
    case {'MSR' 'multi scale retinex'}, f = @MSR;
    case 'MSRCR', f = @MSRCR;
    %case 'MSRCP', f = @MSRCP;
    otherwise
        return;
        %f = str2func(method);
end

OUT = f(I, varargin{:});

end

function OUT = SSR(I, varargin)
T = imgaussfilt(I, varargin{:});
OUT = I./(T); % avoid NaN
end

function OUT = SSRlog(I, varargin)
T = imgaussfilt(I, varargin{:});
OUT = log(I) - log(T+1) + 0.5;
end

function OUT = MSR(I, varargin)
if numel(varargin) == 0
    varargin = {25 100 240};
end
OUT = 0; N = numel(varargin);
for n = 1:N
    OUT = OUT + (1/N)* multiscaleRetinex(I,'SSR',varargin{n});
end
end

function OUT = MSRCR(I, lowScale, medScale, highScale, leftChop, rightChop)
if ~exist('lowScale', 'var'), lowScale = 15; end
if ~exist('MedScale', 'var'), medScale = 80; end
if ~exist('HighScale', 'var'), highScale = 250; end
if ~exist('s1', 'var'), leftChop = 1; end
if ~exist('s2', 'var'), rightChop = 1; end

MSR = multiscaleRetinex(I, 'MSR', lowScale, medScale, highScale);

for c = 1:3
    CR = (log(125*I(:,:,c))-log(I(:,:,1)+I(:,:,2)+I(:,:,3)));
    OUT(:,:,c) = colorBalance(mat2gray(CR.*MSR(:,:,c)), 'simplest', leftChop, rightChop);
end
%OUT = max(0, min(1, OUT));
end


function an=ea_bounding(phi,theta,psi)
% bounding of euler angles
% 0<=phi<2*pi
% 0<=theta<pi
% 0<=psi<2*pi

ifchange=false; % if anles come out from boundaries

if (0<=phi)&&(phi<2*pi)

else
    ifchange=true;
end

if (0<=theta)&&(theta<pi)

else
    ifchange=true;
end

if (0<=psi)&&(psi<2*pi)

else
    ifchange=true;
end

%theta=mod(theta+pi,2*pi)-pi; % -pi<=theta<pi
theta=-(mod(theta*(-1)+pi,2*pi)-pi); % -pi<theta<=pi

if theta<0
    theta=-theta;
    phi=pi+phi;
    psi=pi+psi;
end

phi=mod(phi,2*pi);
psi=mod(psi,2*pi);

an=[ifchange phi theta psi];


%PREX_SOM   PRTools example on training SelfOrganizing Maps
%
% Show the training and plotting of 1- or 2-D Self-Organizing Maps.
%
help prex_som

delfigs
echo on
								% Set size of the SOM
	k = [5 1];
								% Set the number of iterations
   nrruns = [20 40 40];
								% Set desired learning rates
	eta = [0.5 0.1 0.1];
	                     % Set the neighborhood widths
   h = [0.6 0.2 0.01];
								% Generate one banana class:
	A = gendatb([100,100]);
	A = seldat(A,1);
								 % Train a 1D SOM:
  W = som(A,k);  % May take some time
								 % Show the results in a scatter plot
	figure(1); clf;
  scatterd(A); hold on;
	prplotsom(W);
	title('One-dimensional SOM');
	drawnow

								 % Train a 2D SOM:
	k = [5 5];
  W = som(A,k);  % Will take some time
								 % Show the results in a scatter plot
	figure(2); clf;
   scatterd(A); hold on;
	prplotsom(W);
	title('Two-dimensional SOM');
	drawnow

echo off
showfigs


function test_anls(varargin)
%
% demonstration file for NMFLibrary.
%
% This file illustrates how to use this library.
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on June 23, 2022

    if nargin < 1
        clc;
        clear;
        close all;
        rng('default')

        m = 500;
        n = 100;
        V = rand(m,n);
        rank = 20;
        options = [];
        options.verbose = 2;
        options.max_epoch = 100;
        health_check_mode = false;
    else
        V = varargin{1};
        rank = varargin{2};
        options = varargin{3};
        health_check_mode = true;
    end

    % initialize factor matrices
    [x_init, ~] = generate_init_factors(V, rank, []);
    inner_max_epoch = 10;
    rescale = true;

    %% Scale initialization so that argmin_a ||a * WH - X||_F = 1
    W = x_init.W;
    H = x_init.H;
    if rescale
        [W, H] = normalize_WH(V, W, H, rank, 'type1');
    end

    x_init.W = W;
    x_init.H = H;

    %% ANLS
    %algorithm = 'anls_bpp';
    %algorithm = 'anls_asgroup';
    algorithm = 'anls_asgivens';
    % standard
    options.momentum_h = 0;
    options.momentum_w = 0;
    options.inner_max_epoch = inner_max_epoch;
    options.alg = algorithm;

    [w_nmf_anls, infos_nmf_anls] = anls_nmf(V, rank, options);

    % acc only H
    options.momentum_h = 3;
    options.momentum_w = 0;
    options.inner_max_epoch = inner_max_epoch;
    options.alg = algorithm;
    [w_nmf_anls_momemtum_h, infos_nmf_anls_momemtum_h] = anls_nmf(V, rank, options);

    options.momentum_h = 3;
    options.momentum_w = 2;
    options.inner_max_epoch = inner_max_epoch;
    options.alg = algorithm;
    [w_nmf_anls_momemtum_hw, infos_nmf_anls_momemtum_hw] = anls_nmf(V, rank, options);

    options.momentum_h = 3;
    options.momentum_w = 2;
    options.inner_max_epoch = inner_max_epoch;
    options.scaling = false;
    options.alg = algorithm;
    [w_nmf_anls_momemtum_hw_inner, infos_nmf_anls_momemtum_hw_inner] = anls_nmf(V, rank, options);

    if ~health_check_mode

        %% plot
        display_graph('iter','cost', {'ANLS-Standard', 'ANLS-MOMENTUM (H)', 'ANLS-MOMENTUM (W,H)', 'ANLS-MOMENTUM-nonscale (W,H)'}, ...
                                {w_nmf_anls, w_nmf_anls_momemtum_h, w_nmf_anls_momemtum_hw, w_nmf_anls_momemtum_hw_inner}, ...
                                {infos_nmf_anls, infos_nmf_anls_momemtum_h, infos_nmf_anls_momemtum_hw, infos_nmf_anls_momemtum_hw_inner});
    end

end


%% Machine Learning Online Class
%  Exercise 6 | Support Vector Machines
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     gaussianKernel.m
%     dataset3Params.m
%     processEmail.m
%     emailFeatures.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

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

fprintf('Loading and Visualizing Data ...\n')

% Load from ex6data1:
% You will have X, y in your environment
load('ex6data1.mat');

% Plot training data
plotData(X, y);

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

%% ==================== Part 2: Training Linear SVM ====================
%  The following code will train a linear SVM on the dataset and plot the
%  decision boundary learned.
%

% Load from ex6data1:
% You will have X, y in your environment
load('ex6data1.mat');

fprintf('\nTraining Linear SVM ...\n')

% You should try to change the C value below and see how the decision
% boundary varies (e.g., try C = 1000)
C = 100;
model = svmTrain(X, y, C, @linearKernel, 1e-3, 20);
visualizeBoundaryLinear(X, y, model);

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

%% =============== Part 3: Implementing Gaussian Kernel ===============
%  You will now implement the Gaussian kernel to use
%  with the SVM. You should complete the code in gaussianKernel.m
%
fprintf('\nEvaluating the Gaussian Kernel ...\n')

x1 = [1 2 1]; x2 = [0 4 -1]; sigma = 2;
sim = gaussianKernel(x1, x2, sigma);

fprintf(['Gaussian Kernel between x1 = [1; 2; 1], x2 = [0; 4; -1], sigma = 0.5 :' ...
         '\n\t%f\n(this value should be about 0.324652)\n'], sim);

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

%% =============== Part 4: Visualizing Dataset 2 ================
%  The following code will load the next dataset into your environment and
%  plot the data.
%

fprintf('Loading and Visualizing Data ...\n')

% Load from ex6data2:
% You will have X, y in your environment
load('ex6data2.mat');

% Plot training data
plotData(X, y);

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

%% ========== Part 5: Training SVM with RBF Kernel (Dataset 2) ==========
%  After you have implemented the kernel, we can now use it to train the
%  SVM classifier.
%
fprintf('\nTraining SVM with RBF Kernel (this may take 1 to 2 minutes) ...\n');

% Load from ex6data2:
% You will have X, y in your environment
load('ex6data2.mat');

% SVM Parameters
C = 1; sigma = 0.1;

% We set the tolerance and max_passes lower here so that the code will run
% faster. However, in practice, you will want to run the training to
% convergence.
model= svmTrain(X, y, C, @(x1, x2) gaussianKernel(x1, x2, sigma));
visualizeBoundary(X, y, model);

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

%% =============== Part 6: Visualizing Dataset 3 ================
%  The following code will load the next dataset into your environment and
%  plot the data.
%

fprintf('Loading and Visualizing Data ...\n')

% Load from ex6data3:
% You will have X, y in your environment
load('ex6data3.mat');

% Plot training data
plotData(X, y);

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

%% ========== Part 7: Training SVM with RBF Kernel (Dataset 3) ==========

%  This is a different dataset that you can use to experiment with. Try
%  different values of C and sigma here.
%

% Load from ex6data3:
% You will have X, y in your environment
load('ex6data3.mat');

% Try different SVM Parameters here
[C, sigma] = dataset3Params(X, y, Xval, yval);

% Train the SVM
model= svmTrain(X, y, C, @(x1, x2) gaussianKernel(x1, x2, sigma));
visualizeBoundary(X, y, model);

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


function h=joint_histogram(x,y)
%
% Takes a pair of images of equal size and returns the 2d joint histogram.
% used for MI calculation
%
% written by Amir Pasha Mahmoudzadeh
% Wright State University
% Biomedical Imaging Lab

rows=size(x,1);
cols=size(y,2);
N=256;

h=zeros(N,N);

for i=1:rows;
  for j=1:cols;
    h(x(i,j)+1,y(i,j)+1)= h(x(i,j)+1,y(i,j)+1)+1;
  end
end

imshow(h)

end


function eta=shocksim3(cfconds,Fperiods,N,n,fmat,ortirfcell)

% prelminary tasks: initiate R and r
R=[];
r=[];

% loop over forecast periods to build the matrix R and the vector r
for ii=1:Fperiods
   % for period ii, check if there is a condition, variable after variable
   for jj=1:N*n
      % if there is a condition
      if ~isempty(cfconds{ii,jj})
      % fill r with the corresponding condition, minus forecast..
      r=[r;cfconds{ii,jj}-fmat(ii,jj)];
      % .. and R with the corresponding orthogonalised IRFs entries
      % first increment R with a row of zeros of suitable dimension (N*n*Fperiods)
      R=[R;sparse(1,N*n*Fperiods)];
         % loop over periods up to the one on which there is the constraint
         for kk=1:ii
         R(end,(kk-1)*(N*n)+1:kk*(N*n))=ortirfcell{ii-kk+1,ii}(jj,:);
         end
      % if there is no condition, don't do anything
      end
   end
end

% once the linear system is identified, draw a full vector of shocks from the Waggoner-Zha distribution
% realise the singular value decomposition of R as in (3.3.17) and obtain the corresponding matrices
% recover Q and K, the dimensions of the matrix Rtemp
[Q,K]=size(R);
% obtain the singular value decomposition
[U,S,V]=svd(full(R));
% obtain the required matrices
P=S(:,1:Q);
V1=V(:,1:Q);
V2=V(:,Q+1:end);
% draw the vector of constrained shocks from N(etabar,gammabar)
eta=V1/P*U'*r+V2*randn(K-Q,1);


function [ rectangles ] = pruneRectangle( rotImg, rectangles )
%PRUNERECTANGLE Prune rectangles based on gradients
%   Rectangles should present stronger gradients on edges

rotImg = imresize(rotImg, 0.25);
[H,W,~] = size(rotImg);
% img = zeros(H,W);

[FX, FY] = gradient(double(rgb2gray(rotImg)));
F = sqrt(FX.^2 + FY.^2);
[FD, ~, ~] = dt2(F, 0.1, 0, 0.1, 0 );

for vid = 1:6
    numrect = size(rectangles(vid).highPixelBox,1);
    valid = false(numrect, 1);
    rects = rectangles(vid).xyzBox;
    for i = 1:numrect
        rect = [rects(i,1:3); rects(i,4:6); rects(i,7:9); rects(i,10:12)];
        lines = lineFromTwoPoint(rect([1 2 3 4],:), rect([2 3 4 1],:));
        panoReport = paintParameterLine( lines, W, H);
        G = FD(panoReport(:)>0);
        SG = sort(G, 'descend');
        N = round(length(G)/2);

%         valid(i) = sum(SG(1:N))/N;
        if sum(SG(1:N))/N > 20
            valid(i) = true;
        end
    end

    rectangles(vid).highPixelBox = rectangles(vid).highPixelBox(valid,:);
    rectangles(vid).count = sum(valid);
    rectangles(vid).xyzBox = rectangles(vid).xyzBox(valid,:);
    rectangles(vid).score = rectangles(vid).score(valid);
end

end


function Write_CNN_to_binary(location_binary, cnn)

    addpath('../../../PDM_helpers/');

    % use little-endian
    cnn_binary_file = fopen(location_binary, 'w', 'l');

    num_layers = size(cnn.layers,2);

    % Get the number of layers
    fwrite(cnn_binary_file, num_layers, 'uint'); % 4 bytes

    for layers=1:num_layers

        % write layer type: 0 - convolutional, 1 - max pooling, 2 -
        % fully connected, 3 - prelu, 4 - sigmoid
        if(strcmp(cnn.layers{layers}.type, 'conv'))

            % write the type (convolutional)
            fwrite(cnn_binary_file, 0, 'uint'); % 4 bytes

            num_in_map = size(cnn.layers{layers}.weights{1},3);

            % write the number of input maps
            fwrite(cnn_binary_file, num_in_map, 'uint'); % 4 bytes

            num_out_kerns = size(cnn.layers{layers}.weights{1},4);

            % write the number of kernels for each output map
            fwrite(cnn_binary_file, num_out_kerns, 'uint'); % 4 bytes

            % Write output map bias terms
            for k2=1:num_out_kerns
                fwrite(cnn_binary_file, cnn.layers{layers}.weights{2}(k2), 'float32'); % 4 bytes
            end

            for k=1:num_in_map
                for k2=1:num_out_kerns
                    % Write out the kernel
                    W = squeeze(cnn.layers{layers}.weights{1}(:,:,k,k2));
                    writeMatrixBin(cnn_binary_file, W, 5);
                end
            end
        elseif(strcmp(cnn.layers{layers}.type, 'fc'))

            % This is the fully connected layer
            fwrite(cnn_binary_file, 2, 'uint'); % 4 bytes

            % the bias term
            writeMatrixBin(cnn_binary_file, cnn.layers{layers}.weights{2}, 5);
            % the weights
            writeMatrixBin(cnn_binary_file, cnn.layers{layers}.weights{1}, 5);

        elseif(strcmp(cnn.layers{layers}.type, 'max_pooling'))
            fwrite(cnn_binary_file, 1, 'uint'); % 4 bytes, indicate max pooling layer
            % params kernel and stride size
            fwrite(cnn_binary_file, cnn.layers{layers}.kernel_size_x, 'uint'); % 4 bytes
            fwrite(cnn_binary_file, cnn.layers{layers}.kernel_size_y, 'uint'); % 4 bytes
            fwrite(cnn_binary_file, cnn.layers{layers}.stride_x, 'uint'); % 4 bytes
            fwrite(cnn_binary_file, cnn.layers{layers}.stride_y, 'uint'); % 4 bytes

        elseif(strcmp(cnn.layers{layers}.type, 'prelu'))
            fwrite(cnn_binary_file, 3, 'uint'); % 4 bytes, indicate a parametric relu layer
            writeMatrixBin(cnn_binary_file, cnn.layers{layers}.weights{1}, 5);
        end
    end

    fclose(cnn_binary_file);

end


function [nfm_Sharp_lunwrap, mask_sharp] = backgroundRemovalSharp(phase_lunwrap, mask_pad, filterMode)
%BACKGROUNDREMOVALSHARP Background phase removal using SHARP (Sophisticated harmonic artifact reduction for phase data)
%   Code refractored from Berkin Bilgic's scripts: "script_Laplacian_unwrap_Sharp_Fast_TV_gre3D.m"
%   and "script_Laplacian_unwrap_Sharp_Fast_TV_gre3D.m"
%   Original source: https://martinos.org/~berkin/software.html
%
%   phase_lunwrap: unwrapped phase volume
%   mask_pad: zero-padded mask volume
%   filterMode: 'once' or 'iterative'
%
%   Original reference:
%   Bilgic et al. (2014), Fast quantitative susceptibility mapping with
%   L1-regularization and automatic parameter selection. Magn. Reson. Med.,
%   72: 1444-1459. doi:10.1002/mrm.25029
%
%   ... which references:
%
%   Li W, Wu B, Liu C. Quantitative susceptibility mapping of
%   human brain reflects spatial variation in tissue composition.
%   NeuroImage 2011; 55(4): 1645?1656.
%

    if nargin < 4
        filterMode = 'once';
    end
    switch filterMode
        case 'once'
            [nfm_Sharp_lunwrap, mask_sharp] = sharp_once(phase_lunwrap, mask_pad);
        case 'iterative'
            [nfm_Sharp_lunwrap, mask_sharp] = sharp_iterative(phase_lunwrap, mask_pad);
    end

end

function [nfm_Sharp_lunwrap, mask_sharp] = sharp_once(phase_lunwrap, mask_pad)

    N = size(mask_pad);

    ksize = [9, 9, 9];                % Sharp kernel size
    threshold = .05;                  % truncation level

    % calculate del kernel and its inverse
    del_sharp = calc_del_kernel(ksize, N);

    delsharp_inv = zeros(size(del_sharp));
    delsharp_inv( abs(del_sharp) > threshold ) = 1 ./ del_sharp( abs(del_sharp) > threshold );

    % erode mask to remove convolution artifacts
    mask_sharp = erode_mask(mask_pad, ksize);

    % apply Sharp to Laplacian wrapped phase
    phase_del = ifftn(fftn(phase_lunwrap) .* del_sharp);
    Phase_Del = phase_del .* mask_sharp;

    phase_Sharp_lunwrap = real( ifftn(fftn(Phase_Del) .* delsharp_inv) .* mask_sharp );

    nfm_Sharp_lunwrap = phase_Sharp_lunwrap;
end

function [nfm_Sharp_lunwrap, mask_sharp] = sharp_iterative(phase_lunwrap, mask_pad)

    N = size(mask_pad);

    threshold = .05;                     % truncation level

    Kernel_Sizes = 9:-2:3;

    % initiate volumes
    Phase_Del = zeros(N);
    mask_prev = zeros(N);

    for k = 1:length(Kernel_Sizes)

        disp(['Kernel size: ', num2str(Kernel_Sizes(k))])

        Kernel_Size = Kernel_Sizes(k);
        ksize = [Kernel_Size, Kernel_Size, Kernel_Size];                % Sharp kernel size

        % calculate del kernel and its inverse
        del_sharp = calc_del_kernel(ksize, N);

        if k == 1
            delsharp_inv = zeros(size(del_sharp));
            delsharp_inv( abs(del_sharp) > threshold ) = 1 ./ del_sharp( abs(del_sharp) > threshold );
        end

        % erode mask to remove convolution artifacts
        mask_sharp = erode_mask(mask_pad, ksize);

        % apply Sharp to Laplacian unwrapped phase
        phase_del = ifftn(fftn(phase_lunwrap) .* del_sharp);
        Phase_Del = Phase_Del + phase_del .* (mask_sharp - mask_prev);

        mask_prev = mask_sharp;

    end

    phase_Sharp_lunwrap = real( ifftn(fftn(Phase_Del) .* delsharp_inv) .* mask_sharp );
    nfm_Sharp_lunwrap = phase_Sharp_lunwrap;

end

function del_sharp = calc_del_kernel(ksize, N)

    khsize = (ksize-1)/2;
    [a,b,c] = meshgrid(-khsize(2):khsize(2), -khsize(1):khsize(1), -khsize(3):khsize(3));

    kernel = (a.^2 / khsize(1)^2 + b.^2 / khsize(2)^2 + c.^2 / khsize(3)^2 ) <= 1;
    kernel = -kernel / sum(kernel(:));
    kernel(khsize(1)+1,khsize(2)+1,khsize(3)+1) = 1 + kernel(khsize(1)+1,khsize(2)+1,khsize(3)+1);

    Kernel = zeros(N);
    Kernel( 1+N(1)/2 - khsize(1) : 1+N(1)/2 + khsize(1), 1+N(2)/2 - khsize(2) : 1+N(2)/2 + khsize(2), 1+N(3)/2 - khsize(3) : 1+N(3)/2 + khsize(3) ) = -kernel;

    del_sharp = fftn(fftshift(Kernel));

end

function mask_sharp = erode_mask(mask_pad, ksize)

    erode_size = ksize + 1;

    mask_sharp = imerode(mask_pad, strel('line', erode_size(1), 0));
    mask_sharp = imerode(mask_sharp, strel('line', erode_size(2), 90));
    mask_sharp = permute(mask_sharp, [1,3,2]);
    mask_sharp = imerode(mask_sharp, strel('line', erode_size(3), 0));
    mask_sharp = permute(mask_sharp, [1,3,2]);

end


function data = readImage(img)
    data.intensity = mean2(img);
    data.red = mean2(img(:,:,1));
    data.green = mean2(img(:,:,2));
    data.blue = mean2(img(:,:,3));

    gimg = rgb2gray(img);
    range = rangefilt(gimg);
    std = stdfilt(gimg);
    entropy = entropyfilt(gimg);

    data.mean_range = mean2(range);
    data.std_range = std2(range);
    data.mean_std = mean2(std);
    data.std_std = std2(std);
    data.mean_entropy = mean2(entropy);
    data.std_entropy = std2(entropy);

end


function [alpha,ro] = fit_corr(corr)

kmax = length(corr);

[x eval] = fminsearch(@fit_model, [0, 1/2], optimset('TolX',1e-8),kmax,corr);

alpha = x(1);
ro = x(2);