function [y1]=rs(y1,T,f2)
% [y2] = rs(y1, T) resamples y1 to the target sampling rate y2 using T
% [y2] = rs(y1, f1, f2) resamples y1 with f1 to the target sampling rate f2
%
% RS does not require overlap data.
%
% see also: SOPEN, SREAD, SCLOSE, MAT2SEL, SAVE2TXT, SAVE2BKR
%
% Reference(s):

%	$Revision: 1.2 $
%	$Id: rs.m 2202 2009-10-27 12:06:45Z schloegl $
%	Copyright (C) 1997-2004 by Alois Schloegl
%	a.schloegl@ieee.org
%    	This is part of the BIOSIG-toolbox http://biosig.sf.net/

% This library is free software; you can redistribute it and/or
% modify it under the terms of the GNU Library General Public
% License as published by the Free Software Foundation; either
% Version 2 of the License, or (at your option) any later version.
%
% This library 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
% Library General Public License for more details.
%
% You should have received a copy of the GNU Library General Public
% License along with this library; if not, write to the
% Free Software Foundation, Inc., 59 Temple Place - Suite 330,
% Boston, MA  02111-1307, USA.

if nargin==3,
        f1=T;
        if f1==f2
                return;
        elseif f1>f2
                D=f1/f2;
                [yr,yc]=size(y1);
                LEN=yr/D;
		y2=zeros(yr*f2/f1,yc);
                for k=0:LEN-1
                        y2(k+1,:)=sum(y1(k*D+(1:D),:),1)/D;
                end;
		y1=y2;
        else %f1<f2
		y1=y1(ceil((1:size(y1,1)*f2/f1)/f2*f1),:);
        end;

elseif nargin==2,
        [f1,f2]=size(T);
        if f1==f2,
                return;
        end;
        [yr,yc]=size(y1);
        LEN=yr/f1;
	y2=zeros(yr*f2/f1,yc);
        for k=0:LEN-1
                y2(k*f2+(1:f2),:)=T'*y1(k*f1+(1:f1),:);
        end;
	y1=y2;
end;


% sideways motion of center of rotation
function [data,units] = compute_dv_cor(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  if trx(fly).nframes < 2,
    data{i} = [];
  else
    % center of rotation
    [x_cor_curr,y_cor_curr,x_cor_next,y_cor_next] = rfrac2center(trx,fly,[trx(fly).corfrac_maj;trx(fly).corfrac_min]);
    % change in center of rotation
    dx_cor = x_cor_next - x_cor_curr;
    dy_cor = y_cor_next - y_cor_curr;
    % forward motion of center of rotation
    data{i} = (dx_cor.*cos(trx(fly).theta_mm(1:end-1)+pi/2) + dy_cor.*sin(trx(fly).theta_mm(1:end-1)+pi/2))./trx(fly).dt;

  end

end
units = parseunits('mm/s');


function bnet_miss = gener_MAR_net(bnet_orig, base_proba)
% function bnet_miss = gener_MAR_net(bnet_orig, base_proba)
%
%   bnet_orig : a bnet
%   base_proba :  a probability for value to be missing
%
%   bnet_miss : a bnet that could be used in gener_data_from_bnet_miss function
%               to generate incomplete MAR dataset
%
% Francois.Olivier.C.H@gmail.com

%%%%%%%%%%%% INIT
if nargin<2, error('Not enougth arguments'); end

% création du réseau
dag = bnet_orig.dag;
N = size(dag,2);
ns = bnet_orig.node_sizes;

  ns_miss = zeros(1,3*N);
  ns_miss(1:N) = ns;
  ns_miss(N+1:2*N) = 2*ones(1,N); % 1= node i-N present, 2= node i-N missing
  ns_miss(2*N+1:3*N) = ns+1; % 1:ns, absent

  dag_miss = zeros(3*N,3*N);
  dag_miss(1:N,1:N) = dag;
%  dag_miss(2*N+1:3*N,N+1:2*N)=mk_rnd_dag(N,N-ceil(rand*N/2));
  lim = 1+(rand>.4)+(rand>.65)+(rand>.9);
  dag_miss(2*N+1:3*N,N+1:2*N)=mk_rnd_dag(N,lim);

  for i=1:N, dag_miss(i,2*N+i)=1; dag_miss(N+i,2*N+i)=1; dag_miss(2*N+i,i)=0; dag_miss(2*N+i,N+i)=0; end

  bnet_miss = mk_bnet(dag_miss, ns_miss);
  CPT = CPT_from_bnet(bnet_orig, 0);
  for i=1:N
    bnet_miss.CPD{i} = tabular_CPD (bnet_miss, i, CPT{i});
  end

  % CPD of nodes M
  for i=1:N,
     if find(bnet_miss.order==i)<find(bnet_miss.order==N+i),
        CPT_M=[];
        for j=1:ns_miss(i), for l=1:ns_miss(N+i), for k=1:ns_miss(2*N+i),
          CPT_M=[CPT_M (((j==k)&(l==1))|((k==ns_miss(2*N+i))&(l==2)))];
        end, end, end
     else
        CPT_M=[];
        for k=1:ns_miss(2*N+i), for l=1:ns_miss(N+i), for j=1:ns_miss(i),
          CPT_M=[CPT_M (((j==k)&(l==1))|((k==ns_miss(2*N+i))&(l==2)))];
        end, end, end
     end
     bnet_miss.CPD{2*N+i} = tabular_CPD (bnet_miss, 2*N+i, CPT_M);
  end

%%%%%%%%%%%% Base probability of missing value
p = base_proba;
 for i=1:N
  fam = find(dag_miss(:,N+i)==1)';
  semisize = prod(ns_miss(fam)); % as node N+i is binary to say i is present or missing
  CPT = zeros(1,2*semisize);
  CPT(1:semisize) = 1-p;
  CPT(semisize+1:2*semisize) = p;
  bnet_miss.CPD{N+i} = tabular_CPD (bnet_miss, N+i, CPT);
 end

   BETA = gener_discrete_dist(N, base_proba);

   order=[];
   missdagtmp = bnet_miss.dag; %(N+1:2*N,N+1:2*N);
   unprocessed = 1:N;
   while ~isempty(unprocessed)
     npar=[];
     for i=N+1:2*N, npar(end+1)=length(parents(missdagtmp,i));end,  % to be verifie from here
     [npar, ord] = sort(npar);
     while ~ismember(ord(1),unprocessed)
       ord=ord(2:end);
     end
     order = [order, ord(1)];
     missdagtmp(ord(1),:)=0;
     unprocessed = mysetdiff(unprocessed,ord(1));
   end

%%%%%%%%%%%% Update CPT with MCAR process
for i=1:length(BETA)
  fam_miss = find(dag_miss(:,N+order(i))==1)';
  p=BETA(i);
  semisize = prod(ns_miss(fam_miss));

  if isempty(fam_miss),
        CPT = zeros(1,2*semisize);
        CPT(1:semisize) = 1-p;
        CPT(semisize+1:2*semisize) = p;
        bnet_miss.CPD{N+order(i)} = tabular_CPD (bnet_miss, N+order(i), CPT);
  else

        MUi1k = gener_discrete_dist(semisize, p);
        CPT = zeros(1,2*semisize);
        CPT(1:semisize) = 1-MUi1k;
        CPT(semisize+1:2*semisize) = MUi1k;
        bnet_miss.CPD{N+order(i)} = tabular_CPD (bnet_miss, N+order(i), CPT);

  end

end


function [ calmars ] = calmar( cumReturns, mdds, frequency )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Doyen Sahoo
% Contributors: Steven Hoi
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    [r c] = size(cumReturns);
    calmars = zeros(1,c);
    den = 252/frequency;
    Y = r/den;
    annualisedReturns = ((cumReturns(r,:)./cumReturns(1,:)) .^ (1/Y)) - 1;
    calmars = annualisedReturns ./ mdds;
end


function signal = flt_delayembed(varargin)
% Apply delay embedding to epoched data.
% Signal = flt_delayembed(Signal,NumLags)
%
% Delay embedding is essentially appending to each multi-channel samples the subsequent k
% multi-channel samples, and thereby multiplies the number of channels by k. Delay embedding is a
% practical tool to extend linear spatial models (e.g., spatial filters or independent components)
% to linear spatio-temporal (and therefore implicitly spatio-spectral) models, just by applying
% those models to delay-embedded data. As a result, approaches that can learn optimal spatial
% filters (finding sources of interest) can be repurposed to learning optimal spatio-spectral
% filters (jointly finding sources and frequencies of interest).
%
% The tradeoff associated with delay-embedding is that the complexity of the models increases and
% they become harder to estimate, which might require more data or better
% constraints, priors, or regularization.
%
% In:
%   Signal : Epoched data set to be processed
%
%   NumLags : the number of lags that shall be used for delay-embedding (default: 1)
%
%   IncludeIntermediates : Include intermediate lags. If this is set to false, only the 0''th and
%                          the N''th lag will be embedded. (default: true)
%
% Out:
%   Signal : the processed signal; will have more channels
%
% Notes:
%   The temporal filters that can be designed for a small number of lags are often limited to
%   high-frequency responses; the frequency range can often be extended without increasing model
%   complexity by first resampling the data to the lowest acceptable sampling rate (e.g., 60 Hz).
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2013-11-17

% flt_delayembed_version<1.00> -- for the cache

if ~exp_beginfun('filter') return; end

% requires epoched data, works best on spatially filtered data
declare_properties('name','DelayEmbedding', 'depends','set_makepos', 'follows',{'flt_project','flt_window'}, 'independent_channels',false, 'independent_trials',true);

% declare arguments
arg_define(varargin,...
    arg_norep({'signal','Signal'}), ...
    arg({'numlags','NumLags'}, 1, uint32([1 1 20 1000]), 'Number of lags. For delay-embedding.'), ...
    arg({'includeIntermediates','IncludeIntermediates'}, true, [], 'Include intermediate lags. If this is set to false, only the 0''th and the N''th lag will be embedded.'));

for k=quickif(includeIntermediates,0:numlags,[0 numlags])
    tmp{k+1} = signal.data(:,k+(1:end-numlags),:); end
signal.data = cat(1,tmp{:});
[signal.nbchan,signal.pnts,signal.trials] = size(signal.data);
signal.chanlocs = struct('labels', cellfun(@num2str,num2cell(1:signal.nbchan),'UniformOutput',false));

exp_endfun;


function [fusion,w0] = qfuser_v5(w,scores,wfuse)

if nargin==0
    test_this();
    return;
end

% block 1
f1 = linear_fuser([],scores.scores);
w1 = wfuse;
[whead,wtail] = splitvec_fh(length(w1));
f1 = f1(whead);

% block 2
modelQ = scores.modelQ;
[q,n1] = size(modelQ);
modelQ = [modelQ;ones(1,n1)];
segQ = scores.segQ;
[q2,n2] = size(segQ);
segQ = [segQ;ones(1,n2)];
assert(q==q2);
q = q + 1;

wq = q*(q+1)/2;
f2 = AWB_fh(modelQ',segQ,tril_to_symm_fh(q,wtail));
w2 = zeros(wq,1);

% assemble
fusion = sum_of_functions(w,[1,1],f1,f2);
w0 = [w1;w2];

end

function test_this()

m = 5;
k = 2;
n1 = 4;
n2 = 5;

scores.sindx = [1,2,3];
scores.qindx = [4,5];

scores.scores = randn(m,n1*n2);
scores.modelQ = randn(k,n1);
scores.segQ = randn(k,n2);

wfuse = [1,2,3,4]';

[fusion,w0] = qfuser_v4([],scores,wfuse);

%test_MV2DF(fusion,w0);

[fusion(w0),linear_fuser(wfuse,scores.scores(scores.sindx,:))]

%fusion(w0)

end


%Leave One Out Cross Validation with Nearest Neighbor Classifier
%>
%> @param FeatureMatrix: features (dimension iNumFeatures x iNumObservations)
%> @param ClassIdx: vector with class indices (length iNumObservations, starting from 0)
%>
%> @retval Acc overall accuracy after Cross-Validation
% ======================================================================
function [Acc, conf_mat] = ToolLooCrossVal(FeatureMatrix, ClassIdx)

    % initialize
    TP = 0;

    conf_mat = zeros(length(unique(ClassIdx)));

    % loop over observations
    for o = 1:size(FeatureMatrix, 2)
        % remove current observation from 'training set'
        v_train = [FeatureMatrix(:, 1:o-1) FeatureMatrix(:, o+1:end)]';
        C_train = [ClassIdx(1:o-1) ClassIdx(:, o+1:end)]';

        % compute result of Nearest Neighbor Classifier given the traindata
        res = ToolSimpleKnn(FeatureMatrix(:, o)', v_train, C_train, 1);

        conf_mat(ClassIdx(o)+1, res+1) = conf_mat(ClassIdx(o)+1, res+1) + 1;

        % if result is correct increment number of true positives
        if (res == ClassIdx(o))
            TP = TP+1;
        end
    end

    % compute overall (micro) accuracy
    Acc = TP / length(ClassIdx);
end


function [ output_args ] = bz_LFPPowerDist( LFP,varargin )
% bz_LFPPowerDist(LFP) calculates the power distribution of an LFP signal.
%   NOTE: THIS FUNCTION IS UNDER DEVELOPMENT. Feel free to chip in.

%
%INPUTS
%   LFP     [nt x 1] vector of the LFP signal -or- basename
%   (optional)
%   sf          sampling frequency of the LFP (default 1250Hz)
%   int         restrict analysis to specific time intervals
%   spectype    'wavelet' or 'FFT'
%   frange      [lowf highf]
%   nfreqs      number of frequencies to look at
%   SHOWFIG     true/false (default: false)
%   figfolder   folder to save output figures
%
%% DEV
%datasetfolder = '/Users/dlevenstein/Dropbox/Research/Datasets/BWData/';
%figfolder = '/Users/dlevenstein/Dropbox/Research/Current Projects/misc/PowerDistribution/';
%recname = '20140526_277um';

%load(fullfile(datasetfolder,recname,[recname,'_LFP.mat']))
%load(fullfile(datasetfolder,recname,[recname,'_SleepScore.mat']))
%%
sf = 1250;
%int = StateIntervals.NREMpacket;

%LFP = LFP.CTX;
%LFP = NormToInt(LFP,int,sf,'modZ');

spectype = 'FFT';

%%
warning('This function is still under development. No promises')

frange = [1 128];
nfreqs = 100;

switch spectype
    case 'FFT'
        freqlist = logspace(log10(frange(1)),log10(frange(2)),nfreqs);
        window = 1;
        noverlap = 0.8;
        window = window*sf;
        noverlap = noverlap*sf;
        [spec,freqs,t_FFT] = spectrogram(LFP,window,noverlap,freqlist,sf);
        spec = abs(spec)';

        [~,inintIDX] = RestrictInts(t_FFT',int);
        intspec = spec(inintIDX,:);
    case 'wavelet'
        downsamplefactor = 2;
        sf_down = sf./downsamplefactor;
        intLFP = IsolateEpochs2(downsample(LFP,2),int,0,sf_down);
        ncyc = 5;
        [freqlist,t,intspec] = bz_WaveSpec(intLFP,frange,nfreqs,ncyc,1/sf_down,'log');
        intspec = cat(2,intspec{:});
        intspec = abs(intspec)';
end

%%
numpowerbins = 200;

%minpower = min(intspec(:)); maxpower = max(intspec(:));
switch spectype
    case 'FFT'
        minpower = 0.5;maxpower = 2.85e3;
    case 'wavelet'
        minpower = -2;maxpower = 2;
end

powerbins = linspace(log10(minpower),log10(maxpower),numpowerbins);
[powerdist_mean] = hist(log10(intspec),powerbins);

%%
figure
%subplot(2,2,1)
imagesc(log2(freqlist),powerbins,powerdist_mean)
axis xy
LogScale('x',2)

%%
alldists = {'birnbaumsaunders','exponential','extreme value','gamma',...
    'generalized extreme value','generalized pareto','inverse gaussian',...
    'logistic','loglogistic','lognormal','nakagami','normal','rayleigh',...
    'rician','tlocationscale','weibull'};

%Distirbutions were removed that consistently showed bad fit to cortical
%LFP data during NREM
testdists = {'gamma','loglogistic','lognormal',...
    'rayleigh','weibull'};

showexamples = [];

D = {};PF = {};bestfit={};
for ff = 1:nfreqs
    ff
    if ismember(ff,showexamples)
        [distfits] = allfitdist(intspec(:,ff),'PDF');
    else
        [distfits] = allfitdist(intspec(:,ff));
    end

    %Keep only the distributions tested
    distnames = {distfits(:).DistName};
    keepdists = ismember(distnames,testdists);
    distfits = distfits(keepdists);

    %Check the best and worst-fitting distribution
    bestfit{ff} = distfits(1).DistName;
    worstfit{ff} = distfits(end).DistName;

    %Sort alphabetically
    distnames = {distfits(:).DistName};
    [distnames,sortdist] = sort(distnames);

    D{ff} = distfits(sortdist);
end

%%
AICs = cellfun(@(X) [X(:).AIC],D,'UniformOutput',false);
AICs = cat(1,AICs{:});

ndists = length(distnames);
bestfitmat = zeros(nfreqs,ndists);
for nn = 1:ndists
    bestfitmat(:,nn) = nn.*strcmp(distnames{nn},bestfit);
end
bestfitmat(bestfitmat==0)=nan;
%%
figure
subplot(2,2,1)
    imagesc(log2(freqlist),powerbins,powerdist_mean)
    axis xy
    LogScale('x',2);LogScale('y',10)
    xlabel('f (Hz)')
    ylabel('Power (AU)')
    title([spectype,' Power Distribution'])

subplot(4,2,5)
    plot(log2(freqlist),log10(AICs),'LineWidth',1)

    legend(distnames,'location','southwest')
    axis tight
    LogScale('x',2);%LogScale('y',10)
    xlabel('f (Hz)')
    box off
    ylabel('log(AIC)')

subplot(6,2,11)
        plot(log2(freqlist),bestfitmat,'.','markersize',20)
        set(gca,'YTick',1:ndists)
        set(gca,'YTickLabels',distnames)
    LogScale('x',2);%LogScale('y',10)
    xlabel('f (Hz)')
    box off

    NiceSave('LFPPowerDist',figfolder,recname)

end


function [error_x, error_y, fex, fey, ae] = stokespost_q1q1_p(q1q1sol,jmpx,jmpy,els,xy,ev)
%stokespost_q1q1_p  computes Poisson error estimator for Q1-Q1
%   [err_x, err_y, fex, fey, ae] = stokespost_q1p0_p(q1q1sol,jmpx,jmpy,els,xy,ev);
%   input
%          q1q1sol        Q1-Q1 flow solution
%          jmpx, jmpy     component elementwise edge stress jumps
%          els            elementwise edge lengths
%          xy             vertex coordinate vector
%          ev             element mapping matrix
%   output
%          err_x, err_y   component of velocity elementwise error estimate
%          fex, fey       component elementwise rhs vectors
%          ae             elementwise Poisson problem matrices
%
%   IFISS function: DJS; 8 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      fprintf('computing local error estimator... ')
      x=xy(:,1); y=xy(:,2);nvtx=length(x);
      nel=length(ev(:,1));
      error_x=zeros(nel,1);  error_y=zeros(nel,1);
      psol=q1q1sol(2*nvtx+1:end);
%
% set up 3x3 Gauss points
      gpt=sqrt(0.6);
      s(1) = -gpt; t(1) = -gpt; wt(1)=25/81;
      s(2) =  gpt; t(2) = -gpt; wt(2)=25/81;
      s(3) =  gpt; t(3) =  gpt; wt(3)=25/81;
      s(4) = -gpt; t(4) =  gpt; wt(4)=25/81;
      s(5) =  0.0; t(5) = -gpt; wt(5)=40/81;
      s(6) =  gpt; t(6) =  0.0; wt(6)=40/81;
      s(7) =  0.0; t(7) =  gpt; wt(7)=40/81;
      s(8) = -gpt; t(8) =  0.0; wt(8)=40/81;
      s(9) =  0.0; t(9) =  0.0; wt(9)=64/81;
%
% inner loop over elements
        for ivtx = 1:4
        xl_v(:,ivtx) = x(ev(:,ivtx));
        yl_v(:,ivtx) = y(ev(:,ivtx));
        psol_v(:,ivtx) = psol(ev(:,ivtx));
		end

        ae = zeros(nel,5,5); elerrx=zeros(5,nel);  elerry=zeros(5,nel);
        fex = zeros(nel,5); fey = zeros(nel,5);
% loop over Gauss points
         for igpt = 1:9
         sigpt=s(igpt);
         tigpt=t(igpt);
         wght=wt(igpt);
% evaluate derivatives etc
         [jac_v,invjac_v,phi_v,dphidx_v,dphidy_v] = deriv(sigpt,tigpt,xl_v,yl_v);
         [psi_v,dpsidx_v,dpsidy_v] = qderiv(sigpt,tigpt,xl_v,yl_v);

            for j = 1:5
               for i = 1:5
               ae(:,i,j) = ae(:,i,j)+wght*dpsidx_v(:,i+4).*dpsidx_v(:,j+4).*invjac_v(:);
               ae(:,i,j) = ae(:,i,j)+wght*dpsidy_v(:,i+4).*dpsidy_v(:,j+4).*invjac_v(:);
               end
               for ss=1:4
               fex(:,j) = fex(:,j)-wght*dphidx_v(:,ss).*psol_v(:,ss).*psi_v(:,j+4);
               fey(:,j) = fey(:,j)-wght*dphidy_v(:,ss).*psol_v(:,ss).*psi_v(:,j+4);
               end
            end
% end of Gauss point loop
         end
%
% include edge jumps (evaluated at the midpoint)
         for ee = 1:4
         fex(:,ee) = fex(:,ee) - jmpx(:,ee) .* els(:,ee)*(1/3);
         fey(:,ee) = fey(:,ee) - jmpy(:,ee) .* els(:,ee)*(1/3);
         end
%
% solve for local estimate
         for ielem = 1:nel
		 elerrx(:,ielem) = squeeze(ae(ielem,1:5,1:5))\(fex(ielem,1:5)');
		 elerry(:,ielem) = squeeze(ae(ielem,1:5,1:5))\(fey(ielem,1:5)');
	     end
%%
         for ivtx=1:5,
	     error_x(:) = error_x(:) + fex(:,ivtx) .* elerrx(ivtx,:)';
		 error_y(:) = error_y(:) + fey(:,ivtx) .* elerry(ivtx,:)';
         end
%%
         fprintf('done.\n')
		 return


% DEMOIL1 Oil data with fully independent training conditional.

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'oil';
experimentNo = 7;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('fitc');
options.optimiser = 'scg';
latentDim = 2;
d = size(Y, 2);
options.kern = {'gibbs', 'bias', 'white'};
model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model');

if exist('printDiagram') & printDiagram
  fgplvmPrintPlot(model, lbls, capName, experimentNo);
end

% Load the results and display dynamically.
fgplvmResultsDynamic(dataSetName, experimentNo, 'vector')

% compute the nearest neighbours errors in latent space.
errors = fgplvmNearestNeighbour(model, lbls);


function keep = newtonpolytope(exponent_m,exponent_p)
%NEWTONPOLYTOPE  Internal function to remove monimials in SOS programs using Newton polytope

% WARNING : THIS CODE SUCKS AND IS ONLY USED AS A BACK UP PLAN
% IF EVERYTHING ELSE FAILS (CRASHING LP SOLVERS ETC)

% *************************************
% TRY TO CALCULATE CONVEX HULL
% *************************************
try
    cnvhull = convhulln(full(exponent_p));
catch
    keep = 1:size(exponent_m,1);
    return
end

% ***************************************
% GET THE UNIQUE POINTS OF IN CONVEX HULL
% ***************************************
unique_points = unique(cnvhull);

% ***************************************
% CALCULATE A POINT IN INTERIOR
% ***************************************
p_c = sum(exponent_p(unique_points,:),1)'/length(unique_points);

% ***************************************
% CALCULATE HYPER-PLANES Ai^T(x-bi)=0
% ***************************************
j = 1;
A = [];
for i = 1:size(cnvhull,1)
    X = exponent_p(cnvhull(i,:),:)';
    y = X(:,1);
    dX = X(:,2:end)-repmat(X(:,1),1,size(X,2)-1);
    Atemp = null(full(dX'));
    % Is this a full-dimensional facet
    if size(Atemp,2)==1
        direction = (p_c-y)'*Atemp;
        if direction > 0
           A{j}=-Atemp;
       else
           A{j}=Atemp;
       end
        b{j}=y;
        j=j+1;
    end
end

% ***************************************
% CHECK IF MONOMIAL IS IN NEWTON POLYTOPE
% ***************************************
if ~isempty(A)
    keep = [];
    for j = 1:size(exponent_m,1)
        inside = 1;
        y = exponent_m(j,:)';
        if isempty(findrows(exponent_p,y)) % Numerically safe on border
            i = 1;
            while inside & (i<=length(A))
                inside = inside & ((A{i}'*(y-b{i}))<=1e-9);
                i = i+1;
            end
        end
        if inside
            keep = [keep j];
        end
    end
else
    keep = 1:size(exponent_m,1);
end


function res = project_t(pts, H)
%% apply projective transformation H on points pts
%% pts, n x 2
%% H, 3 x 3
%% res, n x 2

pts = [pts, ones(size(pts, 1), 1)];
res = H * pts';
res = [ res(1,:)./res(3,:); res(2,:)./res(3,:) ];
res = res';


function plot_R_cov(Sigma_R_stat, cov_R, Rs)
Lies = dcm2Lie(Rs).';
mean_Lie = mean(Lies, 2);
colors = linspecer(3);
subplot(2, 2, 1);
plot_cov_linestyle(Sigma_R_stat, {'--', 3, colors(1, :)}, cov_R, {'-', 2, colors(3, :)}, Lies, mean_Lie, '.', 1, 2, 3, 10.5);
xlabel('$\xi_1$', 'Interpreter', 'LaTeX', 'FontSize', 15);
ylabel('$\xi_2$', 'Interpreter', 'LaTeX', 'FontSize', 15);

subplot(2, 2, 2);
plot_cov_linestyle(Sigma_R_stat, {'--', 3, colors(1, :)}, cov_R, {'-', 2, colors(3, :)}, Lies, mean_Lie, '.', 1, 3, 3, 10.5);
xlabel('$\xi_1$', 'Interpreter', 'LaTeX', 'FontSize', 15);
ylabel('$\xi_3$', 'Interpreter', 'LaTeX', 'FontSize', 15);

subplot(2, 2, 4);
plot_cov_linestyle(Sigma_R_stat, {'--', 3, colors(1, :)}, cov_R, {'-', 2, colors(3, :)}, Lies, mean_Lie, '.', 2, 3, 3, 10.5);
xlabel('$\xi_2$', 'Interpreter', 'LaTeX', 'FontSize', 15);
ylabel('$\xi_3$', 'Interpreter', 'LaTeX', 'FontSize', 15);

legend({'Stat Covariance', 'Data Points', 'Nguyen et al. Covariance'}, 'Interpreter', 'LaTeX', 'FontSize', 15);

Sigma_Lie_stat = covx(Lies.', Lies.')
end


function out = normest(f)
%NORMEST   Estimate the norm of a SINGFUN.
%   NORMEST(F) returns the NORMEST of the smooth part of a SINGFUN F. Since the
%   function value of a SINGFUN is infinite at -1 and 1 in most of the cases due
%   to the pole(s), NORMEST turn the estimated norm of the smooth part of F.

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

% Call NORMEST() of the smooth part:
out = normest(f.smoothPart);

end


function e = prtRvUtilDiscreteEntropy(q)
% DISCRETEENTROPY

e = q.*log(q);
e(q==0) = 0;
e = sum(e(:));

end


% Script to reproduce the experiments leading to the results provided in the
% Table 2 of the paper "Deep Scattering Spectrum" by J. Andén and S. Mallat.

% Delta-Delta-MFCCs for window size 370 ms

run_name = 'DSS_Table2_GTZAN_mfcc_370ms';

N = 5*2^17;

src = gtzan_src('/path/to/gtzan');

filt1_opt.wavelet_type = {'gabor','morlet'};
filt1_opt.Q = [8 2];
filt1_opt.J = T_to_J(8192,filt1_opt);
filt1_opt.boundary = 'symm';

sc1_opt.M = 1;

filters = filter_bank(N, filt1_opt);

features = {@(x)(feval(@(x)([x; circshift(x,[0 +1]); circshift(x,[0 -1])]), ...
	format_scat(log_scat(spec_freq_average(x,filters,sc1_opt)))))};

db = prepare_database(src,features);
db.features = single(db.features);
db = svm_calc_kernel(db,'gaussian','square',1:2:size(db.features,2));

rs = RandStream.create('mt19937ar','Seed',floor(pi*1e9));
RandStream.setGlobalStream(rs);
[train_set{1}, test_set{1}] = create_partition([src.objects.class], 0.9);
for k = 2:10
	[train_set{k}, test_set{k}] = ...
		next_fold([src.objects.class], train_set{k-1}, test_set{k-1});
end

optt.kernel_type = 'gaussian';
optt.C = 2.^[0:4:8];
optt.gamma = 2.^[-16:4:-8];
optt.search_depth = 3;
optt.full_test_kernel = 1;

for k = 1:10
	[dev_err_grid,C_grid,gamma_grid] = ...
		svm_adaptive_param_search(db,train_set{k},[],optt);

	[dev_err(k),ind] = min(mean(dev_err_grid{end},2));
	C(k) = C_grid{end}(ind);
	gamma(k) = gamma_grid{end}(ind);

	optt1 = optt;
	optt1.C = C(k);
	optt1.gamma = gamma(k);

	model = svm_train(db,train_set{k},optt1);
	labels(:,k) = svm_test(db,model,test_set{k});
	err(k) = classif_err(labels(:,k),test_set{k},db.src);

	fprintf('dev err = %f, test err = %f\n',dev_err(k),err(k));

	save([run_name '.mat'],'labels','dev_err','err','C','gamma');
end


function FR = FbApply2d( I, FB, shape, show )
% Applies each of the filters in the filterbank FB to the image I.
%
% To apply to a stack of images:
%  IFS = fevalArrays( images, @FbApply2d, FB, 'valid' );
%
% USAGE
%  FR = FbApply2d( I, FB, [shape], [show] )
%
% INPUTS
%  I       - 2D input array
%  FB      - filterbank - MxNxK set of K filters each of size MxN
%  shape   - ['full'] option for conv2 'full', 'same', 'valid'
%  show    - [0] first figure to use for optional display
%
% OUTPUTS
%  FR      - 3D set of filtered images
%
% EXAMPLE
%  load trees;  X=imresize(X,.5);  load FbDoG.mat;
%  FR = FbApply2d( X, FB, 'same', 1 );
%
% See also CONV2, FBMAKE
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(shape)); shape = 'full'; end
if( nargin<4 || isempty(show)); show=0; end

nd=ndims(I);  ndf=ndims(FB);  nf=size(FB,3);
if( nd~=2  ); error('I must be an MxN array'); end
if( ndf~=2 && ndf~=3 ); error('FB must be an MxN or MxNxK array'); end
if( ~isa(I,'double')); I = double(I); end

% apply each filter to image
if( ndf==2 )
  FR = conv2( I, FB, shape );
else
  FR = repmat( conv2(I,FB(:,:,1),shape), [1 1 nf] );
  for i=2:nf; FR(:,:,i)=conv2(I,FB(:,:,i),shape); end
end

% optionally display
if( show )
  figure(show); im(I);
  figure(show+1); montage2(FB,struct('extraInfo',1));
  figure(show+2); montage2(FR,struct('extraInfo',1));
end


clc;
close all;
clearvars;

n = 800000;
k = 100;
trials = 100;

sort_elapsed = 0;
quickselect_elapsed = 0;
for i=1:trials
    data = -randperm(n);
    tstart = tic;
    sort(data);
    sort_elapsed = sort_elapsed + toc(tstart);
    tstart = tic;
    y = spx.fast.quickselect(data, k);
    quickselect_elapsed = quickselect_elapsed + toc(tstart);
    fprintf('%d  %d\n', y(k), sum (y(1:k) < -k));
end

t1 = sort_elapsed;
t2 = quickselect_elapsed;
gain_x = t1 / t2;

fprintf('Sort: %.2f sec, quickselect: %.2f sec, Gain: %.2f\n', t1, t2, gain_x);


function [zPred, PzPred,otherInfo]=cubKalMeasPred(xPred,PPred,zDim,h,xi,w,innovTrans,measAvgFun,stateDiffTrans,stateTrans)
%%CUBKALMEASPRED Perform the measurement prediction part of the measurement
%           update step of the cubature Kalman filter with additive
%           measurement noise. The function cubKalUpdateWithPred can be
%           used to complete the measurement update. Separating the
%           measurement prediction step from the rest of the update step
%           can make the creation of multiple measurement association
%           hypotheses from a single target prediction more efficient.
%           The full measurement update function is cubKalUpdate.
%
%INPUTS: xPred The xDimXnumComp predicted target states.
%        PPred The xDimXxDimXnumComp predicted state covariance matrices.
%         zDim The dimensionality of the output of h.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%           xi An xDimXnumCubPoints matrix of cubature points. If this
%              and the next parameter are omitted or empty matrices are
%              passed, then fifthOrderCubPoints(xDim) is used. It is
%              suggested that xi and w be provided to avoid needless
%              recomputation of the cubature points.
%            w A numCubPointsX1 vector of the weights associated with the
%              cubature points.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   measAvgFun An optional function handle that, when given N measurement
%              values with weights, produces the weighted average. This
%              function only has to be provided if the domain of the
%              measurement is not linear. For example, when averaging
%              angular values, then the function meanAng should be used.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states and transforms them however
%              might be necessary. If not transformation is necessary, this
%              parameter can be omitted or an empty matrix passed.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one might generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: zPred The zDimXnumComp measurement predictions from the filter.
%        PzPred The zDimXzDimXnumComp covariance matrices associated with
%               zPred.
%     otherInfo A structure containing members of intermediate results of
%               this function that can be passed to cubKalUpdateWithPred
%               when updating with a measurement.
%
%The mathematics behind the cubature Kalman filter are described in more
%detail in Section IX of [1] and in [2]. See the comments to cubKalUpdate
%for more information.
%
%EXAMPLE:
%With this example, we demonstrate that one gets the same result using
%cubKalUpdate in one step as with using cubKalMeasPred followed by
%cubKalUpdateWithPred.
% xPred=[1e3;-2e3;100;200];
% h=@(x)([sum(x.^2);x(1)-x(4)^(3/2)]);
% PPred=[28,   3.5,    6,  8.5;
%      3.5,    23,  8.5,   11;
%        6,   8.5,   18, 13.5;
%      8.5,    11, 13.5,   13];
% z=1e6*[5.050000548964568;
%       -0.001829553054023];
% zDim=size(z,1);
% R=eye(zDim,zDim);%Measurement covariance matrix.
% %The update in one step.
% [xUpdate,PUpdate,innov,Pzz,W]=cubKalUpdate(xPred,PPred,z,R,h);
% %The update in two steps.
% [zPred, PzPred,otherInfo]=cubKalMeasPred(xPred,PPred,zDim,h);
% [xUpdate1,PUpdate1,innov1,Pzz1,W1]=cubKalUpdateWithPred(z,R,zPred,PzPred,otherInfo);
% %One will see that the one and two step updates agree.
% max(abs([xUpdate1-xUpdate;PUpdate1(:)-PUpdate(:);innov1(:)-innov;Pzz1(:)-Pzz(:);W1(:)-W(:)]))
%
%REFERENCES:
%[1] D. F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems Magazine,
%    vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] I. Arasaratnam and S. Haykin, "Cubature Kalman filters," IEEE
%    Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269,
%    Jun. 2009.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=size(xPred,1);
    numComp=size(xPred,2);

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

    if(nargin<7||isempty(innovTrans))
        %The function just returns the input.
        innovTrans=@(a,b)bsxfun(@minus,a,b);
    end

    if(nargin<8||isempty(measAvgFun))
        measAvgFun=@(zPoints,w)calcMixtureMoments(zPoints,w);
    end

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

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

    numCubPoints=size(xi,2);

    zPred=zeros(zDim,numComp);
    PzPred=zeros(zDim,zDim,numComp);
    xPredCenPoints=zeros(xDim,numCubPoints,numComp);
    zPredCenPoints=zeros(zDim,numCubPoints,numComp);
    Pxz=zeros(xDim,zDim,numComp);

    for k=1:numComp
        %cholSemiDef is used instead of chol in case a positive semi-definite
        %covariance matrix is passed.
        SPred=cholSemiDef(PPred,'lower');
        %Predicted cubature state points
        xPredPoints=stateTrans(transformCubPoints(xi,xPred,SPred));

        %Predicted cubature measurement points
        zPredPoints=zeros(zDim,numCubPoints);
        for curP=1:numCubPoints
            zPredPoints(:,curP)=h(xPredPoints(:,curP));
        end

        %Measurement prediction.
        zPred=measAvgFun(zPredPoints,w);

        %Centered, predicted cubature measurement points, transformed as
        %necessary to keep the values within a desired range.
        zPredCenPoints(:,:,k)=innovTrans(zPredPoints,zPred);
        xPredCenPoints(:,:,k)=stateDiffTrans(bsxfun(@minus,xPredPoints,xPred));
        for curP=1:numCubPoints
            diff=zPredCenPoints(:,curP);
            PzPred(:,:,k)=PzPred(:,:,k)+w(curP)*(diff*diff');
            Pxz(:,:,k)=Pxz(:,:,k)+w(curP)*xPredCenPoints(:,curP)*diff';
        end
        %Pxz is not needed for the measurement prediction, but we compute it
        %here, so that it need not be recomputed again and again if
        %cubKalUpdateWithPred is called for multiple measurements.
    end

    otherInfo.innovTrans=innovTrans;
    otherInfo.stateDiffTrans=stateDiffTrans;
    otherInfo.stateTrans=stateTrans;
    otherInfo.xPredCenPoints=xPredCenPoints;
    otherInfo.zPredCenPoints=zPredCenPoints;
    otherInfo.xPred=xPred;
    otherInfo.w=w;
    otherInfo.Pxz=Pxz;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%{
Authors:
Jonatas Lopes de Paiva
Claudio Fabiano Motta Toledo
Helio Pedrini

%}

function g = mutation(img)

r = randi(3);

switch r

    case 1
        tmp = double(img);
        rate = 0.7 + 0.6*rand;
        i = rate*tmp;
        i = floor(i + 0.5);
    case 2
        t = getFilterSize(2);
        sigma = rand * 5 + 0.05;
        h = fspecial('gaussian', t, sigma);
    case 3
        t = getFilterSize(2);
        h = fspecial('average', t);

end

if (r ==2 || r == 3)
    i = imfilter(img, h);
end

g = uint8(i);

end

function s = getFilterSize(maxSize)
n = randi(maxSize);

s = 2*n+1;
end


function [nstate] = tapas_sampler_mixedlinear_gibbs_node(data, model, ...
    inference, state, node)
%% Samples from a linear multivariate model with fixed and random effects
% and a diagonal covariance matrix.
%
% Input
%
% Output
%
% This level assumes only and unknown mean with known variance for some the
% the elements. The input matrix is required to be simple a weighting of
% prior and mean.
%

% aponteeduardo@gmail.com
% copyright (C) 2016
%

nstate = state;

% Number of subjects and number of chains
[ns, nc] = size(state.graph{node - 1}.y);

% Number of parameters
np = size(state.graph{node}.y{1}.mu, 2);

% Number of regressors
nr = size(state.graph{node}.y{1}.mu, 1);

% First store the values somewhere

for i = 1:nc
    % Get all the parameters at a given temperature
    iy = state.graph{node - 1}.y{i}.mu;

    % Get the grouping of the random variables.
    x = state.graph{node - 1}.u.x;

    % The number of groups is equal to size(x, 2). The ones in a column
    % encode membership to a code. size(x, 1) should be np;

    % Number of groups
    ng = size(x, 2);

    for j = 1:ng
        % Group
        gv = logical(x(:, j));
        ngv = sum(gv);
        % Group mean
        gmu = sum(iy(gv, :), 1) .* state.graph{node - 1}.y{i}.pe;
        gmu = gmu + state.graph{node + 1}.y{i}.pe(j, :) .* ...
            state.graph{node + 1}.y{i}.mu(j, :); % 1 x np

        % Group precision
        gpe = ngv * state.graph{node - 1}.y{i}.pe + ...
            state.graph{node + 1}.y{i}.pe(j, :);

        % Sample using Gibbs
        gmu = gmu./gpe;
        gmu = gmu + sqrt(1./ gpe) .* randn(1, np);

        % Replicate for all the correspondign regressors.
        nstate.graph{node}.y{i}.mu(gv, :) = repmat(gmu, ngv, 1);
    end
end

% Don't need it
%nstate.llh{node} = model.graph{node}.llh(nstate.graph{node}, ...
%    nstate.graph{node + 1}, model.graph{node}.htheta);
%
%nstate.llh{node - 1} = model.graph{node - 1}.llh(...
%    nstate.graph{node - 1}, nstate.graph{node}, model.graph{node - 1}.htheta);

end


function [MPa] = dynpcm22MPa(dynpcm2)
% Convert pressure from dynes per square centimeter to megapascals
% Chad Greene 2012
MPa = dynpcm2*1.00000e-7;


function [tt] = move_tt_block(tt, spos, epos, eps)
%Performs a bubble movement of a block inside a train
%   [TT] = MOVE_TT_BLOCK(TT, SPOS, EPOS, EPOS) Performs the bubble movement
%   of the SPOS-th block to the position EPOS, the intermediate blocks
%   shift to the vacant places, i.e. by (-1) if spos<eps, and +1,
%   otherwise. Requires truncation with the L2-norm accuracy EPSs
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et. al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if (spos==epos)
    return;
end;

d = tt.d;
n = tt.n;
r = tt.r;

% QR to spos
for i=1:spos-1
    cr = reshape(tt{i}, r(i)*n(i), r(i+1));
    [cr, rv]=qr(cr, 0);
    cr2 = reshape(tt{i+1}, r(i+1), n(i+1)*r(i+2));
    cr2 = rv*cr2;
    r(i+1) = size(cr, 2);
    tt{i} = reshape(cr, r(i), n(i), r(i+1));
    tt{i+1} = reshape(cr2, r(i+1), n(i+1), r(i+2));
end;
for i=d:-1:spos+1
    cr = reshape(tt{i}, r(i), n(i)*r(i+1));
    [cr, rv]=qr(cr.', 0);
    cr2 = reshape(tt{i-1}, r(i-1)*n(i-1), r(i));
    cr2 = cr2*(rv.');
    r(i) = size(cr, 2);
    tt{i} = reshape(cr.', r(i), n(i), r(i+1));
    tt{i-1} = reshape(cr2, r(i-1), n(i-1), r(i));
end;

% Now, start permutation
if (spos<epos) % From left to right
    for i=spos:epos-1
        cr1 = reshape(tt{i}, r(i)*n(i), r(i+1));
        cr2 = reshape(tt{i+1}, r(i+1), n(i+1)*r(i+2));
        cr = cr1*cr2;
        cr = reshape(cr, r(i), n(i), n(i+1), r(i+2));
        cr = permute(cr, [1, 3, 2, 4]);
        tempvar = n(i); n(i) = n(i+1); n(i+1) = tempvar;
        cr = reshape(cr, r(i)*n(i), n(i+1)*r(i+2));
        [u,s,v]=svd(cr, 'econ');
        s = diag(s);
        nrm = norm(s);
        r(i+1) = my_chop2(s, eps*nrm/sqrt(d-1));
        tt{i} = reshape(u(:,1:r(i+1)), r(i), n(i), r(i+1));
        v = diag(s(1:r(i+1)))*(v(:,1:r(i+1))');
        tt{i+1} = reshape(v, r(i+1), n(i+1), r(i+2));
    end;
end;

if (epos<spos) % From right  to left
    for i=spos:-1:epos+1
        cr1 = reshape(tt{i}, r(i), n(i)*r(i+1));
        cr2 = reshape(tt{i-1}, r(i-1)*n(i-1), r(i));
        cr = cr2*cr1;
        cr = reshape(cr, r(i-1), n(i-1), n(i), r(i+1));
        cr = permute(cr, [1, 3, 2, 4]);
        tempvar = n(i); n(i) = n(i-1); n(i-1) = tempvar;
        cr = reshape(cr, r(i-1)*n(i-1), n(i)*r(i+1));
        [u,s,v]=svd(cr, 'econ');
        s = diag(s);
        nrm = norm(s);
        r(i) = my_chop2(s, eps*nrm/sqrt(d-1));
        tt{i} = reshape(v(:,1:r(i))', r(i), n(i), r(i+1));
        u = u(:,1:r(i))*diag(s(1:r(i)));
        tt{i-1} = reshape(u, r(i-1), n(i-1), r(i));
    end;
end;

end


function [ varargout ] = TrimmedICP(Md, MovData, RefData, Tf0, MaxIter,TrMin,TrMax,lamda)
if nargin == 0
    clc; close all;
    %load ../../StanfordData/bunny;
    RefData = rand(3, 100); %bunny{1}';
    MovData = rand(3, 100);% bunny{2}';
    Md = createns(RefData');
    Tf0 = [eye(3) zeros(3, 1)];
    MaxIter = 50;
    TrMin = 0.35;
    TrMax = 1.0;
    lamda = 2.0;
end
Dim = size(MovData, 1);
PreMSE= 10^5;   CurMSE= 10^6;  Iter = 1;
RelErr = 1.0;
R0 = Tf0(1:Dim, 1:Dim);
T0 = Tf0(1:Dim, end);
Tf = [R0 T0; zeros(1, Dim) 1 ];
for Iter = 1 : 1 : MaxIter       % (abs(CurMSE-PreMSE)>10^(-9))   % this threshold is sensitive to resolutions....for bunny, this value should be 1e-12.
    TData = Loc2Glo(MovData, Tf(1:Dim, 1:Dim)', Tf(1:Dim, end) );
    [corr,TD] = knnsearch( Md,TData(1:Dim, :)');
    SortTD2 = sortrows(TD.^2); % Sort the correspongding points
    minTDIndex = floor(TrMin*length(TD)); % Get minimum index of TD
    maxTDIndex = ceil(TrMax*length(TD)); % Get maxmum index of TD
    TDIndex = [minTDIndex : maxTDIndex]';
    mTr = TDIndex./length(TD);
    mCumTD2 = cumsum(SortTD2);
    mMSE = mCumTD2(minTDIndex : maxTDIndex)./TDIndex;
    mPhi = ObjectiveFunction(mMSE, mTr);
    PreMSE=CurMSE;
    [CurMSE, nIndex] = min(mPhi);
    Trim = mTr(nIndex); % Update Tr for next step
    corr(:,2) = [1 : length(corr)]';
    % Sort the corresponding points
    corrTD = [corr, TD];
    SortCorrTD = sortrows(corrTD, 3);

    TrLength = floor(Trim*size(SortCorrTD,1)); % The number of corresponding points after trimming
    TCorr = SortCorrTD(1:TrLength, 1:2);     % Trim the corresponding points according to overlap parameter Tr
    % Register MData with TData
    dM = RegFun(RefData(:, TCorr(:, 1)), TData(:, TCorr(:, 2)) );
    % dM = reg(RefData, TData, TCorr);
    Tf = dM * Tf;
    % [M, TCorr, scan] = CalRtPhi(model, data, SortCorrTD, Trim);
    Err = [norm(dM(1:Dim, 1:Dim)-eye(Dim)) norm(dM(1:Dim, end))];
    if max(Err) <= 1e-5
        break;
    end
    RelErr = (PreMSE - CurMSE) / abs(PreMSE);
    if abs(RelErr) <= 1e-3
        break;
    end
end
IS_SHOW = 0;
if IS_SHOW
    Res = CalRes(MovData);
    h = figure;
    ICP_PlotFun(MovData, RefData, Tf, Res, h);
    title('Trimmed ICP');
end;
if nargout == 1
    varargout{1} = Tf;
end
if nargout == 2
    varargout{1} = Tf(1:Dim, 1:Dim);
    varargout{2} = Tf(1:Dim, end);
end
bTest = 1;

end

%%%%%%%%%%%%%%%%%%%%Integrated Function%%%%%%%%%%%%%%%%%%%%
% %% Calculate R,t,Phi based on current overlap parameter
% function [M,TCorr,TData] = CalRtPhi(Model, scan, SortCorrTD,Tr)
%
% TrLength = floor(Tr*size(SortCorrTD,1)); % The number of corresponding points after trimming
% TCorr = SortCorrTD(1:TrLength, 1:2);     % Trim the corresponding points according to overlap parameter Tr
% % Register MData with TData
% [M] = reg(Model(1:3,:), scan(1:3,:), TCorr);
% % To obtain the transformation data
% TData = M*scan;
%
% end
%
% function [M,TCorr,TData] = CalRtPhi(Model, scan, SortCorrTD,Tr)
%
% TrLength = floor(Tr*size(SortCorrTD,1)); % The number of corresponding points after trimming
% TCorr = SortCorrTD(1:TrLength, 1:2);     % Trim the corresponding points according to overlap parameter Tr
% % Register MData with TData
% [M] = reg(Model(1:3,:), scan(1:3,:), TCorr);
% % To obtain the transformation data
% TData = M*scan;
%
% end
%%%%%%%%%%%%%%% Calculate the registration matrix %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% T(TData)->MData %%%%%%%%%%%%%%%%%%%%%%%%%
% SVD solution
function [M] = reg(Model, Data, corr)

n = length(corr);
M = Model(:,corr(:,1));
mm = mean(M,2);
S = Data(:,corr(:,2));
ms = mean(S,2);
Sshifted = [S(1,:)-ms(1); S(2,:)-ms(2); S(3,:)-ms(3)];
Mshifted = [M(1,:)-mm(1); M(2,:)-mm(2); M(3,:)-mm(3)];
K = Sshifted*Mshifted';
K = K/n;
[U A V] = svd(K);
R1 = V*U';
if det(R1)<0
    B = eye(3);
    B(3,3) = det(V*U');
    R1 = V*B*U';
end
t1 = mm - R1*ms;
M=[];
M(1:3,1:3)=R1;
M(1:3,4)=t1;
M(4,:)=[0,0,0,1];
end

function [Phi] = ObjectiveFunction(MSE, TrB)
lamga= 2;
Phi = MSE./((TrB).^((1+lamga)));
end


function [Rot,v,x,PosAmers,P] = iekfPropagation(dt,Rot,v,x,omega_b,a_b,...
    PosAmers,P,omega,acc,Q,g)
NbAmers = size(PosAmers,2);

% state propagation
Rot = Rot*expSO3((omega-omega_b)*dt);
v = v+(Rot*(acc-a_b)+g)*dt;
x = x+v*dt;

% covariance propagation
F = eye(size(P));
F(4:6,1:3) = vecto(g)*dt;
F(7:9,1:3) = vecto(g)*dt*dt;
F(7:9,4:6) = eye(3)*dt;
F(1:3,10:12) = -Rot*dt;
F(4:6,10:12) = -vecto(v)*Rot*dt;
F(7:9,10:12) = -vecto(x)*Rot*dt;
for i = 1:NbAmers
    posAmers_i = PosAmers(:,i);
    F(13+3*i:15+3*i,10:12) = -vecto(posAmers_i)*Rot*dt;
end
F(4:6,13:15) = -Rot*dt;
F(7:9,13:15) = -Rot*dt*dt;

G = zeros(size(P,1),size(Q,1));
G(1:3,1:3) = Rot;
G(4:6,1:3) = vecto(v)*Rot;
G(7:9,1:3) = vecto(x)*Rot;
G(4:6,4:6) = Rot;
G(7:9,4:6) = Rot*dt;
G(10:15,7:12) = eye(6);
G(1:3,7:9) = Rot*dt;
G(4:6,7:9) = vecto(v)*Rot*dt*dt;
G(7:9,7:9) = vecto(x)*Rot*dt*dt*dt;
G(4:6,10:12) = Rot*dt*dt;
G(7:9,10:12) = Rot*dt*dt*dt;

P = F*P*F' + G*(Q*dt)*G'*dt;
end


function layer = genNetworkLSTM(para)
if isfield(para, 'LastActivation4MSE')==0
    para.LastActivation4MSE = 'linear';
end

layer{1}.name = 'Input';        % this is an input layer
layer{end}.inputIdx = 1;    % specifies the index of GCC in Visible_tr
layer{end}.dim = [1 1]*double(para.inputDim);             % [input dim; output dim];

if isfield(para, 'ProjectionSize') &&  ~isempty(para.ProjectionSize) && para.ProjectionSize>0
    layer{end+1}.name = 'Affine';
    layer{end}.prev = -1;
    layer{end}.W = [];
    layer{end}.b = [];
    layer{end}.dim = [para.ProjectionSize layer{end-1}.dim(1)];
    layer{end}.update = 1;
end

for i=1:length(para.hiddenLayerSizeLSTM)
    layer{end+1}.name = 'LSTM';
    layer{end}.prev = -1;   % this is the index of GCC features in Visible_tr; It is the offset to be added to current layer index
    layer{end}.W = []; % to be initialized randomly or by pretraining
    layer{end}.b = [];
    layer{end}.usePastState = para.usePastState(i);
    layer{end}.dim = [para.hiddenLayerSizeLSTM(i) layer{end-1}.dim(1)];
    layer{end}.update = 1;

    if isfield(para, 'useAffineBtwLSTM') && para.useAffineBtwLSTM && i<length(para.hiddenLayerSizeLSTM)
        layer{end+1}.name = 'Affine';
        layer{end}.prev = -1;
        layer{end}.W = [];
        layer{end}.b = [];
        layer{end}.dim = [1 1] * layer{end-1}.dim(1);
        layer{end}.update = 1;
    end
end

layer2 = genNetworkFeedForward_v2(layer{end}.dim(1), para.hiddenLayerSizeFF, para.outputDim, para.costFn, para.LastActivation4MSE);

layer = [layer layer2(2:end)];

if isfield(para, 'labelDelay')
    layer{end}.labelDelay = para.labelDelay;
end
if isfield(para, 'costFrameSelection');
    layer{end}.costFrameSelection = para.costFrameSelection;
end

layer = FinishLayer(layer);
end


function y = log(x)
%LOG          Implements  log(x)  for intervals
%
%   y = log(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, following N.C. Boersken:
%                                  Komplexe Kreis-Standardfunktionen,
%                                  Freiburger Intervallberichte 78/2,
%                                  NaN input, sparse input, log(0),
%                                  major revision, improved accuracy
% modified 12/06/99                branch cut with warning
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/04/05     S.M. Rump  'realstdfctsexcptnignore' added and some
%                                     improvements, tocmplx replaced by cintval,
%                                     extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 10/23/07     S.M. Rump  complex numbers
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/20/08     S.M. Rump  check for zero omitted, improved performance
% modified 10/18/08     S.M. Rump  StdFctsException ignore/NaN
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

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

  if x.complex
    if issparse(x.mid)
      x.mid = full(x.mid);
      x.rad = full(x.rad);
    end
    y = x;

%   y.mid = log(x.mid);
%   y.rad = - log( 1 - x.rad./abs(x.mid) );

    INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');

    xmidre = real(x.mid);
    xmidim = imag(x.mid);
    Xmidim = intval(xmidim);
    Mim = atan(Xmidim./xmidre);      % -pi/2 <= Mim <= pi/2

    % special treatment of imaginary axis
    index = ( xmidre==0 );
    if any(index(:))
      Mim.inf(index) = INTLAB_STDFCTS_PI.PI2INF;          % pi/2
      Mim.sup(index) = INTLAB_STDFCTS_PI.PI2SUP;
      indexneg = index & ( xmidim<0 );
      if any(indexneg(:))
        Mim.inf(indexneg) = -INTLAB_STDFCTS_PI.PI2SUP;    % -pi/2
        Mim.sup(indexneg) = -INTLAB_STDFCTS_PI.PI2INF;
      end
    end

    index = ( xmidre < 0 );
    if any(index(:))
      % correct to atan2:  -pi <= Mim <= pi
      Pi = infsup( INTLAB_STDFCTS_PI.PIINF, ...
                   INTLAB_STDFCTS_PI.PISUP );
      signxmidim = sign(xmidim(index));
      corr = Pi.*(signxmidim+(signxmidim==0));
      Mim.inf(index) = Mim.inf(index) + corr.inf;
      Mim.sup(index) = Mim.sup(index) + corr.sup;
    end

    % x.mid = r*exp(j*phi)  ==>  log(x.mid) = log(r) + j*phi
    R = abs(x.mid);
    Mreinf = log_rnd(R,-1);
    Mresup = log_rnd(R,1);

    setround(1)
    mre = Mreinf + 0.5*(Mresup-Mreinf);
    mim = Mim.inf + 0.5*(Mim.sup-Mim.inf);
    y.mid = mre + j*mim;
    mrad = abs( mre-Mreinf + j*(mim-Mim.inf) );
    % log(R)  in  mre + j*mim +/- mrad

    wng = warning;
    warning off
    % x.rad < R ,  otherwise zero interval
    y.rad = log_rnd( -( x.rad./R - 1 ) , -1);
    warning(wng);
    y.rad = -y.rad + mrad;
    setround(0)

    % special treatment of branch cut
    index0 = ( R <= x.rad );               % zero interval
    index = ( ~index0 ) & ...
            (   ( ( xmidre<0 ) & ( xmidim>=0 ) & ( x.rad>xmidim ) ) ...
              | ( ( xmidre<0 ) & ( xmidim<0 ) & ( x.rad>=-xmidim ) ) ...
            );
    if any(index(:))
      warning('Complex Log: Input interval intersects with branch cut')
    end

    if any(index0(:))
      y.mid(index0) = complex(NaN,NaN);
      y.rad(index0) = NaN;
    end

    if rndold
      setround(rndold)
    end

    return
  end

  if issparse(x.inf)
    x.inf = full(x.inf);
    x.sup = full(x.sup);
  end
  % input x real and full
  % real range of definition:  [0,inf]
  INTLAB_STDFCTS_EXCPTN = getappdata(0,'INTLAB_STDFCTS_EXCPTN');
  index = ( x.inf<0 );                  % (partially) exceptional indices
  if any(index(:))                      % handle input out-of-range
    if INTLAB_STDFCTS_EXCPTN<=1  % out-of-range input handled as complex
      if INTLAB_STDFCTS_EXCPTN==1
        warning('LOG: Real interval input out of range changed to be complex')
      end
      y = x;
      %VVVV  y(index) = log(cintval(x(index)));
      s.type = '()'; s.subs = {index}; y = subsasgn(y,s,log(cintval(subsref(x,s))));
      %AAAA  Matlab bug fix
      index = ~index;
      if any(index(:))
        %VVVV  y(index) = log(x(index));
        s.type = '()'; s.subs = {index}; y = subsasgn(y,s,log(subsref(x,s)));
        %AAAA  Matlab bug fix
      end
      if rndold
        setround(rndold)
      end
      return
    end
    setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
    if INTLAB_STDFCTS_EXCPTN==3    % ignore input out of range (ignore-mode)
      x.inf(index) = realmin*eps;;        % completely exceptional indices treated below
      indexneg = index & ( x.sup<0);      % completely exceptional indices
    end
  else
    indexneg = [];                        % make sure indexneg is not undefined
  end

  % input x real and full
  y = x;
  wng = warning;
  warning off

  % treat non-exceptional cases
  y.inf = log_rnd(x.inf,-1);
  y.sup = log_rnd(x.sup,1);

  if INTLAB_STDFCTS_EXCPTN==3      % ignore input out of range (ignore-mode)
    if ~isempty(find(indexneg))           % completely exceptional arguments to NaN
      y.inf(indexneg) = NaN;
      y.sup(indexneg) = NaN;
    end
  else                                    % any input out of range to NaN (NaN-mode)
    if ~isempty(find(index))              % exceptional arguments to NaN
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  end

  warning(wng)
  setround(rndold)


impts=zeros(yw*exf,xw*exf);
n_rendered=0;
weight=str2double(get(handles.weight,'String'));
size_fac=str2double(get(handles.size_fac_edit,'String'));

for i=nstart:nend
    if xc(i)>=1 && yc(i)>=1 && xc(i)<xw*exf && yc(i)<yw*exf && N(i)>0
      wide=ceil(size_fac*lppix(i)*1.5+1);
%       if wide>20
%           wide=20;
%       end
      if xc(i)-wide>=1 && xc(i)+wide<xw*exf && yc(i)-wide>=1 && yc(i)+wide<yw*exf
        n_rendered=n_rendered+1;
        for j=xc(i)-wide:xc(i)+wide
          for k=yc(i)-wide:yc(i)+wide
            dx=double(j)-xf(i);
            dy=double(k)-yf(i);
            int=pi*lp2pix(i)*size_fac;
            a=exp(-2*(dx*dx+dy*dy)/(size_fac*size_fac*lp2pix(i)))*N(i)*weight/int;
            impts(k,j)=impts(k,j)+a;
          end
        end
      end
    end
    waitbarxmod(i/nend,w); %update
end

thresh=impts*0+1;
impts=thresh.*(impts>thresh)+impts.*(impts<=thresh);
clear thresh;


clear all; close all; clear classes; clc;

%% Set flags.
inspect_only = false;

%% Create shapes.
a = 420;  % lattice constant
t = 1;  % slab thickness
r = 0.29*a;  % hole radius

ad = 25;  % divider for a
td = 10;  % divider for t
dd = 10;  % divider for d = 2*r

mx = 20.5;
my = 7.5;
slab_yn = Box([-mx*a mx*a; -my*a -0.5*a; 0 t], [a/ad, a/ad, t]);
slab_yp = Box([-mx*a mx*a; 0.5*a my*a; 0 t], [a/ad, a/ad, t]);

rod = CircularCylinder(Axis.z, t, [0 0 t/2], r, [2*r/dd, 2*r/dd, t]);

%% Solve the system.
gray = [0.5 0.5 0.5];  % [r g b]
src_loc = 2*a;
[E, H, obj_array, src_array, J] = maxwell_run(...
	'OSC', 1e-9, 1550, ...
	'DOM', {'Johnson/Ag', gray}, [-mx*a mx*a; -my*a my*a; 0 t], [a/ad a/ad t], BC.p, [10*a 2*a 0], ...
	'OBJ', ...
		{'vacuum', 'w', 1}, Box([-mx*a mx*a; -a/2 a/2; 0 t]), ...
	'SRCJ', PointSrc(Axis.z, [src_loc, 0, 0.5]), ...
	inspect_only);

%% Visualize the solution.
if ~inspect_only
	figure;
	clear opts
% 	opts.withgrid = true;
	opts.withobjsrc = false;
	opts.withabs = true;
	opts.withpml = false;
	opts.phase = pi/2;
	vis2d(E{Axis.z}, Axis.z, 0.5, obj_array, src_array, opts)
	%%
	vis2d(H{Axis.y}, Axis.z, 0.5, obj_array, src_array, opts)

	%%
	flux_loc = 3*a;
	power_right = powerflux_patch(E, H, Axis.x, src_loc + flux_loc);
	power_left = -powerflux_patch(E, H, Axis.x, src_loc - flux_loc);
	fprintf('power:\n');
	fprintf('right = %s\n', num2str(power_right));
	fprintf('left = %s\n', num2str(power_left));
	fprintf('error = %s%%\n',num2str((power_left-power_right)/power_right*100));

	%%
	Sx = poynting(Axis.x, E{Axis.y}, E{Axis.z}, H{Axis.y}, H{Axis.z}, Axis.y, 0);
	[array, l] = Sx.data_original;
	plot(l{2}, abs(array))
	mx*a - 10*a
end


function result = CRA_enterFunc(alpha,beta,gamma,lambda,numK,maxIter,flag,inputPath)

    %% ======================================================================
    %%STEP 1: load the data
    fprintf('start load the data...\n');
    [TrainData, TestData, TrainLabel, TestLabel, numX, numS] = CRA_loadData(inputPath);

    numM = size(TrainData,1);                % input data feature dimensions

    %% ======================================================================
    %%STEP 2: Initialize the parameter
    fprintf('start initialize the parameter...\n');
    theta = CRA_initialize(numK, numM, numS, numX,TrainData,TrainLabel,TestData);    % Randomly initialize the parameters

    %% ======================================================================
    %%STEP 3: Training the parameters W1 W2 b1 b2 C
    fprintf('start training the parameter...\n');
    [opttheta, cost] = CRA_Train(numM,numK,numS,numX,maxIter,alpha,beta,gamma,lambda,TrainData,TestData,TrainLabel,theta);

    %% ======================================================================
    %%STEP4: get parameters W1 W2 W11 W22 b1 b2 b11 b22 after training
    fprintf('get the parameter...\n');
    W1 = reshape(opttheta(1:numK*numM), numK, numM);
    b1 = opttheta(2*numK*numM+1:2*numK*numM+numK);
    C = reshape(opttheta(2*numK*numM+numK+numM+1:end), numS, numK);

    %% ======================================================================
    %%STEP4: Testing
    fprintf('testing the model...\n');
    hiddeninputs_train = sigmoid(W1 * TrainData + b1 * ones(1, size(TrainData,2)));
    hiddeninputs_test = sigmoid(W1 * TestData + b1 * ones(1, size(TestData,2)));
    if flag == 1
        predict = CRA_test(hiddeninputs_train, hiddeninputs_test, TrainLabel, TestLabel, numX, numS, C);
    else
        predict = CRA_test_LR(hiddeninputs_train, hiddeninputs_test, TrainLabel, TestLabel, numX, numS);
    end
    result=predict;
    clear hiddeninputs_train hiddeninputs_test
end


% hessMultdalgl - function that computes H*x for DAL with grouped
%                 L1 regularization
%
% Copyright(c) 2009 Ryota Tomioka
% This software is distributed under the MIT license. See license.txt

function yy = hessMultdalgl(xx, A, eta, Hinfo)

yy = Hinfo.hloss*xx;
if ~isempty(Hinfo.precomp)
    % general case
    for p=Hinfo.precomp
        % project & reshape xx's for all spans
        xk = reshape(p.AJ'*xx,[],numel(p.jj));
        % dot product between vn's and xk's, scale by ff's and multiply by vn's (also add xk*(1-ff))
        tmp = bsxfun(@times,p.vn,sum(p.vn.*xk).*p.ff) + bsxfun(@times,xk,p.omff);
        % block-diagonalize tmp
        bd = sparse(p.idxu,p.idxv,tmp,numel(xk),numel(p.jj));
        % map through AJ again, sum and multiply by eta(1), add results to yy
        yy = yy + sum(p.AJ*bd,2)*eta(1);
    end
else
    % very sparse case
    blks =Hinfo.blks;
    hloss=Hinfo.hloss;
    I    =Hinfo.I;
    vv   =Hinfo.vv;
    nm   =Hinfo.nm;
    lambda=Hinfo.lambda;
    for kk=1:length(I)
        jj=I(kk);
        J=Hinfo.blkival{jj};
        vn=vv(J)/nm(jj);
        ff=lambda/nm(jj);
        AJ=A.slice(J);
        xk=AJ'*xx;
        yy = yy + eta(1)*(AJ*((1-ff)*xk + ff*(vn'*xk)*vn));
    end
end

B=Hinfo.B;
if ~isempty(B)
  yy = yy + eta(2)*(B*(B'*xx));
end


 function [xs, ni] = eml_sps(x, Gt, yi, ci, ri, niter, curv)
%function [xs, ni] = eml_sps(x, Gt, yi, ci, ri, niter, curv)
%	One iteration of the ML-SPS algorithm for emission Poisson problem
%	(separable paraboloidal surrogates)
%	model: Y_i ~ Poisson(c_i [G x]_i + r_i)		WITH r_i > 0 REQUIRED!
%	in:
%		Gt	transpose of system matrix
%		see em_fbp.m for model, G, yi, ci, ri
%	out:
%		x [np,niter]	updated image vectors each iteration
%
%	Copyright Mar 2000, Jeff Fessler, The University of Michigan

if nargin < 3, ir_usage, end

[nb, na] = size(yi);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end
if ~isvar('niter') || isempty(niter)
	niter = 1;
end

eml_check(yi, ci, ri)

	gi = sum(Gt)';	% g_i = sum_j g_ij

xs = zeros(numel(x), niter);
xs(:,1) = x(:);
%
%	loop over iterations
%
for ii=2:niter
	li = reshape(Gt' * x(:), size(yi));	% l=G*x "line integrals"
	yb = ci .* li + ri;			% predicted measurement means

	%	curvatures
	ni = eml_curvature(yi, ci, ri, li, yb, curv);

	dothi = ci .* (yi ./ yb - 1);

	x = x + (Gt * dothi(:)) ./ (Gt * (gi .* ni(:)));
	x = max(x,0);

	xs(:,ii) = x;
end


function degree = dunavant_degree ( rule )

%*****************************************************************************80
%
%% DUNAVANT_DEGREE returns the degree of a Dunavant rule for the triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    David Dunavant,
%    High Degree Efficient Symmetrical Gaussian Quadrature Rules
%    for the Triangle,
%    International Journal for Numerical Methods in Engineering,
%    Volume 21, 1985, pages 1129-1148.
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Output, integer DEGREE, the polynomial degree of exactness of
%    the rule.
%
  if ( 1 <= rule & rule <= 20 )
    degree = rule;
  else

    degree = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DUNAVANT_DEGREE - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'DUNAVANT_DEGREE - Fatal error!' )

  end

  return
end


function rhs_spiral_test ( )

%*****************************************************************************80
%
%% RHS_SPIRAL_TEST samples the right hand side at the initial time.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/navier_stokes_2d_exact/rhs_spiral_test.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2015
%
%  Author:
%
%    John Burkardt
%
  nu = 1.0;
  rho = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'RHS_SPIRAL_TEST\n' );
  fprintf ( 1, '  Spiral Flow:\n' );
  fprintf ( 1, '  Sample the Navier-Stokes right hand sides\n' );
  fprintf ( 1, '  at the initial time T = 0, using the unit square.\n' );
  fprintf ( 1, '  Kinematic viscosity NU = %g\n', nu );
  fprintf ( 1, '  Fluid density RHO = %g\n', rho );

  n = 1000;
  xy_lo = 0.0;
  xy_hi = 1.0;
  seed = 123456789;
  [ x, seed ] = r8vec_uniform_ab ( n, xy_lo, xy_hi, seed );
  [ y, seed ] = r8vec_uniform_ab ( n, xy_lo, xy_hi, seed );
  t = 0.0;

  [ f, g, h ] = rhs_spiral ( nu, rho, n, x, y, t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '           Minimum       Maximum\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F:  %14.6g  %14.6g\n', min ( f ), max ( f ) );
  fprintf ( 1, '  G:  %14.6g  %14.6g\n', min ( g ), max ( g ) );
  fprintf ( 1, '  H:  %14.6g  %14.6g\n', min ( h ), max ( h ) );

  return
end


% DEMO of subplot Vs subplot_tight
func_hndl={@subplot;@subplot_tight};
dir_file=dir('*.jpg');
for p=1:length(dir_file)
   img=imread(dir_file(p).name);
   for func_ind=1:length(func_hndl)
      figure(func_ind)
      func_hndl{func_ind}(3,3,p);
      imshow(img);
   end
end
peaks_data=peaks(50);
for func_ind=1:length(func_hndl);
   figure(func_ind)
   func_hndl{func_ind}(3,3,[8,9]);
   surf(peaks_data);
   axis tight;
   xlabel('x');
   ylabel('y');
   zlabel('z');
   title('Peaks Plot','FontSize', 14);
end
set(1,'Name','Matlab SUBPLOT');
set(2,'Name','Our subplot_tight');
set(1:2,'MenuBar','none')


function c = tmat_mxm ( a, b )

%*****************************************************************************80
%
%% TMAT_MXM multiplies two geometric transformation matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Foley, van Dam, Feiner, Hughes,
%    Computer Graphics, Principles and Practice,
%    Addison Wesley, Second Edition, 1990.
%
%  Parameters:
%
%    Input, real A(4,4), the first geometric transformation matrix.
%
%    Input, real B(4,4), the second geometric transformation
%    matrix.
%
%    Output, real C(4,4), the product A * B.
%
  c(1:4,1:4) = a(1:4,1:4) * b(1:4,1:4);

  return
end


function [samecost, sameassignment] = testassignment
%TESTASSIGNMENT  Test and compare assignment algorithms.
%		[SAMECOST, SAMEASSIGNMENT] = TESTASSIGN randomly generates distance
%		matrices and solves the assignment problem using different algorithms.
%		Edit the header of this file to change the simulation parameters.
%
%		assignment.html  File Exchange  Donate via PayPal
%
%		Markus Buehren
%		Last modified 05.07.2011
%
%		See also ASSIGNMENTOPTIMAL, ASSIGNMENTSUBOPTIMAL1,
%		ASSIGNMENTSUBOPTIMAL2, ASSIGNMENTALLPOSSIBLE.

% simulation time in seconds
testTime = 20;

% maximum matrix dimensions
maxOrders = [15, 25];

% If infAllowed in set to false, only distance matrices without infinite
%	costs are used
infAllowed = true;

% If the product of the dimensions of the distance matrix is smaller than
% maxDimProduct, the assignment function computing all possible assignments
% is used as reference. Set this to inf to use always or to 0 to never use.
maxDimProduct = 30;

% set recurstion limit
recursionLimit = get(0,'RecursionLimit');
set(0, 'RecursionLimit', 1000);

% use profiler or not
useProfiler = true;

% start profiler
if useProfiler
	profile clear
	profile on
end

% initialize
startTime = clock;
nassign = 0;
h = waitbar(0, 'Please wait', 'Name', mfilename);

while 1
	for dim1 = 1:maxOrders(1)
		for dim2 = 1:maxOrders(2)

			if ~infAllowed

				% generate distMatrix without infinite elements
				distMatrix = rand(dim1,dim2);

			else

				if rand(1) < 0.5
					% generate distMatrix with some infinite elements
					distMatrix = rand(dim1,dim2);

					if rand(1) < 0.5
						% set some elements to inf
						distMatrix(rand(dim1,dim2) > rand(1)) = inf;
					end

				else

					% generate distMatrix with many infinite elements
					distMatrix = repmat(inf, dim1, dim2);

					if rand(1) < 0.5
						for row=1:dim1
							if rand(1) < 0.8
								% set one element per row to finite number
								distMatrix(row, 1+floor(dim2*rand(1))) = rand(1);
							end
							if rand(1) < 0.3
								% set another element per row to finite number
								distMatrix(row, 1+floor(dim2*rand(1))) = rand(1);
							end
						end
					else
						for col=1:dim2
							if rand(1) < 0.8
								% set one element per column to finite number
								distMatrix(1+floor(dim1*rand(1)), col) = rand(1);
							end
							if rand(1) < 0.3
								% set another element per column to finite number
								distMatrix(1+floor(dim1*rand(1)), col) = rand(1);
							end
						end
					end

				end
			end

			% quantize distMatrix
			if rand(1) < 0.3
				distMatrix = round(max(10, 1000*rand(1)^2)*distMatrix);
			end

			% transpose distMatrix
			if rand(1) < 0.5
				distMatrix = distMatrix';
			end

			%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
			[assignmentCell{1}, costCell{1}] = assignmentoptimal    (distMatrix); %#ok
			[assignmentCell{2}, costCell{2}] = assignmentsuboptimal1(distMatrix); %#ok
			[assignmentCell{3}, costCell{3}] = assignmentsuboptimal2(distMatrix); %#ok
			%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

			% if dimensions are moderate, compute all possible solutions
			if dim1 * dim2 < maxDimProduct
				[assignment, cost] = assignmentallpossible(distMatrix);
			else
				assignment = assignmentCell{1};
				cost       = costCell{1};
			end

			% count trial runs
			nassign = nassign + 1;

			if ~exist('samecost', 'var')
				M = length(costCell);
				samecost        = zeros(M,1);
				sameassignment = zeros(M,1);
			end

			% compute penalty for non-assignments
			finiteIndex = isfinite(distMatrix);
			penalty = max(max(distMatrix(finiteIndex))) * dim1 * dim2;
			if ~isempty(penalty)
				cost = cost + length(find(~assignment)) * penalty;
			end

			for m=1:M

				% penalize non-assignments
				if ~isempty(penalty)
					costCell{m} = costCell{m} + length(find(~assignmentCell{m})) * penalty; %#ok
				end

				% compare costs
				if costCell{m} <= cost + 100*eps
					samecost(m) = samecost(m) + 1;
				end

				% compare assignments
				if all(assignmentCell{m} == assignment)
					sameassignment(m) = sameassignment(m) + 1;
				end

			end
		end
		% set waitbar
		curTime = etime(clock, startTime);
		waitbar(curTime/testTime, h);
	end

	% stop after given time
	if curTime > testTime
		break
	end
end
close(h);

% scale counters
samecost       = samecost/nassign;
sameassignment = sameassignment/nassign;

if useProfiler
	profile off
	profile report
end

% reset recurstion limit
set(0, 'RecursionLimit', recursionLimit);


function [mGal] = uGal2mGal(uGal)
% Convert acceleration from microgals to milligals.
% Chad A. Greene 2012
mGal = uGal*1e-3;
end


function map2ecefTrafo(obj, mstruct, varargin)
% MAP2ECEFTRAFO Coordinate transformation from ecef to map coordinates.
%
% Example: definition of mstruct for UTM33N
% mstruct       = defaultm('utm');
% mstruct.zone  = '33n';
% mstruct.geoid = referenceEllipsoid('GRS 80');
% mstruct       = defaultm(mstruct);

% Input parsing ----------------------------------------------------------------

p = inputParser;
p.addRequired( 'mstruct');
p.parse(mstruct, varargin{:});
p = p.Results;
% Clear required inputs to avoid confusion
clear mstruct

% Start ------------------------------------------------------------------------

procHierarchy = {'POINTCLOUD' 'MAP2ECEFTRAFO'};
msg('S', procHierarchy);
msg('I', procHierarchy, sprintf('Point cloud label = ''%s''', obj.label));

% Conversion to ecef coordinates -----------------------------------------------

[lat, lon, hEll] = minvtran(p.mstruct, obj.X(:,1), obj.X(:,2), obj.X(:,3)); % lat, lon in degrees!

[xEcef, yEcef, zEcef] = geodetic2ecef(lat*pi/180, lon*pi/180, hEll, p.mstruct.geoid); % lat, lon in radian!

% Update coordinates -----------------------------------------------------------

obj.X = [xEcef yEcef zEcef];
obj.info;

% End --------------------------------------------------------------------------

msg('E', procHierarchy);

end


function [CRLB,J]=getEstimatorMinMSEBound(RInv,stateJacob,statJacob,biasVec,biasJacob,numMeasDims,checkPosDef)
%%GETESTIMATORMINMSEBOUND This evaluates a generalization of the Cramer-Rao
%       lower bound (CRLB) specifically for the case where measurements are
%       corrupted with multivariate Gaussian noise. The measurement model
%       is z=h(x)+w, where x is the (deterministic) state, h is the
%       (possible nonlinear) measurement function and w is zero-mean
%       Gaussian noise with covariance matrix R. h can vary between
%       measurements that are being fused. This offers a lower bound on the
%       mean-squared error of a biased multivariate statistic. The
%       statistic is just a transformation of the state, so zStat=f(x).
%       Biased means that if T(x) is the estimator, then the expecttation
%       E{T(x)-f(x)}=b(x), where b(x) is the bias. If one doesn't know the
%       bias and its gradient, omitting those terms provides a lower bound
%       on the covariance matrix of an unbiased statistic. If the statistic
%       Jacobian is omitted, then the bound is directly on the state x.
%       The bias term can be useful when one already has an estimator with
%       a known or approximated bias and they want to approximate its
%       accuracy with the MSE matrix bound rather than analytically
%       evaluating its true MSE.
%
%INPUTS: RInv The zDimXzDimXnumMeas set of inverse covariance matrices
%          associated with the multivariate Gaussian noise corrupting each
%          of the numMeas measurements. If the dimensionality of the
%          measurements varies, then zDim is the maximum
%          dimensionality of any measurement and then numMeasDims is
%          required so that if sel=1:numMeasDims(k), then RInv(sel,sel,k)
%          is the submatrix used for the kth measurement.
% stateJacob The zDimXxDimXnumMeas Jacobian matrices of derivatives of the
%          measurement function h taken with respect to the elements of
%          the target state for every measurement. If a single zDimXxDim
%          matrix is passed, then it is assumed that this is the same for
%          all numMeas measurement. If the dimensionality of the
%          measurements varies, then zDim is the maximum
%          dimensionality of any measurement and then numMeasDims is
%          required so that if sel=1:numMeasDims(k), then
%          stateJacob(sel,:,k) is the submatrix used for the kth
%          measurement.
% statJacob The statDimXxDim matrix of derivatives of the the statistic
%          function f(x) taken with respect to the state x. If this is
%          omitted or an empty matrix is passed, then an xDimXxDim identity
%          matrix is used, which is the same as h(x)=x.
%  biasVec A statDimX1 vector, if provided. This is the bias of the assumed
%          estimator of the statistics. If this value and the next value
%          are omitted or empty matrices are provided, then the CRLB for an
%          unbiased statistic is computed.
% biasJacob A statDimXxDim matrix of derivatives of the estimator bias b(x)
%          taken with respect to the state x. If this is provided, then
%          biasVec must be provided. If omitted or an empty matrix is
%          passed, then this is assumed to be 0.
% numMeasDims If all measurement shave the same dimensionality , then this
%          input should be omitted or an empty matrix passed. Otherwise,
%          this is a length numMeas vector that specified how many
%          dimensions each measurement has.
% checkPosDef If this is true, then a check is performed as to whether the
%          Fisher information matrix in the algorithm is full rank.
%          If it is not, then the CRLB matrix will be returned empty and no
%          attempt will be made to invert it. The default if omitted or an
%          empty matrix is passed is false.
%
%OUTPUTS: CRLB The statDimXstatDim CRLB matrix or, if the FIM was singular,
%              and checkPosDef is true, an empty matrix.
%            J The Fisher infromation matrix that went into computing CRLB.
%
%A simple derivation of the standard multivariate CRLB is derived in [1].
%If in the derivation there, one replaces the difference T_k(x)-x_k (the
%difference of the best estimator of the kth component of x and the state)
%with T_k(x)-f_k(x), where f_k(x) is the kth component of a desired
%statistic and thus T_k(x) becomes an estimator of a component of a
%statistic, then one will derive the CRLB of a statistic instead of just of
%the state. In the scalar case, this was already in Rao's original paper in
%[2].
%
%To consider a biased statistic, one then replaces the assumption in the
%paper of a zero expected value: E{T_k(x)-x_k}=0 with
%E{T_k(x)-f_k(x)}=b_k(x). The rest of the derivation in [1] is pretty much
%the same, making sure that one is evaluating Cov{Z} and not just E{Z*Z'}.
%Completing the derivation just using E{Z*Z'} and not Cov{Z} will lead one
%to have a lower bound that is smaller by a bias*bias' term.
%
%Note that is is possible for a biased estimator to have a lower MSE than
%an unbiased estimator. One such example is given in [3].
%
%EXAMPLE 1:
%This is a simple example, where z=x+w with w having the identity matrix as
%the covariance matrix. We take x to be 2X1. We want to estimate
%f(x)=x'*x. The assumed biased estimator is T(z)=z'*z+sum(z)/100. (Note
%that even T(z)=z'*z is biased, but we choose to make the bias non-
%constant). The bias in 2D can be found to be b(x)=2+sum(x)/100.
%The analytic mean squared error in 2D is
%MSE=(80002+40001*x(1)^2+2*x(1)*(400+x(2))+x(2)*(800+40001*x(2)))/10000
%We plot the MSE and the CRLB for a fixed x(1), while varying x(2). One can
%see a reasonably good level of agreement, though the bound is not tight.
% MSE=@(x)(80002+40001*x(1)^2+2*x(1)*(400+x(2))+x(2)*(800+40001*x(2)))/10000;
% biasVec=@(x)(2+sum(x)/100);
% biasJacob=@(x)[1/100,1/100];
% statJacob=@(x)2*x';
% stateJacob=eye(2,2);
% RInv=eye(2,2);
% numPts=200;
% x1=-1/2;
% x2=linspace(-5,5,numPts);
% MSEVal=zeros(1,numPts);
% CRLB=zeros(1,numPts);
% for curPt=1:numPts
%     xCur=[x1;x2(curPt)];
%     MSEVal(curPt)=MSE(xCur);
%     CRLB(curPt)=getEstimatorMinMSEBound(RInv,stateJacob,statJacob(xCur),biasVec(xCur),biasJacob(xCur));
% end
% figure(1)
% clf
% hold on
% plot(x2,MSEVal,'-b','linewidth',2)
% plot(x2,CRLB,'--k','linewidth',2)
% legend('Actual MSE','CRLB','location','north')
%
%REFERENCES:
%[1] A. Papoulis and S. U. Pillai, Probability, Random Variables and
%    Stochastic Processes, 4th ed. Boston: McGraw Hill, 2002.
%[2] C. R. Rao, "Information and the accuray attainable in the estimation
%    of statistical parameters," Bulletin of the Calcutta Mathematical
%    Society, vol. 37, no. 3, pp. 81-91, 1945.
%[3] P. Stoica and R. L. Moses, "On biased estimators and the unbiased
%    Cramér-Rao lower bound," Signal Processing, vol. 21, no. 4, pp. 349-
%    350, Dec. 1990.
%
%November 2022 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=size(RInv,3);
xDim=size(stateJacob,2);
statDim=size(statJacob,1);

if(nargin<7||isempty(checkPosDef))
    checkPosDef=false;
end

if(nargin<6)
    numMeasDims=[];
end

if(nargin<5||isempty(biasJacob))
    biasJacob=zeros(statDim,xDim);
end

if(nargin<4||isempty(biasVec))
    biasVec=zeros(statDim,1);
end

if(nargin<3||isempty(statJacob))
    statJacob=eye(xDim,xDim);
end

if(numMeas>1&&size(stateJacob,3)==1)
    stateJacob=repmat(stateJacob,[1,1,numMeas]);
end

%First get the FIM
J=zeros(xDim,xDim);
if(isempty(numMeasDims))
    %If all measurements are zDim in size
    for k=1:numMeas
        J=J+stateJacob(:,:,k)'*RInv(:,:,k)*stateJacob(:,:,k);
    end
else
    for k=1:numMeas
        zDimCur=numMeasDims(k);
        sel=1:zDimCur;
        J=J+stateJacob(sel,:,k)'*RInv(sel,sel,k)*stateJacob(sel,:,k);
    end
end

if(checkPosDef)
    %Check whether or not J is positive definite.
    if(matrixRank(J)<xDim)
        CRLB=[];
        return;
    end
end

CRLB=(statJacob+biasJacob)*inv(J)*(statJacob+biasJacob)'+biasVec*biasVec';

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function  prices = SABR_EurBarAmer_func(call, M, T, S0, Kvec, r, CTMCParams, ModParams, contract_type, L)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for European, American, and Barrier Options using
% double Layer CTMC approximation for SABR
%
% Models Supported: SABR
% Returns: price of contract (for vector of strikes)
% Author: Justin Lars Kirkby
%
% References:  (1) General Valuation Framework for SABR and Stochastic Local Volatility
%                   Models. SIAM J. Financial Mathematics, 2018. (w/ Z. Cui
%                   and D. Nguyen)
%
% ----------------------
% Contract/Model Params
% ----------------------
% call = 1 for call option, else Put
% contract_type = type of contact: % 1 = European, 2 = American, 3 = Down and Out Barrier
% Kvec  = strike vector
% S0 = initinal underlying value
% r   = interest rate (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=1)
% ModParams = model parameters: .v0, .alpha, .beta, .rho
% L =  For barrier contract, this is the barrier
%
% ----------------------
% Numerical (CTMC) Params
% ----------------------
% CTMCParams: .m_0 = grid/state size for variance process
%             .N = grid/state stize for underlying
%             .gridMult_v = grid non-uniformity multiplier (for variance)
%             .gridMult_s = grid non-uniformity multiplier (for underlying)
%             .gamma = Grid width param for variance grid
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

m_0        = CTMCParams.m_0;
N          = CTMCParams.N;
gridMult_v = CTMCParams.gridMult_v;
gridMult_s = CTMCParams.gridMult_s;  %Grid mult param for S
gamma      = CTMCParams.gamma;          %Grid width param for variance grid

gridMethod_v = 5;   %%% ALWAYS use 5 for this one (puts v0 on grid)
gridMethod_s = 4;   %%% 5 puts S_0 on grid, but 4 seems better (requires interpolation)

v0    = ModParams.v0;
alpha = ModParams.alpha;
beta  = ModParams.beta;
rho   = ModParams.rho;

%%%%%%%%%%%%%%%%%%%%%%
%%%   Set Asset Grid bounds
%%%%%%%%%%%%%%%%%%%%%%
if S0 < 0.5
    ls = .01*S0;   %lower bound in asset Grid (S_t) space
else
    ls = 0.001*S0;
end

us = max(4.5*S0, S0 + 10*(v0*(S0)^beta)*sqrt(T));  %upper bound in asset Grid (S_t) space

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 1: Variance Grid / Generators
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = T/M;
t = sqrt(T)/2;    %NOTE: this is different than we used to use...

mu_func = @(u) 0*u;
sig_func = @(u) alpha*u;
mu_H = v0;
sig2_H = v0^2*(exp(alpha^2*t) - 1);

lx = max(0.0001,mu_H - gamma*sqrt(sig2_H));
ux = mu_H + gamma*sqrt(sig2_H);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 1: Variance Grid / Generators
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
center_v = v0;
[Q,v] = General_Q_Matrix_Newest(m_0,mu_func,sig_func,lx,ux,gridMethod_v,center_v, gridMult_v);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 2: Asset Grid / Grid For Xtilde
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g = @(s) (s ).^(1-beta)/(1-beta);
invOneBet = 1/(1-beta);

center_s = S0;   %center of asset grid (e.g. center points around the strike)
manualPoint_s = center_s;  %manually places S0 on grid
Xgrid = g(getNonUniformGrid(N, ls, us, gridMethod_s, center_s, manualPoint_s, gridMult_s)) - rho/alpha*v0;  %Grid for Xtilde

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 3: Generators (for Xtilde process)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Nm = N*m_0;
G = zeros(Nm, Nm);  %Big Generator matrix
I = eye(N,N);

Payoff = zeros(Nm, 1);  %Terminal payoff
sqrtRho = sqrt(1-rho^2);

for j = 1:m_0 %loop through rows of big G matrix
    %%%%%%%%%%
    % Step(1): Find G_j (generator with v(j) fixed)
    %%%%%%%%%%
    nu_j = v(j);
    muX_func_nu  = @(x) -.5*beta*(nu_j)^2.*((1-beta)*(x + rho*nu_j/alpha)).^(-1) ;  %Drift function of Xtilde with v(j) fixed
    sigX_func_nu = @(x) sqrtRho*nu_j*[x>-100];  %Constant function for each fixed variance state

    Gnu = getGenerator_Q_MatrixOnly(Xgrid, muX_func_nu, sigX_func_nu, gridMethod_v);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% FORCE absorbing vs reflecting
    Gnu(1,1) = 0; Gnu(1,2) =0;
    %Gnu(N,N) = 0; Gnu(N,N-1) = 0;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%
    % Step(2): Populate the Generator matrix (recall it is block tridiagonal,
    %%%%%%%%%%
    for k = max(1,j-1):min(m_0,j+1)  %%% NOTE: we skip the ones that are known to be zeros (matrix is block tridiagonal)
        lamjk = Q(j,k);
        if j==k  %diagonal block element of G matrix
            G((j-1)*N + 1:j*N, (k-1)*N + 1:k*N ) = Gnu + lamjk*I;
        else
            G((j-1)*N + 1:j*N, (k-1)*N + 1:k*N ) = lamjk*I;
        end
    end
end

%%%% Find bracketing variance gridpoint
k_0 = 2;
while v0 >= v(k_0) && k_0 < m_0
    k_0 = k_0+1;
end
k_0 = k_0 - 1;  %left bracketing point:  v(k_0) <= v0 < v(k_0 +1)

%%%% Find bracketing Xtilde gridpoint
x0 = g(S0) - rho*v0/alpha;  %initial value on Xtilde grid for (S_0, v0)
j_0 = 2;
while x0 >= Xgrid(j_0) && j_0 < N
    j_0 = j_0+1;
end
j_0 = j_0 - 1;  %left bracketing point:   Xtilde(j_0) <= x0 < Xtilde(j_0 +1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% VALUE : using recursive method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

P = expm(G*dt);   %transition matrix of one dimensional CTMC
initialIndex = (k_0-1)*N + j_0;  %index corresponding to initial conditions

prices = zeros(length(Kvec),1);

for k = 1:length(Kvec)
    K = Kvec(k);
    %%% Calculate Payoff
    if call == 1
        for j = 1:m_0
            Payoff((j-1)*N +1:j*N) = max(0, ((1-beta)*(max(0,Xgrid + rho*v(j)/alpha) )).^invOneBet - K) ;  %Portion of payoff corresponding to v(j)
        end
    else
       for j = 1:m_0
            Payoff((j-1)*N +1:j*N) = max(0, K - ((1-beta)*(max(0,Xgrid + rho*v(j)/alpha) )).^invOneBet );  %Portion of payoff corresponding to v(j)
       end
    end
    %%% Now Price
    if contract_type == 3  % Down and out for now
        %determine which states remain alive
        alive = zeros(Nm,1);
        for j = 1:m_0
            cons = g(L) - v(j)*rho/alpha;
            alive((j-1)*N +1:j*N) = (Xgrid > cons);
        end
        pVec = alive.*Payoff;
        for m = M-1:-1:0
            pVec = exp(-r*dt)*alive.*(P*pVec);
        end
    else %% either American or European
        pVec = exp(-r*dt)*(P*Payoff);  %initialize the value (at last period)
        if contract_type == 2
            for m = M-2:-1:0
                pVec = exp(-r*dt)*(P*pVec);  %continuation value
                pVec = max(pVec, Payoff);   %max of continuation and intrinsic value
            end
        elseif contract_type == 1
            for m = M-2:-1:0
                pVec = exp(-r*dt)*(P*pVec);  %continuation value
            end
        end
    end

    if gridMethod_s == 5  %Both v0 and x0 are on grid  (we assume v0 is a member of vol grid, ie as long as gridMethod_v = 5)
        prices(k) = pVec(initialIndex);
    elseif gridMethod_s == 4 %x0 is not on grid (though we assume v0 is, ie as long as gridMethod_v = 5)
        price1 = pVec(initialIndex);    %corresponds to j_0
        price2 = pVec(initialIndex+1);  %corresponds to j_0 + 1
        prices(k) = price1 + (price2 - price1)*(x0 - Xgrid(j_0))/(Xgrid(j_0+1) - Xgrid(j_0));  %LINEAR INTERPOLATION
    end
end

end


function [A,B,C,D] = vibsBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f)
%+========================================================================+
%|                                                                        |
%|                 OPENVIBS - LIBRARY FOR VIBRO-ACOUSTIC                  |
%|           openVibs is part of the GYPSILAB toolbox for Matlab          |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : vibsBlockOperator.m                           |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Constants
mu     = rhoS*cT^2;
lambda = rhoS*(cL^2 - 2*cT^2);
w      = 2*pi*f;
k      = w/c0;

% Dimension
n = size(omega.msh.elt,2)-1;

% Green kernel function
if (n == 2)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[H0(kr)]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]3',k);
    G0          = '[log(r)]';
    gradyG0     = 'grady[log(r)]';
    cteGxy      = 1i/4;
    cteG0       = -1/(2*pi);

elseif (n == 3)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k);
    G0          = '[1/r]';
    gradyG0     = 'grady[1/r]';
    cteGxy      =  1/(4*pi);
    cteG0       =  1/(4*pi);

else
    error('vibsNeumannBW.m : unavailable case.')
end

% Coupling coeff for Brackage-Werner simulation
beta = 1i*k;

%%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ELASTO (A11) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
A = cell(n,n);

% Static part
GG = integral(omega,grad(U),grad(U));
for i = 1:n
    for j = 1:n
        % Operator div(U):div(U)
        DD = integral(omega,grad(U,i),grad(U,j));

        % Operator e(U):e(U)
        EE = integral(omega,grad(U,j),grad(U,i));
        if (i==j)
            EE = EE + GG;
        end

        % Summation
        A{i,j} = lambda.*DD + mu.*EE;
    end
end

% Dynamic part
if (f ~= 0)
    Id = integral(omega,U,U);
    for i = 1:n
       A{i,i} = A{i,i} - (rhoS*w^2) .* Id;
    end
end

% Final form (sparse)
A = cell2mat(A);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A12) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Coupling to FEM
B = cell(n,1);
for i = 1:n
    % Collocation mass operator
    Id = integral(sigma,ntimes(U,i),u);

    % Collocation boundary operator
    S = cteGxy .* integral(sigma,sigma,ntimes(U,i),Gxy,u) + ...
        cteG0  .* regularize(sigma,sigma,ntimes(U,i),G0,u);

    % Collocation boundary operator
    D = cteGxy .* integral(sigma,sigma,ntimes(U,i),gradyGxy,ntimes(u)) + ...
        cteG0  .* regularize(sigma,sigma,ntimes(U,i),gradyG0,ntimes(u));

    % Final operator Brackage-Werner : [1i*k*beta*S - (Id/2 + D)]
    B{i} = beta.*S - (0.5*Id + D);
end

% Final form (full matrix)
B = cell2mat(B);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A21) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
C = cell(1,n);

% Coupling FEM
for i = 1:n
    C{i} = (rho0*w^2) .* integral(sigma,u,ntimes(U,i));
end

% Final format (sparse)
C = cell2mat(C);

%%%%%%%%%%%%%%%%%%%%%%%%%% ACOUSTIC-ACOUSTIC (A22) %%%%%%%%%%%%%%%%%%%%%%%%

% Finite element mass matrix
Id = integral(sigma,u,u);

% Finite element boundary operator
H  = cteGxy .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,ntimes(u)) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,nxgrad(u)));
Hr = cteG0  .* (k^2 * regularize(sigma,sigma,ntimes(u),G0,ntimes(u)) ...
    - regularize(sigma,sigma,nxgrad(u),G0,nxgrad(u)));

% Finite element boundary operator
D  = cteGxy .* integral(sigma,sigma,u,gradyGxy,ntimes(u));
Dr = cteG0  .* regularize(sigma,sigma,u,gradyG0,ntimes(u));

% Final operator Brackage-Werner : - [1i*k*beta*(-Id/2 + Dt) - H]
D = - (beta.*(-0.5*Id + (D+Dr).') - (H+Hr));
end


function traveltime(dbName,varargin)

%TRAVELTIME make travel time plots.
% TRAVELTIME(dbName) creates travel time plots for database dbName. The top
% plot for P and S plots ditance vs. traveltime in seconds adjusted by a
% velocity reduction.
%
% TRAVELTIME(dbName,[Vp Vs]) use the specified velocities to reduce the
% times on the travel time plots. The default values are Vp = 7 and Vs = 4.
%
% see also ttimes.dbload

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

% GET ARGUMENTS
if length(varargin)==1
    VpVs = varargin{1};
else
    VpVs = [7 4];
end

% LOAD DATABASE
[origin,site,arrival,ray] = ttimes.dbload(dbName);

% TRAVEL TIME PLOTS
figure('Position',[0 0 1100 850],'Color','w');
set(gcf,'DefaultAxesFontSize',14);
set(gcf,'DefaultAxesLineWidth',0.25);

% P WAVES
f = find(strcmp(arrival.iphase,'P'));
h1 = subplot(2,2,1);
scatter(ray.flatDist(f),arrival.travelTime(f)-ray.flatDist(f)/VpVs(1),30,ray.originDepth(f),'filled','MarkerEdgeColor','k');
hold on; box on; grid on;
xlabel('Distance (km)');
ylabel(['Traveltime - distance/' num2str(VpVs(1)) 'km/s (s)']);
title('P wave travel times');
xlim1 = get(gca,'xlim');
ylim1 = get(gca,'ylim');
%
h3 = subplot(2,2,3);
scatter(ray.flatDist(f),arrival.timeres(f),30,ray.originDepth(f),'filled','MarkerEdgeColor','k');
hold on; box on; grid on;
xlabel('Distance (km)');
ylabel('Time residual (s)');
title('P wave travel time residuals');
xlim3 = get(gca,'xlim');
ylim3 = get(gca,'ylim');

% S WAVES
f = find(strcmp(arrival.iphase,'S'));
h2 = subplot(2,2,2);
scatter(ray.flatDist(f),arrival.travelTime(f)-ray.flatDist(f)/VpVs(2),30,ray.originDepth(f),'filled','MarkerEdgeColor','k');
hold on; box on; grid on;
xlabel('Distance (km)');
ylabel(['Traveltime - distance/' num2str(VpVs(2)) 'km/s (s)']);
title('S wave travel times');
xlim2 = get(gca,'xlim');
ylim2 = get(gca,'ylim');
%
h4 = subplot(2,2,4);
scatter(ray.flatDist(f),arrival.timeres(f),30,ray.originDepth(f),'filled','MarkerEdgeColor','k');
hold on; box on; grid on;
xlabel('Distance (km)');
ylabel('Time residual (s)');
title('S wave travel time residuals');
xlim4 = get(gca,'xlim');
ylim4 = get(gca,'ylim');
%
h = colorbar('Location','east');
cmap = hot;
%colormap(flipud(cmap));
colormap(cmap);
position = get(h,'Position');
position(3) = position(3)/2;
position(4) = position(4)/2;
set(h,'Position',position);
set(h,'YDir','reverse');
set(h,'FontSize',9);
hh = get(h,'YLabel');
set(hh,'String','Depth (km)');
%
xLim = [0 max(ray.flatDist)];
yLim12 = [ min([ylim1 ylim2]) max([ylim1 ylim2])];
yLim34 = [ min([ylim3 ylim4]) max([ylim3 ylim4])];
set(h1,'xlim',xLim,'ylim',yLim12);
set(h2,'xlim',xLim,'ylim',yLim12);
set(h3,'xlim',xLim,'ylim',yLim34);
set(h4,'xlim',xLim,'ylim',yLim34);
%
set(gcf, 'paperorientation', 'landscape');
set(gcf, 'paperposition', [.5 .5 10 7.5] );
print(gcf, '-dpsc2', 'FIG_tt_curve.ps');