function q = wrap2 ( m, q )

%*****************************************************************************80
%
%% WRAP2 is a circular wrap of the pointer offset Q.
%
%  Discussion:
%
%    Input values of Q between 0 and M are 'legal'.
%    Values of Q below 0 are incremented by M + 1 until they are legal.
%    Values of Q above M are decremented by M + 1 until they become legal.
%    The legal value is the output value of the function.
%
%  Example:
%
%    M  Qin  Qout
%
%    3  -5   3
%    3  -4   0
%    3  -3   1
%    3  -2   2
%    3  -1   3
%    3   0   0
%    3   1   1
%    3   2   2
%    3   3   3
%    3   4   0
%    3   5   1
%    3   6   2
%    3   7   3
%    3   8   0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2010
%
%  Author:
%
%    Original C version by Sophocles Orfanidis.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sophocles Orfanidis,
%    Introduction to Signal Processing,
%    Prentice-Hall, 1995,
%    ISBN: 0-13-209172-0,
%    LC: TK5102.5.O246.
%
%  Parameters:
%
%    Input, integer M, the maximum acceptable value for outputs.
%    M must be at least 0.
%
%    Input, integer Q, the value to be wrapped.
%
%    Output, integer Q, the wrapped value.
%
  if ( m < 0 )
    fprintf ( 2, '\n' );
    fprintf ( 2, 'WRAP2 - Fatal error!\n' );
    fprintf ( 2, '  M < 0.\n' );
    error ( 'WRAP2 - Fatal error!' );
  end
%
%  When Q = M + 1, it wraps to Q = 0.
%
  while ( m < q )
    q = q - m - 1;
  end
%
%  When Q = - 1, it wraps to Q = M.
%
  while ( q < 0 )
    q = q + m + 1;
  end

  return
end


function c8_atan_test ( )

%*****************************************************************************80
%
%% C8_ATAN_TEST tests C8_ATAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2015
%
%  Author:
%
%    John Burkardt
%
  seed = 123456678;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8_ATAN_TEST\n' );
  fprintf ( 1, '  C8_ATAN computes the inverse tangent of a C8.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '       C1=C8_UNIFORM_01          C2 = C8_ATAN(C1)           C3 = C8_TAN(C2)\n' );
  fprintf ( 1, '     ---------------------     ---------------------     ---------------------\n' );
  fprintf ( 1, '\n' );

  for test = 1 : 10

    [ c1, seed ] = c8_uniform_01 ( seed );

    c2 = c8_atan ( c1 );

    c3 = c8_tan ( c2 );

    fprintf ( 1, '  (%12f  %12f)  (%12f  %12f)  (%12f  %12f)\n', ...
      real ( c1 ), imag ( c1 ), real ( c2 ), imag ( c2 ), real ( c3 ), imag ( c3 ) );

  end

  return
end


function [ amin, amax ] = r8vec_range_2 ( n, a, amin, amax )

%*****************************************************************************80
%
%% R8VEC_RANGE_2 updates a range to include a new array.
%
%  Discussion:
%
%    Given a range AMIN to AMAX, and an array A, the routine will
%    decrease AMIN if necessary, or increase AMAX if necessary, so that
%    every entry of A is between AMIN and AMAX.
%
%    However, AMIN will not be increased, nor AMAX decreased.
%
%    This routine may be used to compute the maximum and minimum of a
%    collection of arrays one at a time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array.
%
%    Input, real AMIN, AMAX, the current legal range of values for A.
%
%    Output, real AMIN, AMAX, unchanged, or else "widened" so that all entries
%    of A are within the range.
%
  amax = max ( amax, max ( a(1:n) ) );
  amin = min ( amin, min ( a(1:n) ) );

  return
end


%DEMO_BAYESIANOPTIMIZATION1  A demonstration program for Bayesian
%                            optimization in 1 dimension
%
% The set of BO demos
%  Part 1:  this file
%  One dimensional example
%
%  Part 2:  see demo_bayesoptimization2
%  Two dimensional example
%
%  Part 3:  see demo_bayesoptimization3
%  Two dimensional example with constraints
%  * The implementation of constraints follows Gelbart et al. (2014)
%
%  References:
%    Jones, D., Schonlau, M., & Welch, W. (1998). Efficient global
%    optimization of expensive black-box functions. Journal of Global
%    Optimization, 13(4), 455-492. doi:10.1023/a:1008306431147
%
%    Michael A. Gelbart, Jasper Snoek, and Ryan P. Adams
%    (2014). Bayesian Optimization with Unknown Constraints.
%    http://arxiv.org/pdf/1403.5607v1.pdf
%
%    Snoek, J., Larochelle, H, Adams, R. P. (2012). Practical Bayesian
%    Optimization of Machine Learning Algorithms. NIPS 25
%
%  Copyright (c) 2015-2017 Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%%  Part 1:
%  One dimensional example
% For testing purposes:
stack = dbstack;
if (~isempty(stack) && (strcmp(stack(end).name, 'runtestset') || strcmp(stack(end).name, 'runtests'))) test = 1; else test = 0; end;

% Construct a function to be optimized
xl = linspace(0,10,100)';
fx = @(x) 0.6*x -0.1*x.^2 + sin(2*x);

% construct GP
cfse = gpcf_sexp('lengthScale',1,'magnSigma2',1,'magnSigma2_prior',prior_sqrtt('s2',10^2));
lik = lik_gaussian('sigma2', 0.001, 'sigma2_prior', prior_fixed);
gp = gp_set('cf', {cfse}, 'lik', lik);

% ----- conduct Bayesian optimization -----
% draw initial point

% Set the options for optimizer of the acquisition function
optimf = @fmincon;
optdefault=struct('GradObj','on','LargeScale','off','Algorithm','SQP','TolFun',1e-6,'TolX',1e-3);
opt=optimset(optdefault);
lb=0;     % lower bound of the input space
ub=10;    % upper bound of the input space

% draw initial point
rng(3)
x = 10*rand;
y = fx(x);

figure, % figure for visualization
i1 = 1;
maxiter = 15;
improv = inf;   % improvement between two successive query points
while i1 < maxiter && improv>1e-6
%while i1 < maxiter

    % Train the GP model for objective function and calculate variables
    % that are needed when calculating the Expected improvement
    % (Acquisition function)
    if i1>1
        gp = gp_optim(gp,x,y);
    end
    [K, C] = gp_trcov(gp,x);
    invC = inv(C);
    a = C\y;
    fmin = min( fx(x) );

    % Calculate EI and posterior of the function for visualization purposes
    EI = expectedimprovement_eg(xl, gp, x, a, invC, fmin);
    [Ef,Varf] = gp_pred(gp, x, y, xl);

    % optimize acquisition function
    %    Note! Opposite to the standard notation we minimize negative Expected
    %    Improvement since Matlab optimizers seek for functions minimum
    % Here we use multiple starting points for the optimization so that we
    % don't crash into suboptimal mode
    fh_eg = @(x_new) expectedimprovement_eg(x_new, gp, x, a, invC, fmin); % The function handle to the Expected Improvement function
    indbest = find(y == fmin);
    xstart = [linspace(0.5,9.5,5) x(indbest)+0.1*randn(1,2)];
    for s1=1:length(xstart)
        x_new(s1) = optimf(fh_eg, xstart(s1), [], [], [], [], lb, ub, [], opt);
    end
    EIs = expectedimprovement_eg(x_new(:), gp, x, a, invC, fmin);
    x_new = x_new( find(EIs==min(EIs),1) ); % pick up the point where Expected Improvement is maximized

    % put new sample point to the list of evaluation points
    x(end+1) = x_new;
    y(end+1) = fx(x(end));  % calculate the function value at query point
    x=x(:);y=y(:);

    % visualize
    clf
    subplot(2,1,1),hold on, title('function to be optimized and GP fit')
    %plot(xl,fx(xl))
    box on
    plot(xl,fx(xl),'r')
    % The function evaluations so far
    plot(x(1:end-1),y(1:end-1), 'ko')
    % The new sample location
    plot(x(end),y(end), 'ro')
    % the posterior of the function
    plot(xl,Ef, 'k')
    plot(xl,Ef + 2*sqrt(Varf), 'k--')
    plot(xl,Ef - 2*sqrt(Varf), 'k--')
    legend('objective function', 'function evaluations', 'next query point', 'GP mean', 'GP 95% interval','location','southwest')
    % The expected information
    subplot(2,1,2)
    plot(xl,EI, 'r'), hold on
    plot(x(end),0, 'r*')
    plot(x(end)*[1 1],ylim, 'r--')
    title('acquisition function')

    improv = abs(y(end) - y(end-1));
    i1=i1+1;

    if test == 0
        pause
    end
end
%subplot(2,1,1)
%plot(xl,fx(xl),'r')


function sw = BLanalyticalCoreyFoam(mug, muw)
%BLANALYTICAL Analytical solution of Buckley-Leverett equation
% Written by Ali A. Eftekhari
% See the license file
%muw = 10e-3;
%muo = 10e-3;
u = 1e-6/(pi()*0.038^2/4)/60;
phi = 0.2;
k = 2e-12; %[m^2]
sw_end = .5;
fmmob = 1.01889e+05;
fmdry = 1.20000e-01;
epdry = 5.0000e+03;
swc = 0.07;
sw_in = swc;
sgr = 0.0;
krg0 = 1;
ng = 2;
krw0 = 1;
nw = 2;
sws = @(sw)((sw>swc).*(sw-swc)/(1-sgr-swc));
kr = @(sw)(krg0*(1-sws(sw)).^ng);
% fm = @(sw)((sw>fmdry).*(1+fmmob*(0.5+atan(epdry.*(sw-fmdry))/pi()))+(sw<=fmdry));
fm = @(sw)(1+fmmob*(0.5+atan(epdry.*(sw-fmdry))/pi()));
krg = @(sw)(kr(sw)./fm(sw));
krw = @(sw)(krw0*sws(sw).^nw);
dkrwdsw = @(sw)(nw*krw0*(1/(1-sgr-swc))*sws(sw).^(nw-1));
dkrdsw = @(sw)((-krg0*ng*(1-sws(sw)).^(ng-1))/(-swc-sgr+1));
% fm = @(sw)((sw>fmdry).*(1+fmmob*(0.5+atan(epdry.*(sw-fmdry))/pi()))+(sw<=fmdry));
dfmdsw = @(sw)(((epdry*fmmob)./(pi*(epdry^2*(sw-fmdry).^2+1))));
% dfmdsw = @(sw)((epdry*fmmob)./(pi*(epdry^2*(sw-fmdry).^2+1)));
dkrgdsw = @(sw)((dkrdsw(sw).*fm(sw)-dfmdsw(sw).*kr(sw))./fm(sw).^2);
fw = @(sw)((krw(sw)/muw)./(krw(sw)/muw+krg(sw)/mug));
dfwdsw = @(sw)((dkrwdsw(sw)/muw.*(krw(sw)/muw+krg(sw)/mug)- ...
    (dkrwdsw(sw)/muw+dkrgdsw(sw)/mug).*krw(sw)/muw)./ ...
    (krg(sw)/mug+krw(sw)/muw).^2);
s = linspace(swc,1,1000);
figure(1)
subplot(2,2,1);
plot(s, krw(s), s, kr(s), s, krg(s));
xlabel('S_w'); ylabel('rel perms'); legend('liq', 'gas', 'foam');
F = @(sw)(dfwdsw(sw)-(fw(sw_end)-fw(sw))/(sw_end-sw));
sw_shock = fzero(F, [swc+eps,0.3]);
subplot(2,2,2);
plot(s, fw(s), [sw_shock sw_end], [fw(sw_shock) fw(sw_end)]);
xlabel('S_w'); ylabel('f_w');
% plot(s, fw(s), [sw_end sw_shock], [fw(sw_end) fw(sw_shock)]);
s1 = linspace(sw_in, sw_shock, 50);
xt_s1 = u/phi*dfwdsw(s1);
xt_s = u/phi*dfwdsw(s);
xt_shock = u/phi*dfwdsw(sw_shock);
subplot(2,2,3);
plot(xt_s, s, '--', ...
    [xt_s1 xt_shock xt_shock max(xt_s)], [s1 sw_shock sw_end sw_end])
xlabel('x/t [m/s]'); ylabel('S_w');
subplot(2,2,4);
plot([xt_s1 xt_shock xt_shock 10*xt_shock], [s1 sw_shock sw_end sw_end])
xlabel('x/t [m/s]'); ylabel('S_w');
sw_prf = [s1 sw_shock sw_end sw_end];
xt_prf = [xt_s1 xt_shock xt_shock max(xt_s)];
sw = @(xt)([interp1(xt_s1, s1, xt(xt<xt_shock)) sw_end*xt(xt>=xt_shock)]);
% Now I can use the data to calculate the pressure drop across my domain
L = 0.17; % [m]
t = eps:20:5*40*60; % [s] time
dp = zeros(1,length(t));
for i =1:length(t)
    x = 0:0.001:L;
    xt = x/t(i);
    dp(i) = trapz(x, u./(k*(krg(sw(xt))/mug+krw(sw(xt))/muw)));

end
figure(2);plot(t/60, dp/1e5, '.')
xlabel('time [min]'); ylabel('pressure drop [bar]');
end


function accuracy = lcdrc(TrainSet, TestSet, train_num, test_num, class_num, reduce_dimension, options)
% Linear collaborative discriminant regression classificatoin (LCDRC) algorithm
%
% Inputs:
%       TrainSet            train sets of size dxn, where d is dimension and n is number of sets
%       TestSet             test sets of size dxn, where d is dimension and n is number of sets
%       test_num            numner of test sets
%       class_num           numner of classes
%       options             options
% Output:
%       accuracy            classification accurary
%
% References:
%       X. Qu, S. Kim, R. Cui and H. J. Kim,
%       "Linear collaborative discriminant regression classification for face recognition,"
%       J. Visual Communication Image Represetation, vol.31, pp. 312-319, 2015.
%
%
% Created by H.Kasai on July 04, 2017

    % extract options
    if ~isfield(options, 'verbose')
        verbose = false;
    else
        verbose = options.verbose;
    end

    if verbose
        fprintf('# LCDRC: calculate projection matrix U ...');
    end
    Eb = 0;
    Ew = 0;
    for j = 1 : train_num

        j_class = TrainSet.y(1, j);

        % calcuate Ew
        intra_class_index = find(TrainSet.y == j_class);
        intra_class_index(intra_class_index == j) = [];
        X_intra_wo_j = TrainSet.X(:, intra_class_index);                                    % X^{intra}
        alpha_intra = X_intra_wo_j * pinv(X_intra_wo_j' * X_intra_wo_j) * X_intra_wo_j';    % alpha^{intra} in Eq.(2)
        error_intra = TrainSet.X(:,j) - alpha_intra * TrainSet.X(:,j);
        Ew = Ew + error_intra * error_intra';

        % calcuate Eb
        inter_class_index = find(TrainSet.y ~= j_class);
        X_inter = TrainSet.X(:, inter_class_index);%                                        % X^{iner}
        alpha_inter= X_inter * pinv(X_inter' * X_inter) * X_inter';                         % alpha^{inter} in Eq.(2)
        error_inter = TrainSet.X(:,j) - alpha_inter * TrainSet.X(:,j);
        Eb = Eb + error_inter * error_inter';
    end

    % calc
    Ew = Ew/train_num;
    Eb = Eb/train_num;
    lambda = 0.001;
    Ew = Ew + lambda*eye(size(Ew,2));

    [V, ~] = eig(Eb, Ew);
    for i = 1:reduce_dimension
        U(:,i) = V(:,size(V,2)+1-i);                                                        % Eq.(13)
    end
    if verbose
        fprintf('done\n');
    end

    % reduce dimention
    TrainSet.X_red  = U' * TrainSet.X;
    TestSet.X_red   = U' * TestSet.X;

    % prepare projection matrix (hat matrix)
    H = cell(1, class_num);
    for i = 1 : class_num
        class_index = find(TrainSet.y == i);

        X_i = TrainSet.X_red(:, class_index);           % Eq.(1)
        alpha_i = pinv(X_i' * X_i) *  X_i';                  % Eq.(2) exept y (beta_i = inv(X_i' * X_i) * X_i')
        H{i} = X_i * alpha_i;                           % Eq.(3) exept y

        if verbose
            fprintf('# LCDRC: calc Hi for class : %03d/%03d (samples: %03d)\n', i, class_num, length(class_index));
        end
    end

    % prepare predicted label array
    identity = zeros(1, test_num);

    % predict the class
    for j = 1 : test_num

        dis = zeros(1, class_num);
        y = TestSet.X_red(:, j);
        for i = 1 : class_num
            y_pred = H{i} * y;
            dis(1, i) = norm(y - y_pred);               % Eq.(5)
        end
        [~, label] = min(dis);                          % Eq.(6)
        identity(j) = label;

        if verbose
            correct = (label == TestSet.y(1, j));
            fprintf('# LCDRC: test:%03d, predict class: %03d --> ground truth :%03d (%d)\n', j, label, TestSet.y(1, j), correct);
        end
    end

    % calculate accuracy
    correct_num = sum(identity == TestSet.y);
    accuracy = correct_num / test_num;
end


function out = DN_HistogramMode(y,numBins,doSimple,doPlot)
% DN_HistogramMode      Mode of a data vector.
%
% Measures the mode of the data vector using histograms with a given number
% of bins.
%
%---INPUTS:
%
% y, the input data vector.
% numBins, the number of bins to use in the histogram.
% doSimple, whether to use a simple binning method (linearly spaced bins).
% doPlot, whether to show a plot of what was computed.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
% Check inputs and set defaults:
%-------------------------------------------------------------------------------
if nargin < 2
    numBins = 'auto';
end
if nargin < 3
    doSimple = true;
end
if nargin < 4
    doPlot = false;
end
%-------------------------------------------------------------------------------

% Compute the histogram from the data:
if isnumeric(numBins)
    if doSimple
        [N,binEdges] = BF_SimpleBinner(y,numBins);
    else
        [N,binEdges] = histcounts(y,numBins);
    end
elseif ischar(numBins)
    [N,binEdges] = histcounts(y,'BinMethod',numBins);
else
    error('Unknown format for numBins');
end

% Compute bin centers from bin edges:
binCenters = mean([binEdges(1:end-1); binEdges(2:end)]);

% Mean position of maximums (if multiple):
out = mean(binCenters(N == max(N)));

% Plot a summary of what was computed:
if doPlot
    histogram('BinEdges',binEdges,'BinCounts',N,'EdgeColor','k','FaceColor',0.6*ones(1,3));
    hold('on');
    plot(out*ones(2,1),[0,max(N)],'r','LineWidth',2);
    hold('off')
end

end


function H=calcSpherHessian(x,systemType,useHalfRange,lTx,lRx,M)
%%CALCSPHERHESSIAN Calculate the Hessian matrix (a matrix of second partial
%          derivatives) for a monostatic or bistatic spherical measurement,
%          ignoring atmospheric effects, with respect to 3D Cartesian
%          position.
%
%INPUTS: x The 3X1 position of the target in Cartesian coordinates in the
%          order [x;y;z].
% systemType An optional parameter specifying the axis from which the
%          angles are measured in radians. Possible values are
%          0 (The default if omitted) Azimuth is measured
%            counterclockwise from the x-axis in the x-y plane. Elevation
%            is measured up from the x-y plane (towards the z-axis). This
%            is consistent with common spherical coordinate systems for
%            specifying longitude (azimuth) and geocentric latitude
%            (elevation).
%          1 Azimuth is measured counterclockwise from the z-axis in the
%            z-x plane. Elevation is measured up from the z-x plane
%            (towards the y-axis). This is consistent with some spherical
%            coordinate systems that use the z axis as the boresight
%            direction of the radar.
%          2 This is the same as 0 except instead of being given
%            elevation, one desires the angle away from the z-axis, which
%            is (pi/2-elevation).
%          3 This is the same as 0 except azimuth is measured clockwise
%            from the y-axis in the x-y plane instead of counterclockwise
%            from the x-axis. This coordinate system often arises when
%            given "bearings" in a local East-North-Up coordinate system,
%            where the bearing directions are measured East of North.
% useHalfRange An optional boolean value specifying whether the bistatic
%          (round-trip) range value has been divided by two. This normally
%          comes up when operating in monostatic mode (the most common
%          type of spherical coordinate system), so that the range
%          reported is a one-way range (or just half a bistatic range).
%          The default if this parameter is not provided is false if lTx
%          is provided and true if it is omitted (monostatic).
%      lTx The 3X1 transmitter position in the global coordinate system
%          with [x;y;z] components. If omitted or an empty matrix is
%          passed, then a vector of zeros is used.
%      lRx The 3X1 receiver position in the global coordinate system
%          with [x;y;z] components. If omitted or an empty matrix is
%          passed, then a vector of zeros is used.
%        M A rotation matrix from the global Coordinate system to the
%          orientation of the coordinate system at the receiver. This is
%          only necessary if UV direction components are desired. If
%          omitted, it is assumed to be the identity matrix.
%
%OUTPUTS: H The 3X3X3 Hessian matrix, where H(:,:,1) is the Hessian for the
%           range component, H(:,:,2) is the Hessian for the azimuth
%           component, and H(:,:,3) is the Hessian for the elevation
%           component. The ordering of the derivatives in each matrix is:
%                  [d^2/(dxdx), d^2/(dxdy), d^2/(dxdz);
%                   d^2/(dydx), d^2/(dydy), d^2/(dydz);
%                   d^2/(dzdx), d^2/(dzdy), d^2/(dzdz)];
%          note that each matrix is symmetric (i.e.
%                   d^2/(dydx)=d^2/(dxdy) ).
%
%This function just calls rangeHessian and spherAngHessian.
%
%June 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(M))
   M=eye(3,3);
end

if((nargin<4||isempty(lTx))&&(nargin<3||isempty(useHalfRange)))
    useHalfRange=true;
elseif(nargin<3||isempty(useHalfRange))
    useHalfRange=false;
end

if(nargin<5||isempty(lRx))
    lRx=zeros(3,1);
end

if(nargin<4||isempty(lTx))
    lTx=zeros(3,1);
end

if(nargin<2||isempty(systemType))
    systemType=0;
end

H=zeros(3,3,3);
H(:,:,1)=rangeHessian(x,useHalfRange,lTx,lRx);
H(:,:,2:3)=spherAngHessian(x,systemType,lRx,M);
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 [ t, det, info ] = ctrdi ( t, ldt, n, job )

%*****************************************************************************80
%
%% CTRDI computes the determinant and inverse of a complex triangular matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex T(LDT,N), the triangular matrix.  The zero
%    elements of the matrix are not referenced, and the corresponding
%    elements of the array can be used to store other information.
%
%    Input, integer LDT, the leading dimension of T.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer JOB.
%    010, no determinant,    inverse, matrix is lower triangular.
%    011, no determinant,    inverse, matrix is upper triangular.
%    100,    determinant, no inverse.
%    110,    determinant,    inverse, matrix is lower triangular.
%    111,    determinant,    inverse, matrix is upper triangular.
%
%    Output, complex T(LDT,N), if an inverse was requested, then T has
%    been overwritten by its inverse.
%
%    Output, complex DET(2), the determinant of the original matrix,
%    if requested.  Otherwise not referenced.
%    Determinant = DET(1) * 10.0**DET(2) with 1.0 <= cabs1 ( DET(1) ) < 10.0
%    or DET(1) == 0.0.  Also, DET(2) is strictly real.
%
%    Output, integer INFO.
%    0, an inverse was requested and the matrix is nonsingular.
%    K, an inverse was requested, but the K-th diagonal element
%    of T is zero.
%
  det = [];
  info = 0;

  if ( floor ( job / 100 ) ~= 0 )

    det(1) = 1.0;
    det(2) = 0.0;

    for i = 1 : n

      det(1) = det(1) * t(i,i);

      if ( cabs1 ( det(1) ) == 0.0 )
        break
      end

      while ( cabs1 ( det(1) ) < 1.0 )
        det(1) = det(1) * 10.0;
        det(2) = det(2) - 1.0;
      end

      while ( 10.0 <= cabs1 ( det(1) ) )
        det(1) = det(1) / 10.0;
        det(2) = det(2) + 1.0;
      end

    end

  end
%
%  Compute inverse of upper triangular matrix.
%
  if ( mod ( floor ( job / 10 ), 10 ) ~= 0 )

    if ( mod ( job, 10 ) ~= 0 )

      info = 0;

      for k = 1 : n

        if ( cabs1 ( t(k,k) ) == 0.0 )
          info = k;
          break
        end

        t(k,k) = 1.0 / t(k,k);
        temp = -t(k,k);
        t(1:k-1,k) = t(1:k-1,k) * temp;

        for j = k+1 : n
          temp = t(k,j);
          t(k,j) = 0.0;
          t(1:k,j) = t(1:k,j) + temp * t(1:k,k);
        end

      end
%
%  Compute inverse of lower triangular matrix.
%
    else

      info = 0;

      for k = n : -1: 1

        if ( cabs1 ( t(k,k) ) == 0.0 )
          info = k;
          break
        end

        t(k,k) = 1.0 / t(k,k);

        if ( k ~= n )
          temp = -t(k,k);
          t(k+1:n,k) = t(k+1:n,k) * temp;
        end

        for j = 1 : k-1
          temp = t(k,j);
          t(k,j) = 0.0;
          t(k:n,j) = t(k:n,j) + temp * t(k:n,k);
        end

      end

    end

  end

  return
end


% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function sample = SampleMultinomial(probabilities)

dice = rand(1,1);
accumulate = 0;
for i=1:length(probabilities)
    accumulate = accumulate + probabilities(i);
   if accumulate/sum(probabilities) > dice
       break
   end
end
sample = i;


function s = logsumexp(a, dim, method)
% Returns log(sum(exp(a),dim)) while avoiding numerical underflow.
% Default is dim = 1 (columns).
% logsumexp(a, 2) will sum across rows instead of columns.
% Unlike matlab's "sum", it will not switch the summing direction
% if you provide a row vector.
%
% Example: s = logsumexp(rand(10,3),2)

% Written by Tom Minka
% (c) Microsoft Corporation. All rights reserved.

if nargin < 2, dim = 2; end
if nargin < 3, method = 1; end

% subtract the largest in each column
y = max(a,[],dim);
if method==1
  dims = ones(1,ndims(a));
  dims(dim) = size(a,dim);
  a = a - repmat(y, dims);
else
  % Added by KPM. Just a hair faster (see timing comparison below).
  % A=rand(1000,100);N=100;
  % tic; for i=1:N, s1 = logsumexp(A,2,1); end; t1=toc;
  % tic; for i=1:N, s2=logsumexp(A,2,2); end; t2=toc;
  % assert(approxeq(s1,s2)); [t1 t2]
  a = bsxfun(@minus, a, y);
end

s = y + log(sum(exp(a),dim));
i = find(~isfinite(y));
if ~isempty(i)
  s(i) = y(i);
end


function [ mOutputImage ] = ApplyBoxFilter( mInputImage, boxRadius, borderType, borderValue, normalizeFlag )
% ----------------------------------------------------------------------------------------------- %
% [ mFilteredImage ] = ApplyBoxFilter( mInputImage, boxBlurKernelRadius )
%   Applies Box Filter using Integral Images.
% Input:
%   - mInputImage   -   Input Image.
%                       Structure: Image Matrix (Single Channel)
%                       Type: 'Single' / 'Double'.
%                       Range: [0, 1].
%   - boxRadius     -   Box Radius.
%                       The radius of the box neighborhood for the
%                       summation process.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ..., }.
% Output:
%   - mOutputImage  -   Output Image.
%                       Structure: Image Matrix (Single Channel)
%                       Type: 'Single' / 'Double'.
%                       Range: [0, 1].
% Remarks:
%   1.  References: "..."
%   2.  The running sum matches Intel IPP 'SumWindowRow' / 'SumWindowColumn'.
% TODO:
%   1.  s
%   Release Notes:
%   -   1.2.001     29/04/2016  Royi Avital
%       *   Fixed bug were the border pixels weren't calculated correctly.
%   -   1.2.000     29/04/2016  Royi Avital
%       *   Updated function input.
%   -   1.1.000     04/01/2016  Royi Avital
%       *   Using "Running Sum" instead of Integral Images / Summed Area
%           Table due to numeric issues.
%   -   1.0.000     14/03/2015  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF = 0;
ON  = 1;

BORDER_TYPE_CONSTANT    = 1;
BORDER_TYPE_CIRCULAR    = 2;
BORDER_TYPE_REPLICATE   = 3;
BORDER_TYPE_SYMMETRIC   = 4;

numRows = size(mInputImage, 1);
numCols = size(mInputImage, 2);

boxBlurKernelLength     = (2 * boxRadius) + 1;

mOutputImage    = zeros([(numRows + boxBlurKernelLength - 1), (numCols + boxBlurKernelLength - 1)]);
switch(borderType)
    case(BORDER_TYPE_CONSTANT)
        mInputImage     = padarray(mInputImage, [boxRadius, boxRadius], borderValue, 'both');
    case(BORDER_TYPE_CIRCULAR)
        mInputImage     = padarray(mInputImage, [boxRadius, boxRadius], 'circular', 'both');
    case(BORDER_TYPE_REPLICATE)
        mInputImage     = padarray(mInputImage, [boxRadius, boxRadius], 'replicate', 'both');
    case(BORDER_TYPE_SYMMETRIC)
        mInputImage     = padarray(mInputImage, [boxRadius, boxRadius], 'symmetric', 'both');
end

vRowsIdx = boxRadius + [1:numRows];
vColsIdx = boxRadius + [1:numCols];
removedIdx = boxRadius + 1;

vCurrSum = sum(mInputImage(1:boxBlurKernelLength, :), 1);
mOutputImage((boxRadius + 1), :) = vCurrSum;

for iRow = boxRadius + [2:numRows]
    vRowToAdd       = mInputImage((iRow + boxRadius) ,:);
    vRowToRemove    = mInputImage((iRow - removedIdx) ,:);
    vCurrSum        = vCurrSum + vRowToAdd - vRowToRemove;

    mOutputImage(iRow, :) = vCurrSum;
end

mInputImage = mOutputImage;

vCurrSum = sum(mInputImage(:, 1:boxBlurKernelLength), 2);
mOutputImage(:, (boxRadius + 1)) = vCurrSum;

for iCol = boxRadius + [2:numCols]
    vColToAdd       = mInputImage(:, (iCol + boxRadius));
    vColToRemove    = mInputImage(:, (iCol - removedIdx));
    vCurrSum        = vCurrSum + vColToAdd - vColToRemove;

    mOutputImage(:, iCol) = vCurrSum;
end

mOutputImage = mOutputImage(vRowsIdx, vColsIdx);

if(normalizeFlag == ON)
    mOutputImage = mOutputImage ./ (boxBlurKernelLength * boxBlurKernelLength);
end

end


function [err,time,solver,eqn,node,elem] = afemPoisson(mesh,pde,option,varargin)

%% Check input arguments
if isfield(mesh,'node') && isfield(mesh,'elem')
    node = mesh.node;
    elem = double(mesh.elem);
end
if ~exist('node','var') || ~exist('elem','var')
    % default mesh: Lshape
    [node,elem] = squaremesh([-1,1,-1,1],1);
    [node,elem] = delmesh(node,elem,'x>0 & y<0');
end
if isfield(mesh,'bdFlag')
    bdFlag = mesh.bdFlag;
else % default boundary condition
    bdFlag = setboundary(node,elem,'Dirichlet');
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = Lshapedata;                          % default data
end

%% Parameters
option = afemoption(option,2);
maxIt = option.maxIt;
refType = option.refType;
elemType = option.elemType;
theta = option.theta;

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1); erreta = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
estimateTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Generate an initial mesh
for k = 1:option.L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
end

%%  Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*
for k = 1:maxIt
    % Step 1: SOLVE
    switch elemType
        case 'P1'     % piecewise linear function P1 element
            [soln,eqn,info] = Poisson(node,elem,bdFlag,pde,option);
        case 'CR'     % piecewise linear function CR element
            [soln,eqn,info] = PoissonCR(node,elem,bdFlag,pde,option);
        case 'P2'     % piecewise quadratic function
            [soln,eqn,info] = PoissonP2(node,elem,bdFlag,pde,option);
        case 'WG'     % weak Galerkin element
            [soln,eqn,info] = PoissonWG(node,elem,bdFlag,pde,option);
    end
    uh = soln.u;
    % compute error
    t = cputime;
    if isfield(pde,'Du')
        if ~isfield(pde,'d')
            pde.d = [];
        end
        if isfield(soln,'Du') && ~isempty(soln.Du) % Du is in the output
            errH1(k) = getH1error(node,elem,pde.Du,soln.Du,pde.d);
        else
            errH1(k) = getH1error(node,elem,pde.Du,soln.Du,pde.d);
        end
    end
    if isfield(pde,'exactu')
        errL2(k) = getL2error(node,elem,pde.exactu,uh);
        % interpolation
        switch elemType
            case 'P1'
                uI = Lagrangeinterpolate(pde.exactu,node,elem);
            case 'Q1'
                uI = Lagrangeinterpolate(pde.exactu,node,elem);
            case 'CR'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',eqn.edge);
            case 'P2'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'P2',eqn.edge);
            case 'P3'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'P3',eqn.edge);
            case 'WG'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'WG',eqn.edge);
        end
        erruIuh(k) = sqrt((uh-uI)'*eqn.A*(uh-uI));
        errMax(k) = max(abs(uh-uI));
    end
    errTime(k) = cputime - t;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
            errTime(k),errH1(k), errL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = length(uh);
    if  strcmp(elemType,'WG') % modify size for WG
        if ~isfield(option,'reducesystem') || (option.reducesystem == 1)
            N(k) = N(k) - size(elem,1); % reduced system
        end
    end
    if option.plotflag % show mesh and solution
       figure(1);
       showresult(node,elem,uh,option.viewangle);
    end
    % Step 2: ESTIMATE
    t = cputime;
    switch option.estType
        case 'recovery' % recovery type
            eta = estimaterecovery(node,elem,uh);
        case 'residual' % residual type
            switch elemType
                case 'P1'
                    eta = estimateresidual(node,elem,uh,pde,bdFlag);
                case 'WG'
                    eta = estimateresidualWG(node,elem,uh,soln.Du,pde);
            end
    end
    erreta(k) = sqrt(sum(eta.^2));
    estimateTime(k) = cputime - t;
    % Step 3: MARK
    switch option.markType
        case 'L2'
            markedElem = mark(elem,eta,theta);
        case 'MAX'
            markedElem = mark(elem,eta,theta,'MAX');
    end
    % Step 4: REFINE
    t = cputime;
    if (N(k) > option.maxN) || (k == maxIt) % no refinement
        break;
    end
    [node,elem,bdFlag,HB,tree] = bisect(node,elem,markedElem,bdFlag); %#ok<ASGLU>
    % Step 4.2: COARSEN
    if option.coarsenflag
        eta = eleminterpolate(eta,tree);
        markedElem = mark(elem,eta,0.25*theta,'COARSEN');
        [node,elem,bdFlag] = coarsen(node,elem,markedElem,bdFlag);
    end
    meshTime(k) = cputime - t;
end

%% Plot convergence rates
if option.rateflag
    if ~isfield(option,'rateshift')
        option.rateshift = 10;
    end
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrate2(N(1:k),errH1(1:k),option.rateshift,'-*','|| Du-Du_h||',...
              N(1:k),errL2(1:k),option.rateshift,'k-+','|| u-u_h||');
    subplot(1,2,2)
    showrate2(N(1:k),erruIuh(1:k),option.rateshift,'m-+','||Du_I-Du_h||',...
              N(1:k),errMax(1:k),option.rateshift,'r-*','||u_I-u_h||_{\infty}');
end

%% Output
err = struct('N',N(1:k),'H1',errH1(1:k),'L2',errL2(1:k),...
             'uIuhH1',erruIuh(1:k),'uIuhMax',errMax(1:k),'eta',erreta(1:k));
time = struct('N',N(1:k),'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assemble',assembleTime(1:k),'mesh',meshTime(1:k),...
              'estimate',estimateTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error and CPU time
if option.dispflag
    if ~isfield(option,'dispspace')
       option.dispspace = 5;    % display error for every 5 iterations
    end
    idx = 1:option.dispspace:k;

    fprintf('\n');
    disp('Table: Error')
    colname = {'#Dof','||u-u_h||','||Du-Du_h||','||DuI-Du_h||','||uI-u_h||_{max}','eta'};
    disptable(colname,err.N(idx),[],err.L2(idx),'%0.5e',err.H1(idx),'%0.5e',...
          err.uIuhH1(idx),'%0.5e',err.uIuhMax(idx),'%0.5e',err.eta(idx),'%0.5e');
%
%     disp('Table: CPU time')
%     colname = {'#Dof','Assemble','Solve','Error','Mesh'};
%     disptable(colname,time.N,[],time.assemble,'%0.2e',time.solver,'%0.2e',...
%                       time.err,'%0.2e',time.mesh,'%0.2e');
    fprintf('\n');
end


%     MOD1(x,y) is like MOD(x,y), except that it returns a value between 1
%     and y, instead of between 0 and y-1.

function v = mod1(x,y)

v = mod(x-1,y)+1;


function [DX] = spm_diff_dx(varargin)
% optimisation of finite difference for numerical differentiation
% FORMAT [dx] = spm_diff_dx(f,x,...,n)
% FORMAT [dx] = spm_diff_dx(f,x,...,n,V)
% FORMAT [dx] = spm_diff_dx(f,x,...,n,'q')
%
% f      - [inline] function f(x{1},...)
% x      - input argument[s]
% n      - arguments to differentiate w.r.t.
%
% dx     - 'best' step size
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_diff_dx.m 7143 2017-07-29 18:50:38Z karl $

% Stability of numerical gradients
%==========================================================================
global GLOBAL_DX

% line search of step sizes
%--------------------------------------------------------------------------
dh    = 1;
dx    = -8:dh:0;
nd    = numel(dx);
for i = 1:nd
    GLOBAL_DX = exp(dx(i));
    dxdp{i}   = spm_diff(varargin{:});
end
np    = size(dxdp{1},2);
for i = 1:(nd - 1)
    for j = 1:np
        dgdh     = spm_vec(dxdp{i}{j}) - spm_vec(dxdp{i + 1}{j});
        ssd(i,j) = mean(abs(dgdh/dh).^2);
    end
end

% graphics
%--------------------------------------------------------------------------
dx    = dx(1:end - 1);
mss   = mean(log(ssd),2);
[~,j] = min(mss);
DX    = dx(j);

% graphics
%--------------------------------------------------------------------------
if nargout, return, end

str   = sprintf('Log stability: dx = exp(%.1f)',DX);

subplot(2,2,1), plot(dx,mss,dx,log(ssd),':')
title('Log stability','Fontsize',16), xlabel('log(dx)')
axis square, spm_axis tight

subplot(2,2,2), imagesc(1:np,dx,log(ssd))
title('Log stability','Fontsize',16), xlabel('Paramter mode')
axis square, ylabel('log(dx)')

subplot(2,2,3), plot(log(ssd(j,:)))
title(str,'Fontsize',16), xlabel('Paramter mode')
axis square, spm_axis tight

if iscell(varargin{end})
    V = varargin{end};
    subplot(2,2,4), imagesc(V{1})
    title('Paramter modes','Fontsize',16), xlabel('Paramter mode')
    axis square, ylabel('parameter')
end


function [state_red,inv_state_red,n_g_red] = krylov_reduction(g0,g1,n_v,n_g,n_arnoldi,varargin)
% Reduce the dimensionality of state space using Krylov subspace method
%
% by SeHyoun Ahn, Sept 2016
%
% REFERENCE: Ahn, SeHyoun, Greg Kaplan, Benjamin Moll, Thomas Winberry, and
%    Christian Wolf. "When Inequality Matters for Macro and Macro Matters
%    for Inequality."
%
% PARAMETERS:
%    g0 = LHS matrix, only used to check it satisfies require form
%    g1 = Dynamics matrix
%    n_v = number of jump variables
%    n_g = number of state variables to be reduced
%    n_Z = number of state variables not being reduced
%    n_arnoldi = dimension of Krylov subspace
%    varargin(1) = other observables
%    varargin(2) = updating parameter for B_gv
%    varargin(3) = number of variables to be NOT reduced; it is assumed
%                  that the last varargin(3) variables are not being
%                  reduced, so reorder as necessary
%
% OUTPUTS:
%    state_red = tranformation to get from full grid to reduced states
%    inv_state_red = inverse transform
%    n_g_red = number of state variables after reduction
%
% SYNTAX:
% [state_red,inv_state_red,n_g_red] = krylov_reduction(g0,g1,n_v,n_g,n_arnoldi,varargin)

if nargin == 8
    observable = varargin{1};
    F = varargin{2};
    n_Z = varargin{3};
elseif nargin == 7
    observable = varargin{1};
    F = varargin{2};
    n_Z = 0;
elseif nargin == 6
    observable = varargin{1};
    F = [];
    n_Z = 0;
else
    observable = [];
    F = [];
    n_Z = 0;
end
n_p = size(g1,1) - n_v - n_g;
n_total = n_v + n_g;
n_g = n_g - n_Z;

%% Check to make sure that g0 is an identity matrix
%     This reducetion step assumes that g0 has been cleaned, so if
%     this part fails, make sure to run <clean_g.m>
if (max(max(abs(g0(1:n_total,1:n_total)-speye(n_total))))~=0)
    error('Make sure that g0 is normalized.');
end

% Slice Dynamics Equation into Different Parts
B_pv = -g1(n_total+1:n_total+n_p,n_total+1:n_total+n_p)\g1(n_total+1:n_total+n_p,1:n_v);
B_pg = -g1(n_total+1:n_total+n_p,n_total+1:n_total+n_p)\g1(n_total+1:n_total+n_p,n_v+1:n_v+n_g);
B_pZ = -g1(n_total+1:n_total+n_p,n_total+1:n_total+n_p)\g1(n_total+1:n_total+n_p,n_v+n_g+1:n_v+n_g+n_Z);
B_gg = g1(n_v+1:n_v+n_g,n_v+1:n_v+n_g);
B_gv = g1(n_v+1:n_v+n_g,1:n_v);
B_gp = g1(n_v+1:n_v+n_g,n_total+1:n_total+n_p);
% B_vp = g1(1:n_v,n_v+n_g+1:n_v+n_g+n_p);

%% Drop redundant Directions in B_pg
% In theory, since we do deflated block arnoldi, this step
%    might be redundant. Will be tested later.
obs = [B_pg;observable];
[~,d0,V_g] = svd(full(obs),'econ');
aux = diag(d0);
aux = aux/aux(1);
n_Bpg = sum(aux>10*eps);
V_g = bsxfun(@times,V_g(:,1:n_Bpg),aux(1:n_Bpg)');

% Compute Krylov Subspace
if isa(F,'function_handle')
    A = @(x) F(B_gv'*x)+B_gg'*x + B_pg'*(B_gp'*x);
elseif F
    A = @(x) F'*(B_gv'*x)+B_gg'*x + B_pg'*(B_gp'*x);
else
    A = @(x) B_gg'*x + B_pg'*(B_gp'*x);
end
[V_g,~] = deflated_block_arnoldi(A,V_g,n_arnoldi);
n_g_red = size(V_g,2);

% Build State-Space Reduction transform
state_red = sparse(n_v+n_g_red,n_total+n_p);
state_red(1:n_v,1:n_v) = speye(n_v);
state_red(n_v+1:n_v+n_g_red,n_v+1:n_v+n_g) = V_g';
state_red(n_v+n_g_red+1:n_v+n_g_red+n_Z,n_v+n_g+1:n_v+n_g+n_Z) = speye(n_Z);
%state_red(:,n_total+1:n_total+n_p) = 0;

% Build inverse transform
inv_state_red = sparse(n_total+n_p,n_v+n_g_red);
inv_state_red(1:n_v,1:n_v) = speye(n_v);
inv_state_red(n_v+1:n_v+n_g,n_v+1:n_g_red+n_v) = V_g;
inv_state_red(n_total+1:n_total+n_p,1:n_v) = B_pv;
inv_state_red(n_total+1:n_total+n_p,n_v+1:n_v+n_g_red) = B_pg*V_g;
inv_state_red(n_total+1:n_total+n_p,n_v+n_g_red+1:n_v+n_g_red+n_Z) = B_pZ;
inv_state_red(n_v+n_g+1:n_total,n_v+n_g_red+1:n_v+n_g_red+n_Z) = speye(n_Z);

n_g_red = n_g_red + n_Z;


%% LSHAPEAFEMQUADCURL quad curl equations on the Lshaped domain
%
%   LSHAPEAFEMQUADCURL computes ND0-(CR-P0)-(linear ND) nonconforming approximations
%   of the quad curl equations in the unit cube. The mesh is refined
%   adaptively guided by a residual-based estimator using a separate marking
%   strategy with two marking parameters for eta_1 which estimates the error
%   for w and eta_2 for phi.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear;

%% Set up
maxN = 6e5;
maxIt = 24;
theta = 0.3;
theta1 = 0.5;
nDofMaxwell = zeros(maxIt,1);
nDofStokes = zeros(maxIt,1);
h = zeros(maxIt,1);
erru = zeros(maxIt,1);
erruL2 = zeros(maxIt,1);
errw = zeros(maxIt,1);
errphiL2 = zeros(maxIt,1);
errphi = zeros(maxIt,1);
etaTotal = zeros(maxIt,1);
etaw = zeros(maxIt,1);
etaphi = zeros(maxIt,1);
etau = zeros(maxIt,1);

%% Generate initial mesh
load meshLshape3.mat % a minimal Lshape mesh
bdFlag = setboundary3(node,elem,'Dirichlet');
for i = 1
    [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
end
[elem,bdFlag,HB] = label3(node,elem,'all',bdFlag);

%% PDE and options
gamma = 3+1/6; alpha = 2/3;
% gamma = 8/3; alpha = 2/3;
% gamma = 5/2; alpha = 1/2;
% gamma = 7/3; alpha = 1/3; % extreme case
pde = quadCurlDataLshape1(gamma,alpha);
% pde = quadCurlDataLshape2(gamma,alpha);
% pde = quadCurlDataLshape(gamma,alpha);
option.printlevel = 0;

%% AFEM cycles using only 1st Maxwell+ Stokes, 2nd Maxwell solved only for ref
for k = 1:maxIt
    fprintf('\n\n\nRefinement level %d\n', k);
    NT = size(elem,1);

    %% SOLVE the 1st Maxwell problem

    pdeCurl1.J = pde.quadcurlu;
    pdeCurl1.g_D = @(p) zeros(size(p,1),3);
    option.solver = 'direct';
    if NT > 2e4; option.solver = 'diag'; end
    fprintf('\n**************Solving the first Maxwell problem for w**************');
    soln.w = Maxwellsaddle(node,elem,bdFlag,pdeCurl1,option,HB);
%     soln.w = Maxwellsaddle(node,elem,bdFlag,pdeCurl1,option);
    fprintf('*********************************************************************\n');
    %% SOLVE the Stokes problem

    % RHS of Stokes
    [curlwh, volume, curlbasis] = curlu3(node,elem,soln.w);
    pdeStokes.f = curlwh;
    pdeStokes.g_D = pde.curlu;
    option.solver = 'diag';
    option.printlevel = 1;
    fprintf('\n**************Solving the Stokes problem for phi*******************\n');
    solnStokes = Stokes3CRP0(node,elem,bdFlag,pdeStokes,option,HB);
    fprintf('*********************************************************************\n');
    %% SOLVE the 2nd Maxwell problem (not used in the indicator for mesh refinement)

    % RHS os the 2nd Maxwell
    [elem2face,face] = dof3face(elem);
    NF = size(face,1);
    soln.phi = reshape(solnStokes.u,NF,3);

    phihCenter = zeros(NT,3); % at each elem center
    for j = 1:3 % each component
        phihj = soln.phi(:,j);
        phihj2elem = phihj(elem2face);
        phihCenter(:,j) = sum(phihj2elem,2)/4;
        % Crouzeix-Raviart shape function 1-3\lambda = 1/4 at center
    end

    [elem2edge,edge] = dof3edge(sort(elem,2)); % sort for edge element
    NE = size(edge,1);
    bt = zeros(NT,6);
    for j = 1:6 % 6 basis of the edge element
        bt(:,j) = dot(curlbasis(:,:,j),phihCenter,2).*volume;
    end
    fCurl = zeros(2*NE,1);
    fCurl(1:NE,:) = accumarray(elem2edge(:),bt(:),[NE 1]);

    % solve the last Maxwell problem
    option.solver = 'direct';
    if NE > 2e4; option.solver = 'diag'; end
    pdeCurl2.J = fCurl;
    pdeCurl2.g_D = pde.exactu;
    pdeCurl2.g = pde.g;
    fprintf('\n*****Solving the second Maxwell problem for u (for reference)*****');
    soln.u = Maxwell1saddle(node,elem,bdFlag,pdeCurl2,option,HB);
    fprintf('*********************************************************************\n');
    %% ESTIMATE
    soln.curlw = curlwh;
    nDofMaxwell(k) = 2*NE;
    nDofStokes(k) = 3*NF;

    option.includeU = true; % only use the indicator by w_h and phi_h
    option.scaling = false; % no scaling and separate marking
    [etaK, est] = estimatequadcurl(node,elem,soln,pde,option);

    hK = est.hK;
%     h = sqrt(sum(hK.^2)/NT);
    Dlambda = gradbasis3(node,elem);
    curlphi = curlu3CR(elem2face,soln.phi,Dlambda);
    [errphi(k), errphiK] = getL2error3(node,elem,pde.curlcurlu,curlphi);
    [errphiL2(k), errKphiL2] = getL2error3(node,elem,pde.curlu,soln.phi);
    etaTotal(k) = sqrt(sum(etaK.^2));
    curluh = curlu3(node,elem,soln.u);
    [erru(k), errK] = getL2error3(node,elem,pde.curlu,curluh);
    erruL2(k) = getL2error3ND1(node,elem,pde.exactu,soln.u);
%     est.eta1 = est.eta1.*hK;
%     est.eta1 = est.eta1*min(hK);
    etaw(k) = sqrt(sum((est.eta1).^2));
    etaphi(k) = sqrt(sum((est.eta2).^2));
    etau(k) = sqrt(sum((est.eta3).^2));

    fprintf('Total eta          = %g \n', etaTotal(k));
%     fprintf('Mesh size          = %g \n', max(hK));
    fprintf('# face             = %g \n', NF);
    fprintf('# edge             = %g \n', NE);
    fprintf('||phi_h - curl u|| = %g \n', errphiL2(k));
    %% Visualize
    figure(1);
    subplot(1,3,1)
    etawE2V = accumarray(elem(:),repmat(est.eta1,[4,1]),[size(node,1), 1]);
    showsolution3(node,elem,etawE2V,'z<0.25 ','EdgeColor','k','facecolor','flat');
    colorbar;
    title('eta for w')
    view(120,45)

    subplot(1,3,2)

    % errphiL2E2V = accumarray(elem(:),repmat(errphiL2K,[4,1]),[size(node,1), 1]);
    % showsolution3(node,elem,errphiL2E2V,'z<0.25 ','EdgeColor','k','facecolor','flat');

    etaphiE2V = accumarray(elem(:),repmat(est.eta2,[4,1]),[size(node,1), 1]);
    showsolution3(node,elem,etaphiE2V,'z<0.25 ','EdgeColor','k','facecolor','flat');
    colorbar;
    title('eta for phi')
    view(120,45)

    subplot(1,3,3)
%     errU2v = accumarray(elem(:),repmat(erruK,[4,1]), [size(node,1), 1]);
%     showsolution3(node,elem,errU2v,'z<0.25','EdgeColor','k','facecolor','flat');
    % errL2U2v = accumarray(elem(:),repmat(abs(erruL2K),[4,1]),[size(node,1), 1]);
    % showsolution3(node,elem,errL2U2v,'z<0.25 ','EdgeColor','k','facecolor','flat');
    errphiE2V = accumarray(elem(:),repmat(errphiK,[4,1]),[size(node,1), 1]);
    showsolution3(node,elem,errphiE2V,'z<0.25 ','EdgeColor','k','facecolor','flat');
    title('H^1 error for phi')
    colorbar;
    view(120,45)

    set(gcf,'color','w','Position', [100 600 1400 300])
    drawnow;

    %% MARK and REFINE
    if nDofStokes(k) > maxN; break; end
%
%     markedElem = mark(elem,etaK,theta);
%     markedElemC = mark(elem,est.elemRescurlwh,2/3*theta);
%     markedElem = unique([markedElem; markedElemC]);

    % markedElem = mark(elem,errKphiL2,theta); % cheating
    markedElem = mark2(elem,est.eta1,theta1,est.eta2,theta);

    fprintf('eta1(marked)^2/eta1^2 = %g \n', sum(est.eta1(markedElem).^2)/sum(est.eta1.^2));
    fprintf('eta2(marked)^2/eta2^2 = %g \n', sum(est.eta2(markedElem).^2)/sum(est.eta2.^2));
    if k < maxIt
        [node,elem,bdFlag,HB] = bisect3(node,elem,markedElem,bdFlag,HB);
    end

end

%%
close all;
figure(1);
nDofMaxwell = nDofMaxwell(1:k);
nDofStokes = nDofStokes(1:k);
erru = erru(1:k);
errphiL2 = errphiL2(1:k);
etaTotal = etaTotal(1:k);
erruL2 = erruL2(1:k);
etaw = etaw(1:k);
etaphi = etaphi(1:k);
etau = etau(1:k);
errphi = errphi(1:k);
ITER_THRESH = ceil(k/2);
r1 = showrate(nDofMaxwell,erru,ITER_THRESH,'k-o');
r2 = showrate(nDofStokes,errphiL2,ITER_THRESH,'r-o');
% r3 = showrate(nDofMaxwell+nDofStokes,etaTotal,ITER_THRESH,'b-*');
r3 = showrate(nDofMaxwell,erruL2,ITER_THRESH,'b-*');
r4 = showrate(nDofMaxwell/2,etaw,ITER_THRESH,'color',[0.7,0.2,0.7],'marker','*');
r5 = showrate(nDofStokes,etaphi,ITER_THRESH,'color',[0.2,0.8,0.7],'marker','*');
r6 = showrate(nDofMaxwell,etau,ITER_THRESH,'color',[0.7,0.8,0.2],'marker','*');
r7 = showrate(nDofStokes,errphi,ITER_THRESH,'g-o');
set(gca,'xscale','log','yscale','log');
T = title('Convergence');
XL = xlabel('$\#$ DoFs');
YL = ylabel('Error Magnitudes');
L = legend('$\Vert  \nabla\times(u-u_h)\Vert$', ...
    ['$(\# \mbox{edge})^{' num2str(r1) '}$'],...
    '$\Vert  \nabla\times u - \phi_h\Vert$', ...
    ['$(\# \mbox{face})^{' num2str(r2) '}$'],...
        '$\Vert u - u_h\Vert$', ...
    ['$(\# \mbox{edge})^{' num2str(r3) '}$'],...
     '$\eta_1(w_h)$', ['$(\# \mbox{edge})^{' num2str(r4) '}$'],...
    '$\eta_2(w_h, \phi_h)$', ['$(\# \mbox{face})^{' num2str(r5) '}$'],...
    '$\eta_3(\phi_h, u_h)$', ['$(\# \mbox{edge})^{' num2str(r6) '}$'],...
    '$|\nabla\times u-\phi_h|_{1,h}$', ['$(\# \mbox{face})^{' num2str(r7) '}$'],...
'LOCATION','southwest');

%     '$\eta(w_h,\phi_h)$', ...
%     ['$(\# \mbox{DoF})^{' num2str(r3) '}$'],...

set([XL,YL,L],'Interpreter','latex','FontSize', 16);
set(T,'Interpreter','latex','FontSize',20);
set(gca,'TickLabelInterpreter', 'latex');
grid on;
% set(gcf,'color','w','Position', [100 200 300 600])
set(gcf,'color','w','Position',[613 598 820 820])
drawnow;

%% sanity check
if nDofMaxwell<1e4
    figure(2);
    subplot(1,2,1);
    option.scale = 2;
    option.plot = 'quiver3';
    center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
        node(elem(:,3),:) + node(elem(:,4),:))/4;
    showvector3(center,pde.curlu(center),option);
    subplot(1,2,2);
    curluh = curlu3(node,elem,soln.u);
    showvector3(center,curluh,option);

    set(gcf,'color','w','Position', [500 600 750 300])
    drawnow;
end


function [C, H, W, M] = FindLargestRectangles(I, crit, minSize)
% finds largest rectangle regions within all points set to 1.
% input: I       - B/W boolean matrix or output of FindLargestSquares
%        minSize - [height width] - minimum width and height on regions of
%                  interest (used to restrict final choise)
%        crit    - Optimazation Criteria parameters to optimize:
%                   crit(1)*height + crit(2)*width + crit(3)*height*width
% output:
%         C    - value of the optimization criteria "crit" calculated for
%                each pixel
%         W, H - for each pixel I(r,c) return height and width of the largest
%                all-white rectangle with its upper-left corner at I(r,c)
%         M    - Mask the largest all-white rectangle of the image

if (nargin<2)
  crit = [1 1 0];
end
if (nargin<3)
  minSize = [1 1];
end
p = crit;
[nR nC] = size(I);
if (minSize(1)<1), minSize(1)= floor(minSize(1)*nR); end
if (minSize(2)<1), minSize(2)= floor(minSize(2)*nC); end
if (max(I(:)) - min(I(:))==1),
  S = FindLargestSquares(I);
else
  S = I;
end
n = max(S(:));
W = S; % make a carbon copy of the matrix data
H = S;
C = ((p(1)+p(2)) + p(3)*S) .* S; % p(1)*width + p(2)*height + p(3)*height*width for height=width=S;
d = round((3*n)/4);
minH = max(min(minSize(1), d),1);
minW = max(min(minSize(2), d),1);

%% look for rectangles with width>height
hight2width = zeros(n+1,1);  % Store array with largest widths aviable for a given height
for r = 1 : nR               % each row is processed independently
  hight2width(:) = 0;        % reset the List
  for c = nC: -1 : 1         % go through all pixels in a row right to left
    s = S(r,c);              % s is a size of a square with its corner at (r,c)
    if (s>0)                 % if pixel I(r,c) is true
      MaxCrit = C(r,c);      % initialize the Max Criteria using square
      for hight = s:-1:1     % go through all possible width&hight combinations. Start with more likely to be the best
        width = hight2width(hight); % look up width for a given hight
        width = max(width+1,s);
        hight2width(hight) = width;
        Crit = p(1)*hight + p(2)*width + p(3)*width*hight;
        if (Crit>MaxCrit),   % check if it produces larger Criteria
          MaxCrit = Crit;    % if it does than save the results
          W(r,c)  = width;
          H(r,c)  = hight;
        end % if Crit
      end % for hight
      C(r,c)  = MaxCrit;
    end % if s
    hight2width((s+1):end) = 0;    % hights>s will not be aviable for the next pixel
  end % for c
end
clear hight2width

%% look for rectangles with width<height
width2hight = zeros(n+1,1);  % Store array with largest widths aviable for a given height
for c = 1 : nC               % each column is processed independently
  width2hight(:) = 0;        % reset the List
  for r = nR: -1 : 1         % go through all pixels in a column bottom to top
    s = S(r,c);              % s is a size of a square with its corner at (r,c)
    if (s>0)                 % if pixel I(r,c) is true
      MaxCrit = C(r,c);      % initialize the Max Criteria using square
      for width = s:-1:1     % go through all possible width&hight combinations. Start with more likely to be the best
        hight = width2hight(width); % look up hight for a given width
        hight = max(hight+1,s);
        width2hight(width) = hight;
        Crit = p(1)*hight + p(2)*width + p(3)*width*hight;
        if (Crit>MaxCrit),   % check if it produces larger Criteria
          MaxCrit = Crit;    % if it does than save the results
          W(r,c)  = width;
          H(r,c)  = hight;
        end % if Crit
      end % for width
      C(r,c)  = MaxCrit;
    end % if s
    width2hight((s+1):end) = 0;    % hights>s will not be aviable for the next pixel
  end % for r
end

%% Create container mask
CC = C;
CC( H<minH | W<minW ) = 0; % first try to obey size restrictions
[~, pos] = max(CC(:));
if (isempty(pos)), [~, pos] = max(C(:)); end % but when it fails than drop them
[r c] = ind2sub(size(C), pos);
M = false(size(C));
M( r:(r+H(r,c)-1), c:(c+W(r,c)-1) ) = 1;


function [theta_gibbs,sigma_gibbs,sigmatilde_gibbs,sig_gibbs]=panel5gibbs(y,Y,Xtilde,Xdot,N,n,T,d,theta0,Theta0,alpha0,delta0,It,Bu,pick,pickf)

% compute first  preliminary elements
% compute alphabar
alphabar=N*n*T+alpha0;
% compute the inverse Theta0
invTheta0=sparse(diag(1./diag(Theta0)));
% initiate the Gibbs sampler
% initiate the counting of iterations
count=1;
pickcount=1;
% initiate the record matrices
theta_gibbs=zeros(d,It-Bu);
sigmatilde_gibbs=zeros((N*n)^2,It-Bu);
sig_gibbs=zeros(1,It-Bu);
sigma_gibbs=zeros((N*n)^2,It-Bu);

% step 1: compute initial values
% initial value for theta (use OLS values)
theta=(Xtilde*Xtilde')\(Xtilde*y);
% initial value for sigmatilde (use residuals form OLS values)
eps=y-Xtilde'*theta;
eps=reshape(eps,T,N*n);
sigmatilde=eps'*eps;
% initiate value for the sigma, the scaling term for the errors
sig=1;
% initiate value for the matrix sigma, the residual variance-covariance matrix
sigma=sig*sigmatilde;
% initiate value for eyesigma
% compute the inverse of sigma
C=bear.trns(chol(bear.nspd(sigma),'Lower'));
invC=C\speye(N*n);
invsigma=invC*invC';
% then compute eyesigma
eyesigma=kron(speye(T),invsigma);
% finally, initiate eyetheta
eyetheta=kron(speye(T),theta);

hbar = bear.parfor_progressbar(It-Bu,'Progress of Panel BVAR Gibbs Sampler.');  %create the progress bar

% run the Gibbs sampler
while count<=It

% step 2: obtain sigmatilde
% compute Sbar
Sbar=(1/sig)*(Y-Xdot*eyetheta)*(Y-Xdot*eyetheta)';
sigmatilde=bear.iwdraw(Sbar,T);

% step 3: obtain sig
% compute the inverse of sigmatilde
C=bear.trns(chol(bear.nspd(sigmatilde),'Lower'));
invC=C\speye(N*n);
invsigmatilde=invC*invC';
% compute deltabar
deltabar=trace((Y-Xdot*eyetheta)*(Y-Xdot*eyetheta)'*invsigmatilde)+delta0;
% draw sig
sig=bear.igrandn(alphabar/2,deltabar/2);

% step 4: compute sigma and eyesigma
sigma=sig*sigmatilde;
C=bear.trns(chol(bear.nspd(sigma),'Lower'));
invC=C\speye(N*n);
invsigma=invC*invC';
eyesigma=kron(speye(T),invsigma);

% step 5: obtain theta
% compute Thetabar
invThetabar=full((Xtilde*eyesigma*Xtilde'+invTheta0));
C=bear.trns(chol(bear.nspd(invThetabar),'Lower'));
invC=C\speye(d);
Thetabar=invC*invC';
% compute thetabar
thetabar=Thetabar*(Xtilde*eyesigma*y+invTheta0*theta0);
% draw theta
theta=thetabar+chol(bear.nspd(Thetabar),'lower')*mvnrnd(zeros(d,1),eye(d))';

% step 6: obtain eyetheta
eyetheta=kron(speye(T),theta);

   % record phase
   % if the burn-in sample phase is not yet over
   if count<=Bu
   % simply add 1 to the iteration count
   count=count+1;
   % on the other hand, if the burn-in sample phase is over
   elseif count>Bu
   % adding one iteration to the count will depend on wether post-burn selection applies
      % if there is no post burn selection
      if pick==0
      % record the draw
      theta_gibbs(:,count-Bu)=theta;
      sigmatilde_gibbs(:,count-Bu)=bear.vec(sigmatilde);
      sig_gibbs(1,count-Bu)=sig;
      sigma_gibbs(:,count-Bu)=bear.vec(sigma);
      % and add one to the count
      count=count+1;
      % if there is post burn selection, only one draw over 'fpick' draws will be retained
      elseif pick==1
         % if the iteration does not correspond to fpick, don't record the results, don't increase the regular count, but do increase pickcount by 1
         if pickcount~=pickf
         pickcount=pickcount+1;
         % on the other hand, if the iteration does correspond to fpick
         elseif pickcount==pickf
         % do record the results
         theta_gibbs(:,count-Bu)=theta;
         sigmatilde_gibbs(:,count-Bu)=bear.vec(sigmatilde);
         sig_gibbs(1,count-Bu)=sig;
         sigma_gibbs(:,count-Bu)=bear.vec(sigma);
         % then increase the regular count by 1 and re-initialise pickcount
         count=count+1;
         pickcount=1;
         end
      end
   end

   hbar.iterate(1);   % update progress by one iteration

end

close(hbar);   %close progress bar


function cdf = stdnormal_cdf (x)
% For each component of x, compute the CDF of the standard normal
% distribution at x.

% Description: CDF of the standard normal distribution

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
% Copyright (C) 1995-2011 Kurt Hornik
% Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:       KH <Kurt.Hornik@wu-wien.ac.at>
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

if (nargin ~= 1)
    error('Requires one input arguments.');
end

if (numel(x) == 0)
    error ('stdnormal_cdf: X must not be empty');
end

cdf = erfc (x / (-sqrt(2))) / 2;


function [MPa] = ftH2O2MPa(ftH2O)
% Convert pressure from feet of water column at 4 degrees to megapascals
% Chad Greene 2012
MPa = ftH2O*0.00298907;


function nx= sizeXrec(i,P)
% returns the number of k-ToM's hidden sates
% function nx = sizeXrec(i,P)
% Marie Devaine wrote this in November 2015 (comments: JD).
% The formula follows from an analysis of the recursive sequence.
% IN:
%   - i: k-ToM's sophistication level
%   - P: total nb of evol/obs params
% OUT:
%   - nx: number of k-ToM's hidden sates

if i==0
    nx = 2;
else
    i_1 = i-1;
    if i_1>0
        power2 = exp(((i_1-1):-1:0).*log(2));
        S = 2^i + power2*((1:i_1)*(3*P+2)-1)';
    else
        S = 2;
    end
    nx = S+ i*(3*P+2)-1;
end
end


function [ w, success ] = quad_form( x, Q, v, w )

%QUAD_FORM quadratic form.
%   QUAD_FORM(x,Q) is real(x'*Q*x) = x'*((Q+Q')/2)*x.
%   QUAD_FORM(x,Q,v,w) is real(x'*(Q*x+v)+w).
%
%   x must be a row or column vector, and Q must either be a scalar or
%   a square matrix with the same number of rows as x. If supplied, v must
%   be a scalar or a vector of the same size as x, and w must be a scalar.
%   If x is a row vector, then x (and v) are Hermitian transpoed before
%   evaluation.
%
%   NOTE: The use of QUAD_FORM can often be replaced by a call to NORM. For
%   example, if Q is positive definite, then the constraint
%       quad_form(x,Q) <= 1
%   is equivalent to
%       norm(sqrtm(Q)*x) <= 1
%   Generally speaking, the NORM version will be more reliable and more
%   accurate, so we encourage you to make similar conversions whenever
%   possible. We *strongly* discourage the QP-era practice of converting
%   NORM expressions into quadratic forms.
%
%   Disciplined convex programming information:
%       QUAD_FORM(x,Q,v,w) is neither convex nor concave in x and (Q,v)
%       jointly, so at least one of the two must be constant.
%
%       If (Q,v) is constant, then QUAD_FORM is convex if Q is positive
%       semidefinite, and concave if Q is negative semidefinite. An error
%       is generated if Q is indefinite (unless x is also constant).
%       QUAD_FORM is nonmonotonic in x, so x must be affine.
%
%       If x is constant, then QUAD_FORM is affine in Q, v, and w. The
%       signs of x will govern whether the elements of Q, v, and w may
%       be convex, concave, or affine.

tol = 16 * eps;
tolLDL = 4 * eps;
if nargin < 4,
    w = 0;
    if nargin < 3,
        v = 0;
    end
end

sx = size( x );
if length( sx ) ~= 2 || all( sx ~= 1 ),
    cvx_throw( 'The first argument must be a vector.' );
end
nx = prod( sx );

sQ = size( Q );
if length( sQ ) ~= 2 || sQ( 1 ) ~= sQ( 2 ),
    cvx_throw( 'The second argument must be a scalar or a square matrix.' );
elseif sQ( 1 ) ~= 1 && sQ( 1 ) ~= nx,
    cvx_throw( 'The size of Q is incompatible with the size of x.' );
end

if nargin < 3,
    v = sparse( sx(1), sx(2) );
elseif ~isequal( size( v ), sx ),
    cvx_throw( 'The size of v is incompatible with the size of x.' );
end

if nargin < 4,
    w = 0;
elseif numel(w) ~= 1 || ~isreal(w),
    cvx_throw( 'The fourth argument must be a real scalar.' );
end

if sx(1) ~= nx,
    x = x';
    v = v';
end

success = true;
cvx_optval = [];
if cvx_isconstant( x ),
    x = cvx_constant( x );
    cvx_optval = real( x' * Q * x ) + sum( real( v' * x ) ) + w;
    return
elseif ~cvx_isconstant( Q ) || ~cvx_isconstant( v ),
    cvx_throw( 'Either x or (Q,v) must be constant.' );
elseif ~cvx_isaffine( x ),
    cvx_throw( 'First argument must be affine.' );
end

Q = cvx_constant( Q );
Q = 0.5 * ( Q + Q' );
v = cvx_constant( v );
dQ = diag( Q );

if nnz( Q ) == 0,
    cvx_optval = real( v' * x ) + w;
    return
end

if sQ( 1 ) == 1,
    x = x + 0.5 * ( v / Q );
    w = w - 0.25 * ( v' * v ) / Q;
    if ~isreal( x ), x = abs( x ); end
    w = real( Q ) * sum_square( x ) + w;
    return
end

while true,

    %
    % Remove zero rows and columns from Q. If a diagonal element of Q is
    % zero but there are elements on that row or column that are not,
    % then we know that neither Q nor -Q is PSD.
    %

    dQ = diag( Q );
    trQ = sum( dQ );
    if ~all( dQ ),
        nnzQ = nnz( Q );
        tt = dQ ~= 0;
        Q = Q( tt, tt );
        if nnz( Q ) ~= nnzQ,
            success = false;
            break
        end
        dQ = dQ( tt );
        w = w + real( v( ~tt, : )' * cvx_subsref( x, ~tt, ':' ) );
        v = v( tt, : );
        x = cvx_subsref( x, tt, ':' );
        sx = length( x );
    end

    %
    % Determine the sign of the elements of Q. If they are not all of
    % the same sign, then neither Q nor -Q is PSD. Note that trQ has
    % preserved the sign of our quadratic form, so setting Q=-Q here
    % in the concave case does not cause a problem.
    %

    dQ = dQ > 0;
    if ~all( dQ ),
        if any( dQ ),
            success = false;
        else
            Q = -Q;
        end
    end

    %
    % We've had to modify this portion of the code because MATLAB has
    % removed support for the CHOLINC function.
    %
    % First, try a Cholesky. If it successfully completes its
    % factorization without fail, we accept it without question. If
    % it terminates early, we perform a numerical test to see if the
    % result still approximates the square root to good precision.
    %
    % If Cholesky fails, then we assume the matrix is either rank
    % deficient or indefinite. For sparse matrices, we perform an LDL
    % factorization, and remove the contributions of any 2x2 blocks,
    % negative 1x1 blocks, and near-zero 1x1 blocks on the diagonal.
    % If there are no such blocks, we accept it without question; if
    % so, we perform the same numerical test. If the test fails, we
    % assume, for sparse matrices, at least, that the matrix is
    % indefinite.
    %
    % If the matrix is dense, our final test is an eigenvalue
    % decomposition, the most expensive but the most accurate.
    %

    spQ = nnz(Q) <= 0.1 * nx * nx;
    if spQ,
        Q = sparse( Q );
        [ R, p, prm ] = chol( Q, 'upper', 'vector' );
        if any( diff(prm) ~= 1 ),
            R( :, prm ) = R; %#ok
        end
    else
        Q = full( Q );
        [ R, p ] = chol( Q, 'upper' );
        if p > 1,
            R = [ R , R' \ Q(1:p-1,p:end) ]; %#ok
        end
    end
    valid = p == 0;
    if ~valid,
        tolQ = tol * norm( Q, 'fro' );
        if p > 1,
            valid = norm( Q - R' * R, 'fro' ) < tolQ;
        end
    end
    if ~valid && spQ,
        [ R, DD, prm ] = ldl( sparse( Q ), 'upper', 'vector' );
        if nnz( R ) > max( sx, 0.1 * sx * ( sx + 1 ) / 2 ), spQ = false; end
        tt = diag(DD,1) == 0;
        tt = [ tt ; true ] & [ true ; tt ] & ( diag(DD) > tolLDL * trQ );
        DD = diag(DD);
        R  = bsxfun( @times, sqrt( DD(tt,:) ), R(tt,:) );
        if any( diff(prm) ~= 1 ), R( :, prm ) = R; end
        valid = all( tt ) || norm( Q - R' * R, 'fro' ) < tolQ;
    end
    if ~valid && ~spQ,
        [ V, D ] = eig( full( Q ) );
        if nnz( V ) <= max( length(V), 0.1 * numel(V) ), V = sparse(V); end
        D = diag( D );
        if any( D(2:end) < D(1:end-1) ),
            [D,ndxs] = sort(D);
            V = V(:,ndxs);
        end
        valid = D(1) > -tol * D(end);
        if valid,
            nzero = nnz( cumsum(D) < tol * abs(trQ) );
            V = V(:,nzero+1:end);
            D = sqrt(D(nzero+1:end));
            R = diag(sparse(D)) * V';
        end
    end
    if ~valid,
        success = false;
        break;
    end

    %
    % Scale so that the mean eigenvalue of (1/alpha)*R'*R is one.
    % Hopefully this will minimize scaling issues.
    %

    alpha = trQ / size(R,1);
    w = w + alpha * sum_square_abs( ( R * x ) / sqrt(alpha) ) + real( v' * x );
    break;

end

if ~success && nargout == 1,
    cvx_throw( 'Disciplined convex programming error:\n    The second argument must be positive or negative semidefinite.' );
end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [cha, chp] = convexhullProps(grains)
% Area of the convex hull of grains
% Note: this is only reasonable for smooth and sufficiently large grains
%
%
% Syntax
%   [cha, chp] = convexhullArea(grains)
%
% Input
%  grains - @grain2d
%
% Output
%  cha - area of convex hull
%  chp - perimeter of covex hull
%
%

p = zeros(size(grains));

% store this in local variables for speed reasons
X = grains.V(:,1);
Y = grains.V(:,2);

poly = grains.poly;
% remove inclusions
incl = grains.inclusionId;
for i = find(incl>0).'
  poly{i} = poly{i}(1:end-incl(i));
end

% compute convex hull perimeters
for id = 1:length(grains)

  % extract coordinates
  xGrain = X(poly{id});
  yGrain = Y(poly{id});

  % compute convex hull
  ixy = convhull(xGrain,yGrain);

  % area
  cha(id) = polySgnArea(xGrain(ixy),yGrain(ixy));

  % perimeter
  chp(id) = sum(sqrt(...
    (xGrain(ixy(1:end-1)) - xGrain(ixy(2:end))).^2 + ...
    (yGrain(ixy(1:end-1)) - yGrain(ixy(2:end))).^2));

end


 function st = newfft(om_in, Nd_in, varargin)
%function st = newfft(om, Nd, [options])
%|
%| New version of NUFFT (pun intended) that uses real interpolation kernels.
%| (The original NUFFT code used complex interpolation needlessly.)
%|
%| This returns a "strum" object with methods for both forward and adjoint
%| d-dimensional NUFFT operations.
%| The forward operation is:
%| X(om_m) = \sum_{n=0}^{N-1} x[n] exp(-1i * om_m * n) for m=1,...,M
%| The adjoint operation is:
%| x_adj[n] = \sum_{m=1}^M X(om_m) exp(+1i * om_m * n) for n=0,...,N-1
%| Note that the adjoint is not the "inverse" NUFFT in general.
%|
%| This routine has numerous options for investigative purposes, but is
%| designed so that the default options should be very good choices.
%| Providing the frequencies and the image size and should suffice.
%| Reducing the neighborhood size 'Jd' (default 6) and/or reducing the
%| over-sampled DFT size 'Kd' (default 2*Nd) may be useful for acceleration.
%|
%| in
%|	om [M,d]	"digital" frequencies in radians (can be empty!)
%|			(if empty, then user must provide 'om' to methods)
%|	Nd [d]		image dimensions (N1,N2,...,Nd)
%|
%| options
%|	'Jd' [d]	# of neighbors used (in each direction). (default: 6)
%|	'Kd' [d]	FFT sizes (should be >= Nd). (default: 2*Nd)
%|	n_shift [d]	n = 0-n_shift to N-1-n_shift (default: 0)
%|		Like fft(), the NUFFT expects the signals to be x(0,0), ...
%|		Use n_shift = [N1/2, N2/2, ...] for x(-N1/2,-N2/2,...), ...
%|
%|	'mode'	char	how to compute the NUFFT (default: 'table1')
%|			'exact'	slow FT
%|			'table0' table with nearest neighbor interpolation
%|			'table1' table with linear interpolation (default)
%|			'gg' gaussian factorization of Greengard & Lee 04 (todo)
%|			'sparse' precompute large sparse matrix
%|				caution: this option may require lots of memory!
%|				(this option used internally to make tables too)
%|			recommended: table0 or table1 to save memory.
%|
%|	'oversample' int table oversampling factor (default: 2^9 for table1)
%|	'gram' 0|1	if 1, precompute additional terms needed for gram matrix
%|	'printmem' 0|1	if 1, print memory usage
%|	'phasing' char	'real' : new real kernels (strongly recommended default)
%|			'complex' : original nufft.m complex kernels
%|			'none' : no phase (straw man; table uses it internally)
%|			'flipreal' : real kernels with sign flips (unsupported)
%|	'dotime' char	report cpu time? (default: '')
%|
%|	'ktype'	char	type of interpolation kernel (default: 'minmax:kb')
%|
%| ktype options:
%|	'diric'		Dirichlet interpolator (exact only if Jd = Kd)
%|	'linear'	linear interpolator (a terrible straw man)
%|	'minmax:kb'	minmax interpolator with excellent KB scaling!
%|	'minmax:tuned'	minmax interpolator, somewhat numerically tuned scaling
%|	'minmax:unif'	minmax with uniform scaling factors (not recommended)
%|	'minmax:user'	minmax interpolator with user parameters required:
%|				'alpha', {alpha}, 'beta', {beta}
%|	'kb:minmax'	kaiser-bessel (KB) interpolator (minmax best alpha, m)
%|	'kb:beatty'	KB with parameters from Beatty et al T-MI Jun 2005
%|	'kb:user'	KB with user-specified KB parameters required:
%|				'kb_m', [m] 'kb_alf', [alpha]
%|	@kernel		user-provided inline interpolation kernel(k,J)
%|			(or a cell array of kernels, one for each dimension)
%|			example ..., 'table', 2^11, 'minmax:kb'
%|			where 2^11 is the table over-sampling factor.
%|
%| out
%|	st	strum object with several methods including the following:
%|		st.fft(x, [om])
%|		st.adj(Xo, [om])
%|		st.p		[M, *Kd]	sparse interpolation matrix
%|						(or empty if table-based)
%|		st.sn		[(Nd)]		scaling factors
%|		st.Nd,Jd,Kd,om	copies of inputs
%|
%| *Nd is shorthand for prod(Nd).
%| (Nd) is shorthand for (N1,N2,...,Nd)
%|
%| Copyright 2007-6-3, Jeff Fessler, The University of Michigan

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

cpu etic

% inputs
st.om = om_in; % [M,d] frequency samples
st.Nd = Nd_in; % [d] signal dimentions
st.dd = length(st.Nd); % dimensionality of input space (usually 2 or 3)

% defaults
st.gram = false;
st.Jd = 6 * ones(1, st.dd);
st.Kd = 2 * st.Nd;
st.n_shift = zeros(1, st.dd);
st.mode = 'table1';
st.oversample = []; % aka Ld for table mode
st.order = []; % for table mode
st.ktype = 'kb:minmax';
st.kb_m = []; % [dd] KB parameters
st.kb_alf = [];
st.alpha = {}; % [dd] minmax parameters
st.beta = {};
st.tol = 0;
st.printmem = false;
st.is_kaiser_scale = false;
st.phasing = 'real'; % use new real table by default
st.dotime = '';

% options
st = vararg_pair(st, varargin);
dotime = st.dotime; st = rmfield(st, 'dotime');

st.phase_before = []; % place holders
st.phase_after = [];
st.flips = [];

% special cases of input sampling pattern
if ischar(st.om)
	st.om = nufft_samples(st.om, st.Nd);
end

% checks
if st.dd ~= length(st.Jd) || st.dd ~= length(st.Kd)
	error 'inconsistent dim'
end
if st.dd ~= length(st.n_shift)
	fail('n_shift needs %d columns', st.dd)
end

if ~isempty(st.om) && st.dd ~= size(st.om,2)
	fail('omega needs %d columns', st.dd)
end

if st.gram, fail 'todo: gram not done', end

if any(st.Kd < st.Nd), warning 'Kd < Nd unlikely to work.  Try Kd=2*Nd', end

%
% "midpoint" of scaling factors
%
switch st.phasing
case 'real'
	st.Nmid = floor(st.Nd / 2); % new
otherwise
	st.Nmid = (st.Nd - 1) / 2; % old
end

%
% different interpolation modes
%
switch st.mode

case 'exact' % exact interpolation for testing
	st = strum(st, { ...
		'fft', @newfft_exact_for, '(x, [om])';
		'adj', @newfft_exact_adj, '(X, [om])';
		'p', @newfft_exact_p, '([om])';
		'sn', @(st) ones(st.Nd), '()';
		});

case {'table0', 'table1'} % precomuted interpolator table
	st = newfft_table_init(st);

	% set up phase corrections not included in table initialization
	if streq(st.phasing, 'real') || streq(st.phasing, 'flipreal')
		st.phase_before = newfft_phase_before(st.Kd, st.Nmid);
		st.phase_after = @(om) newfft_phase_after(om, st.Nmid, st.n_shift);
		if ~isempty(st.om) % phase that goes after interpolation:
			st.phase_after = st.phase_after(st.om);
		end
	end

case 'gg' % greengard's gaussian
	error 'todo: gg'
	st = strum(st, { ...
		'fft', @newfft_gg_for, '(x, [om])';
		'adj', @newfft_gg_adj, '(X, [om])';
		'p', @newfft_gg_p, '([om])';
		'sn', @newfft_gg_sn, '()';
		});

case 'sparse' % interpolator based on a sparse matrix
	if isempty(st.om), error 'sparse mode requires "om"', end
	st = newfft_init_sparse(st);

otherwise
	fail('unknown mode %s', st.mode)
end

if ~isempty(dotime)
	tmp = whos('st');
	tmp = num2str(tmp.bytes);
	tmp = ['newfft setup ' st.mode ' ' st.phasing(1) ' ' tmp ' ' dotime];
	cpu('etoc', tmp)
end

%
% newfft_init_sparse()
%
% create an interpolator based on a sparse matrix.
% this matrix will be large for intersting problem sizes, so this
% mode is not recommended.  but it is supported in part because it
% is needed for generating samples of the interpolator for the table mode.
%
function st = newfft_init_sparse(st)

om = st.om;

%
% different interpolation kernel mechanisms
%
ktype = st.ktype;

switch class(ktype)
case 'cell' % cell array of kernel functions: {kernel1, kernel2, ..., kernelD}
	if isa(ktype{1}, 'inline') || isa(ktype{1}, 'function_handle')
		if length(ktype) ~= dd, error 'wrong # of kernels', end
		st.kernel = ktype;
		ktype = 'inline';
	else
		error 'cell array should be inline kernels!?'
	end

case {'inline', 'function_handle'} % single inline kernel for all dimensions
	for id = 1:st.dd
		st.kernel{id} = ktype; % all same
	end
	ktype = 'inline';

case 'char'
	% a string that describes the type of interpolator, see below

otherwise
	fail('unknown kernel type %s', class(ktype))
end

%
% interpolator set up
%
Nd = st.Nd;
Jd = st.Jd;
Kd = st.Kd;
dd = st.dd;
is_kaiser_scale = false;

switch ktype
case 'inline'
	% already did it above

case 'diric' % exact interpolator
	if any(Jd ~= Kd), warn 'diric inexact unless Jd=Kd', end
	ktype = 'inline';
	for id = 1:dd
		N = Nd(id);
		K = Kd(id);
		if 1 && streq(st.phasing, 'real')
			N = 2 * floor((K+1)/2) - 1; % trick
		end
		st.kernel{id} = @(k,J) N / K * nufft_diric(k, N, K, true);
	end

case 'linear' % linear interpolator straw man
	ktype = 'inline';
	kernel = inline('(1 - abs(k/(J/2))) .* (abs(k) < J/2)', 'k', 'J');
	for id = 1:dd
		st.kernel{id} = kernel;
	end

case 'kb:beatty' % KB with Beatty et al parameters
	is_kaiser_scale = true;
	if ~isempty(st.kb_alf) || ~isempty(st.kb_m)
		warn 'kb_alf and kb_m ignored'
	end
	K_N = Kd ./ Nd;
	st.kb_alf = pi * sqrt( Jd.^2 ./ K_N.^2 .* (K_N - 1/2).^2 - 0.8 );
%	pr st.kb_alf ./ Jd % approximately 2.34 for K_N = 2
	st.kb_m = zeros(1,dd);
	for id = 1:dd
		st.kernel{id} = kaiser_bessel('inline', Jd(id), ...
				st.kb_alf(id), st.kb_m(id));
	end

case 'kb:minmax' % KB with minmax-optimized parameters
	is_kaiser_scale = true;

	if ~isempty(st.kb_alf) || ~isempty(st.kb_m)
		warn 'kb_alf and kb_m ignored'
	end
	for id = 1:dd
		[st.kernel{id} st.kb_alf(id) st.kb_m(id)] = ...
			kaiser_bessel('inline', Jd(id));
	end

case 'kb:user' % KB with user-defined parameters
	is_kaiser_scale = true;

	if isempty(st.kb_alf) || isempty(st.kb_m)
		fail 'kb_alf and kb_m required'
	end
	if (length(st.kb_alf) ~= dd) || (length(st.kb_m) ~= dd)
		fail('#alpha=%d #m=%d vs dd=%d', ...
			length(st.kb_alf), length(st.kb_m), dd)
	end
	for id = 1:dd
		st.kernel{id} = kaiser_bessel('inline', Jd(id), ...
				st.kb_alf(id), st.kb_m(id));
	end

case 'minmax:kb' % minmax interpolator with KB scaling factors
	for id = 1:dd
		[st.alpha{id}, st.beta{id}] = ...
			nufft_alpha_kb_fit(Nd(id), Jd(id), Kd(id), ...
				'Nmid', st.Nmid(id));
	end

case 'minmax:tuned' % minmax with numerically "tuned" scaling factors
	for id = 1:dd
		[st.alpha{id}, st.beta{id}, ok] = ...
			nufft_best_alpha(Jd(id), 0, Kd(id)/Nd(id));
		if ~ok, error 'unknown J,K/N', end
	end

case 'minmax:user' % minmax interpolator with user-provided scaling factors
	if isempty(st.alpha) || isempty(st.beta)
		error 'user must provide alpha/beta'
	end
	if length(st.alpha) ~= dd || length(st.beta) ~= dd
		error 'alpha/beta size mismatch'
	end

case 'minmax:unif' % minmax with straw man uniform scaling factors
	for id = 1:dd
		st.alpha{id} = 1;
		st.beta{id} = 0;
	end

otherwise
	fail('unknown kernel type %s', ktype)
end

%
% scaling factors: "outer product" of 1D vectors
%
st.sn = 1;
for id=1:dd
	if 1 && streq(st.ktype, 'linear')
		tmp = newfft_scale_tri(Nd(id), Jd(id), Kd(id), st.Nmid);
	elseif streq(st.ktype, 'diric')
		tmp = ones(Nd(id),1);
	elseif is_kaiser_scale
		nc = [0:Nd(id)-1]' - st.Nmid(id);
		tmp = 1 ./ kaiser_bessel_ft(...
			nc/Kd(id), Jd(id), st.kb_alf(id), st.kb_m(id), 1);
	elseif streq(ktype, 'inline')
		tmp = 1 ./ nufft_interp_zn(0, Nd(id), Jd(id), Kd(id), ...
			st.kernel{id}, st.Nmid(id));
	else
		tmp = nufft_scale(Nd(id), Kd(id), ...
			st.alpha{id}, st.beta{id}, st.Nmid(id));
	end
	tmp = reale(tmp);
	st.sn = st.sn(:) * tmp';
end
if length(Nd) > 1
	st.sn = reshape(st.sn, Nd);	% [(Nd)]
else
	st.sn = st.sn(:);	% [(Nd)]
end

%
% [J?,M] interpolation coefficient vectors.  will need kron of these later
%
for id=1:dd
	N = Nd(id);
	J = Jd(id);
	K = Kd(id);
	if isfield(st, 'kernel')
		[c, arg] = ...
		nufft_coef(om(:,id), J, K, st.kernel{id});	% [J?,M]
	else
		alpha = st.alpha{id};
		beta = st.beta{id};
		T = nufft_T(N, J, K, st.tol, alpha, beta);	% [J?,J?]
		[r, arg] = ...
		nufft_r(om(:,id), N, J, K, alpha, beta);	% [J?,M]
		c = T * r;
		clear T r
	end

	%
	% indices into oversampled FFT components
	%
	koff = nufft_offset(om(:,id), J, K);	% [M,1] to leftmost near nbr
	k0 = outer_sum([1:J]', koff');	% [J?,M] arbitrary integers
	kd{id} = mod(k0, K);	% [J?,M] {0,...,K?-1} (DFT indices)

	gam = 2*pi/K;

	switch st.phasing
	case {'real', 'none'}
		phase = 1;
	case 'complex'
		phase_scale = 1i * gam * (N-1)/2;
		phase = exp(phase_scale * arg);	% [J?,M] linear phase
	case 'flipreal'
		isodd = @(n) mod(n,2) == 1;
		phase = ones(size(k0)); % [J?,M]
		flip = isodd((kd{id} - k0) / K * (N-1)); % sign flip every K
		phase(flip) = -1; % sign flip (if N is even)
	otherwise
		fail('unknown phasing %s', st.phasing)
	end
	ud{id} = phase .* c;		% [J?,M]

end % id
clear c arg gam phase phase_scale koff k0 N J K

% st.M = size(om,1);
M = size(om,1);

%
% build sparse matrix that is [M,*Kd]
% with *Jd nonzero entries per frequency point
%
if st.printmem
	printm('Needs at least %g Gbyte RAM', prod(Jd)*M*8/2^30*2)
end

kk = kd{1};	% [J1,M]
uu = ud{1};	% [J1,M]
for id = 2:dd
	Jprod = prod(Jd(1:id));
	tmp = kd{id} * prod(Kd(1:(id-1)));
	kk = block_outer_sum(kk, tmp);		% outer sum of indices
	kk = reshape(kk, Jprod, M);
	uu = block_outer_prod(uu, ud{id});	% outer product of coefficients
	uu = reshape(uu, Jprod, M);
end % now kk and uu are [*Jd, M]

%
% handle phase shifts
%
uu = conj(uu); % [*Jd,M] ala Hermitian transpose of interpolation coefficients
switch st.phasing
case 'complex'
	phase = exp(1i * (om * st.n_shift(:))).';	% [1,M]
	uu = uu .* phase(ones(1,prod(Jd)),:);	% [*Jd,M]
	if streq(st.mode, 'table', 5) % moved from newfft_table_init.m to here
		st.phase_after = @(om) exp(1i * (om * col(st.n_shift))); % [M,1]
	end
case {'real', 'flipreal'}
	st.phase_before = newfft_phase_before(Kd, st.Nmid);
	if ~isempty(st.om) % precompute phase that goes after interpolation
		st.phase_after = newfft_phase_after(st.om, st.Nmid, st.n_shift);
	else
		st.phase_after = @(om) newfft_phase_after(om, st.Nmid, st.n_shift);
	end
case 'none'
	% do nothing
otherwise
	error 'bug'
end

mm = repmat(1:M, prod(Jd), 1); % [*Jd,M]
st.p = sparse(mm(:), 1+kk(:), uu(:), M, prod(Kd)); % [M, *Kd] sparse matrix
% sparse object, to better handle single precision operations!
st.p = Gsparse(st.p, 'odim', [M 1], 'idim', [prod(Kd) 1]);

st = strum(st, { ...
	'fft', @newfft_approx_for, '(x, [om])';
	'adj', @newfft_approx_adj, '(X, [om])';
	});

%
% in
%	x1	[J1,M]
%	x2	[J2,M]
% out
%	y	[J1,J2,M]	y(i1,i2,m) = x1(i1,m) + x2(i2,m)
%
function y = block_outer_sum(x1, x2)
[J1 M] = size(x1);
[J2 M] = size(x2);
xx1 = reshape(x1, [J1 1 M]);	% [J1,1,M] from [J1,M]
xx1 = xx1(:,ones(J2,1),:);	% [J1,J2,M], emulating ndgrid
xx2 = reshape(x2, [1 J2 M]);	% [1,J2,M] from [J2,M]
xx2 = xx2(ones(J1,1),:,:);	% [J1,J2,M], emulating ndgrid
y = xx1 + xx2;			% [J1,J2,M]

function y = block_outer_prod(x1, x2)
[J1 M] = size(x1);
[J2 M] = size(x2);
xx1 = reshape(x1, [J1 1 M]);	% [J1,1,M] from [J1,M]
xx1 = xx1(:,ones(J2,1),:);	% [J1,J2,M], emulating ndgrid
xx2 = reshape(x2, [1 J2 M]);	% [1,J2,M] from [J2,M]
xx2 = xx2(ones(J1,1),:,:);	% [J1,J2,M], emulating ndgrid
y = xx1 .* xx2;			% [J1,J2,M]

%
% newfft_phase_before()
% phase factor that gets multiplied by DFT (before interpolation)
%
function phase = newfft_phase_before(Kd, Nmid)

phase = 0;
for id = 1:length(Kd)
	tmp = 2 * pi * [0:Kd(id)-1] / Kd(id) * Nmid(id);
	phase = outer_sum(phase, tmp); % [(Kd)] when done
end
phase = exp(1i * phase);

%
% newfft_phase_after()
% phase factor that multiplies the DTFT (after interpolation)
%
function phase = newfft_phase_after(om, Nmid, n_shift)

phase = exp(1i * (om * col(n_shift - Nmid))); % [M,1]

%
% newfft_scale_tri()
% scale factors when kernel is 'linear'
% tri(u/J) <-> J sinc^2(J x)
%
function sn = newfft_scale_tri(N, J, K, Nmid)
nc = [0:N-1] - Nmid;
fun = @(x) J * nufft_sinc(J * x / K).^2;
cent = fun(nc);
sn = 1 ./ cent;

% try the optimal formula
tmp = 0;
LL = 3;
for ll=-LL:LL
	tmp = tmp + abs(fun(nc - ll*K)).^2;
end
sn = cent ./ tmp;

%
% newfft_test_time()
% compare compute times of real vs complex, sparse vs table
%
function newfft_test_time

Nd = [1 1] * 2^8;
[tmp om] = mri_trajectory('radial', {}, Nd, Nd);
pr length(om)
rng(0)
x0 = rand([Nd 1]);

arg = {om, Nd, 'dotime', ' '};
s0r = newfft(arg{:}, 'mode', 'table0');
s0c = newfft(arg{:}, 'mode', 'table0', 'phasing', 'complex');
s1r = newfft(arg{:}, 'mode', 'table1');
ssr = newfft(arg{:}, 'mode', 'sparse');
ssc = newfft(arg{:}, 'mode', 'sparse', 'phasing', 'complex');

tmp = @(st) newfft_test_time_one(st, x0, 2);
tmp(s0r)
tmp(s0c)
tmp(s1r)
tmp(ssr)
tmp(ssc)

function newfft_test_time_one(st, x0, nn)
st.fft(x0); % warm up
tmp = [st.mode ' ' st.phasing(1)];
cpu etic
for ii=1:nn
	st.fft(x0); % trial
end
cpu('etoc', tmp)

%
% newfft_test()
%
function newfft_test

newfft_test_time
prompt

modes = {'sparse', 'table0', 'table1'};

phasings = {'complex', 'real'};
% 'flipreal' no longer needed thanks to floor(N/2)
% 'none' is for internal table only

Nd_list = [20 10 8];
for id=0:3
	if id == 0
		Nd = 1 + Nd_list(1); % test odd case
	else
		Nd = Nd_list(1:id);
	end
	dd = length(Nd);

	rng(0)
	om = 3 * 2 * pi * (rand(100,length(Nd)) - 0.5);
%	om = sort(om);
%	om = linspace(-1,1,601)' * 3*pi;
%	om = [-8:8]'/2 * pi;
	%om = 'epi';
	x0 = rand([Nd 1]);
%	x0 = zeros([Nd 1]); x0(1) = 1; % unit vector for testing
%	x0 = ones([Nd 1]);

	% exact
	st_e = newfft(om, Nd, 'mode', 'exact');
	Xe = st_e.fft(x0);
	xe = st_e.adj(Xe);
	if 0
		pe = st_e.p;
		equivs(pe * x0(:), Xe)
		equivs(reshape(pe' * Xe, [Nd 1]), xe)
	end

	ktypes = {{'linear', 'Jd', 2*ones(1,dd)}, ...
		'minmax:unif', ...
		{'minmax:user', 'alpha', num2cell(ones(1,dd)), ...
			'beta', num2cell(0.5 * ones(1,dd))}, ...
		{'diric', 'Jd', 2*Nd-0, 'oversample', []}, ...
		'minmax:kb', 'minmax:tuned', ...
		'kb:minmax', 'kb:beatty', ...
		{'kb:user', 'kb_m', 0*Nd, 'kb_alf', 2.34 * 6 + 0*Nd}
		};

	for ii=4:length(ktypes) % skip poor ones
		ktype = ktypes{ii};
		if ~iscell(ktype), ktype = {ktype}; end

	for jj=1:length(modes)
	for ip=1:length(phasings)
		sc = newfft(st_e.om, st_e.Nd, 'phasing', 'complex', ...
			'mode', 'table0', 'ktype', ktype{:});

		st = newfft(st_e.om, st_e.Nd, 'phasing', phasings{ip}, ...
			'mode', modes{jj}, 'ktype', ktype{:});

		if streq(st.phasing, 'complex') && streq(st.mode, 'table1')
			continue
		end

%		pr minmax(st.sn)
		pad = @(s,n) [s blanks(n-length(s))];
		key = [sprintf('%2d ', st.Jd(1)) st.ktype];
		key = [st.mode ' ' num2str(id) st.phasing(1) ' ' pad(key,16)];
		Xs = st.fft(x0);
		max_percent_diff(Xe, Xs, key)
%		plot(abs(Xe), abs(Xs), 'o'), prompt

		xs = st.adj(Xe);
		max_percent_diff(xe, xs, key)
	end % ip
	end % jj
	end % ii
end


function element_node = grid_q9_element ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_Q9_ELEMENT produces a grid of 9 node quadrilaterals.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NODE =
%         1,  3, 17, 15,  2, 10, 16,  8,  9;
%         3,  5, 19, 17,  4, 12, 18, 10, 11;
%         5,  7, 21, 19,  6, 14, 20, 12, 13;
%        15, 17, 31, 29, 16, 24, 30, 22, 23;
%        17, 19, 33, 31, 18, 26, 32, 24, 25;
%        19, 21, 35, 33, 20, 28, 34, 26, 27.
%
%  Grid:
%
%   29---30---31---32---33---34---35
%    |    .    |    .    |    .    |
%    |    .    |    .    |    .    |
%   22 . 23 . 24 . 25 . 26 . 27 . 28
%    |    .    |    .    |    .    |
%    | 4  .    | 5  .    | 6  .    |
%   15---16---17---18---19---20---21
%    |    .    |    .    |    .    |
%    |    .    |    .    |    .    |
%    8 .  9 . 10 . 11 . 12 . 13 . 14
%    |    .    |    .    |    .    |
%    | 1  .    | 2  .    | 3  .    |
%    1----2----3----4----5----6----7
%
%  Reference Element Q9:
%
%    |
%    1  4--7--3
%    |  |     |
%    |  |     |
%    S  8  9  6
%    |  |     |
%    |  |     |
%    0  1--5--2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NODE(9,NELEMX*NELEMY), the nodes that form
%    each element.
%

%
%  Node labeling:
%
%    NW----N----NE
%     |          |
%     W    C     E
%     |          |
%    SW----S----SE
%
  element = 0;

  for j = 1 : nelemy
    for i = 1 : nelemx

      sw = 2 * ( j - 1 )  * ( 2 * nelemx + 1 ) + 2 * ( i - 1 ) + 1;
      w  = sw +               2 * nelemx + 1;
      nw = sw +         2 * ( 2 * nelemx + 1 );

      s  = sw + 1;
      c  = sw + 1 +               2 * nelemx + 1;
      n  = sw + 1 +         2 * ( 2 * nelemx + 1 );

      se = sw + 2;
      e  = sw + 2 +               2 * nelemx + 1;
      ne = sw + 2 +         2 * ( 2 * nelemx + 1 );

      element = element + 1;

      element_node(1,element) = sw;
      element_node(2,element) = se;
      element_node(3,element) = ne;
      element_node(4,element) = nw;
      element_node(5,element) = s;
      element_node(6,element) = e;
      element_node(7,element) = n;
      element_node(8,element) = w;
      element_node(9,element) = c;

    end
  end

  return
end


function [xUpdate,LUpdate,PUpdate,innov,Pzz,W]=separatedCovUpdate(xPred,LPred,TPred,z,R,H,c)
%%SEPARATEDCOVUPDATE Perform the measurement update step in the separated
%                    covariance filter. This filter takes a maximum assumed
%                    acceleration (or other moment) for the target and
%                    provides the optimum estimate in terms of a cost
%                    function that trades off between estimation accuracy
%                    and estimator delay. The filter assumes a linear
%                    measurement model.
%
%INPUTS: xPred The xDimX1 predicted target state.
%        LPred The xDimXzDim predicted delay vector (defined before
%              Equation 12 in [1]). The use of multiple columns represents
%              a choice in how the algorithm was generalized to multiple
%              dimensions.
%        TPred The xDimXxDim total error matrix. This is a combination of
%              errors due to measurement noise and filter lag.
%            z The zDimX1 vector measurement. zDim should be the same as
%              the number of position components in the state.
%            R The zDimXzDim measurement covariance matrix.
%            H The zDimXxDim measurement matrix for a linear measurement
%              model. That is z=H*x+w, where w is measurement noise having
%              covariance matrix R.
%            c The confidence region under consideration by the filter.
%              0<c<1. If this parameter is omitted or an empty matrix is
%              passed, the default value of c=0.99 is used.
%
%OUTPUTS: xUpdate The xDimX1 updated state vector.
%         LUpdate The xDimXzDim updated delay vector.
%         PUpdate The xDimXxDim covariance matrix of the state estimate.
%                 This is a combination of LUpdate and PUpdate.
%      innov, Pzz The zDimX1 innovation and a zDimXzDim matrix S that is
%                 akin to an innovation covariance matrix are returned in
%                 case one wishes to analyze the consistency of the
%                 estimator or use those values in gating or likelihood
%                 evaluation.
%               W The gain used in the update. This can be useful when
%                 gating and using the function calcMissedGateCov.
%
%The equations for the algorithm are given in Table 1 of [1]. The algorithm
%is presented in 1D for a state consisting of position and velocity.
%However, the equations are given in vector form and can thus be used in
%multiple dimensions, which is done here.
%
%The filter in 1 is 1D. To generalize the filter to 3D, L is redefined so
%that each column contains the lag for one particular dimension of motion.
%To keep the solution the same as in Table 1, L becomes a matrix where the
%elements in each column that do not correspond to components for that
%dimensions of motion are zero.
%
%In [1], no clear method of initializing this type of tracking filter is
%provided. A simple way to initialize the filter would be to use two
%Cartesian converted measurements to obtain a state estimate and covariance
%PInit as one would do with a normal Kalman filter (one could, for example,
%use the KalmanFIRSmoother function) and then set TUpdate=c^2*PInit;
%LUpdate=zeros(xDim,zDim); and PUpdate=PInit.
%
%REFERENCES:
%[1] G. J. Portmann, J. R. Moore, and W. G. Bath, "Separated covariance
%    filtering," in Proceedings of the IEEE International Radar Conference,
%    Arlington, VA, 7-10 May 1990, pp. 456-460.
%
%September 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7||isempty(c))
   c=0.99;
end

Pzz=H*TPred*H'+c^2*R;
%Ensure symmetry
Pzz=(Pzz+Pzz')/2;

W=TPred*H'/Pzz;%The gain

innov=z-H*xPred;%The innovation
xUpdate=xPred+W*innov;

xDim=size(xPred,1);
diff=eye(xDim,xDim)-W*H;

LUpdate=diff*LPred;
TUpdate=diff*TPred;

%Ensure symmetry
TUpdate=(TUpdate+TUpdate')/2;

PUpdate=(TUpdate-LUpdate*LUpdate')/c^2;

%Ensure symmetry
PUpdate=(PUpdate+PUpdate')/2;
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.