function path = imGeodesicPath(img, source, target, varargin)
%IMGEODESICPATH Compute a geodesic path between two markers in an image
%
%   PATH = imGeodesicPath(MASK, SOURCE, TARGET)
%   Computea minimal geodesic-distance path between the two markers SOURCE
%   and TARGET. Both SOURCE and TARGET can be either a binary image the
%   same size as MASK, or a N-by-2 array containing a list of coordinates
%   (x first, y second).
%   The result PATH is a P-by-2 array containing coordinates of a polyline
%   with minimal geodesic length starting from one of the points specified
%   by SOURCE, and terminating at one of the points specified by TARGET.
%   Note that the result is not uniquely defined, and the returned solution
%   is one of the possible solutions.
%
%   PATH = imGeodesicPath(..., WEIGHTS)
%   Specify the weights to use for propagating chamfer distance. Default is
%   [3 4], as suggested by Borgefors.
%
%   PATH = imGeodesicPath(..., 'verbose', V)
%   Specify the verbosity. V can be either TRUE or FALSE.
%
%
%   Example
%     % read circle image, and create 2 markers
%     img = imread('circles.png');
%     imshow(img); hold on;
%     p1 = [130 130]; % (x1,y1)
%     p2 = [170 170]; % (x2,y2)
%     plot(p1(1), p1(2), 'bo');
%     plot(p2(1), p2(2), 'ro');
%     % Compute and display the path as a polyline
%     path = imGeodesicPath(img, p1, p2);
%     plot(path(:,1), path(:,2), 'color', 'm', 'linewidth', 2);
%
%   See also
%   imGeodesics, imGeodesicDistanceMap, imMaxGeodesicPath

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-04-04,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% Default values

% weights for computing geodesic lengths
ws = [3 4];

% no verbosity by default
verbose = 0;

%% process input arguments

% extract weights if present
if ~isempty(varargin)
    if isnumeric(varargin{1})
        ws = varargin{1};
        varargin(1) = [];
    end
end

% Extract options
while ~isempty(varargin)
    paramName = varargin{1};
    if strcmpi(paramName, 'verbose')
        verbose = varargin{2};
    else
        error(['Unkown option in imGeodesicLength: ' paramName]);
    end
    varargin(1:2) = [];
end

% forces input image to binary
img = img > 0;

%% Propagate distance from destination markers

% initialize marker image
if sum(size(target) == size(img)) == 2
    % initialize from binary mask
    markers = target > 0;

else
    % initialize from set of points
    markers = false(size(img));
    for i = 1:size(target, 1)
        markers(target(i,2), target(i,1)) = true;
    end
end

% compute distance map from marker
dist = imGeodesicDistanceMap(img, markers, ws, 'verbose', verbose);

%% Find position of first source point

% find position of closest point belonging to the source
if sum(size(source) == size(img)) == 2
    % compute in a binary mask
    [minDist, ind] = min(dist(source));
    [ys, xs] = ind2sub(size(source), ind);

else
    % initialize from set of points
    minDist = inf;
    for i = 1:size(source, 1)
        value = dist(source(i,2), source(i,1));
        if value < minDist
            xs = source(i,1);
            ys = source(i,2);
            minDist = value;
        end
    end
end

% check existence of path
if isempty(minDist)
    warning([mfilename ':NoPathFound'], ...
        'No path could be found between the two markers');
    path = [];
    return;
end

%% Create the path by returning to destination marker

% add a 1-pixel wide border around image, initialized with very high value
tmp = dist;
dist = zeros(size(dist)+2, class(dist));
dist(:) = max(tmp(:)) + 2;
dist(2:end-1, 2:end-1) = tmp;

% keep coordinates of geodesic extremity
x = xs + 1;
y = ys + 1;

% initialize path
path = [x y];

while true
    % check around current point
    neigh = dist(y-1:y+1, x-1:x+1);

    % look for the minimum
    [mini, ind] = min(neigh(:));

    % if minimum is the same as current value, minima is found
    if mini == dist(y, x)
        break;
    end

    % convert index to sub
    [iy, ix] = ind2sub([3 3], ind(1));

    % update coord
    x = x + ix - 2;
    y = y + iy - 2;

    % stores result
    path = [path; x y]; %#ok<AGROW>
end

% subtract 1, because of border
path = path - 1;


function y = erfc(x)
%ERFC         Implements  erfc(x)  for real intervals
%
%   y = erfc(x)
%
%interval standard function implementation
%

% written  05/30/13     S.M. Rump
%

  if x.complex
    error('Error function only for real arguments')
  end

  if issparse(x)
    index = ( x==0 );
    if any(index(:))                    % treat zero indices
      y = intval(ones(size(x)));
      index = ~index;
      %VVVV  y(index) = erfc(full(x(index)));
      s.type = '()'; s.subs = {index}; y = subsasgn(y,s,erfc(full(subsref(x,s))));
      %AAAA  Matlab bug fix
    else
      y = erfc(full(x));
    end
    return
  end

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

  y = x;

  if all( x.inf(:)==x.sup(:) );         % thin input

    [y.inf,y.sup] = erfc_rnd(x.inf(:),[]);

  else                                  % thick input

    y.inf = erfc_rnd(x.sup(:),-1);
    y.sup = erfc_rnd(x.inf(:),1);

  end

  y.inf = reshape(y.inf,size(x.inf));
  y.sup = reshape(y.sup,size(x.sup));

  setround(rndold)


% OP_GRADSYMU_GRADSYMV_MP: assemble the stiffness matrix A = [a(i,j)], a(i,j) = (epsilon gradsym u_j, gradsym v_i), in a multipatch domain.
%
%   mat = op_gradsymu_gradsymv_mp (spu, spv, msh, [epsilon], [patches]);
%
% INPUT:
%
%   spu:     object representing the space of trial functions (see sp_multipatch)
%   spv:     object representing the space of test functions (see sp_multipatch)
%   msh:     object defining the domain partition and the quadrature rule (see msh_multipatch)
%   epsilon: function handle to compute the diffusion coefficient. Equal to one if left empty.
%   patches: list of patches where the integrals have to be computed. By default, all patches are selected.
%
% OUTPUT:
%
%   mat:    assembled stiffness matrix
%
% Copyright (C) 2015, 2016, 2020 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function A = op_gradsymu_gradsymv_mp (spu, spv, msh, coeff, patch_list)

  if (nargin < 5)
    patch_list = 1:msh.npatch;
  end

  if ((spu.npatch ~= spv.npatch) || (spu.npatch ~= msh.npatch))
    error ('op_gradsymu_gradsymv_mp: the number of patches does not coincide')
  end

  ncounter = 0;
  for iptc = patch_list
    if (nargin < 4 || isempty (coeff))
      [rs, cs, vs] = op_gradsymu_gradsymv_tp (spu.sp_patch{iptc}, spv.sp_patch{iptc}, msh.msh_patch{iptc});
    else
      [rs, cs, vs] = op_gradsymu_gradsymv_tp (spu.sp_patch{iptc}, spv.sp_patch{iptc}, msh.msh_patch{iptc}, coeff);
    end
    rows(ncounter+(1:numel (rs))) = spv.gnum{iptc}(rs);
    cols(ncounter+(1:numel (rs))) = spu.gnum{iptc}(cs);

    if (~isempty (spv.dofs_ornt))
      vs = spv.dofs_ornt{iptc}(rs)' .* vs;
    end
    if (~isempty (spu.dofs_ornt))
      vs = vs .* spu.dofs_ornt{iptc}(cs)';
    end

    vals(ncounter+(1:numel (rs))) = vs;
    ncounter = ncounter + numel (rs);
  end

  A = sparse (rows, cols, vals, spv.ndof, spu.ndof);
  clear rows cols vals rs cs vs

end


function n = kpu_order ( l )

%*****************************************************************************80
%
%% KPU_ORDER computes the order of a KPU rule from the level.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 May 2012
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, integer L, the level of the rule.
%    1 <= L <= 25
%
%    Output, integer N, the order of the rule.
%
  if ( l < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KPU_ORDER - Fatal error!\n' );
    fprintf ( 1, '  1 <= L <= 25 required.\n' );
    fprintf ( 1, '  Input L = %d\n', l );
    error ( 'KPU_ORDER - Fatal error!' );
  elseif ( l == 1 )
    n = 1;
  elseif ( l <= 3 )
    n = 3;
  elseif ( l <= 6 )
    n = 7;
  elseif ( l <= 12 )
    n = 15;
  elseif ( l <= 24 )
    n = 31;
  elseif ( l <= 25 )
    n = 63;
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KPU_ORDER - Fatal error!\n' );
    fprintf ( 1, '  1 <= L <= 25 required.\n' );
    fprintf ( 1, '  Input L = %d\n', l );
    error ( 'KPU_ORDER - Fatal error!' );
  end

  return
end


% LORENZ system
% Train ensemble of models (e.g. for varying training data length, noise
% level, etc.)
% System identification: SINDYc

clear all, close all, clc

SystemModel = 'LORENZ';
DERIV_NOISE = 0;
TRACK_MODEL_BEST = 0;
%% Set Paths
figpath = '../FIGURES/EX_LORENZ_Dependencies/SINDYc/'; mkdir(figpath)
datapath = '../DATA/EX_LORENZ_Dependencies/SINDYc/'; mkdir(datapath)

% Overwrite if necessary
if exist('DERIV_NOISE')
    if DERIV_NOISE == 1
        figpath = '../FIGURES/EX_LORENZ_Dependencies/SINDYc/TVRegDiff/'; mkdir(figpath)
        datapath = '../DATA/EX_LORENZ_Dependencies/SINDYc/TVRegDiff/'; mkdir(datapath)
    end
end
addpath('../utils');

%% Parameters
ModelName = 'SINDYc';
polyorder = 2;
usesine = 0;
lambda0 = 0.1;     % lambda is our sparsification knob.
dep_trainlength = 1;
dep_noise = 0;

MOD_VAL = 10;

%% Select case 1 or 2
% 1) Dependency on training length
Ntrain_vec = [5:15,20:5:95,100:100:1000];%,1250,1500,2000,3000];%,1500:500:3000];
eta_vec = 0;%0.05;
Nr = 1;%50;

% 2) Noise dependency
% Ntrain_vec = 3000;
% eta_vec = [0.01 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5];
% Nr = 50;

N_LENGTHS = length(Ntrain_vec);
N_ETA = length(eta_vec);

%% Parameters
SAVE_MODEL = 0;
SHOW_RESULTS = 0;
SHOW_STATS = 1;
ONLY_TRAINING_LENGTH = 1; % if 0 : 3000 time unit, otherwise 1000

if ONLY_TRAINING_LENGTH == 1
    PostName = ['_TrainLength'];
else
    PostName = [];
end
%% Generate Data %{'sine2', 'chirp','prbs', 'sphs'}
InputSignalType = 'sphs'; %prbs; chirp; noise; sine2; sphs; mixed
getTrainingData

DataTrain.x = x;
DataTrain.t = t;
DataTrain.tspan = tspan;
DataTrain.u = u;

xstd = std(xv(:,3));

close all
rng(0,'twister')

errBest = inf*ones(N_LENGTHS,N_ETA);
% BestModels(1:N_LENGTHS,1:N_ETA) = struct('name', [], 'polyorder', [], 'usesine', [], 'Xi', [], ...
%     'dt', [], 'N', [], 'Ttraining', [], 'Err', [], 'ErrM', []);
BestModelsList = zeros(N_LENGTHS,N_ETA);
Lambda = zeros(N_LENGTHS,N_ETA);
Nvar = 3;
%% SINDYc
if ONLY_TRAINING_LENGTH == 0
    for iN = 1:N_LENGTHS
        for iNoise = 1:N_ETA
            disp(['Running for noise case ', num2str(iNoise), ' of ', num2str(N_ETA)])

            Results = struct('err', zeros(Nr,1), 'errM', zeros(Nr,1), 'xA', zeros(length(tv),Nvar), 'xB', zeros(length(tv),Nvar,Nr),'Ttraining',zeros(Nr,1));

            for iR = 1:Nr

                % Setup data
                x = DataTrain.x(1:Ntrain_vec(iN),:);
                u = DataTrain.u(1:Ntrain_vec(iN));
                t = DataTrain.t(1:Ntrain_vec(iN));
                tspan = DataTrain.tspan(1:Ntrain_vec(iN));

                % Add noise
                eps = eta_vec(iNoise)*xstd;
                x = x + eps*randn(size(x));

                % Train model
                lambda = lambda0;
                tic
                trainSINDYc
                telapsed = toc

                % Prediction over training phase
                p.ahat = Xi(:,1:3);
                p.polyorder = polyorder;
                p.usesine = usesine;
                p.dt = dt;
                [N,Ns] = size(DataTrain.x);
                xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x0';
                for ct=1:N-1
                    xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
                end
                xSINDYc = xSINDYc';

                % Show validation
                SHOW_PREDICTION_FOR_TRAINING_PHASE

                %% Prediction
                % Reference
                xA      = [xv];
                tA      = tv;

                % Model
                [N,Ns] = size(xA);
                xB = zeros(Ns,N); xB(:,1) = DataTrain.x(end,:)';
                for ct=1:N
                    xB(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xB(:,ct),uv(ct),dt,1,[],p);
                end
                xB = xB(:,1:N+1)';
                tB = [tv(1)-dt;tv];

                % Show training and prediction
                SHOW_PREDICTION_FOR_VALIDATION_PHASE

                %% Error
                err = mean(sum((xB(2:end,:)-xA).^2,2));
                errM = mean(sum((xB(2:250+1,:)-xA(1:250,:)).^2,2));

                %% Save Data
                Model.name = 'SINDYc';
                Model.polyorder = polyorder;
                Model.usesine = usesine;
                Model.Xi = Xi;
                Model.dt = dt;
                Model.N  = Ntrain_vec(iN);
                Model.Ttraining = telapsed;
                Model.Err = err;
                Model.ErrM = errM;

                if SAVE_MODEL == 1
                    if mod(iR,MOD_VAL) == 0 || iR == 1

                        save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_iR',num2str(iR),'.mat']),'Model')
                    end
                end

                Results.err(iR) = err;
                Results.errM(iR) = errM;
                Results.xA = xA;
                Results.xB(:,1:3,iR) = xB(2:end,:);
                Results.Ttraining(iR) = telapsed;

                %% Track best model
                % errBest = 10^12*ones(N_LENGTHS,N_ETA)
                if TRACK_MODEL_BEST == 1
                    if Results.err(iR)<errBest(iN,iNoise) || iR == 1
                        errBest(iN,iNoise) = Results.err(iR);

                        BestModel = Model;
                        save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_BEST_MODEL.mat']),'Model')

                        %BestModels(iN,iNoise) = Model;
                        BestModelsList(iN,iNoise) = iR;
                        Lambda(iN,iNoise) = lambda;

                        clear ph
                        f1 = figure('visible','off');box on, hold on,
                        ccolors = get(gca,'colororder');
                        plot([tB(1),tB(1)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                        plot([t(end),t(end)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                        ylim([-25 65])
                        text(5,55,'Training', 'FontSize',12)
                        %                     text(10+tA(1),230,'Validation', 'FontSize',12)
                        text(3+tA(1),55,'Validation', 'FontSize',12)

                        if eps~=0
                            ph(4) = plot(tspan,x(:,1),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(1,:)+[0.15 0.3 0.25]
                            plot(tspan,x(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(2,:)+[0.15 0.3 0.25]
                            plot(tspan,x(:,3),'-','Color',0.7*ones(1,3),'LineWidth',1);
                            ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',0.5);
                            ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',0.5);
                            ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',0.5);
                        else
                            ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
                            ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
                            ph(3) = plot(t,x(:,1),'--','Color',[0 1 0],'LineWidth',1); % Training data
                            plot(t,x(:,2),'--','Color',[0 1 0],'LineWidth',1);
                        end

                        ph(5) = plot(tB,xB(:,1),'-.','Color',ccolors(1,:)-[0 0.2 0.2],'LineWidth',2);
                        ph(6) = plot(tB,xB(:,2),'-.','Color',ccolors(2,:)-[0.1 0.2 0.09],'LineWidth',2);
                        ph(7) = plot(tB,xB(:,3),'-.','Color',ccolors(3,:)-[0.1 0.2 0.09],'LineWidth',2);
                        grid off
                        xlim([0 tv(end)])
                        xlabel('Time')
                        ylabel('xi')
                        set(gca,'LineWidth',1, 'FontSize',14)
                        set(gcf,'Position',[100 100 300 200])
                        set(gcf,'PaperPositionMode','auto')
                        print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_BEST_MODEL_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_noleg.eps']);

                        if eps~=0
                            lh = legend(ph([1,4,5]),'Truth','Training',ModelName,'Location','NorthWest');
                            %                         lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
                            lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
                        else
                            lh = legend(ph([1,4,5]),'Truth','Training',ModelName,'Location','NorthWest');
                            lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
                        end
                        print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_BEST_MODEL_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'.eps']);
                        close(f1);
                    end
                end
                lambda = lambda0;
            end
            if SHOW_STATS == 1
                %%
                xBmin = min(Results.xB(:,1:3,:),[],3);
                xBmax = max(Results.xB(:,1:3,:),[],3);
                clear ph
                figure('visible','off'),box on, hold on,
                ccolors = get(gca,'colororder');
                plot([tB(1),tB(1)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                plot([t(end),t(end)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
                ylim([-25 65])
                t1 = text(5,55,'Training', 'FontSize',12);
                t2 = text(5+tA(1),55,'Validation', 'FontSize',12);

                X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
                Y=[xBmin(:,1)',flipud(xBmax(:,1))'];              %#create y values for out and then back
                fillh1 = fill(X,Y,ccolors(1,:)-[0 0.2 0.2]);                  %#plot filled area
                fillh1.EdgeColor = ccolors(1,:)-[0 0.2 0.2]; fillh1.FaceAlpha = 0.5;

                X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
                Y=[xBmin(:,2)',flipud(xBmax(:,2))'];              %#create y values for out and then back
                fillh = fill(X,Y,ccolors(2,:)-[0.1 0.2 0.09]);                  %#plot filled area
                fillh.EdgeColor = ccolors(2,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

                X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
                Y=[xBmin(:,3)',flipud(xBmax(:,3))'];              %#create y values for out and then back
                fillh = fill(X,Y,ccolors(3,:)-[0.1 0.2 0.09]);                  %#plot filled area
                fillh.EdgeColor = ccolors(3,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

                if eps~=0
                    ph(4) = plot(tspan,x(:,1),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(1,:)+[0.15 0.3 0.25]
                    plot(tspan,x(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(2,:)+[0.15 0.3 0.25]
                    plot(tspan,x(:,3),'-','Color',0.7*ones(1,3),'LineWidth',1);
                    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
                    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
                    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
                else
                    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
                    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
                    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
                    ph(4) = plot(t,x(:,1),'--','Color',[0 1 0],'LineWidth',1); % Training data
                    plot(t,x(:,2),'--','Color',[0 1 0],'LineWidth',1);
                    plot(t,x(:,3),'--','Color',[0 1 0],'LineWidth',1);
                end

                grid off
                xlim([0 tv(end)])
                xlabel('Time')
                ylabel('xi')
                set(gca,'LineWidth',1, 'FontSize',14)
                set(gcf,'Position',[100 100 300 200])
                set(gcf,'PaperPositionMode','auto');

                if eps~=0
                    lh = legend([ph([1,4]),fillh1],'Truth','Training',ModelName,'Location','NorthWest');
                    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
                else
                    lh = legend(ph([1,4]),'Truth','Training',ModelName,'Location','NorthWest');
                    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
                end

                print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS.eps']);

                delete(lh), delete(t1), delete(t2)
                print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS_noleg.eps']);
            end
            %%
            save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS.mat']),'Results')
        end
    end
    % % save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_BEST_MODELS.mat']),'BestModels')
    save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_BEST_MODELS_LIST',PostName,'.mat']),'BestModelsList')
    save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_BEST_MODELS_Lambda',PostName,'.mat']),'Lambda')
end

%%
if ONLY_TRAINING_LENGTH == 1
    length_vec = 5:2:Ntrain-1;
    Results = struct('err', zeros(length(length_vec),2),'PredHor_Ball', zeros(length(length_vec),2), 'PredLength', zeros(length(length_vec),2),'RelErr', zeros(length(length_vec),2),'RelErrMax', zeros(length(length_vec),2), 'Ttraining', zeros(length(length_vec),1));
    Results.Ntrain_vec = length_vec;
    for iL = 1:length(length_vec)
        iN = length_vec(iL);
        if mod(iN,50)
            disp(['Running for length ', num2str(iN), 'of ', num2str(Ntrain-1)])
        end
        % Setup data
        x = DataTrain.x(1:iN,:);
        u = DataTrain.u(1:iN);
        t = DataTrain.t(1:iN);

        % Train model
        lambda = lambda0;
        tic
        trainSINDYc
        telapsed = toc

        % Prediction over training phase
        p.ahat = Xi(:,1:3);
        p.polyorder = polyorder;
        p.usesine = usesine;
        p.dt = dt;
        [N,Ns] = size(DataTrain.x);
        xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x0';
        for ct=1:N-1
            xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
        end
        xSINDYc = xSINDYc';

        %% Error over training phase
        Results.err(iL,1) = mean(sum((xSINDYc(1:end,:)-DataTrain.x).^2,2));
        Results.RelErr(iL,1) = mean( sum( abs( (DataTrain.x - xSINDYc)./DataTrain.x ),2) );
        Results.RelErrMax(iL,1) = mean( max( abs( (DataTrain.x - xSINDYc)./DataTrain.x ),[],2) );

        tmp = abs( (DataTrain.x - xSINDYc)./DataTrain.x );
        TF = tmp>0.1;
        idx = [];
        for iVar = 1:Nvar
            idx = [idx ; find(TF(:,iVar)==1,1,'first')];
        end
        Results.PredLength(iL,1) = max(idx);

        tmp2 = sqrt(sum(abs(DataTrain.x - xSINDYc).^2,2));
        idx = find(tmp2>3,1,'first');
        Results.PredHor_Ball(iL,1) = idx-1;
        %% Prediction
        % Reference
        xA      = [xv];
        tA      = tv;

        % Model
        [N,Ns] = size(xA);
        xB = zeros(Ns,N); xB(:,1) = DataTrain.x(end,:)';
        for ct=1:N
            xB(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xB(:,ct),uv(ct),dt,1,[],p);
        end
        xB = xB(:,1:N+1)';
        tB = [tv(1)-dt;tv];

        %% Error over prediction phase
        xB = xB(2:end,:);
        Results.err(iL,2) = mean(sum((xB-xA).^2,2));
        Results.RelErr(iL,2) = mean( sum( abs( (xA - xB)./xA ),2) );
        Results.RelErrMax(iL,2) = mean( max( abs( (xA - xB)./xA ),[],2) );
        Results.Ttraining(iL) = telapsed;

        tmp = abs( (xA - xB)./xA );
        TF = tmp>0.1;
        idx = [];
        for iVar = 1:Nvar
            idx = [idx ; find(TF(:,iVar)==1,1,'first')];
        end
        Results.PredLength(iL,2) = max(idx);

        tmp2 = sqrt(sum(abs(xA - xB).^2,2));
        idx = find(tmp2>3,1,'first');
        Results.PredHor_Ball(iL,2) = idx-1;
    end

    Results.DataTrain = DataTrain;
    Results.DataValid.x = xv;
    Results.DataValid.u = uv;
    Results.DataValid.t = tv;
    Results.DataValid.tspan = tspanv;
    save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_Nevol','_STATS.mat']),'Results')
end


function jed = transition_to_jed_english ( )

%*****************************************************************************80
%
%% TRANSITION_TO_JED_ENGLISH returns the English calendar transition as a JED.
%
%  Discussion:
%
%    In the English calendar, the last moment of the Julian calendar was
%      11:59 pm, 2 September 1752 Julian/English,
%      11:59 pm, 13 September 1752 Gregorian/CE.
%    The first minute of the Gregorian calendar ended at
%      12:01 am, 3 September 1752 Julian,
%      12:01 am, 15 September 1752 Gregorian/CE/English.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real JED, the Julian Ephemeris Date of the date.
%
  jed = 2361221.5;

  return
end


function f = non_domination_sort_mod(x, M, V)

%% function f = non_domination_sort_mod(x, M, V)
% This function sort the current popultion based on non-domination. All the
% individuals in the first front are given a rank of 1, the second front
% individuals are assigned rank 2 and so on. After assigning the rank the
% crowding in each front is calculated.

%  Copyright (c) 2009, Aravind Seshadri
%  All rights reserved.

% [N, m] = size(x);
% clear m;
N = size(x,1); % modified by zzb
x_temp = zeros(N,M + V + 1); % modified by zzb
x_temp(:,1:M + V) = x(:,1:M + V); % modified by zzb
% Initialize the front number to 1.
front = 1;
% There is nothing to this assignment, used only to manipulate easily in
% MATLAB.
% F(front).f = [];
% individual = [];
F = struct('f',cell(1,N)); % modified by zzb
individual = struct('n',cell(1,N),'p',cell(1,N)); % modified by zzb

%% Non-Dominated sort.
% The initialized population is sorted based on non-domination. The fast
% sort algorithm [1] is described as below for each

% ?for each individual p in main population P do the following
%   ?Initialize Sp = []. This set would contain all the individuals that is
%     being dominated by p.
%   ?Initialize np = 0. This would be the number of individuals that domi-
%     nate p.
%   ?for each individual q in P
%       * if p dominated q then
%           ?add q to the set Sp i.e. Sp = Sp ? {q}
%       * else if q dominates p then
%           ?increment the domination counter for p i.e. np = np + 1
%   ?if np = 0 i.e. no individuals dominate p then p belongs to the first
%     front; Set rank of individual p to one i.e prank = 1. Update the first
%     front set by adding p to front one i.e F1 = F1 ? {p}
% ?This is carried out for all the individuals in main population P.
% ?Initialize the front counter to one. i = 1
% ?following is carried out while the ith front is nonempty i.e. Fi != []
%   ?Q = []. The set for storing the individuals for (i + 1)th front.
%   ?for each individual p in front Fi
%       * for each individual q in Sp (Sp is the set of individuals
%         dominated by p)
%           ?nq = nq-1, decrement the domination count for individual q.
%           ?if nq = 0 then none of the individuals in the subsequent
%             fronts would dominate q. Hence set qrank = i + 1. Update
%             the set Q with individual q i.e. Q = Q ? q.
%   ?Increment the front counter by one.
%   ?Now the set Q is the next front and hence Fi = Q.
%
% This algorithm is better than the original NSGA ([2]) since it utilize
% the informatoion about the set that an individual dominate (Sp) and
% number of individuals that dominate the individual (np).

for i = 1 : N
    % Number of individuals that dominate this individual
    individual(i).n = 0;
    % Individuals which this individual dominate
    individual(i).p = [];
    for j = 1 : N
        dom_less = 0;
        dom_equal = 0;
        dom_more = 0;
        for k = 1 : M
            if (x_temp(i,V + k) < x_temp(j,V + k))
                dom_less = dom_less + 1;
            elseif (x_temp(i,V + k) == x_temp(j,V + k))
                dom_equal = dom_equal + 1;
            else
                dom_more = dom_more + 1;
            end
        end
        if dom_less == 0 && dom_equal ~= M
            individual(i).n = individual(i).n + 1;
        elseif dom_more == 0 && dom_equal ~= M
            individual(i).p = [individual(i).p j];
        end
    end
    if individual(i).n == 0
        x_temp(i,M + V + 1) = 1;
        F(front).f = [F(front).f i];
    end
end
% Find the subsequent fronts
while ~isempty(F(front).f)
   Q.temp = []; % modified by zzb
   for i = 1 : length(F(front).f)
       if ~isempty(individual(F(front).f(i)).p)
            for j = 1 : length(individual(F(front).f(i)).p)
            	individual(individual(F(front).f(i)).p(j)).n = ...
                	individual(individual(F(front).f(i)).p(j)).n - 1;
                if individual(individual(F(front).f(i)).p(j)).n == 0
                    x_temp(individual(F(front).f(i)).p(j),M + V + 1) = front + 1;
                    Q.temp = [Q.temp individual(F(front).f(i)).p(j)]; % modified by zzb
                end
            end
       end
   end
   front =  front + 1;
   F(front).f = Q.temp; % modified by zzb
end

%% Crowding distance
%The crowing distance is calculated as below
% ?For each front Fi, n is the number of individuals.
%   ?initialize the distance to be zero for all the individuals i.e. Fi(dj ) = 0,
%     where j corresponds to the jth individual in front Fi.
%   ?for each objective function m
%       * Sort the individuals in front Fi based on objective m i.e. I =
%         sort(Fi,m).
%       * Assign infinite distance to boundary values for each individual
%         in Fi i.e. I(d1) = ? and I(dn) = ?
%       * for k = 2 to (n-1)
%           ?I(dk) = I(dk) + (I(k + 1).m ? I(k ? 1).m)/fmax(m) - fmin(m)
%           ?I(k).m is the value of the mth objective function of the kth
%             individual in I

% Find the crowding distance for each individual in each front
[~,index_of_fronts] = sort(x_temp(:,M + V + 1));
sorted_based_on_front = x_temp; % Initialize by x % modified by zzb
for i = 1 : length(index_of_fronts)
    sorted_based_on_front(i,:) = x_temp(index_of_fronts(i),:);
end
current_index = 0;

% for front = 1 : (length(F) - 1) % The last front of F is []
for front_count = 1 : (front - 1) % modified by zzb
    % objective = [];
    % distance = 0;
    % y = [];
    y = zeros(length(F(front_count).f),2*M + V + 1); % modified by zzb
    previous_index = current_index + 1;
    for i = 1 : length(F(front_count).f) % modified by zzb
        %y(i,:) = sorted_based_on_front(current_index + i,:);
        y(i,1:M + V + 1) = sorted_based_on_front(current_index + i,:); % modified by zzb
    end
    current_index = current_index + i;
    % Sort each individual based on the objective
    % sorted_based_on_objective = []; % modified by zzb
    for i = 1 : M
        [~, index_of_objectives] = sort(y(:,V + i)); % modified by zzb
        % sorted_based_on_objective = [];
        sorted_based_on_objective = y;  % Initialize by y  % modified by zzb
        for j = 1 : length(index_of_objectives)
            sorted_based_on_objective(j,:) = y(index_of_objectives(j),:);
        end
        f_max = sorted_based_on_objective(length(index_of_objectives), V + i);
        f_min = sorted_based_on_objective(1, V + i);
        y(index_of_objectives(length(index_of_objectives)),M + V + 1 + i) = Inf;
        y(index_of_objectives(1),M + V + 1 + i) = Inf;
         for j = 2 : length(index_of_objectives) - 1
            next_obj = sorted_based_on_objective(j + 1,V + i);
            previous_obj = sorted_based_on_objective(j - 1,V + i);
            if (f_max - f_min == 0)
                y(index_of_objectives(j),M + V + 1 + i) = Inf;
            else
                y(index_of_objectives(j),M + V + 1 + i) = ...
                     (next_obj - previous_obj)/(f_max - f_min);
            end
         end
    end
    % distance = [];
    % distance(:,1) = zeros(length(F(front_count).f),1); % modified by zzb
    distance = zeros(length(F(front_count).f),1); % modified by zzb
    for i = 1 : M
       % distance(:,1) = distance(:,1) + y(:,M + V + 1 + i);% modified by zzb
       distance = distance + y(:,M + V + 1 + i);% modified by zzb
    end
    y(:,M + V + 2) = distance;
    % y = y(:,1 : M + V + 2);
    % z(previous_index:current_index,:) = y;
    z(previous_index:current_index,:) = y(:,1 : M + V + 2);
end
% f = z();
f = z;
%% References
% [1] *Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan*, |A Fast
% Elitist Multiobjective Genetic Algorithm: NSGA-II|, IEEE Transactions on
% Evolutionary Computation 6 (2002), no. 2, 182 ~ 197.
%
% [2] *N. Srinivas and Kalyanmoy Deb*, |Multiobjective Optimization Using
% Nondominated Sorting in Genetic Algorithms|, Evolutionary Computation 2
% (1994), no. 3, 221 ~ 248.


function [Fm,Vm]=cap_patchcylinder(F1,V1,F2,V2,nr,nz)

% function [Fm,Vm]=cap_patchcylinder(F1,V1,F2,V2,nr,nz)
% ------------------------------------------------------------------------
% This function assumes the inputs F1, V1 and F2, V2 define the faces and
% vertices of two concentrid cylinders and closes to top and bottom faces
% by connecting the cylinders together. The input nr defines the number of
% radial points to add for the connection. The input nz defines the number
% of steps used in the z-direction for the cylinders.
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% Change log:
% 2014/09/25
% 2019/03/29 Updated help documentation
%
%------------------------------------------------------------------------

%%

Fm=[F1; (F2+size(V1,1))];
Vm=[V1; V2];

%Coordinates created from nr*nz matrix

%Top edge indices
It=ones(nr-1,1); Jt=(1:1:nr-1)';
INDt=sub2ind([nz,nr-1],It,Jt);
% Vt1=V1(INDt,:); Vt2=V2(INDt,:); Vt=[Vt1; Vt2];

%Bottom edge indices
Ib=nz.*ones(nr-1,1); Jb=(1:1:nr-1)';
INDb=sub2ind([nz,nr-1],Ib,Jb);
% Vb1=V1(INDb,:); Vb2=V2(INDb,:); Vb=[Vb1; Vb2];

Fp=(ones(nr-1,1)*[1 2 2 1])+(((1:1:nr-1*ones)'-1)*ones(1,4)); Fp(Fp==nr)=1; %Quad order

Fq=[INDt(Fp(:,1:2)) INDt(Fp(:,3:4))+size(V1,1)];
Ftt=[Fq(:,[1 3 2]); Fq(:,[1 4 3])]; %Tri order

Fq=[INDb(Fp(:,1:2)) INDb(Fp(:,3:4))+size(V1,1)];
Ftb=[Fq(:,[1 3 2]); Fq(:,[1 4 3])]; %Tri order

Fm=[Fm; Ftt; Ftb];

end

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


function [ hyps ] = generateHypsB( lines, vp, tol, omap, gc )
%GENERATEHYPS generate hypothesis, screen out inconsistent ones according
%to OM, GC, and merged map
%   lines: line segment
%   vp: vanishing point, in z,y,x order
%   tol: tolerance when assigning lines to vp
%   omap & gc: evidence
%   hyps: all hyps with >0.5 consistency with OM will be returned;

%% >> ASSIGN LINE SEGMENTS WITH TYPE

%% get line segments align with vanishing point
lines1 = getVanishingLine(lines, vp(1,:), tol);
lines2 = getVanishingLine(lines, vp(2,:), tol);
lines3 = getVanishingLine(lines, vp(3,:), tol);

%% extend line representation with line type
% line: [1x3 n] [planeID] [1x2 uv1] [u/d f/b r/l] (1/-1, 0 not sure, 2 not available)
lines1_attr = zeros(size(lines1,1),9);
lines2_attr = zeros(size(lines2,1),9);
lines3_attr = zeros(size(lines3,1),9);
lines1_attr(:,1:6) = lines1(:,1:6);
lines2_attr(:,1:6) = lines2(:,1:6);
lines3_attr(:,1:6) = lines3(:,1:6);

%% horizontal line, line connecting vp2 and vp3, u=1, d=-1
horizontal = cross(vp(2,:),vp(3,:));
horizontal = horizontal./norm(horizontal);
if horizontal(:,3)<0
    horizontal = -horizontal;
end
% line belong to vp2 and vp3 should have this attribute
% lines1 not available
lines1_attr(:,7) = 2;
% lines2
for i = 1:size(lines2_attr,1)
    l = lines2(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN([u' v'], l(4));
    b1 = dot(xyz(1,:), horizontal)>0;
    b2 = dot(xyz(2,:), horizontal)>0;
    if b1&&b2
        lines2_attr(i,7) = 1;
    elseif ~b1&&~b2
        lines2_attr(i,7) = -1;
    else
        lines2_attr(i,7) = 0;
    end
end
% lines3
for i = 1:size(lines3_attr,1)
    l = lines3(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN([u' v'], l(4));
    b1 = dot(xyz(1,:), horizontal)>0;
    b2 = dot(xyz(2,:), horizontal)>0;
    if b1&&b2
        lines3_attr(i,7) = 1;
    elseif ~b1&&~b2
        lines3_attr(i,7) = -1;
    else
        lines3_attr(i,7) = 0;
    end
end

%% front and back, line connecting vp1 and vp3, f=1, b=-1
frontNback = cross(vp(1,:),vp(3,:));
frontNback = frontNback./norm(frontNback);
if frontNback(:,2)<0
    frontNback = -frontNback;
end
% line belong to vp1 and vp3 should have this attribute
% lines2 not available
lines2_attr(:,8) = 2;
% lines1
for i = 1:size(lines1_attr,1)
    l = lines1(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), frontNback)>0;
    b2 = dot(xyz(2,:), frontNback)>0;
    if b1&&b2
        lines1_attr(i,8) = 1;
    elseif ~b1&&~b2
        lines1_attr(i,8) = -1;
    else
        lines1_attr(i,8) = 0;
    end
end
% lines3
for i = 1:size(lines3_attr,1)
    l = lines3(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), frontNback)>0;
    b2 = dot(xyz(2,:), frontNback)>0;
    if b1&&b2
        lines3_attr(i,8) = 1;
    elseif ~b1&&~b2
        lines3_attr(i,8) = -1;
    else
        lines3_attr(i,8) = 0;
    end
end

%% left and right, line connecting vp1 and vp2, r=1, l=-1
leftNright = cross(vp(1,:),vp(2,:));
leftNright = leftNright./norm(leftNright);
if leftNright(:,1)<0
    leftNright = -leftNright;
end
% line belong to vp1 and vp2 should have this attribute
% lines3 not available
lines3_attr(:,9) = 2;
% lines1
for i = 1:size(lines1_attr,1)
    l = lines1(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), leftNright)>0;
    b2 = dot(xyz(2,:), leftNright)>0;
    if b1&&b2
        lines1_attr(i,9) = 1;
    elseif ~b1&&~b2
        lines1_attr(i,9) = -1;
    else
        lines1_attr(i,9) = 0;
    end
end
% lines2
for i = 1:size(lines2_attr,1)
    l = lines2(i,:);
    u = l(5:6)*2*pi-pi;
    v = computeUVN( l(1:3), u, l(4));
    xyz = uv2xyzN( [u' v'], l(4));
    b1 = dot(xyz(1,:), leftNright)>0;
    b2 = dot(xyz(2,:), leftNright)>0;
    if b1&&b2
        lines2_attr(i,9) = 1;
    elseif ~b1&&~b2
        lines2_attr(i,9) = -1;
    else
        lines2_attr(i,9) = 0;
    end
end

%% save line segments according to type
typ = [1 1 2; 1 2 -1; 1 -1 2; 1 2 1; ...
       -1 1 2; -1 2 -1; -1 -1 2; -1 2 1; ...
       2 1 -1; 2 1 1; 2 -1 1; 2 -1 -1];
allLines = [lines1_attr; lines2_attr; lines3_attr];
typLine = repmat(struct('lines', [], 'lsLength', [], 'lspool', [], ...
                        'poolLength', []), [size(typ,1) 1]);
for i = 1:size(typ,1)
    valid = (allLines(:,7)==typ(i,1)) & (allLines(:,8)==typ(i,2)) & (allLines(:,9)==typ(i,3));
    typLine(i).lines = allLines(valid,:);
    lsLength = typLine(i).lines(:,6)-typLine(i).lines(:,5);
    lsLength(lsLength<0) = lsLength(lsLength<0) + 1;
    sumLength = sum(lsLength);
    lspool = zeros( ceil(sumLength*100), 1);
    accumuLen = 0;
    poolLength = length(lspool);
    for j = 1:length(lsLength)
        startid = round(accumuLen/sumLength*poolLength) + 1;
        accumuLen = accumuLen + lsLength(j);
        endid = round(accumuLen/sumLength*poolLength);
        lspool(startid:endid) = j;
    end

    typLine(i).lsLength = lsLength;
    typLine(i).lspool = lspool;
    typLine(i).poolLength = poolLength;
end

%% >> BUILD UP RECIPE FROM LINE TYPE TO BOX
% +1: max coords; -1: min coords;
% uf [ 0 +1 +1]  df [ 0 +1 -1]  lf [-1 +1  0];
% ul [-1  0 +1]  dl [-1  0 -1]  rf [+1 +1  0];
% ub [ 0 -1 +1]  db [ 0 -1 -1]  rb [+1 -1  0];
% ur [+1  0 +1]  dr [+1  0 -1]  lb [-1 -1  0];

%% generate recipe: from line type to box by check rank of matrix
allFunc = [ 0 0 0 1 0 -1; ...
            1 0 0 0 0 -1; ...
            0 0 1 0 0 -1; ...
            0 1 0 0 0 -1; ...
            0 0 0 1 -1 0; ...
            1 0 0 0 -1 0; ...
            0 0 1 0 -1 0; ...
            0 1 0 0 -1 0; ...
            1 0 0 -1 0 0; ...
            0 1 0 -1 0 0; ...
            0 1 -1 0 0 0; ...
            1 0 -1 0 0 0];
% combos = combntns(1:size(allFunc,1), 5);
combos = nchoosek(1:size(allFunc,1), 5);
degen = false(size(combos,1),1);
for i = 1:size(combos,1)
    equset = allFunc(combos(i,:), :);
    if rank(equset)<5
        degen(i) = true;
    end
end
cube_recipe = combos(~degen,:);

%% define geometry of cuboid: correlation among corner, edge, and face
% 8 corners, each is the intersection of 3 edges from 3 vps respectively
edgeToPoint = [ 9  2  1; ...
               12  2  3; ...
               11  4  3; ...
               10  4  1; ...
                9  6  5; ...
               12  6  7; ...
               11  8  7; ...
               10  8  5];
% 8 corners, each is assigned with type [u(+1)/d(-1) f(+1)/b(-1) r(+1)/l(-1)]
pointDirect = [ 1  1 -1; ...
                1 -1 -1; ...
                1 -1  1; ...
                1  1  1; ...
               -1  1 -1; ...
               -1 -1 -1; ...
               -1 -1  1; ...
               -1  1  1];
% 12 edges, each connects two corners
PointToEdge = [ 1  4; ...
                2  1; ...
                2  3; ...
                3  4; ...
                5  8; ...
                6  5; ...
                6  7; ...
                7  8; ...
                5  1; ...
                8  4; ...
                7  3; ...
                6  2];
% 6 faces, each face has 4 corners
% ranked as u d f b r l; start from left bottom, clockwise
PointToSurface = [1 2 3 4; ...
                  6 5 8 7; ...
                  5 1 4 8; ...
                  7 3 2 6; ...
                  8 4 3 7; ...
                  6 2 1 5];
SurfaceNm = [1;1;2;2;3;3];

%% >> START TO GENERATE CUBOID HYPOTHESIS, VALIDATE WITH OMAP
maxSample = 200000;
validHyps = repmat(struct('extLine', [], 'hCorner', [], 'srcLine', [], ...
                          'omapScr', 0, 'vpLength', [], 'recipe', [], 'gcScr', -1, 'mgScr', -1, 'opScr', -1 ), maxSample, 1);

%% sample line segments, long line segments are more likely to be selected.
rcpNum = size(cube_recipe,1);
for samid = 1:maxSample
    if rem(samid,1000)==0
%         fprintf('%d/%d hypothesis validated, score: %f/%f\n', samid, maxSample, maxScore, numPoint);
        fprintf('%d/%d hypothesis sampled\r', samid, maxSample);
    end
    rcpid = cube_recipe(randsample(rcpNum, 1),:);

    if any([typLine(rcpid).poolLength]==0)
        validHyps(samid).srcLine = [];
        validHyps(samid).recipe = rcpid;
        continue;
    end

    lineID1 = typLine(rcpid(1)).lspool(randsample(typLine(rcpid(1)).poolLength, 1));
    lineID2 = typLine(rcpid(2)).lspool(randsample(typLine(rcpid(2)).poolLength, 1));
    lineID3 = typLine(rcpid(3)).lspool(randsample(typLine(rcpid(3)).poolLength, 1));
    lineID4 = typLine(rcpid(4)).lspool(randsample(typLine(rcpid(4)).poolLength, 1));
    lineID5 = typLine(rcpid(5)).lspool(randsample(typLine(rcpid(5)).poolLength, 1));

    hlines = zeros(12,9);
    hlines(rcpid(1),:) = typLine(rcpid(1)).lines(lineID1,:);
    hlines(rcpid(2),:) = typLine(rcpid(2)).lines(lineID2,:);
    hlines(rcpid(3),:) = typLine(rcpid(3)).lines(lineID3,:);
    hlines(rcpid(4),:) = typLine(rcpid(4)).lines(lineID4,:);
    hlines(rcpid(5),:) = typLine(rcpid(5)).lines(lineID5,:);

    validHyps(samid).srcLine = hlines;
    validHyps(samid).recipe = rcpid;
end
fprintf('\n');

%% finish up unknown edges, compute corner, check if ls locates in line
for samid = 1:maxSample
    if rem(samid,1000)==0
%         fprintf('%d/%d hypothesis validated, score: %f/%f\n', samid, maxSample, maxScore, numPoint);
        fprintf('%d/%d hypothesis validated\r', samid, maxSample);
    end
    if isempty(validHyps(samid).srcLine)
        validHyps(samid).omapScr = -1;
        continue;
    end
    hlines = validHyps(samid).srcLine;
    hCorner = zeros(8,3);
    lineExist = false(12,1);
    lineExist(validHyps(samid).recipe) = true;
    rcpid = validHyps(samid).recipe;
    % >> compute all the other edges, corner
    bDegenerate = false;
    while any(~lineExist)
        hits = lineExist(edgeToPoint);
        actLine = sum(hits,2)==2;
        i = find(actLine,1)';
        srcLineID = edgeToPoint(i,hits(i,:));
        dstLineID = edgeToPoint(i,~hits(i,:));
        dstVpID = find(~hits(i,:));
        dstCor = cross( hlines(srcLineID(1),1:3), hlines(srcLineID(2),1:3), 2);
        if norm(dstCor)<0.1
            bDegenerate = true;
            break;
        end

        hCorner(i,:) = dstCor./norm(dstCor);
        % check if intersection locates at right position
        v = dot([dstCor;dstCor;dstCor],[horizontal;frontNback;leftNright],2);
        if sign(v(1))~=pointDirect(i,1) ...
         ||sign(v(2))~=pointDirect(i,2) ...
         ||sign(v(3))~=pointDirect(i,3)
            hCorner(i,:) = -hCorner(i,:);
        end

        dstNM = cross(hCorner(i,:), vp(dstVpID,:), 2);
        hlines(dstLineID,1:3) = dstNM./norm(dstNM);
        lineExist(dstLineID) = true;
    end

    if bDegenerate
        validHyps(samid).omapScr = -1;
        continue;
    end

    I = find(actLine); i = I(end);
    srcLineID = edgeToPoint(i,hits(i,:));
    dstCor = cross( hlines(srcLineID(1),1:3), hlines(srcLineID(2),1:3), 2);
    hCorner(i,:) = dstCor./norm(dstCor);
    % check if intersection locates at right position
    v = dot([dstCor;dstCor;dstCor],[horizontal;frontNback;leftNright],2);
    if sign(v(1))~=pointDirect(i,1) ...
     ||sign(v(2))~=pointDirect(i,2) ...
     ||sign(v(3))~=pointDirect(i,3)
        hCorner(i,:) = -hCorner(i,:);
    end

    % >> compute extended line representation
    rcpidR = setdiff(1:12, rcpid);
    areaXY = abs(sum(hlines(rcpidR,1:3).*repmat([0 0 1], [length(rcpidR) 1]),2));
    areaYZ = abs(sum(hlines(rcpidR,1:3).*repmat([1 0 0], [length(rcpidR) 1]),2));
    areaZX = abs(sum(hlines(rcpidR,1:3).*repmat([0 1 0], [length(rcpidR) 1]),2));
    [~, planeIDs] = max([areaXY areaYZ areaZX], [], 2); % 1:XY 2:YZ 3:ZX
    hlines(rcpidR,4) = planeIDs;

    extLine = hlines(:,1:6);
    for i = 1:12
        endPoint = hCorner(PointToEdge(i,:),:);
        uv = xyz2uvN(endPoint,extLine(i,4));
        umin = (min(uv(:,1))+pi)/2/pi;    umax = (max(uv(:,1))+pi)/2/pi;
        if umax-umin>0.5
            extLine(i,5) = umax;
            extLine(i,6) = umin;
        else
            extLine(i,5) = umin;
            extLine(i,6) = umax;
        end
    end

    % check if line segments locates in line
    validated = true;
    for i = rcpid
        lineRange = extLine(i,5:6);
        lsegRange = hlines(i,5:6);
        b = insideRange(lsegRange, lineRange);
        if ~b(1) || ~b(2)
            validated = false;
            break;
        end
    end
    if ~validated
        validHyps(samid).omapScr = -1;
        continue;
    end
    validHyps(samid).srcLine = hlines(rcpid,1:6);
    validHyps(samid).extLine = extLine;
    validHyps(samid).hCorner = hCorner;
end
fprintf('\n');

%% compute the checking direction on omap
[candiSetXYZ, ~] = icosahedron2sphere(6);
[ohei, owid, ~] = size(omap);
candiSetUV = uv2coords(xyz2uvN(candiSetXYZ), owid, ohei);
candiInd = sub2ind([ohei owid], candiSetUV(:,2), candiSetUV(:,1));
% convert gc to omap
gndGC = zeros(ohei, owid, 3);
gndGC(:,:,1) = max(gc(:,:,5),gc(:,:,6));
gndGC(:,:,2) = max(gc(:,:,1),gc(:,:,3));
gndGC(:,:,3) = max(gc(:,:,2),gc(:,:,4));
normGndGC = sum(gndGC(candiInd))+sum(gndGC(candiInd+1*ohei*owid))+sum(gndGC(candiInd+2*ohei*owid));
% omap
gndOmap = omap;
gndOmap(gndOmap>0) = 1;
normGndOmap = sum(gndOmap, 3);
gndOmap = gndOmap./(repmat(normGndOmap+0.0001, [1 1 3]));
normGndOmap = sum(gndOmap(candiInd))+sum(gndOmap(candiInd+1*ohei*owid))+sum(gndOmap(candiInd+2*ohei*owid));
numPoint = size(candiSetXYZ,1);

% merge
gndMerge = zeros(ohei, owid, 3);
gndMerge(1:ohei/2,:,:) = gndOmap(1:ohei/2,:,:);
gndMerge(ohei/2+1:ohei, :, :) = gndGC(ohei/2+1:ohei, :, :);
% normMegMap = sum(gndOmap, 3);
% gndOmap = gndOmap./(repmat(normMegMap+0.0001, [1 1 3]));
normMegMap = sum(gndMerge(candiInd))+sum(gndMerge(candiInd+1*ohei*owid))+sum(gndMerge(candiInd+2*ohei*owid));

% optimal merge
gndOptim = zeros(ohei, owid, 3);
gndOptim(1:685,:,:) = gndOmap(1:685,:,:);
gndOptim(686:ohei, :, :) = gndGC(686:ohei, :, :);
% normMegMap = sum(gndOmap, 3);
% gndOmap = gndOmap./(repmat(normMegMap+0.0001, [1 1 3]));
normOptMap = sum(gndOptim(candiInd))+sum(gndOptim(candiInd+1*ohei*owid))+sum(gndOptim(candiInd+2*ohei*owid));

%% check consistency of orientation on uniformly sampled point on sphere
for samid = 1:maxSample
    hypVp = false(numPoint,3);
    if validHyps(samid).omapScr>-0.5
        hCorner = validHyps(samid).hCorner;
        for i = 1:6
%             suf1 = cross(hCorner(PointToSurface(i,2),:), hCorner(PointToSurface(i,1),:), 2);
%             suf2 = cross(hCorner(PointToSurface(i,3),:), hCorner(PointToSurface(i,2),:), 2);
%             suf3 = cross(hCorner(PointToSurface(i,4),:), hCorner(PointToSurface(i,3),:), 2);
%             suf4 = cross(hCorner(PointToSurface(i,1),:), hCorner(PointToSurface(i,4),:), 2);
%             vad1 = dot(candiSetXYZ, repmat(suf1, [numPoint 1]), 2);
%             vad2 = dot(candiSetXYZ, repmat(suf2, [numPoint 1]), 2);
%             vad3 = dot(candiSetXYZ, repmat(suf3, [numPoint 1]), 2);
%             vad4 = dot(candiSetXYZ, repmat(suf4, [numPoint 1]), 2);
%             vadIN = vad1<0 & vad2<0 & vad3<0 & vad4<0;
            vadIN = insideCone( hCorner(PointToSurface(i,[4 3 2 1]),:), candiSetXYZ, 0);
            hypVp( vadIN, SurfaceNm(i)) = true;
        end
%         response = selVp & hypVp;
%         validHyps(samid).omapScr = sum( sum(response,2)>0 )/size(response,1);
        response = sum(gndOmap(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndOmap(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndOmap(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).omapScr = response/normGndOmap;
        response = sum(gndGC(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndGC(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndGC(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).gcScr = response/normGndGC;
        response = sum(gndMerge(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndMerge(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndMerge(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).mgScr = response/normMegMap;
        response = sum(gndOptim(candiInd(hypVp(:,1))+0*ohei*owid)) ...
                 + sum(gndOptim(candiInd(hypVp(:,2))+1*ohei*owid)) ...
                 + sum(gndOptim(candiInd(hypVp(:,3))+2*ohei*owid));
        validHyps(samid).opScr = response/normOptMap;
    end
end

%% save only validate hypothesis, with high score
omapScr = [validHyps.omapScr];
% [B, IX] = sort(omapScr, 'descend');
% selIX = IX(B>-0.5);
% hyps = validHyps(selIX);

hyps = validHyps(omapScr>-0.5);
end

function b = insideRange(pt, range)
range = range + [-0.02 +0.02];
if range(2)>range(1)
    b = pt>=range(1) & pt<=range(2);
else
    b1 = pt>=range(1) & pt<=1;
    b2 = pt>=0 & pt<=range(2);
    b = b1 | b2;
end

end


function [ug] = amu2ug(amu)
% Convert mass from atomic mass units to micrograms.
% Chad Greene 2012
ug = amu*1.6605402e-18;


function [varargout] = solver_RPCA_Lagrangian(Y,lambda_L,lambda_S,varargin)
% [L,S,errHist] = solver_RPCA_Lagrangian(Y,lambda_L,lambda_S, A_cell, opts)
% Solves the problem
%   minimize_{L,S} .5|| L + S - Y ||_F^2 + lambda_L ||L||_* + lambda_S ||S||_1
%
%   or if A_cell is provided, where A_cell = {A, At}
%   (A is a function handle, At is a function handle to the transpose of A)
%   then
%
%   minimize_{L,S} .5|| A(L + S) - Y ||_F^2 + lambda_L ||L||_* + lambda_S ||S||_1
%   (here, Y usually represents A(Y) )
%
%   errHist(:,1) is a record of the residual
%   errHist(:,2) is a record of the full objective (.f*resid^2 + lambda_L,
%      etc.)
%   errHist(:,3) is the output of opts.errFcn if provided
%
% opts is a structure with options:
%   opts.sum, opts.max  (as described above)
%   opts.L0         initial guess for L (default is 0)
%   opts.S0         initial guess for S (default is 0)
%   opts.tol        sets stopping tolerance
%   opts.maxIts     sets maximum number of iterations
%   opts.printEvery will print information this many iterations
%   opts.displayTime will print out timing information (default is true for large problems)
%   opts.errFcn     a function of (L,S) that records information
%   opts.trueObj    if provided, this will be subtracted from errHist(2,:)
%   opts.Lip        Lipschitz constant, i.e., 2*spectralNorm(A)^2
%                       by default, assume 2 (e.g., good if A = P_Omega)
%   opts.FISTA      whether to use FISTA or not. By default, false
%     opts.restart  how often to restart FISTA; set to -Inf to make it automatic
%   opts.BB         whether to use the Barzilai-Borwein spectral steplength
%     opts.BB_type  which BB stepsize to take. Default is 1, the larger step
%     opts.BB_split whether to calculate stepslengths for S and L independently.
%       Default is false, which is recommended.
%   opts.quasiNewton  uses quasi-Newton-like Gauss-Seidel scheme. By
%                       default, true
%     opts.quasiNewton_stepsize     stepsize length. Default is .8*(2/Lip)
%     opts.quasinewton_SLS          whether to take S-L-S sequence (default is true)
%                                   otherwise, takes a L-S Gauss-Seidel sequence
%   opts.SVDstyle   controls what type of SVD is performed.
%       1 = full svd using matlab's "svd". Best for small problems
%       2 = partial svd using matlab's "svds". Not recommended.
%       3 = partial svd using PROPACK, if installed. Better than option 2, worse than 4
%       4 = partial svd using randomized linear algebra, following
%           the Halko/Tropp/Martinnson "Structure in Randomness" paper
%       in option 4, there are additional options:
%       opts.SVDwarmstart   whether to "warm-start" the algorithm
%       opts.SVDnPower  number of power iterations (default is 2 unless warm start)
%       opts.SVDoffset  oversampling, e.g., "rho" in Tropp's paper. Default is 5
%
%   opts.L1L2      instead of using l1 penalty, e.g., norm(S(:),1), we can
%       also use block norm penalties, such as (if opts.L1L2 = 'rows')
%       the sum of the l2-norm of rows (i.e., l1-norm of rows),
%       or if opts.L1L2='cols', the sum of the l2-norms of colimns.
%       By default, or if opts.L1L2 = [] or false, then uses usual l1 norm.
%       [Feature added April 17 2015]
%
% Stephen Becker, March 6 2014
% See also solver_RPCA_constrained.m

[varargout{1:nargout}] = solver_RPCA_constrained(Y,lambda_S/lambda_L, -lambda_L, varargin{:});


function [ x, y ] = dswap ( n, x, incx, y, incy )

%*****************************************************************************80
%
%% DSWAP interchanges two vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real X(*), one of the vectors to swap.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Input, real Y(*), one of the vectors to swap.
%
%    Input, integer INCY, the increment between successive elements of Y.
%
%    Output, real X(*), the swapped vector.
%
%    Output, real Y(*), the swapped vector.
%
  if ( n <= 0 )

  elseif ( incx == 1 & incy == 1 )

    m = mod ( n, 3 );

    for i = 1 : m
      temp = x(i);
      x(i) = y(i);
      y(i) = temp;
    end

    for i = m+1 : 3 : n

      temp = x(i);
      x(i) = y(i);
      y(i) = temp;

      temp = x(i+1);
      x(i+1) = y(i+1);
      y(i+1) = temp;

      temp = x(i+2);
      x(i+2) = y(i+2);
      y(i+2) = temp;

    end

  else

    if ( 0 <= incx )
      ix = 1;
    else
      ix = ( - n + 1 ) * incx + 1;
    end

    if ( 0 <= incy )
      iy = 1;
    else
      iy = ( - n + 1 ) * incy + 1;
    end

    for i = 1 : n
      temp = x(ix);
      x(ix) = y(iy);
      y(iy) = temp;
      ix = ix + incx;
      iy = iy + incy;
    end

  end

  return
end


 function xs = eml_inc_em(x, Gb, yi, ci, ri, varargin)
%|function xs = eml_inc_em(x, Gb, yi, ci, ri, varargin)
%| E-ML-INC-EM algorithm for image reconstruction from Poisson emission data
%| (incremental EM algorithm)
%| model: Y_i ~ Poisson(c_i [G x]_i + r_i)
%| in
%|	x	[np,1]		initial estimate
%|	Gb	[nd,np]		Gblock object (see eml_osem_test.m)
%|	yi,ci,ri [nb,na]	see em_fbp.m (for model too)
%|
%| options
%|	'niter'	(#)		# of iterations
%|	'isave'	[]		list of iterations to archive
%|				(default: [] 'last)
%|	chat	(0 | 1)		verbosity
%|	pixmax	(value)		upper constraint for pixel values
%|	hds	(value)		'1' or '3' hidden data space
%|	os	(#)		how many "warmup" osem iterations
%|
%| out
%|	xs [np nsave]		estimates each (saved) iteration
%|
%| Copyright 2004-3-20, Jeff Fessler, University of Michigan

if nargin < 3, ir_usage, end

% options
arg.chat = 0;
arg.pixmax = inf;
arg.hds = 1;
arg.niter = 1;
arg.isave = 'last';
arg.os = 0;
arg.userfun = [];

arg = vararg_pair(arg, varargin);
arg.isave = iter_saver(arg.isave, arg.niter);

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

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

eml_check(yi, ci, ri, 'os', nblock);
[nb na] = size(yi);

ticker(mfilename, 1, arg.niter)

% precompute hidden data space factor
gam = eml_hds(Gb, ci, ri, arg.hds);
if arg.chat, printf('hds = %g', gam), end

% system (block) sensitivities
np = length(x);
asum = zeros(np, 1);
precon = zeros(np, nblock); % classic OSEM preconditioner
for iset=1:nblock
	ticker([mfilename ' : precon'], iset, nblock)
	istart = starts(iset);
	ia = istart:nblock:na;
	asum_m = Gb{istart}' * col(ci(:,ia));
	asum = asum + asum_m;
	asum_m(asum_m == 0) = Inf; % avoid divide by 0
	precon(:, iset) = 1 ./ asum_m;
end, clear asum_m
% asum = Gb' * ci(:);

x = max(x,0);
x = min(x,arg.pixmax);
xs = zeros(numel(x), length(arg.isave));
if any(arg.isave == 0)
	xs(:,find(arg.isave == 0)) = x;
end

%
% precompute sufficient statistics,
% possibly using osem warmup iterations
%
ff = zeros(np,nblock);
if arg.os == 0
	for iblock=1:nblock
		ff(:,iblock) = compute_fm(x, Gb, yi, ci, ri, gam, iblock);
	end

else
	for iter = 1:arg.os
		for iset = 1:nblock
			ticker([mfilename ': osem'], [iter iset], [arg.os nblock])
			iblock = starts(iset);
			ia = iblock:nblock:na;

			tmp = compute_fm(x, Gb, yi, ci, ri, gam, iblock);

			pre = precon(:,min(iset,ncol(precon)));	% 1 or iset
			x = max(pre .* tmp - gam, 0);

			if iter == arg.os
				ff(:,iblock) = tmp;
			end
		end

		if any(arg.isave == iter)
			xs(:, arg.isave == iter) = x;
		end
	end
end
fsum = sum(ff, 2);

%
% loop over incremental EM iterations
%

for iter = (2+arg.os):arg.niter

	%
	% loop over subsets
	%
	for iset=1:nblock
		ticker(mfilename, [iter iset], [arg.niter nblock])
		iblock = starts(iset);

		x = fsum ./ asum - gam;
		x = max(x,0);
		x = min(x,arg.pixmax);

		fsum = fsum - ff(:,iblock);
		ff(:,iblock) = compute_fm(x, Gb, yi, ci, ri, gam, iblock);
		fsum = fsum + ff(:,iblock);
	end

	if arg.chat, printf('Range %g %g', min(x), max(x)), end
	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = x;
	end
end

%
% compute_fm()
% compute sufficient statistics
%
function fm = compute_fm(x, Gb, yi, ci, ri, gam, iblock)
eterm = eml_eterm(x, Gb, yi, ci, ri, iblock);
fm = (x + gam) .* eterm;


%EIMM_SMOOTH  EKF based fixed-interval IMM smoother using two IMM-EKF filters.
%
% Syntax:
%   [X_S,P_S,X_IS,P_IS,MU_S] = EIMM_SMOOTH(MM,PP,MM_i,PP_i,MU,p_ij,mu_0j,ind,dims,A,a,a_param,Q,R,H,h,h_param,Y)
%
% In:
%   MM    - Means of forward-time IMM-filter on each time step
%   PP    - Covariances of forward-time IMM-filter on each time step
%   MM_i  - Model-conditional means of forward-time IMM-filter on each time step
%   PP_i  - Model-conditional covariances of forward-time IMM-filter on each time step
%   MU    - Model probabilities of forward-time IMM-filter on each time step
%   p_ij  - Model transition probability matrix
%   ind   - Indices of state components for each model as a cell array
%   dims  - Total number of different state components in the combined system
%   A     - Dynamic model matrices for each linear model and Jacobians of each
%           non-linear model's measurement model function as a cell array
%   a     - Cell array containing function handles for dynamic functions
%           for each model having non-linear dynamics
%   a_param - Parameters of a as a cell array.
%   Q     - Process noise matrices for each model as a cell array.
%   R     - Measurement noise matrices for each model as a cell array.
%   H     - Measurement matrices for each linear model and Jacobians of each
%           non-linear model's measurement model function as a cell array
%   h     - Cell array containing function handles for measurement functions
%           for each model having non-linear measurements
%   h_param - Parameters of h as a cell array.
%   Y     - Measurement sequence
%
% Out:
%   X_S  - Smoothed state means for each time step
%   P_S  - Smoothed state covariances for each time step
%   X_IS - Model-conditioned smoothed state means for each time step
%   P_IS - Model-conditioned smoothed state covariances for each time step
%   MU_S - Smoothed model probabilities for each time step
%
% Description:
%   EKF based two-filter fixed-interval IMM smoother.
%
% See also:
%   EIMM_UPDATE, EIMM_PREDICT

% History:
%   09.01.2008 JH The first official version.
%
% Copyright (C) 2007,2008 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [x_sk,P_sk,x_sik,P_sik,mu_sk] = eimm_smooth(MM,PP,MM_i,PP_i,MU,p_ij,mu_0j,ind,dims,A,a,a_param,Q,R,H,h,h_param,Y)
    % Default values for mean and covariance
    MM_def = zeros(dims,1);
    PP_def = diag(ones(dims,1));

    % Number of models
    m = length(A);

    % Number of measurements
    n = size(Y,2);

    % The prior model probabilities for each step
    p_jk = zeros(m,n);
    p_jk(:,1) = mu_0j;
    for i1 = 2:n
        for i2 = 1:m
            p_jk(i2,i1) = sum(p_ij(:,i2).*p_jk(:,i1-1));
        end
    end

    % Backward-time transition probabilities
    p_ijb = cell(1,n);
    for k = 1:n
        for i1 = 1:m
            % Normalizing constant
            b_i = sum(p_ij(:,i1).*p_jk(:,k));
            for j = 1:m
                p_ijb{k}(i1,j) = 1/b_i.*p_ij(j,i1).*p_jk(j,k);
            end
        end
    end

    % Space for overall smoothed estimates
    x_sk = zeros(dims,n);
    P_sk = zeros(dims,dims,n);
    mu_sk = zeros(m,n);

    % Values of smoothed estimates at the last time step.
    x_sk(:,end)   = MM(:,end);
    P_sk(:,:,end) = PP(:,:,end);
    mu_sk(:,end)  = MU(:,end);

    % Space for model-conditioned smoothed estimates
    x_sik = cell(m,n);
    P_sik = cell(m,n);

    % Values for last time step
    x_sik(:,end) = MM_i(:,end);
    P_sik(:,end) = PP_i(:,end);

    % Backward-time estimated model probabilities
    mu_bp = MU(:,end);

    % Space for model-conditioned backward-time updated means and covariances
    x_bki = cell(1,m);
    P_bki = cell(1,m);

    % Initialize with default values
    for i1 = 1:m
       x_bki{i1} = MM_def;
       x_bki{i1}(ind{i1}) = MM_i{i1,end};
       P_bki{i1} = PP_def;
       P_bki{i1}(ind{i1},ind{i1}) = PP_i{i1,end};
    end

    % Space for model-conditioned backward-time predicted means and covariances
    x_kp = cell(1,m);
    P_kp = cell(1,m);

    % Initialize with default values
    for i1 = 1:m
       x_kp{i1} = MM_def;
       P_kp{i1} = PP_def;
    end

    for k = n-1:-1:1
        % Space for normalizing constants and conditional model probabilities
        a_j = zeros(1,m);
        mu_bijp = zeros(m,m);

        for i2 = 1:m
            % Normalizing constant
            a_j(i2) = sum(p_ijb{k}(:,i2).*mu_bp(:));
             % Conditional model probability
            mu_bijp(:,i2) = 1/a_j(i2).*p_ijb{k}(:,i2).*mu_bp(:);

            % Retrieve the transition matrix or the Jacobian of the dynamic model
            if isnumeric(A{i2})
                A2 = A{i2};
            elseif isstr(A{i2}) | strcmp(class(A{i2}),'function_handle')
                A2 = feval(A{i2},x_bki{i2}(ind{i2}),a_param{i2});
            else
                A2 = A{i2}(x_bki{i2}(ind{i2}),a_param{i2});
            end

            % Backward-time EKF prediction step
            [x_kp{i2}(ind{i2}), P_kp{i2}(ind{i2},ind{i2})] = ekf_predict1(x_bki{i2}(ind{i2}),...
                                                              P_bki{i2}(ind{i2},ind{i2}),...
                                                              inv(A2),Q{i2},a{i2},[],a_param{i2});

        end

        % Space for mixed predicted mean and covariance
        x_kp0 = cell(1,m);
        P_kp0 = cell(1,m);

        % Space for measurement likelihoods
        lhood_j = zeros(1,m);

        for i2 = 1:m
            % Initialize with default values
            x_kp0{i2} = MM_def;
            P_kp0{i2} = PP_def;
            P_kp0{i2}(ind{i2},ind{i2}) = zeros(length(ind{i2}),length(ind{i2}));

            % Mix the mean
            for i1 = 1:m
                x_kp0{i2}(ind{i2}) = x_kp0{i2}(ind{i2}) + mu_bijp(i1,i2)*x_kp{i1}(ind{i2});
            end

            % Mix the covariance
            for i1 = 1:m
                P_kp0{i2}(ind{i2},ind{i2}) = P_kp0{i2}(ind{i2},ind{i2}) + mu_bijp(i1,i2)*(P_kp{i1}(ind{i2},ind{i2})+(x_kp{i1}(ind{i2})-x_kp0{i2}(ind{i2}))*(x_kp{i1}(ind{i2})-x_kp0{i2}(ind{i2}))');
            end

            % Backward-time EKF update
            %
            % If the measurement model is linear don't pass h and h_param to ekf_update1
            if isempty(h) | isempty(h{i2})
                [x_bki{i2}(ind{i2}), P_bki{i2}(ind{i2},ind{i2}),K,MUP,S,lhood_j(i2)] = ekf_update1(x_kp0{i2}(ind{i2}),P_kp0{i2}(ind{i2},ind{i2}),Y(:,k),H{i2},R{i2},[],[],[]);
            else
                [x_bki{i2}(ind{i2}), P_bki{i2}(ind{i2},ind{i2}),K,MUP,S,lhood_j(i2)] = ekf_update1(x_kp0{i2}(ind{i2}),P_kp0{i2}(ind{i2},ind{i2}),Y(:,k),H{i2},R{i2},h{i2},[],h_param{i2});
            end
        end

        % Normalizing constant
        a_s = sum(lhood_j.*a_j);
        % Updated model probabilities
        mu_bp = 1/a_s.*a_j.*lhood_j;

        % Space for conditional measurement likelihoods
        lhood_ji = zeros(m,m);
        for i1 = 1:m
            for i2 = 1:m
                d_ijk = MM_def;
                D_ijk = PP_def;
                d_ijk = d_ijk + x_kp{i1};
                d_ijk(ind{i2}) = d_ijk(ind{i2}) - MM_i{i2,k};
                PP2 = zeros(dims,dims);
                PP2(ind{i2},ind{i2}) = PP_i{i2,k};
                D_ijk = P_kp{i1} + PP2;

                % Calculate the (approximate) conditional measurement likelihoods
                lhood_ji(i2,i1) = gauss_pdf(d_ijk,0,D_ijk);
            end
        end

        d_j = zeros(m,1);
        for i2 = 1:m
           d_j(i2) = sum(p_ij(i2,:).*lhood_ji(i2,:));
        end
        d = sum(d_j.*MU(:,k));

        mu_ijsp = zeros(m,m);
        for i1 = 1:m
            for i2 = 1:m
                mu_ijsp(i1,i2) = 1./d_j(i2)*p_ij(i2,i1)*lhood_ji(i2,i1);
            end
        end

        mu_sk(:,k) = 1/d.*d_j.*MU(:,k);

        % Space for two-step conditional smoothing distributions p(x_k^j|m_{k+1}^i,y_{1:N}),
        % which are a products of two Gaussians
        x_jis = cell(m,m);
        P_jis = cell(m,m);
        for i2 = 1:m
            for i1 = 1:m
                MM1 = MM_def;
                MM1(ind{i2}) = MM_i{i2,k};

                PP1 = PP_def;
                PP1(ind{i2},ind{i2}) = PP_i{i2,k};

                iPP1 = inv(PP1);
                iPP2 = inv(P_kp{i1});

                % Covariance of the Gaussian product
                P_jis{i2,i1} = inv(iPP1+iPP2);
                % Mean of the Gaussian product
                x_jis{i2,i1} = P_jis{i2,i1}*(iPP1*MM1 + iPP2*x_kp{i1});
            end
        end

        % Mix the two-step conditional distributions to yield model-conditioned
        % smoothing distributions.
        for i2 = 1:m
            % Initialize with default values
            x_sik{i2,k} = MM_def;
            P_sik{i2,k} = PP_def;
            P_sik{i2,k}(ind{i2},ind{i2}) = zeros(length(ind{i2}),length(ind{i2}));

            % Mixed mean
            for i1 = 1:m
                x_sik{i2,k} = x_sik{i2,k} + mu_ijsp(i1,i2)*x_jis{i2,i1};
            end

            % Mixed covariance
            for i1 = 1:m
                P_sik{i2,k} = P_sik{i2,k} + mu_ijsp(i1,i2)*(P_jis{i2,i1} + (x_jis{i2,i1}-x_sik{i2,k})*(x_jis{i2,i1}-x_sik{i2,k})');
            end
        end

        % Mix the overall smoothed mean
        for i1 = 1:m
            x_sk(:,k) = x_sk(:,k) + mu_sk(i1,k)*x_sik{i1,k};
        end

        % Mix the overall smoothed covariance
        for i1 = 1:m
            P_sk(:,:,k) = P_sk(:,:,k) + mu_sk(i1,k)*(P_sik{i1,k} + (x_sik{i1,k}-x_sk(:,k))*(x_sik{i1,k}-x_sk(:,k))');
        end

    end


function c = rdivide(a,b)
%RDIVIDE      Interval elementwise right division a ./ b
%

% written  10/16/98     S.M. Rump
% modified 11/30/98     S.M. Rump  modified for infinity
% modified 06/06/98     S.M. Rump  modified for NaN+Nan*i
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    remove check for 'double'
%                                    take care of Matlab sparse Inf/NaN bug
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/03/05     S.M. Rump  sparse flag corrected
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 02/11/06     S.M. Rump  SparseInfNanFlag removed
%                                    improved performance
% modified 05/23/06     S.M. Rump  sparse Inf/NaN bug corrected in Version 7.2+
% modified 12/03/06     S.M. Rump  Sparse Bug global flag (thanks to Arnold)
% modified 10/23/07     S.M. Rump  complex numbers
% modified 02/18/09     S.M. Rump  NaN performance improved
%

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

  % no need to take care about huge matrices: result would be almost full anyway
  % also no care necessary about previous Matlab sparse NaN bug (would be helpful, in fact)
  % make sure a full except b is scalar, b full anyway
  if isa(b,'intval')
    bcomplex = b.complex;
    if bcomplex
      b.mid = full(b.mid);
      b.rad = full(b.rad);
      makefull = ( prod(size(b.mid))~=1 );
      nanindex = isnan(b.mid) | isnan(b.rad);
    else
      b.inf = full(b.inf);
      b.sup = full(b.sup);
      makefull = ( prod(size(b.inf))~=1 );
      nanindex = isnan(b.inf) | isnan(b.sup);
    end
  else
    bcomplex = ~isreal(b);
    b = full(b);
    makefull = ( prod(size(b))~=1 );
    nanindex = isnan(b);
  end
  nanindex = sparse(nanindex);      % careful: full(True) | sparse = full
  if isa(a,'intval')
    acomplex = a.complex;
    if acomplex
      if makefull
        a.mid = full(a.mid);
        a.rad = full(a.rad);
      end
      nanindex = nanindex | isnan(a.mid) | isnan(a.rad);
    else
      if makefull
        a.inf = full(a.inf);
        a.sup = full(a.sup);
      end
      nanindex = nanindex | isnan(a.inf) | isnan(a.sup);
    end
  else
    acomplex = ~isreal(a);
    if makefull
      a = full(a);
    end
    nanindex = nanindex | isnan(a);
  end
  anynanindex = any(nanindex);
  anynanindex = any(anynanindex(:));

  ws = warning;
  warning off

  if acomplex | bcomplex                % numerator complex
    b = intval(b);                      % make sure b is interval
    if ~bcomplex                        % denominator is real
      c = a.*(1./b);
      return
    end
    x = real(b.mid);                    % denominator is complex
    y = imag(b.mid);
    setround(-1)
    Ninf = x.*x + y.*y + (-b.rad).*b.rad;
    index = ( Ninf<=0 );
    setround(1)
    Nsup = x.*x + y.*y + (-b.rad).*b.rad;
    x2 = max( x./Ninf , x./Nsup );
    y2 = max( y./Ninf , y./Nsup );
    setround(-1)
    x1 = max( x./Ninf , x./Nsup );
    y1 = max( y./Ninf , y./Nsup );
    c1 = x1 - j*y2;
    setround(1)
    c2 = x2 - j*y1;
    binv.complex = 1;
    binv.inf = [];
    binv.sup = [];
    binv.mid = c1 + 0.5*(c2-c1);
    binv.rad = abs( binv.mid - c1 ) + b.rad./Ninf;
    index = index | ( binv.rad<0 );
    if any(index(:))                          % division by zero
      binv.mid(index) = complex(NaN,NaN);
      binv.rad(index) = NaN;
    end
    binv = class(binv,'intval');
    c = a.*binv;
    if anynanindex
      c.mid(nanindex) = NaN;
      c.rad(nanindex) = NaN;                  % radius for sparse cannot be 0
    end
  else                                        % both a and b real
    c.complex = 0;
    if ~isa(a,'intval')                       % R ./ IR
      % be sure min/max works correct for zero upper bounds in b
      b.sup(b.sup==0) = -0;
      setround(-1)
      c.inf = min( a./b.inf , a./b.sup );
      setround(1)
      c.sup = max( a./b.inf , a./b.sup );
      index = ( b.inf<=0 ) & ( b.sup>=0 );    % 0 in b
      if ~isempty(find(index))
        c.inf(index) = -inf;
        c.sup(index) =  inf;
        index = index & ( a==0 );             % 0/0
        if any(index(:))
          c.inf(index) = NaN;
          c.sup(index) = NaN;
        end
        if anynanindex
          c.inf(nanindex) = NaN;
          c.sup(nanindex) = NaN;
        end
      end
    elseif ~isa(b,'intval')                     % IR ./ R
      setround(-1)
      c.inf = min( a.inf./b , a.sup./b );
      setround(1)
      c.sup = max( a.inf./b , a.sup./b );
      index = ( b==0 );                         % numerator/0
      if ~isempty(find(index))
        c.inf(index) = -inf;
        c.sup(index) =  inf;
        index = index & ( a.inf<=0 ) & ( 0<=a.sup );
        if any(index(:))
          c.inf(index) = NaN;
          c.sup(index) = NaN;
        end
        if anynanindex
          c.inf(nanindex) = NaN;
          c.sup(nanindex) = NaN;
        end
      end
    else                                        % IR ./ IR
      % be sure min/max works correct for zero upper bounds in b
      b.sup(b.sup==0) = -0;
      setround(-1)
      c.inf = min( a.inf./b.inf , a.inf./b.sup );
      c.inf = min( c.inf , a.sup./b.inf );
      c.inf = min( c.inf , a.sup./b.sup );
      setround(1)
      c.sup = max( a.inf./b.inf , a.inf./b.sup );
      c.sup = max( c.sup , a.sup./b.inf );
      c.sup = max( c.sup , a.sup./b.sup );
      index = ( b.inf<=0 ) & ( b.sup>=0 );      % 0 in b
      if ~isempty(find(index))
        if prod(size(b.inf))==1
          c.inf = -inf*ones(size(c.inf));
          c.sup = -c.inf;
        else
          c.inf(index) = -inf;
          c.sup(index) =  inf;
        end
        index = index & ( a.inf<=0 ) & ( a.sup>=0 );   % 0./0
        if any(index(:))
          c.inf(index) = NaN;
          c.sup(index) = NaN;
        end
      end
      if anynanindex
        c.inf(nanindex) = NaN;
        c.sup(nanindex) = NaN;
      end
    end
    c.mid = [];
    c.rad = [];
    c = class(c,'intval');
  end

  if issparse(b) & ( prod(size(c))~=1 )
    c = sparse(c);
  end

  warning(ws)
  setround(rndold)


% [INPUT]
% pods = A vector of floats [0,1] of length n representing the probabilities of default.
% g = A boolean n^2-by-n matrix representing the posterior density orthants.
% p = A vector of floats [0,1] of length n^2 representing the posterior density probabilities.
%
% [OUTPUT]
% jpod = A float [0,1] representing the Joint Probability of Default.
% fsi = A float [1,n] representing the Financial Stability Index.
% pce = A float [0,1] representing the Probability of Cascade Effects.
% dide = A float n-by-n matrix [0,1] representing the Distress Dependency.
% si = A row vector of floats [0,1] of length n representing the Systemic Importance.
% sv = A row vector of floats [0,1] of length n representing the Systemic Vulnerability.
% cojpods = A row vector of floats [0,1] of length n representing the Conditional Joint Probabilities of Default.

function [jpod,fsi,pce,dide,si,sv,cojpods] = cross_entropy_metrics(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('pods',@(x)validateattributes(x,{'double'},{'real' 'finite' '>=' 0 '<=' 1 'vector' 'nonempty'}));
        ip.addRequired('g',@(x)validateattributes(x,{'double'},{'real' 'finite' 'binary' '2d' 'nonempty'}));
        ip.addRequired('p',@(x)validateattributes(x,{'double'},{'real' 'finite' '>=' 0 '<=' 1 'vector' 'nonempty'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [pods,g,p] = validate_input(ipr.pods,ipr.g,ipr.p);

    nargoutchk(7,7);

    [jpod,fsi,pce,dide,si,sv,cojpods] = cross_entropy_metrics_internal(pods,g,p);

end

function [jpod,fsi,pce,dide,si,sv,cojpods] = cross_entropy_metrics_internal(pods,g,p)

    n = numel(pods);
    g_refs = sum(g,2);

    jpod = p(g_refs == n,:);
    fsi = min(max(sum(pods,'omitnan') / (1 - p(g_refs == 0,:)),1),n);
    pce = sum(p(g_refs >= 2,:)) / sum(p(g_refs >= 1,:));

    dide = eye(n);

    for i = 1:n
        for j = 1:n
            if (isnan(pods(j)))
                dide(i,j) = NaN;
            elseif (i ~= j)
                dide(i,j) = p((g_refs == 2) & (g(:,i) == 1) & (g(:,j) == 1),:) / pods(j);
            end
        end
    end

    dide_pods = ((dide - eye(n)) .* repmat(pods,1,n));
    si = sum(dide_pods,2);
    sv = sum(dide_pods,1).';

    jpods = ones(n,1) .* jpod;
    cojpods = (jpods ./ pods).';

end

function [pods,g,p] = validate_input(pods,g,p)

    pods = pods(:);
    n = numel(pods);

    if (n < 2)
        error('The value of ''pods'' is invalid. Expected input to be a vector containing at least 2 elements.');
    end

    k = n^2;

    [kg,ng] = size(g);

    if ((kg ~= k) || (ng ~= n))
        error(['The value of ''g'' is invalid. Expected input to be a matrix of size ' num2str(k) 'x' num2str(n) '.']);
    end

    kp = numel(p);

    if (kp ~= k)
        error(['The value of ''p'' is invalid. Expected input to be a vector containing ' num2str(k) ' elements.']);
    end

    p = p(:);

end


function value = r4_besi0e ( x )

%*****************************************************************************80
%
%% R4_BESI0E evaluates the exponentially scaled Bessel function I0(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the exponentially scaled Bessel function I0(X).
%
  persistent ai02cs
  persistent ai0cs
  persistent bi0cs
  persistent ntai0
  persistent ntai02
  persistent nti0
  persistent xsml

  if ( isempty ( nti0 ) )
    ai02cs = [ ...
      0.05449041101410882E+00, ...
      0.00336911647825569E+00, ...
      0.00006889758346918E+00, ...
      0.00000289137052082E+00, ...
      0.00000020489185893E+00, ...
      0.00000002266668991E+00, ...
      0.00000000339623203E+00, ...
      0.00000000049406022E+00, ...
      0.00000000001188914E+00, ...
     -0.00000000003149915E+00, ...
     -0.00000000001321580E+00, ...
     -0.00000000000179419E+00, ...
      0.00000000000071801E+00, ...
      0.00000000000038529E+00, ...
      0.00000000000001539E+00, ...
     -0.00000000000004151E+00, ...
     -0.00000000000000954E+00, ...
      0.00000000000000382E+00, ...
      0.00000000000000176E+00, ...
     -0.00000000000000034E+00, ...
     -0.00000000000000027E+00, ...
      0.00000000000000003E+00 ]';
    ai0cs = [ ...
      0.07575994494023796E+00, ...
      0.00759138081082334E+00, ...
      0.00041531313389237E+00, ...
      0.00001070076463439E+00, ...
     -0.00000790117997921E+00, ...
     -0.00000078261435014E+00, ...
      0.00000027838499429E+00, ...
      0.00000000825247260E+00, ...
     -0.00000001204463945E+00, ...
      0.00000000155964859E+00, ...
      0.00000000022925563E+00, ...
     -0.00000000011916228E+00, ...
      0.00000000001757854E+00, ...
      0.00000000000112822E+00, ...
     -0.00000000000114684E+00, ...
      0.00000000000027155E+00, ...
     -0.00000000000002415E+00, ...
     -0.00000000000000608E+00, ...
      0.00000000000000314E+00, ...
     -0.00000000000000071E+00, ...
      0.00000000000000007E+00 ]';
    bi0cs = [ ...
     -0.07660547252839144951E+00, ...
      1.927337953993808270E+00, ...
      0.2282644586920301339E+00, ...
      0.01304891466707290428E+00, ...
      0.00043442709008164874E+00, ...
      0.00000942265768600193E+00, ...
      0.00000014340062895106E+00, ...
      0.00000000161384906966E+00, ...
      0.00000000001396650044E+00, ...
      0.00000000000009579451E+00, ...
      0.00000000000000053339E+00, ...
      0.00000000000000000245E+00 ]';
    nti0 = r4_inits ( bi0cs, 12, 0.1 * r4_mach ( 3 ) );
    ntai0 = r4_inits ( ai0cs, 21, 0.1 * r4_mach ( 3 ) );
    ntai02 = r4_inits ( ai02cs, 22, 0.1 * r4_mach ( 3 ) );
    xsml = sqrt ( 4.0 * r4_mach ( 3 ) );
  end

  y = abs ( x );

  if ( y <= xsml )
    value = 1.0;
  elseif ( y <= 3.0 )
    value = exp ( - y ) * ( 2.75 + ...
      r4_csevl ( y * y / 4.5 - 1.0, bi0cs, nti0 ) );
  elseif ( y <= 8.0 )
    value = ( 0.375 + r4_csevl ...
      ( ( 48.0 / y - 11.0 ) / 5.0, ai0cs, ntai0 ) ) / sqrt ( y );
  else
    value = ( 0.375 + r4_csevl ...
      ( 16.0 / y - 1.0, ai02cs, ntai02 ) ) / sqrt ( y );
  end

  return
end


 function ob = Jop(varargin)
%function ob = Jop(varargin)
%J = Jop({'J', 4, 'L', 4, 'kspace', kspace, 'fov', fov, ...
%     'kn.ktype', 'kaiser', 'kn.kb_alf', kb_alf, 'kn.kb_m', kb_m});
%
% Creates fatrix J for use in operations of the form y = J * x.  Designed
% for use in iterative algorithms for estimating density compensation
% weights for conjugate phase MRI reconstruction.
%
% J is an MxM symmetric banded matrix, with entries corresponding to a
% gridding kernel, C, convolved with itself, evaluated at values
% corresponding to the difference between kspace sample locations, i.e.
%       J(i,l) = C(*)C(v_i - v_l)
% where C is the gridding kernel (usually a Kaiser-Bessel), (*) represents
% the convolution operator, and kspace sample locations are {v_m},
% m = 1,..., M.
%
% in:
%   kspace          kspace sample locations
%   fov             Field of View
%
%  optional inputs:
%   J               neighborhood for interpolation kernel
%   L               samples per integer
%   kn.*            various information to define kernel
%                   e.g. kn.ktype = 'kaiser', kn.kb_alf = 10, kn.kb_m = 0
% out:
%   J   [nd nd]     Fatrix operator
%
% K. Khalsa, Mar. 2006

% default assignments:

arg.J = 4;
arg.L = 4;
arg.kspace = [];
arg.fov = [];
arg.del = [];

arg.kn.ktype = 'kaiser';
arg.kn.kernel = {};     %*** find out what to do with this, see Gn.arg.st
arg.kn.kb_alf = [];     %
arg.kn.kb_m = [];       %

% pair input arguments
if iscell(varargin)
    varargs = varargin{1};
    arg = vararg_pair(arg, varargs);
else
    printf('input needs to be in a cell array.');
    help Jop;
    return;
end

if isempty(arg.kspace)
    error('kspace sample locations are a required input argument');
else
    arg.M = size(arg.kspace, 1);
    arg.dim = [arg.M arg.M];
end

arg.del = 1 ./ (arg.fov * arg.L);

kappa = linspace(-arg.J/2, arg.J/2, arg.J * arg.L + 1)';
%kappa = [-arg.J/2 : 1/arg.L : arg.J/2]';   %equivalently

if (arg.kn.ktype == 'kaiser')

    if xor(isempty(arg.kn.kb_alf), isempty(arg.kn.kb_m))
        printf('For Kaiser-Bessel kernel, both m and alpha are required, or neither');
        return;
    elseif (isempty(arg.kn.kb_alf) && isempty(arg.kn.kb_m))
        % use parameters from Jackson's 1991 gridding paper
        arg.kn.kb_m = 0;
        ww = [1.5 2.0 2.5 3.0 3.5 4.0];
        jack_alf = [6.6875 9.1375 11.5250 13.9086 16.2734 18.5547];
        arg.kn.kb_alf = interp1(ww, jack_alf, arg.J, 'linear', 'extrap');

    end
    % keyboard
    arg.kn.C = kaiser_bessel(kappa, arg.J, arg.kn.kb_alf, ...
        arg.kn.kb_m);
    if size(arg.kspace, 2) == 2
        arg.kn.C = arg.kn.C * arg.kn.C';    % 2D kernel
    end
    arg.kn.c0 = kaiser_bessel_ft(0, arg.J, arg.kn.kb_alf, arg.kn.kb_m);

elseif (arg.kn.ktype == 'function_handle')
    % figure out what to do here
    % see Gn.arg.st.kernel = {[1x1] function_handle}... ??
else
    error('kn.ktype must be kaiser or function_handle');
end

% Build Fatrix object
ob = Fatrix(arg.dim, arg, 'forw', @J_forw, 'back', @J_forw, ...
    'caller', mfilename);
% transpose multiplication = same as fwd multiplication b/c we
% stipulate that all kernels must be real and symmetric

%--------------------------
% multiplication
function y = J_forw(arg, x)

if (size(x,1) ~= arg.dim(2))
    error('dimension mismatch in matrix vector multiplication');
end

% 1D case
if size(arg.kspace, 2) == 1
    k0 = abs(min(arg.kspace / arg.del) - arg.J);
    k = round(arg.kspace / arg.del + k0);
    N = max(k) + arg.J;
    xtmp = full(sum(sparse(1:arg.M, k, x, arg.M, N), 1))';

    CC = conv(arg.kn.C, arg.kn.C);
    Jxtmp = conv(xtmp, CC);
    Jxtmp = arg.del * Jxtmp;
    Jxtmp = Jxtmp / max(CC);    % adjust scaling?
    y = Jxtmp(k + arg.J * arg.L);

    % 2D case
elseif size(arg.kspace, 2) == 2

    k01 = abs(min(arg.kspace(:,1) / arg.del(1)) - arg.J);
    k02 = abs(min(arg.kspace(:,2) / arg.del(2)) - arg.J);
    k0 = [k01, k02];

    k1 = round(arg.kspace(:,1) / arg.del(1) + k0(1));
    k2 = round(arg.kspace(:,2) / arg.del(2) + k0(2));
    N1 = max(k1); N2 = max(k2);

    xtmp = full(sparse(k1, k2, x, N1, N2));

    CC = conv2(arg.kn.C, arg.kn.C);
    Jxtmp = conv2(xtmp, CC, 'same');
    ind = sub2ind(size(Jxtmp), k1,k2);

    y = Jxtmp(ind);
    %  y = y / max(CC(:));  % adjust scaling?
else
    error('only 1D and 2D currently supported')
end
y = y / ((arg.kn.c0)^2); % adjust scaling??
% keyboard

% %plot to see if it's working right 2D
%     figure(11), clf
%     subplot(211)
%     plot3(arg.kspace(:,1), arg.kspace(:,2), x, 'x');
%     title('ks locs before regridding'), xlabel('kx'), ylabel('ky')
%     subplot(212)
%     plot3(k1, k2, x, 'x'), title('ks locs after regridding'), xlabel('kapx')
%
%
%   figure(12), clf
%   subplot(211), imagesc(xtmp), axis square, colorbar, title('xtmp')
%   subplot(212), imagesc(Jx), axis square, colorbar, title('Jx')
% keyboard

%%plot to see if it's working right 1D
%     figure(11), clf
%     subplot(211)
%     stem(arg.kspace, x), title('wi before regridding'), xlabel('kx')
%     subplot(212)
%     stem(1:N, xtmp), title('wi after regridding'), xlabel('kap')

%   figure(12), clf
%     subplot(211), stem(1:length(Jxkap), Jxkap), title('Jxkap')
%     subplot(212), stem(arg.kspace, y), title('y = J * x'), xlabel('kx')
% keyboard