  function [sino, hn, nn, Hk] = fbp2_sino_hilbert(sino, varargin)
%|function [sino, hn, nn, Hk] = fbp2_sino_hilbert(sino, [options])
%|
%| Apply band-limited Hilbert-transform filter to 2D sinogram.
%| Frequency response: H(u) = -1i * sign(u) * rect(u/2/umax)
%|
%| in
%|	sino	[nb (L)] sinogram(s)
%|
%| options
%|	dr | ds	(real)	sample spacing (in distance units, e.g., cm) (default 1)
%|	npad		# of padded samples. (default: 0, means next power of 2)
%|	decon1		deconvolve effect of linear interpolator? (default: 0)
%|	window	[npad]	samples of apodization window function
%|			for [-np/2,...,np/2-1]. (default: '' = plain ramp)
%|			or a string like 'hann' for some predefined windows
%| out
%|	sino	[nb (L)] filtered sinogram rows
%|	hn	[npad]	samples of band-limited Hilbert transform filter
%|	nn	[npad]	[-np/2,...,np/2-1] vector for convenience
%|	Hk	[npad]	spectral samples on [0 ... np-1]
%|
%| Copyright 2011-07-16, Jeff Fessler, University of Michigan

if nargin == 1 && streq(sino, 'test'), fbp2_sino_hilbert_test, clear, return, end
if nargin < 1, ir_usage, end

arg.dr = 1;
arg.window = '';
arg.npad = 0;
arg.decon1 = false;
arg = vararg_pair(arg, varargin, 'subs', {'ds', 'dr'});

dims = size(sino);
sino = reshape(sino, dims(1), []);
[sino hn nn Hk] = fbp2_sino_hilbert_do(sino, ...
	arg.dr, arg.window, arg.npad, arg.decon1);
sino = reshape(sino, [size(sino, 1) dims(2:end)]);

% fbp2_sino_hilbert_do()
function [sino, hn, nn, Hk] ...
	= fbp2_sino_hilbert_do(sino, dr, window, npad, decon1);

[nb na] = size(sino);
if ~npad
	npad = 2^ceil(log2(2*nb-1)); % padded size
end
sino = [sino; zeros(npad-nb,na)]; % padded sinogram

[hn nn] = fbp2_filter_hilbert_make(npad, dr);

Hk = 1i * imag_check(fft(fftshift(hn))); % trick: pure imaginary!

Hk = Hk .* fbp2_window(npad, window);

Hk = dr * Hk; % differential for discrete-space convolution vs integral

% linear interpolation is like blur with a triangular response,
% so we can compensate for this approximately in frequency domain
if decon1
	Hk = Hk ./ fftshift(nufft_sinc(nn / npad).^2);
end

sino = ifft_sym( fft(sino, [], 1) .* repmat(Hk, [1 na]), [], 1); % apply filter

% fbp2_filter_hilbert_make()
function [hn, nn] = fbp2_filter_hilbert_make(n, dr)
nn = [-(n/2):(n/2-1)]';
u0 = 1/2/dr;
hn = 2 * u0 * nufft_sinc(nn/2) .* sin(pi/2 * nn);

% imag_check()
% take imaginary part but check that real part is negligible
function out = imag_check(in, varargin)
out = reale(-1i * in, varargin{:});

% fbp2_sino_hilbert_test()
function fbp2_sino_hilbert_test
nb = 2^5;
dr = 0.1;
[sino1 h1 nn H1] = fbp2_sino_hilbert(zeros(nb,2), 'dr', dr);
h2 = 1 ./ (pi * nn * dr); % samples of ideal non-bandlimited

if im
	clf, subplot(121)
	plot(nn, h1, '.-', nn, h2, '-')
	xlabel 'n', ylabel 'h[n]'
	legend('band-limited', 'ideal')

	subplot(122)
	plot(nn, imag_check(fftshift(H1)), '.-', nn, -sign(nn), '-')
	xlabel 'k', ylabel 'imag(H[k])'
	legend('band-limited', 'ideal', 'location', 'north')
end


function [fe] = tapas_ti_trapezoidal(llh, T)
%% Computes the free energy using the trapezoidal rule
%
% Input
%       llh     -- Samples from the log likelihod
%       T       -- Temperatures at which the samples were drawn
% Output
%       fe      -- Free energy
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

ellh = mean(llh, 1);
fe = trapz(T, llh);

end


function img = dtiSmooth3(img, sigma)
% img = dtiSmooth3(img, sigma)
%
% Gaussian smoothing (faster than matlab's smooth3)
%
% NOTE: We should add some zero-padding to avoid wrap-around issues.

persistent gauss;
persistent sigmaCache;
persistent dimsCache;

if(numel(sigma==1))
    sigma = [sigma sigma sigma];
end
if(all(sigma<=0))
    return;
end
dims = size(img);
if ~isa(img, 'double'), type = class(img); img = double(img); end
% there should be a more elegant way of preserving the image intensity
% range!
scale = max(abs(img(:)));
if(scale==0), return; end

if(isempty(gauss) || numel(sigma)~=numel(sigmaCache) || any(sigma~=sigmaCache) || numel(dims)~=numel(dimsCache) || any(dims~=dimsCache))
    gauss = newGauss(dims, sigma);
    sigmaCache = sigma;
    dimsCache = dims;
end

% Allow for 4d timeseries or 5d tensor data.
for(ii=1:size(img,4))
    for(jj=1:size(img,5))
        ft = fftshift(fftn(img(:,:,:,ii,jj)));
        ft = gauss.*ft;
        img(:,:,:,ii,jj) = real(ifftn(ifftshift(ft)));
    end
end
img = img./max(abs(img(:))).*scale;

if exist('type', 'var')
	% we had a non-double matrix, convert back
	img = feval(type, img);
end
return;

function gauss = newGauss(dimSrc,variance)
gaussx = myGausswin(dimSrc(1),variance(1));
gaussy = myGausswin(dimSrc(2),variance(2));
gaussz = myGausswin(dimSrc(3),variance(3));
gaussxy = gaussx*gaussy';
gauss = repmat(gaussxy,[1 1 dimSrc(3)]);
for i = 1:dimSrc(1)
    for j = 1:dimSrc(2)
        gauss(i,j,:) = gaussxy(i,j)*gaussz;
    end
end
return

function w = myGausswin(n, alpha)
% alpha is 1/stdev (width of the gaussian in Fourier space).
k = [-(n-1)/2:(n-1)/2];
w = exp((-1/2)*(alpha * k/(n/2)).^2)';
% Make sure the peak is at 1 so that we'll preserve to DC level
w(w==max(w)) = 1;
return


%TESTR MSE for regression
%
%      E = TESTR(X,W,TYPE)
%      E = TESTR(X*W,TYPE)
%      E = X*W*TESTR([],TYPE)
%      E = X*W*TESTR(TYPE)
%
% INPUT
%   X    Regression dataset
%   W    Regression mapping
%   TYPE Type of error measure, default: mean squared error
%
% OUTPUT
%   E    Mean squared error
%
% DESCRIPTION
% Compute the error of regression W on dataset X. The following error
% measures have been defined for TYPE:
% 'mse'    mean squared error (default)
% 'mad'    mean absolute deviation
%
% SEE ALSO (PRTools Guide)
%  RSQUARED, TESTC

% Copyright: D.M.J. Tax, D.M.J.Tax@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function e = testr(varargin)

  argin = shiftargin(varargin,'char');
  argin = setdefaults(argin,[],[],'mse');

  if mapping_task(argin,'definition')
    e = define_mapping(argin,'fixed');

  else	% Evaluate.

    [x,w,type] = deal(argin{:});

    if (ismapping(w) & istrained(w))
      x = x*w;
    end
    if ischar(w)
      type = w;
    end
    switch type
      case 'mse'
        e = mean((+x(:,1) - gettargets(x)).^2);
      case 'mad'
        e = mean(abs(+x(:,1) - gettargets(x)));
      otherwise
        error('Error %s is not implemented.',type);
    end

    if nargout==0
      %display results on the screen:
      fprintf('Error on %d objects: %f.\n',...
        size(x,1), e);
      clear e;
    end

  end


function Neff = getNeff(dens)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% getNeff(P)
%  returns an estimate of Neff, the 'effective' number of points in the NPDE
%   (= sum(1/weights^2) )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

  Neff = 1/sum(getWeights(dens).^2);


% Function that optimizes Pose given an initial solution of the rotation only
%
% IMPORTANT: THIS FUNCTION ONLY DEALS WITH THE ORTHOGRAPHIC CASE RIGHT NOW
%
% This function optimizes the pose estimation of one camera with respect to
% another given only the 2 corresponding views.  The data is supposed to be
% centered and only the rotation has to be estimated. I it is here
% performed using simple gradient descent.
%
% The following code explains how the code to compute the
% error/gradient/hessian was created using matlab symbolic toolbox.
%
% Vincent's Structure From Motion Toolbox      Version 2.0\n
% Copyright (C) 2009 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

%%%%%  Define the rotation matrix %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
syms a b c d real

R = [ a^2+b^2-c^2-d^2 2*b*c-2*a*d 2*a*c+2*b*d; ...
 2*a*d+2*b*c a^2-b^2+c^2-d^2 2*c*d-2*a*b; ...
 2*b*d-2*a*c 2*a*b+2*c*d a^2-b^2-c^2+d^2 ]/(a^2+b^2+c^2+d^2);
R = R(1:2,:);

%%%%%  Compute its derivatives with respect to the quaternions %%%%%%%%%%%%%%%%%%%%%%
l = R;
l = [ diff(l,a) diff(l,b) diff(l,c) diff(l,d) ];
l = simple( l );

%%% Now, use the following:
%%% You need an expression of the form : '[ a[1][1] = x, a[1][2] =y^2 ]'
%%% not the overall brackets and the indexing starting from 1 (as it is
%%% maple. The quotes: simply coz it has to be a string
%%% so you create an array of assigments basically
maple restart;
com = 'res := [ ';
k = 1;
for i = 1 : 12
  for j = 1 : 2
    com = [ com 'dR[' num2str(k) ']=' char(l(j,i)) ', '];
	k = k + 1;
  end
end
com = [ com(1:end-2) '];' ];

%%%%%  Below is the code to generate C-code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Mupad try, but it sucks ...
%  reset(symengine);
%  out = evalin(symengine, [ 'opts := generate::optimize([R=expr(' char(Rp') ') ]);' ] );
%
%  out = evalin(symengine, [ 'opts := generate::optimize([R=expr(' char(Rp') '), dR=expr(' char(l(:,3:10)) '), ddR=expr(' char(l(:,11:end)) ')]);' ] );
%  evalin(symengine, [ ':=rhs(opts[-1]);' ] );
%
%
%
%  out = [ char(evalin(symengine, [ 'generate::C(opts[1..-2]);' ] )) ...
%    char( evalin(symengine, [ 'generate::C(rhs(opts[-1]))' ] )) ];
%  out = strrep( out, '"', '' );
%  out = strrep( out, [ var '[0][' ], [ var '[' ] );

%%% now, save your crazy array into the res variable
maple( com );

%  %%% cost of the assigments with no optimization
%  maple('codegen[cost](res)')
%  % maple( 'opt1 := [codegen[optimize](l)]' );
%  %%% cost of the assigment with the normal optimization
%  in = [ 'l:=' char( l ) ]; maple( in ); maple('codegen[cost](codegen[optimize](l))')

%%% code for better optimization and corresponding cost
maple( 'opt2 := [codegen[optimize](res,tryhard)]' );
maple('codegen[cost](opt2)')

%%% Convert to C code
out = maple( 'codegen[C](opt2)' );

%%% clean the C code and convert it to Matlab code (replace [] by ()
out = strrep( out, ';   ', ';\n' );
out = strrep( out, '~', '' );

tout = ''
existing = cell(1,0);
tmp = regexp( out,'(t\d*)', 'tokens' )
for i = 1 : length(tmp)
  ii = tmp(i);
  ii = ii{1}{1};
  doExist = false;
  for j = 1 : size(existing,2)
    if strcmp(existing{j},ii)
	  doExist = true;
	  break;
	end
  end
  if ~doExist
    existing{1,end+1} = ii;
    tout = [ tout, ', ', ii ];
  end
end
out = [ 'double ', tout(2:end), ';\n', out ];
fprintf(out)


function fom = figuremerit(ideal,detected)

    Ni = numel(find(ideal == 1));
    Nd = numel(find(detected == 1));

    [h w c] = size(ideal);

    alpha = 1/9;
    sum = 0.0;
    for r = 1:h
        for c = 1:w
            if (detected(r,c) == 1)
                dist = distance(ideal,detected,r,c)^2;
                sum = sum + (1/(1+alpha*dist));
            end
        end
    end

    fom = (1/max(Ni,Nd))*sum;

function d = distance(ideal,detected,rind,cind)

    [h w c] = size(ideal);

    diff  = 1;
    found = 0;
    d = 10.0^100;

    if (ideal(rind,cind) == detected(rind,cind))
        d     = 0.0;
        found = 1;
    end

    left   = cind - diff;
    right  = cind + diff;
    top    = rind - diff;
    bottom = rind + diff;
    while (~found)

        for c = left:right
            if ((top >= 1) && (c >= 1) && (c <= w))
                if (ideal(top,c) == detected(rind,cind))
                    tmp = sqrt((top-rind)^2 + (c-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        for c = left:right
            if ((bottom <= h) && (c >= 1) && (c <= w))
                if (ideal(bottom,c) == detected(rind,cind))
                    tmp = sqrt((bottom-rind)^2 + (c-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        for r = top+1:bottom-1
            if ((left >= 1) && (r >= 1) && (r <= h))
                if (ideal(r,left) == detected(rind,cind))
                    tmp = sqrt((r-rind)^2 + (left-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        for r = top+1:bottom-1
            if ((right <= w) && (r >= 1) && (r <= h))
                if (ideal(r,right) == detected(rind,cind))
                    tmp = sqrt((r-rind)^2 + (right-cind)^2);
                    found = 1;

                    if (tmp < d)
                        d = tmp;
                    end
                end
            end
        end

        diff = diff + 1;
        left   = cind - diff;
        right  = cind + diff;
        top    = rind - diff;
        bottom = rind + diff;
        if ((left < 1) && (right > w) && (top < 1) && (bottom > h))
            found = 1;
        end
    end


D = 2; % we consider a two-dimensional problem
N = 100; % we will generate 100 observations

%% Generate a random Covariance matrix
tmp = randn(D);
Sigma = tmp.' * tmp;

%% Sample from the corresponding Gaussian
X = mvnrnd(zeros(D, 1), Sigma, N);

%% Estimate the leading component
comp = grassmann_average(M, 1); % the second input is the number of component to estimate
comp = trimmed_grassmann_average(M', 50, 1);
show_2dvideo(comp,m,n);
show_2dvideo(comp(:,1),m,n);
show_2dvideo(comp(:,2),m,n);
show_2dvideo(comp(:,3),m,n);
show_2dvideo(comp(:,4),m,n);
show_2dvideo(comp(:,5),m,n);

%% Plot the results
plot(X(:, 1), X(:, 2), 'ko', 'markerfacecolor', [255,153,51]./255);
axis equal
hold on
plot(3*[-comp(1), comp(1)], 3*[-comp(2), comp(2)], 'k', 'linewidth', 2)
hold off
axis off


function [W_RF,W_BB]=OMP_Combining(F_RF,F_BB)
global  Nr Ns Nrf H Vn Codebook_w
W_RF = [];
W_MMSE = ((F_BB' * F_RF' * H' * H * F_RF * F_BB + Vn * Ns * eye(Ns))^(-1) * F_BB' * F_RF' * H')';
Wres = W_MMSE;
n = 1 / Ns * H * F_RF * F_BB * F_BB' * F_RF' *H' + Vn * eye(Nr);
for i = 1 : Nrf
    y = Codebook_w' * n * Wres;
    k = find(diag(y * y')==max(diag(y * y')));
    W_RF(:,i) = Codebook_w(:,k);
    W_BB = (W_RF' * n * W_RF)^(-1) * W_RF' * n * W_MMSE;
    Wres = (W_MMSE - W_RF * W_BB) / norm(W_MMSE - W_RF * W_BB,'fro');
end


function [H] = scale(f)

% SCALE returns the homogenous coordinate transformation matrix
% corresponding to a scaling along the x, y and z-axis
%
% Use as
%   [H] = translate(S)
% where
%   S       [sx, sy, sz] scaling along each of the axes
%   H   corresponding homogenous transformation matrix

% Copyright (C) 2000-2005, Robert Oostenveld
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if numel(f)~=3
  ft_error('incorrect input vector');
end

H = [
  f(1) 0    0    0
  0    f(2) 0    0
  0    0    f(3) 0
  0    0    0    1
  ];


function [h, compUpVV, compUpVP, compUp] =  lfmComputeH3VV(gamma1_p, gamma1_m, sigma2, t1, ...
    t2, preFactor, mode)

% LFMCOMPUTEH3VV Helper function for computing part of the LFMVXLFMV kernel.
% FORMAT
% DESC computes a portion of the LFMVXLFMV kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG preFactor : precomputed constants.
% ARG mode: indicates the correct precomputations.
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

% Evaluation of h

if nargout>1
    [compUpVV{1}, compUpVP{1}, compUp{1}] = lfmvvComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode);
    [compUpVV{2}, compUpVP{2}, compUp{2}] = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
    h = preFactor(1)*compUpVV{1} + preFactor(2)*compUpVV{2};
else
    h = preFactor(1)*lfmvvComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode) ...
        + preFactor(2)*lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);

%     %h = preFactor(2)*lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%
%      h = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%
%      epsilon = 1e-6;
%      valg = gamma1_m;
%      gamma1_m = valg + epsilon;
%      h1 = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%      gamma1_m = valg - epsilon;
%      h2 = lfmvvComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
%      gamma1_m = valg;
%      numerics = 0.5*(h1-h2)/epsilon;
%      theo = lfmvvGradientUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);

end


function hypercube_monte_carlo_test ( )

%*****************************************************************************80
%
%% HYPERCUBE_MONTE_CARLO_TEST tests the HYPERCUBE_MONTE_CARLO library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERCUBE_MONTE_CARLO_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the HYPERCUBE_MONTE_CARLO library.\n' );

  hypercube_monte_carlo_test01 ( );
  hypercube_monte_carlo_test02 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERCUBE_MONTE_CARLO_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function coef = sandia_sgmgg_coef_naive ( dim_num, point_num, sparse_index )

%*****************************************************************************80
%
%% SANDIA_SGMGG_COEF_NAIVE returns the combinatorial coefficients.
%
%  Discussion:
%
%    The coefficient of point I is calculated as follows:
%
%    *) point J is a "neighbor" of point I if every entry of the sparse
%       index for point J is either equal to, or 1 greater than, the
%       corresponding entry of the sparse index of point I.
%
%    *) If point J is a neighbor of point I, then it contributes
%       (-1)^D to the coefficient, where D is the sum of the differences
%       between the sparse indices of point I and point J.
%
%    This is a completely naive implementation of the calculation,
%    intended simply as a demonstration for small examples.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the vector.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer SPARSE_INDEX(DIM_NUM,POINT_NUM),
%    the indices that define the points.
%
%    Output, integer COEF(POINT_NUM), the coefficients.
%
  coef = zeros ( point_num, 1 );

  for j1 = 1 : point_num

    for j2 = 1 : point_num

      neighbor = 1;
      term = + 1;

      for i = 1 : dim_num

        dif = sparse_index(i,j2) - sparse_index(i,j1);

        if ( dif == 0 )

        elseif ( dif == 1 )
          term = - term;
        else
          neighbor = 0;
          break;
        end

      end

      if ( neighbor )
        coef(j1) = coef(j1) + term;
      end

    end

  end

  return
end


%TEST_DGT  Test DGT full window backend
%
%  This script runs a throrough test of the COMP_DGT_FAC
%  and COMP_IDGT_FAC testing them on a range of input parameters.
%
%  Use TEST_WFAC first, to verify that COMP_WFAC and COMP_IWFAC
%  are working, since this tester depends on them.

Lr=[24,16,144,108,144,24,135,35,77,20];
ar=[ 4, 4,  9,  9, 12, 6,  9, 5, 7, 1];
Mr=[ 6, 8, 16, 12, 24, 8,  9, 7,11,20];

test_failed=0;

disp('--- Used subroutines ---');

which comp_wfac
which comp_iwfac
which comp_dgt_fac
which comp_idgt_fac

for ii=1:length(Lr);

  L=Lr(ii);

  M=Mr(ii);
  a=ar(ii);

  b=L/M;
  N=L/a;
  c=gcd(a,M);
  d=gcd(b,N);
  p=a/c;
  q=M/c;

  for W=1:3

    for R=1:3

      for rtype=1:2
	if rtype==1
	  rname='REAL ';
	  f=tester_rand(L,W);
	  g=tester_rand(L,R);
	else
	  rname='CMPLX';
	  f=tester_crand(L,W);
	  g=tester_crand(L,R);
	end;

	gf=comp_wfac(g,a,M);
	cc=comp_dgt_fac(f,gf,a,M);
	cc2=ref_dgt(f,g,a,M);

	res=norm(cc(:)-cc2(:));

	failed='';
	if res>10e-10
	  failed='FAILED';
	  test_failed=test_failed+1;
	end;

	s=sprintf('DGT  %s L:%3i W:%2i R:%2i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i %0.5g %s',rname,L,W,R,a,b,c,d,p,q,res,failed);
	disp(s)

      end;

      for rtype=1:2
	if rtype==1
	  rname='REAL ';
	  g=tester_rand(L,R);
	else
	  rname='CMPLX';
	  g=tester_crand(L,R);
	end;

	cc=tester_crand(M,N*R*W);

	gf=comp_wfac(g,a,M);
	f=comp_idgt_fac(ifft(cc)*sqrt(M),gf,L,a,M);
	f2=ref_idgt(reshape(cc,M*N*R,W),g,a,M);

	res=norm(f(:)-f2(:));

	failed='';
	if res>10e-10
	  failed='FAILED';
	  test_failed=test_failed+1;
	end;

	s=sprintf('IDGT %s L:%3i W:%2i R:%2i a:%3i b:%3i c:%3i d:%3i p:%3i q:%3i %0.5g %s',rname,L,W,R,a,b,c,d,p,q,res,failed);
	disp(s)

      end;

    end;

  end;

end;

test_failed


function [IM] = InterpMatrix2D(rout, sout)

% function [IM] = InterpMatrix2D(rout, sout)
% Purpose: Compute local elemental interpolation matrix

Globals2D;

% compute Vandermonde at (rout,sout)
Vout = Vandermonde2D(N, rout, sout);

% build interpolation matrix
IM = Vout*invV;
return


function [ R t s ]=computeOrientation(x,xp,method,varargin)
% Compute Absolute or Exterior Orientation (Pose Estimation)
%
% Absolute is : given x and xp (2 arrays of 3D coordinates), find the best
% transformation such that x=s*R*xp+t
%
% Exterior is : 1 array of 3D position x is known and its 2D orthographic
% projection xp.
% Find the best transformation such that xp=projection*(s*R*x+t)
% (same as Pose Estimation, ePNP). Projection is ortho for now
%
% The routines below are only for the orthographic case for now
%
% USAGE
%  [R,t,s]=computeOrientation(x,xp,method)
%
% INPUTS
%  x,xp       - 3xN or 2xN array of points or right/left.
%               For 'exteriorSequence', it can be of size
%               2 x nPoint x nFrame and 3 x nPoint x nFrame
%  method     - 'absolute', 'absoluteHard' (try more if Horn's fails)
%               or 'exterior' and 'exteriorSequence'
%  varargin   - list of paramaters in quotes alternating with their values
%       - 'RIni' initial rotation from which the optimization will be
%       started (only for 'exterior' and 'exteriorSequence')
%
% OUTPUTS
%  R       - rotation matrix
%  t       - translation vector
%  s       - scale factor (if not requested, s=1)
%
% EXAMPLE
%
% See also
%
% Vincent's Structure From Motion Toolbox      Version 3.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

nPoint=size(x,2);

switch method
  case { 'absolute', 'absoluteHard' }
    % Computes the absolute orientation between 2 sets of 3D points
    % Xright=s*R*Xleft + t   . We want to recover s,R,T
    % Ref: B.K.P. Horn, H.M. Hilden, and S. Negahdaripour, Closed-Form
    % Solution of Absolute Orientation Using Orthonormal Matrices
    rr=normalizePoint(x,4); rl=normalizePoint(xp,4);
    rrBar=mean(rr,2); rlBar=mean(rl,2);
    rrp=rr-rrBar(:,ones(1,nPoint)); rlp=rl-rlBar(:,ones(1,nPoint));

    M=zeros(3); for i=1:nPoint; M=M+rrp(:,i)*rlp(:,i)'; end
    [V,D]=eig(M'*M);
    V=real(V);

    R=1/sqrt(D(2,2))*V(:,2)*V(:,2)' + 1/sqrt(D(3,3))*V(:,3)*V(:,3)';
    temp=V(:,1)*V(:,1)';
    if D(1,1)>0
      R=R+1/sqrt(D(1,1))*temp;
    else
      if det(R+temp)>0; R=R+temp; else R=R-temp; end
    end
    R=M*R;

    R=rotationMatrix(R);

    if det(R)<0 && strcmp(method,'absoluteHard') % if Horn's method fails
      warning('Horn''s method failed. Trying GloptiPoly3 solution');
      RHorn=R;
      try
        % Sedumi and GloptiPoly3 must be installed and in the path !
        mpol R 3 3;

        g0=rlp-R*rrp;
        g0=g0(1,:)*g0(1,:)'+g0(2,:)*g0(2,:)'+g0(3,:)*g0(3,:)';

        % define the rotation constraints
        K = [ R(1,:)*R(1,:)' == 1, R(2,:)*R(2,:)'==1, R(3,:)*R(3,:)'==1,...
          R(1,:)*R(2,:)' == 0, R(1,:)*R(3,:)' == 0, R(2,:)*R(3,:)' == 0,...
          det(R)>=0 ];

        % define the problem and solve it
        P=msdp(min(g0),K);
        [ status obj ] = msol(P);

        R=rotationMatrix( double(R) );
      catch %#ok<CTCH>
        R = RHorn;
      end
    end

    % Figure out s and t
    if nargout==2
      s=1;
    else
      s=norm(rrp,'fro')/norm(rlp,'fro');
      if norm( rrp+s*R*rlp, 'fro' ) < norm( rrp-s*R*rlp, 'fro' ); s=-s; end
    end

    t=rrBar-s*R*rlBar;
  case 'exterior'
    if ~isempty(varargin); RIni = varargin{0};
    else RIni=[];
    end

    if isempty(RIni)
      try
        % Sedumi and GloptiPoly3 must be installed and in the path !
        mpol R 2 3;
        if nargout<2; t=0; else mpol t 2 1; end
        if nargout<3; s=1; else mpol s; end

        g0=0;
        for i=1:nPoint
          tmp=xp(:,i) - s*(R*x(:,i)+t); %#ok<NODEF>
          g0 = g0 + tmp'*tmp;
        end

        % define the rotation constraints
        K = [ R(1,:)*R(1,:)' == 1, R(2,:)*R(2,:)'==1, R(1,:)*R(2,:)' == 0];

        % define the problem and solve it
        mset('verbose',false);
        P=msdp(min(g0),K);

        [ status obj ] = msol(P);

        R=rotationMatrix( double(R) );

        if nargout>=2; t=[ double(t); 0]; end
        if nargout==3; s=double(s); end
      catch %#ok<CTCH>
        warning(['GloptiPoly3 not installed or Gloptypoly crashed,' ...
          'using ePnP']);
        try
          mset clear;
        catch %#ok<CTCH>
          [ R t ] = efficient_pnp( x', xp', eye(3,3) );
          t=t(1:2);
        end
      end
      if any(isnan(R))
        warning(['GloptiPoly failed, using ePnP']);
        [ R t ] = efficient_pnp( x', xp', eye(3,3) );
        t=t(1:2);
      end
    else
      R=RIni;
    end

    % perform gradient descent to optimize the rotation
    Q = refineExteriorOrientation(x,xp,quaternion(R));
    R=quaternion( Q );
  case 'exteriorSequence'
    RIni=getPrmDflt( varargin, {'RIni' [] }, 1 );
    if length(RIni)>1
      Q=quaternion(RIni);
    else
      Q=2*rand(4,size(xp,3))-1;
    end
    [ Q err ]=refineExteriorOrientation(x,xp,Q);

    for i = 1 : 10
      [ QNew errNew ]=refineExteriorOrientation(x,xp,2*rand(4,size(xp,3))-1);
      temp = errNew<err;
      Q(:,temp) = QNew(:,temp);
    end

    R=quaternion( Q );
end
end


function [iPlot,minDist] = BF_ClosestPoint_ginput(xy,inputPoint,doNormalize)
% ClosestPoint_ginput   The closest point in a dataset to the input co-ordinates given.
%
%---INPUTS:
% xy, A Nx2 vector of x-y co-ordinates.
% inputPoint, The output of a ginput.
% doNormalize, whether to normalize the input space so that relative differences
%               in x and y are treated (rather than raw Euclidean distances).
%
%---OUTPUT:
% plotMe, the closest point.

% ------------------------------------------------------------------------------
% 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 work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International License. To view a copy of
% this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or send
% a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View,
% California, 94041, USA.
% ------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
% Check inputs:
%-------------------------------------------------------------------------------
if nargin < 2 || isempty(inputPoint)
    inputPoint = ginput(1);
end
if nargin < 3
    doNormalize = true;
end

%-------------------------------------------------------------------------------
% Normalize
%-------------------------------------------------------------------------------
% Normalizing the space to a unit square treats distances in both axes similarly
% relative to their ranges.
if doNormalize
    minMaxx = [min(xy(:,1)),max(xy(:,1))];
    minMaxy = [min(xy(:,2)),max(xy(:,2))];
    xy(:,1) = (xy(:,1) - minMaxx(1))/(minMaxx(2)-minMaxx(1));
    inputPoint(1) = (inputPoint(1) - minMaxx(1))/(minMaxx(2)-minMaxx(1));
    xy(:,2) = (xy(:,2) - minMaxy(1))/(minMaxy(2)-minMaxy(1));
    inputPoint(2) = (inputPoint(2) - minMaxy(1))/(minMaxy(2)-minMaxy(1));
end

%-------------------------------------------------------------------------------
% Calculate distances from each point to input inputPoint
%-------------------------------------------------------------------------------
dpxy = sum((xy - repmat(inputPoint,size(xy,1),1)).^2,2); % Euclidean distances to the input point
[minDist,iPlot] = min(dpxy);

end


function out = DN_Moments(y,theMom)
% DN_Moments    A moment of the distribution of the input time series.
%
% Normalizes by the standard deviation
% Uses the moment function from Matlab's Statistics Toolbox
%
%---INPUTS:
% y, the input data vector
% theMom, the moment to calculate (a scalar)

% ------------------------------------------------------------------------------
% 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/>.
% ------------------------------------------------------------------------------

out = moment(y,theMom) / std(y); % normalized

end


% SHUFFLE - shuffle a given dimension in an array
%
% Usage: >> Y = shuffle(X)
%        >> [Y = shuffle(X, DIM)
%
% Inputs:
%   X   - input array
%   DIM - dimension index (default is first non-singleton dimension)
%
% Outputs:
%    Y - shuffled array
%    I - forward indices (Y = X(I) if 1D)
%    J - reverse indices (X(J) = Y if 1D)
%
% Author: Arnaud Delorme, SCCN/INC/UCSD USA, Dec 2000

% Copyright (C) Arnaud Delorme, SCCN/INC/UCSD USA, Dec 2000
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [x, i, j]=shuffle( y, dim)

if nargin < 1
	help shuffle;
	return;
end
if nargin < 2
	if size(y,1) ~= 1
		dim = 1;
	else
		if size(y,2) ~= 1
			dim = 2;
		else
			dim = 3;
		end
	end
end

r =size(y, dim);
a = rand(1,r);
[tmp i] = sort(a);
switch dim
	case 1
		x = y(i,:,:,:,:);
	case 2
		x = y(:,i,:,:,:);
	case 3
		x = y(:,:,i,:,:);
	case 4
		x = y(:,:,:,i,:);
	case 5
		x = y(:,:,:,:,i);
end;
[tmp j] = sort(i); % unshuffle

return;


function pco = ge(p,q)
% function pco = ge(p,q)
%
% DESCRIPTION
%   Creates a polynomial constraint object representing the
%   constraint p>=q.  sosopt interprets this constraint as
%           p-q is sum of squares
%
% INPUTS
%   p: polynomial
%   q: polynomial
%
% OUTPUT
%   pco: polynomial constraint object
%
% SYNTAX
%   p>=q
%     Creates a polynomial constraint object.  If p is a scalar and
%     q is a vector then p will be expanded to have the same dimension as
%     q.  Similarly, if q is a scalar and p is a vector then q will be
%     expanded.
%

% 10/22/2010:   PJS  Initial Coding

% Create polynomial constraint object
pco = polyconstr(p,q,'>=');


function [cx, cy, w, h] = get_axis_aligned_BB(region)
% GETAXISALIGNEDBB extracts an axis aligned bbox from the ground truth REGION with same area as the rotated one
nv = numel(region);
assert(nv==8 || nv==4);

if nv==8
    cx = mean(region(1:2:end));
    cy = mean(region(2:2:end));
    x1 = min(region(1:2:end));
    x2 = max(region(1:2:end));
    y1 = min(region(2:2:end));
    y2 = max(region(2:2:end));
    A1 = norm(region(1:2) - region(3:4)) * norm(region(3:4) - region(5:6));
    A2 = (x2 - x1) * (y2 - y1);
    s = sqrt(A1/A2);
    w = s * (x2 - x1) + 1;
    h = s * (y2 - y1) + 1;
else
    x = region(1);
    y = region(2);
    w = region(3);
    h = region(4);
    cx = x+w/2;
    cy = y+h/2;
end


% Coupled nonlinear PDE's
% Buckley Leverett equation
% dependent variables: pressure and water saturation
% Prepared for educational purposes by ** AAE **
% Spontaneous imbibition in a core, IMPES, works fine
% not as fast as expected, can be improved
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
%% define the geometry
Nx = 20; % number of cells in x direction
Ny = 50; % number of cells in y direction
W = 0.02; % [m] length of the domain in x direction
H = 0.07; % [m] length of the domain in y direction
% m = createMesh1D(Nx, W);
m = createMeshCylindrical2D(Nx, Ny, W, H); % creates a 2D mesh
%% define the physical parametrs
krw0_ww = 0.3;
krw0_ow = 1.0;
kro0_ww = 0.6;
kro0_ow = 0.76;
nw = 2.4;
no = 2.0;
sor_ww=0.1;
sor_ow=0.12;
swc_ww=0.09;
swc_ow=0.09;
SF=createFaceVariable(m, 0.0); % 1 is water wet, 0 is oil wet
krw0=krw0_ww*SF+krw0_ow*(1-SF);
kro0=kro0_ww*SF+kro0_ow*(1-SF);
sor=sor_ww*SF+sor_ow*(1-SF);
swc=swc_ww*SF+swc_ow*(1-SF);
sws=@(sw, sor, swc)((sw>swc).*(sw<1-sor).*(sw-swc)./(1-sor-swc)+(sw>=1-sor));
kro=@(sw, kro0, sor, swc)((sw>=swc).*kro0.*(1-sws(sw, sor, swc)).^no+(sw<swc).*(1+(kro0-1)./swc.*sw));
krw=@(sw, krw0, sor, swc)((sw<=1-sor).*krw0.*sws(sw, sor, swc).^nw+(sw>1-sor).*(-(1-krw0)./sor.*(1.0-sw)+1.0));
dkrwdsw=@(sw, krw0, sor, swc)((sw<=1-sor).*nw.*krw0.*(1./(1-sor-swc)).*sws(sw, sor, swc).^(nw-1)+(sw>1-sor).*((1-krw0)./sor));
dkrodsw=@(sw, kro0, sor, swc)((sw>=swc).*(-kro0.*no.*(1-sws(sw, sor, swc)).^(no-1))./(-swc-sor+1)+(sw<swc).*((kro0-1)./swc));
p0 = 100e5; % [bar] pressure
pin = 150e5; % [bar] injection pressure at the left boundary
u_in= 1.0/(24*3600); % [m/s] equal to 1 m/day
sw0 = swc_ww+0.01;
% sw0(10:end-10, 10:end-10)=swc+0.2;
% sw0 = swc+0.1; % initial water saturation
sw_in = 1;
mu_oil = 2e-3; % [Pa.s] oil viscosity
mu_water = 1e-3; % [Pa.s] water viscosity
% reservoir
k0 = 0.005e-12; % [m^2] average reservoir permeability
phi0 = 0.45; % average porosity
teta_ow=deg2rad(30);
gama_ow=0.03; % N/m
labda=2.4;
eps1=1e-6;
clx=0.05;
cly=0.05;
V_dp=0.01; % Dykstra-Parsons coef.
perm_val= k0; %field2d(Nx,Ny,k0,V_dp,clx,cly);
k=createCellVariable(m, perm_val);
phi=createCellVariable(m, phi0);
pce=0.1*gama_ow*cos(teta_ow)*(phi./k).^0.5;
pce_face=0.05*gama_ow*cos(teta_ow)*(arithmeticMean(phi)./geometricMean(k)).^0.5;
sco=swc+0.01;
pc=@(sw, sor, swc)(pce.*(sws(sw, sor, swc)+eps).^(-1/labda)); % it can also be defined for each block
dpc=@(sw, sor, swc)((-1/labda)*(1./(1-sor-swc)).*pce_face.*(sws(sw, sor, swc)+eps).^(-1/labda-1));
dpcdk=@(sw, sor, swc)(0.5*gama_ow*cos(teta_ow)*(geometricMean(k)./arithmeticMean(phi)).^0.5.*(sws(sw, sor, swc)+eps).^(-1/labda));
grad_phik=gradientTerm(phi./k);
% sw_plot=linspace(0,1, 10000);
% plot(sw_plot, pc(sw_plot))
lw = geometricMean(k)/mu_water;
lo = geometricMean(k)/mu_oil;
Lw = @(sw)(krw(sw));
Lo = @(sw)(k/mu_oil*kro(sw));
dLwdsw = @(sw)(k/mu_water*dkrwdsw(sw));
dLodsw = @(sw)(k/mu_oil*dkrodsw(sw));
%% Define the boundaries: all fixed Sw=1, fixed pressure everywhere(?)
BCp = createBC(m); % Neumann BC for pressure
BCs = createBC(m); % Neumann BC for saturation
% left boundary pressure gradient
% BCp.left.a(:)=(krw(sw_in)*lw.xvalue(1,:)+kro(sw_in)*lo.xvalue(1,:)); BCp.left.b(:)=0; BCp.left.c(:)=-u_in;
% change the right boandary to constant pressure (Dirichlet)
% BCp.left.a(:)=0; BCp.left.b(:)=1; BCp.left.c(:)=p0;
BCp.right.a(:)=0; BCp.right.b(:)=1; BCp.right.c(:)=p0;
BCp.top.a(:)=0; BCp.top.b(:)=1; BCp.top.c(:)=p0;
BCp.bottom.a(:)=0; BCp.bottom.b(:)=1; BCp.bottom.c(:)=p0;
% change the left boundary to constant saturation (Dirichlet)
% BCs.left.a(:)=0; BCs.left.b(:)=1; BCs.left.c(:)=1.0-sor;
BCs.right.a(:)=0; BCs.right.b(:)=1; BCs.right.c(:)=1.0;
BCs.top.a(:)=0; BCs.top.b(:)=1; BCs.top.c(:)=1.0;
BCs.bottom.a(:)=0; BCs.bottom.b(:)=1; BCs.bottom.c(:)=1.0;
%% define the time step and solver properties
% dt = 1000; % [s] time step
% dt=(W/Nx)/u_in/20; % [s]
dt=1;
t_end = 10*3600*24; % [s] final time
eps_p = 1e-5; % pressure accuracy
eps_sw = 1e-4; % saturation accuracy
%% define the variables
sw_old = createCellVariable(m, sw0, BCs);
p_old = createCellVariable(m, p0, BCp);
sw = sw_old;
oil_init=domainInt(1-sw_old);
p = p_old;
uw = -gradientTerm(p_old); % an estimation of the water velocity
%% start the main loop
% generate intial pressure profile (necessary to initialize the fully
% implicit solver)
rec_fact=0;
t_day=0;
t = 0;
dt0=dt;
dsw_alwd= 0.01;
dp_alwd= 100; % Pa
while (t<t_end)
% for i=1:5
    error_p = 1e5;
    error_sw = 1e5;
    % Implicit loop
%     while ((error_p>eps_p) || (error_sw>eps_sw))
    while(1)
        % calculate parameters
        pgrad = gradientTerm(p);
%         pcgrad=gradientTerm(pc(sw));
        sw_face = upwindMean(sw, -pgrad); % average value of water saturation
        sw_grad=gradientTerm(sw);
        sw_ave=arithmeticMean(sw);
        pcgrad=dpc(sw_ave, sor, swc).*sw_grad+dpcdk(sw_ave, sor, swc).*grad_phik;
        % solve for pressure at known Sw
        labdao = lo.*funceval(kro, sw_face, kro0, sor, swc);
        labdaw = lw.*funceval(krw, sw_face, krw0, sor, swc);
        labda = labdao+labdaw;
        % compute [Jacobian] matrices
        Mdiffp1 = diffusionTerm(-labda);
        RHSpc1=divergenceTerm(labdao.*pcgrad);
        [Mbcp, RHSbcp] = boundaryCondition(BCp);
        RHS1 = RHSpc1+RHSbcp; % with capillary
        p_new=solvePDE(m, Mdiffp1+Mbcp, RHS1);

        % solve for Sw
        pgrad = gradientTerm(p_new);
        uw=-labdaw.*pgrad;
        [Mbcsw, RHSbcsw] = boundaryCondition(BCs);
        RHS_sw=-divergenceTerm(uw);
        sw_new=solveExplicitPDE(sw_old, dt, RHS_sw, BCs, phi);

        error_p = max(abs((p_new.value(:)-p.value(:))./p_new.value(:)))
        error_sw = max(abs(sw_new.value(:)-sw.value(:)))
        dt_new=dt*min(dp_alwd/error_p, dsw_alwd/error_sw);
        % assign new values of p and sw
        if error_sw>dsw_alwd
            dt=dt*(dsw_alwd/error_sw)
        else
            t=t+dt;
            p = p_new;
            sw = sw_new;
            p_old = p;
            sw_old = sw;
            dt=min(dt*(dsw_alwd/error_sw), 10*dt);
            break;
        end
    end

    rec_fact=[rec_fact (oil_init-domainInt(1-sw))/oil_init];
    t_day=[t_day t];
    figure(1);visualizeCells(1-sw); drawnow;
    figure(2); plot(t_day/3600/24, rec_fact)
    xlabel('time [day]');
    ylabel('recovery factor');
    title([num2str(t/3600/24) ' day']); drawnow;
end


% AUSM (Liou-Steffen) scheme for one-dimensional Euler equations

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory in Section 10.5 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program makes Figs. 10.17, 10.20 in the book

% Function called: f
% Programs called: problem_specification, Riemann

clear all

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

		% .....................Input............................
gamma = 1.4; 	% Ratio of specific heats
J = 48;		% Number of grid cells
bouncon = 0;	% bouncon chooses outflow boundary conditions
		% = 0: Nothing happens: infinite domain
		% = 1: Solid wall at x = 1 with direct prescription of uwall
		% = 2: Solid wall at x = 1 with reflection b.c.
		% ....................End of input........................

gammab = 1/(gamma - 1); gam1 = gamma-1; gamgam = gamma^gamma;
problem_specification

h = 1/J;  				% Cell size
dt = lambda*h;				% Time step
n = floor(tend/dt);			% Number of time-steps

% 		Definition of grid numbering
%       x=0    					 x=1
% grid   |---o---|---o---|---o---  ...  --|---o---|
%            1   1   2   2   3           J-1  J

xcenter = h*[1:J] - h/2;		% Location of cell centers

press = zeros(size(xcenter));		% Preallocation of pressure,
rhoold = press; uold = press;		%       density and velocity
rhonew = press; mnew = press;		%       momentum
totenew = press; enthalpy = press;	%	total energy and enthalpy

for j = 1:length(xcenter)		% Initial conditions
  if xcenter(j) < 0.5, press(j) = pleft; rhoold(j) = rholeft;  uold(j) = uleft;
  else,  	     press(j) = pright;  rhoold(j) = rhoright; uold(j) = uright;
  end
end

	% Initialization of cell center variables
totenold = rhoold.*(0.5*uold.*uold + gammab*press./rhoold); % Total energy rho*E
totenleft = totenold(1); totenright = totenold(J);
mold = rhoold.*uold;					    % Momentum m
c = sqrt(gamma*press./rhoold);				    % Sound speed
mach = uold./c;						    % Mach number
enthalpy =  0.5*uold.*uold + gammab*c.^2;		    % Enthalpy

machplus = mach; machminus = mach;		% Preallocation of
presplus = mach; presminus = mach;		%     	split fluxes
flux1 = zeros(J-1,1); flux2 = flux1;		% 	and Liou-Steffen
flux3 = flux1; machhalf = flux1;		%	fluxes
m1 = flux1; m2 = flux1;

t = 0;
for i = 1:n,  t = t + dt;
  for j = 1:J
    if mach(j) > 1
      machplus(j) = mach(j);  machminus(j) = 0;
      presplus(j) = press(j); presminus(j) = 0;
    elseif mach(j) < -1
      machplus(j) = 0; machminus(j) = mach(j);
      presplus(j) = 0; presminus(j) = press(j);
    else
      machplus(j)  =  0.25*(mach(j) + 1)^2;
      machminus(j) = -0.25*(mach(j) - 1)^2;
      presplus(j)  =  0.5*press(j)*(1 + mach(j));
      presminus(j) =  0.5*press(j)*(1 - mach(j));
    end
  end

	% Liou-Steffen fluxes
  for j = 1:J-1,  machhalf(j) = machplus(j) + machminus(j+1); end
  m1 = 0.5*(machhalf + abs(machhalf));  m2 = 0.5*(machhalf - abs(machhalf));
  rhoc = rhoold.*c;
  for j = 1:J-1
    flux1(j) = m1(j)*rhoc(j) + m2(j)*rhoc(j+1);
    flux2(j) = m1(j)*rhoc(j)*uold(j) +  m2(j)*rhoc(j+1)*uold(j+1) +...
      presplus(j) + presminus(j+1);
    flux3(j) =  m1(j)*rhoc(j)*enthalpy(j)  + m2(j)*rhoc(j+1)*enthalpy(j+1);
  end

	% Update of state variables
  rhonew(1)  = rholeft;  	rhonew(J)  = rhoright;
  mnew(1)    = rholeft*uleft;	mnew(J)    = rhoright*uright;
  totenew(1) = totenleft; 	totenew(J) = totenright;
  for j = 2:J-1
    rhonew(j)  = rhoold(j)   - lambda*(flux1(j) - flux1(j-1));
    mnew(j)    = mold(j)     - lambda*(flux2(j) - flux2(j-1));
    totenew(j) = totenold(j) - lambda*(flux3(j) - flux3(j-1));
  end
  uold   = mnew./rhonew;   press = gam1*(totenew - 0.5*mnew.*uold);
  rhoold = rhonew; 	totenold = totenew; 	mold = mnew;
  c = sqrt(gamma*press./rhoold); 	mach = uold./c;
  enthalpy =  0.5*uold.*uold + gammab*c.^2;
end
entropy = log(press./rhoold.^gamma);

figure(1), clf
subplot(2,3,1),hold on,title('DENSITY','fontsize',14),plot(xcenter,rhonew,'o')
subplot(2,3,2),hold on,title('VELOCITY','fontsize',14),plot(xcenter,uold,'o')
subplot(2,3,3),hold on,title('PRESSURE','fontsize',14),plot(xcenter,press,'o')
subplot(2,3,4),hold on,title('MACHNUMBER','fontsize',14),plot(xcenter,mach,'o')
subplot(2,3,5),hold on,title('ENTROPY','fontsize',14),plot(xcenter,entropy,'o')
subplot(2,3,6),axis('off'),title('Liou-Steffen scheme','fontsize',14)

Riemann		% Plot exact solution


% SPEC_FREQ_AVERAGE Calulates the frequency-averaged spectrogram
%
% Usage
%    spec = SPEC_FREQ_AVERAGE(x, filters, options)
%
% Input
%    x (numeric): The signal to be transformed.
%    filters (cell): The set of filters used to calculate the spectrogram
%       and the frequency averaging.
%    options (struct): Various options to the function:
%       options.oversampling (int): The amount of oversampling with respect to
%          the critical bandwidth, as a power of 2 (default 1).
%
% Output
%    spec (numeric): The frequency-averaged spectrogram, arranged in as a
%       first-order scattering transform.
%
% Description
%    First, the spectrogram of x is computed with respect to the second-order
%    lowpass filter filters{2}.phi, if it is present, otherwise filters{1}.phi
%    is used. Once a spectrogram is obtained, the filters in filters{1}.psi are
%    averaged with the spectrogram to each yield a time-varying coefficient.
%    The resulting signals are arranged in spec in the format of a first-order
%    scattering transform. As a result, spec{1}.signal is empty, since no
%    zeroth-order coefficients are present. However, spec{2}.signal contains
%    the frequency-averaged spectrogram coefficients, arranged in order of
%    decreasing frequeny.
%
%    The accompanying meta structure is also analogous to the scattering
%    transform. Notably, spec{2}.meta.order is equal to 1 for each coefficient
%    and spec{2}.meta.j gives the index of the filter used to compute the
%    frequency averaging.
%
% See also
%   MORLET_FILTER_BANK_1D, WAVELET_FACTORY_1D

function [out,meta] = spec_freq_average(in,filters,options)
	options = fill_struct(options,'oversampling',1);

	filters1 = filters{1};
	filters2 = filters{min(2,numel(filters))};

	[temp1,temp2,phi2_bw] = filter_freq(filters{2}.meta);

	supp_mult = 4;

	N = size(in,1);
	sig_count = size(in,3);
	Nfilt = filters1.meta.size_filter;
	N1 = 2^round(log2(2*pi/phi2_bw));

	fs = zeros(N1*supp_mult,length(filters1.psi.filter));

	for k1 = 0:length(filters1.psi.filter)-1
		f_temp = realize_filter(filters1.psi.filter{k1+1});
		fs(:,k1+1) = abs(f_temp(1:Nfilt/(N1*supp_mult):Nfilt));
	end

	window = ifft(realize_filter(filters2.phi.filter));
	window = [window(Nfilt-N1*supp_mult/2+1:Nfilt); window(1:N1*supp_mult/2)];

	frames = zeros(length(window),round(N/N1*2^options.oversampling),sig_count);

	out = zeros(size(frames,2),size(fs,2),sig_count);

	for t = 0:size(out,1)-1
		ind = round(t*N1/2^options.oversampling+[-N1*supp_mult/2:N1*supp_mult/2-1]+1);

		% symmetric extension
		ind(ind<1) = 1-ind(ind<1);
		ind(ind>N) = 2*N+1-ind(ind>N);

		frames(:,t+1,:) = in(ind,:);
	end

	frames = bsxfun(@times,frames,window);

	frame_fm = abs(fft(frames,[],1));

	out = fs.'*reshape(frame_fm,[size(frame_fm,1) size(frame_fm,2)*size(frame_fm,3)]);
	out = reshape(out,[size(fs,2) size(frame_fm,2) size(frame_fm,3)]);

	resolution = round(log2(N1))-options.oversampling;

	meta.order = ones(1,size(fs,2));
	meta.scale = [0:size(fs,2)-1];
	meta.bandwidth = phi2_bw*ones(1,size(fs,2));
	meta.resolution = resolution*ones(1,size(fs,2));

	X = cell(1,2);
	X{1}.signal = {};
	X{1}.meta.bandwidth = zeros(1,0);
	X{1}.meta.scale = zeros(1,0);

	X{2}.signal = cell(1,size(out,1));
	X{2}.meta = meta;

	for k0 = 0:size(out,1)-1
		X{2}.signal{k0+1} = reshape(out(k0+1,:,:),[size(out,2) 1 size(out,3)]);
	end

	out = X;
end


%DEMO_REGRESSION_HIER  Hierarchical regression demonstration using
%                      Rats data
%
%  Description
%    The example data is taken from section 6 of Gelfand et al
%    (1990) (also used in WinBUGS/OpenBUGS), and concerns 30 young
%    rats whose weights were measured weekly for five week. This
%    demo demosntrates how to make hierarchical linear and
%    non-linear models.
%
%  Reference
%    Gelfand, A. E., Hills, S. E., Racine-Poon, A. and Smith, A. F.
%    M. (1990) Illustration of Bayesian Inference in Normal Data
%    Models Using Gibbs Sampling. Journal of the American
%    Statistical Association 85(412):972-985.
%

% Copyright (c) 2010 Aki Vehtari

% 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.

S = which('demo_regression_hier');
L = strrep(S,'demo_regression_hier.m','demodata/rats.mat');
data=load(L);
xx = data.x;
yy = data.y;
% Show data : 5 weight measurements per rat for 30 rats
figure
plot(xx,yy,'o-')
axis([7 37 100 400])
title('Data')
drawnow

% Reshape data
ntime = size(xx,2);
nrats = size(yy,1);
% All y's to one vector
y=yy(:);
% Repeat x for each rat
x=reshape(repmat(xx,nrats,1),ntime*nrats,1);
% Add ratid
x=[x repmat([1:nrats]',ntime,1)];
% Now 'x' consist of the inputs (ratid,time) and 'y' of the output (weight).
% Normalize x and y
[xn,xmean,xstd]=normdata(x);
[yn,ymean,ystd]=normdata(y);

% optmization options
opt=optimset('TolFun',1e-4,'TolX',1e-4,'Display','on');

% common Gaussian likelihood with weakly informative prior for variance
lik=lik_gaussian('sigma2',.1,...
                 'sigma2_prior',prior_sinvchi2('s2',0.01,'nu',1));
% common categorical covariance term
cc=gpcf_cat('selectedVariables',2);

% 1) Linear model with intercept and slope wrt time
disp('1) Linear model with intercept and slope wrt time')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfl});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,1)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Linear model')
drawnow

% 2) Linear model with hierarchical intercept
disp('2) Linear model with hierarchical intercept')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,2)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Linear model with hierarchical intercept')
drawnow

% 3) Linear model with hierarchical intercept and slope
disp('3) Linear model with hierarchical intercept and slope')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% linear covariance term for each rat
cfli=gpcf_prod('cf',{cfl cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl cfli});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,3)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Linear model with hierarchical intercept and slope')
drawnow

% 4) Nonlinear model with hierarchical intercept
% include linear part, too
disp('4) Nonlinear model with hierarchical intercept')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part
cfs=gpcf_sexp('selectedVariables',1);
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl cfs});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,4)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear model with hierarchical intercept')
drawnow

% 5) Nonlinear model with hierarchical intercept and curve
% include linear part, too
disp('5) Non-linear hierarchical model 1 with MAP')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
cfl=gpcf_linear('coeffSigma2',1,'selectedVariables',1,...
                'coeffSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% linear covariance term for each rat
cfli=gpcf_prod('cf',{cfl cc});
% nonlinear part
cfs=gpcf_sexp('selectedVariables',1);
% nonlinear covariance term for each rat
cfsi=gpcf_prod('cf',{cfs cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfl cfli cfs cfsi},...
          'jitterSigma2',1e-6);
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,5)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 1 with MAP')
drawnow

% 6) With increasing flexibility of the modeling function
%    we need to integrate over the parameteres
% integrate over parameters
disp('6) Non-linear hierarchical model 1 with IA')
[gps,pth,th]=gp_ia(gp,xn,yn);
% predict and plot
Ef=gp_pred(gps,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,6)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 1 with IA')
drawnow

% 7) Nonlinear model with hierarchical intercept and curve
%    Same as 5, but with no linear and product covariances
disp('7) Non-linear hierarchical model 2 with MAP')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part with delta distance for ratid
cfs=gpcf_sexp('metric',metric_euclidean('components',{[1] [2]},...
                                        'deltadist', [0 1], ...
                                        'lengthScale_prior',prior_t()));
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfs});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% predict and plot
Ef=gp_pred(gp,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,7)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 2 with MAP')
drawnow

% 8) With increasing flexibility of the modeling function
%    we need to integrate over the parameteres
% integrate over parameters
disp('8) Non-linear hierarchical model 2 with IA')
gps=gp_ia(gp,xn,yn);
% predict and plot
Ef=gp_pred(gps,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,8)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 2 with IA')
drawnow

% 9) With neuralnetwork covariance and integration over the parameters
disp('9) Non-linear hierarchical model 3 with IA')
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_t());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part with neuralnetwork covariance
cfnn=gpcf_neuralnetwork('selectedVariables',1,'biasSigma2_prior',prior_t(),...
                        'weightSigma2_prior',prior_t());
% nonlinear covariance term for each rat
cfnni=gpcf_prod('cf',{cfnn cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfnn cfnni});
% optimize
gp=gp_optim(gp,xn,yn,'opt',opt);
% integrate over parameters
gps=gp_ia(gp,xn,yn);
% predict and plot
Ef=gp_pred(gps,xn,yn,xn);
Eff=reshape(denormdata(Ef,ymean,ystd),nrats,ntime);
subplot(3,3,9)
plot(xx,Eff,'o-')
axis([7 37 100 400])
title('Non-linear hierarchical model 3 with IA')
drawnow

%*** Missing Data Example ***
% In the original paper (Gelfand et al, 1990) data was also to
% demonstrate missing data handling, by removing 1-4 weeks of data
% for part of the rats. Handling missing data in this case is trivial
% for GPs, too

disp('10) Missing data example')
S = which('demo_regression_hier');
L = strrep(S,'demo_regression_hier.m','demodata/rats.mat');
data=load(L);
xx = data.x;
yy = data.y;
yymiss = data.ymiss;
% Show data : 5 weight measurements per rat for 30 rats
figure
plot(xx,yymiss,'o-')
axis([7 37 100 400])
title('Data')
% Reshape data
ntime = size(xx,2);
nrats = size(yy,1);
% All y's to one vector
ym=yymiss(:);
% Repeat x for each rat
xm=reshape(repmat(xx,nrats,1),ntime*nrats,1);
% Add ratid
xm=[xm repmat([1:nrats]',ntime,1)];
% Now 'x' consist of the inputs (ratid,time) and 'y' of the output (weight).
% Normalize x and y
[xmn,xmmean,xmstd]=normdata(xm);
[ymn,ymmean,ymstd]=normdata(ym);
% test x is the complete x
xmnt=xmn;
% remove missing data from the training data
missi=isnan(ym);
ymn(missi,:)=[];
xmn(missi,:)=[];

% 10) neuralnetwork covariance, IA and missing data
cfc=gpcf_constant('constSigma2',1,'constSigma2_prior',prior_gaussian());
% own constant term for each rat
cfci=gpcf_prod('cf',{cfc cc});
% nonlinear part with neuralnetwork covariance
cfnn=gpcf_neuralnetwork('selectedVariables',1,'biasSigma2_prior',prior_gaussian('s2',10));
% nonlinear covariance term for each rat
cfnni=gpcf_prod('cf',{cfnn cc});
% construct GP
gp=gp_set('lik',lik,'cf',{cfc cfci cfnn cfnni});
% optimize
gp=gp_optim(gp,xmn,ymn);
% integrate over parameters
gps=gp_ia(gp,xmn,ymn);
% predict and plot
Ef=gp_pred(gps,xmn,ymn,xmnt);
Eff=reshape(denormdata(Ef,ymmean,ymstd),nrats,ntime);
Effc=Eff;Effc(isnan(ym))=NaN;
plot(xx,Effc,'bo-',xx,Eff,'bo--')
axis([7 37 100 400])
title('Non-linear hierarchical model 3 with IA and missing data')


function perimeterEdges=findPerimeter(mesh)
% function perimeterPoints=findPerimeter(mesh,faceIndexList)
% Finds perimeter points.
% Works by looking at the connection matrix (which is a list of edges)
% The perimeter points are those points that are on edges that are used by only one face

%nVerts=length(mesh.uniqueVertices);

% Create a list of edges...
[edgeList1,edgeList2]=find(triu(mesh.connectionMatrix));

edgeList=[edgeList1,edgeList2];

disp ('Number of edges found:');
numUniqueEdges=length(edgeList);

disp (length(edgeList));

sortedEdgeList=sort(edgeList')';
sortedEdgeList=sub2ind([numUniqueEdges,numUniqueEdges],sortedEdgeList(:,1),sortedEdgeList(:,2));

% Now convert the list of faces into three sets of (sorted!) edges
% And convert the x,y coordinates into unique indices
FaceEdges1=[mesh.uniqueFaceIndexList(:,1),mesh.uniqueFaceIndexList(:,2)];
FaceEdges2=[mesh.uniqueFaceIndexList(:,1),mesh.uniqueFaceIndexList(:,3)];
FaceEdges3=[mesh.uniqueFaceIndexList(:,2),mesh.uniqueFaceIndexList(:,3)];

% Sort them so that [a b] and [b a] are seen as the same edge.
sortedFaceEdges1=sort(FaceEdges1')';
sortedFaceEdges2=sort(FaceEdges2')';
sortedFaceEdges3=sort(FaceEdges3')';

% Make them into unique index numbers to help searching and sorting
[FaceEdges1]=sub2ind([numUniqueEdges,numUniqueEdges],sortedFaceEdges1(:,1),sortedFaceEdges1(:,2));
[FaceEdges2]=sub2ind([numUniqueEdges,numUniqueEdges],sortedFaceEdges2(:,1),sortedFaceEdges2(:,2));
[FaceEdges3]=sub2ind([numUniqueEdges,numUniqueEdges],sortedFaceEdges3(:,1),sortedFaceEdges3(:,2));

% concatenate the list of Face edges into one long list of indices
FaceEdges=[FaceEdges1,FaceEdges2,FaceEdges3]';

% Sort them - should produce doublets of most edges 'cos most edges are part of two faces
sortedFaceEdges=sort(FaceEdges(:));

% SortedFaceEdges is a sorted list of all the edges in the face list. If all the edges are
% members of two faces, it'll just be pairs of numbers.
% Lone edges (part of only one face) will be on their own here as well.
% So the list might look like
% 1,1,2,2,3,3,4,4,5,5,6,6,7,8,9,9,10,10....
% Where edge 7 and edge 8 are lone...
% if n(i)==n(i+1) or n(i)==n(i-1) then n(i) is an internal edge
% So in fact we're looking for cases where the above fails...
diffFromUpper=sortedFaceEdges-shift(sortedFaceEdges,[1,0]);
diffFromLower=sortedFaceEdges-shift(sortedFaceEdges,[-1,0]);
loneEdges=sortedFaceEdges(find(diffFromUpper.*diffFromLower));

[a b c]=unique(sortedFaceEdges(:));
% Just a quick check:
% 'a' tells you what unique edges there are in the faces. This should be the same as edgeList

nFound=length(loneEdges);
if (nFound>0)
   disp(nFound);
   disp ('Perimeter edges found');

% In the test cases, the mesh is closed so there's no perimeter, loneEdges should be empty
% But let's pretend we've found some...
[perimeterEdges(:,1),perimeterEdges(:,2)]=ind2sub([numUniqueEdges,numUniqueEdges],loneEdges);
else
   disp ('No Perimeter edges found - mesh is closed');
   perimeterEdges=[];
end


function [grp_phase, cep, ts] = modified_group_delay_feature(file_name, rho, gamma, num_coeff, frame_shift)

%input:
%     file_name: path for the waveform. The waveform should have a header
%     rho: a parameter to control the shape of modified group delay spectra
%     gamma: a parameter to control the shape of the modified group delay spectra
%     num_coeff: the desired feature dimension
%     [frame_shift]:
%
%output:
%     grp_phase: modifed gropu delay spectrogram
%     cep: modified group delay cepstral feature.
%     ts: time instants at the center of each analysis frame.
%
%Example:
%     [grp_phase, cep, ts] = modified_group_delay_feature('./100001.wav', 0.4, 0.9, 12);
% Please tune rho and gamma for better performance
%     See also: howtos/HOWTO_features.m
%
% by Zhizheng Wu (zhizheng.wu@ed.ac.uk)
% http://www.zhizheng.org
%
% The code has been used in the following three papers:
% Zhizheng Wu, Xiong Xiao, Eng Siong Chng, Haizhou Li, "Synthetic speech detection using temporal modulation feature", IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP) 2013.
% Zhizheng Wu, Tomi Kinnunen, Eng Siong Chng, Haizhou Li, Eliathamby Ambikairajah, "A study on spoofing attack in state-of-the-art speaker verification: the telephone speech case", Asia-Pacific Signal and Information Processing Association Annual Summit and Conference (APSIPA ASC) 2012.
% Zhizheng Wu, Eng Siong Chng, Haizhou Li, "Detecting Converted Speech and Natural Speech for anti-Spoofing Attack in Speaker Recognition", Interspeech 2012.
%
% feel free to modify the code and welcome to cite above papers :)

[speech,fs]  = wavread(file_name);

if nargin<2;
    rho = 0.4;
end
if nargin<3;
    gamma = 0.9;
end
if nargin<4;
    num_coeff = 12;
end
frame_length = 0.025; %msec
if nargin<5;
    frame_shift  = 0.010; %msec
end
NFFT         = 512;
pre_emph     = true;

%%% Pre-emphasis + framing
if (pre_emph)
    speech = filter([1 -0.97], 1, speech);
end;
frame_length = round((frame_length)*fs);
frame_shift = round((frame_shift)*fs);
[frames, ts] = enframe(speech, hamming(frame_length), frame_shift);
ts = (ts-1)/fs;

frame_num    = size(frames, 1);
frame_length = size(frames, 2);
delay_vector = [1:1:frame_length];
delay_matrix = repmat(delay_vector, frame_num, 1);

delay_frames = frames .* delay_matrix;

x_spec = fft(frames', NFFT);
y_spec = fft(delay_frames', NFFT);
x_spec = x_spec(1:NFFT/2+1, :);
y_spec = y_spec(1:NFFT/2+1, :);

temp_x_spec = abs(x_spec);

dct_spec = dct(medfilt1(log(temp_x_spec), 5));
smooth_spec = idct(dct_spec(1:30,:), NFFT/2+1);

grp_phase1 = (real(x_spec).*real(y_spec) + imag(y_spec) .* imag(x_spec)) ./(exp(smooth_spec).^ (2*rho));
grp_phase = (grp_phase1 ./ abs(grp_phase1)) .* (abs(grp_phase1).^ gamma);
grp_phase = grp_phase ./ (max(max(abs(grp_phase))));

grp_phase(isnan(grp_phase)) = 0.0;

cep = dct(grp_phase);
cep = cep(2:num_coeff+1, :)';


% NOLM_Bifurcation_Diagram.
% Copyright Springer 2013 A.L. Steele and S. Lynch.
clear
N=49999;halfN=N/2;
lambda=1.55E-6;n2=3.2E-20;Aeff=30E-12;L=80;
E1(1)=0;Pmax=40;phi=0;Gauss(1)=0;
Pin(1:N)=0; Pout(1:N)=0;
phif=0*pi; E6p=0;
kappa1=0.25;kappa2=0.8;kappa3=0.8;
rootk1 = sqrt(kappa1); irootk1 = 1i*sqrt(1-kappa1);
rootk2 = sqrt(kappa2); irootk2 = 1i*sqrt(1-kappa2);
rootk3 = sqrt(kappa3); irootk3 = 1i*sqrt(1-kappa3);
G=sqrt((1-kappa2)*(1-kappa3));
% Ramp the power up and down
for n=1:N

    Ein = sqrt(Pmax*exp(-0.02*((n*Pmax/N-Pmax/2))^2));
    E1 = rootk3*Ein+irootk3*E6p;
    P1 = abs(E1)^2;

    E3 = rootk1*E1;
    E4 = irootk1*E1;

    phic=2*pi*n2*L*(2-kappa1)*P1/(lambda*Aeff);
    phicc=2*pi*n2*L*(1+kappa1)*P1/(lambda*Aeff);

    E3p = E3*exp(-1i*(phi+phic));
    E4p = E4*exp(-1i*(phi+phicc));

    E5 = rootk1*E3p+irootk1*E4p;

    Pout(n) = abs(rootk2*E5)^2;
    Pin(n) = abs(Ein)^2;
    E6 = irootk2*E5;

    E6p = E6*exp(1i*phif);

end

% Plot the bifurcation diagrams
figure(1)
clf
fsize=15;
subplot(2,1,1)
hold on
plot(Pout(1:N),'.','MarkerSize',1)
plot(Pin(1:N),'.','MarkerSize',1);
xlabel('Number of Ring Passes','FontSize',fsize);
ylabel('Output Power','FontSize',fsize);
hold off

subplot(2,1,2)
hold on
plot(Pin,Pout,'.','MarkerSize',1);
xlabel('Input Power','FontSize',fsize);
ylabel('Output Power','FontSize',fsize);
hold off


% GPCG algorithm adapted for NMF
function X = gpcg_nmf(Y,A,X,MaxIter,CostFun,Alpha)
% Our implementation of GPCG;

[R,T] = size(X); [M,T] = size(Y);
beta = 0.5; mu = 0.01; MaxSearchIter = 10;
Diff_DF_max = 0; gamma_GP = 0.01;

if CostFun == 1
   H = spalloc(R*T,R*T,T*R^2);
   Hx = A'*A;
   H = kron(speye(T),Hx); % Hessian
end

for k = 1:MaxIter

% Step 1
    Xp = X;
    if CostFun ~= 1
       H = CostFunHess(Y,A,X,Alpha); % Hessian
    end
    P = -CostFunGrad(Y,A,X,CostFun,Alpha); % Gradient Matrix
    p = P(:); % Vectorization

% Step 2
     eta = (p'*p)/(p'*(H*p));
     for i = 0:MaxSearchIter % Armijo rule
         eta = eta*beta.^i;
         X = max(X + eta*P,0);
         if (CostFunEval(Y,A,X,CostFun,Alpha) - CostFunEval(Y,A,Xp,CostFun,Alpha)) <=  (norm(X - Xp,'fro')*mu/eta)
             break;
         end
     end

% Step 3
  Z = zeros(size(X));
  Z(X > eps) = 1;
  z = vec(Z);

% Step 4
  Pc = CostFunGrad(Y,A,X,CostFun,Alpha); % Gradient Matrix
  pc = Pc(:); % Vectorization

% Step 5
  pR = z.*pc; % Reduced gradient
  if CostFun ~= 1
     H = CostFunHess(Y,A,X,Alpha); % Hessian
  end

HR = repmat(z,1,R*T).*H.*repmat(z',R*T,1) + speye(R*T) - spdiags(z,0,R*T,R*T); % reduced Hessian

% Step 6
 [pc,flag,relres,iter,resvec] = pcg(HR,-pR);
 P = reshape(pc,R,T); % Matricization

 % Step 7
     DF = CostFunEval(Y,A,X,CostFun,Alpha);
     eta = 1;
     for j = 0:MaxSearchIter % Armijo rule
         eta = eta*beta.^j;
         X = max(X + eta*P,0);
         if (CostFunEval(Y,A,X,CostFun,Alpha) < DF)
             break;
         end
     end

   Fn(k) = CostFunEval(Y,A,X,CostFun,Alpha);

 % Stopping criterion
   DF_old = norm(Y - A*Xp,'fro');  DF = norm(Y - A*X,'fro');
   Diff_DF = DF_old - DF;  Diff_DF_max = max(Diff_DF,Diff_DF_max);
   if (Diff_DF <= gamma_GP*Diff_DF_max) & (k > 1)
       break;
   end

end

% Cost Function
function F = CostFunEval(Y,A,X,CostFun,Alpha);

 switch CostFun

     case 1 % Eucliden distance

         F = norm(Y - A*X,'fro');

     case 2 % Alpha divergence

         Z = A*X + 1E2*eps;
         Y = Y + 1E2*eps;
         if Alpha == 1 % KL divergence
            F = sum(sum(Y.*log(Y./Z) + Z - Y));
         elseif Alpha == 0 % Dual KL divergence
            F = sum(sum(Z.*log(Z./(Y+eps)) + Y - Z));
         else % Alpha-divergence
            F = (1/Alpha)*sum(sum(Y.*( (Y./Z).^(Alpha - 1) - 1)/(Alpha - 1) + Z - Y));
         end
 end

% Gradient of Cost Function
function G = CostFunGrad(Y,A,X,CostFun,Alpha);

 switch CostFun

     case 1 % Eucliden distance

         G = A'*(A*X - Y);

     case 2 % Alpha divergence

         Z = A*X+1E2*eps;
         if ~Alpha % Dual KL divergence
            G = A'*log(Z./(Y + 1E2*eps));
         else
            G = (1/Alpha)*A'*(1 - ((Y+1E2*eps)./Z).^Alpha);
         end
  end

% Hessian of Cost Function
function H = CostFunHess(Y,A,X,Alpha);

[R,T] = size(X);
M = size(Y,1);
H = spalloc(R*T,R*T,T*R^2);
Z = A*X+1E2*eps;
if ~Alpha % Dual KL divergence
    Zx = 1./Z + 1E2*eps;
else
    Zx = ((Y+1E2*eps).^Alpha)./(Z.^(Alpha + 1));
end
 for t = 1:T
     H(((t-1)*R+1):t*R,((t-1)*R+1):t*R) = A'*repmat(Zx(:,t),1,M)*A;
 end


function [x,fval,exitflag,output]=cplexmiqcp(H, f, Aineq, bineq, Aeq, beq, l, Q, r, sostype, sosind, soswt, lb, ub, ctype, x0, options)
%%
% Purpose
% Solve quadratically constrained linear or quadratic integer programming
% problems.
%
% Syntax
%    x = cplexmiqcp(H,f,Aineq,bineq)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,
%        ub)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,
%        ub,ctype,x0)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,
%        ub,ctype,x0,options)
%    x = cplexmiqcp(problem)
%    [x,fval] = cplexmiqcp(...)
%    [x,fval,exitflag] = cplexmiqcp(...)
%    [x,fval,exitflag,output] = cplexmiqcp(...)
%    [x,fval,exitflag,output,lambda] = cplexmiqcp(...)
%
% Description
% Finds the minimum of a problem specified by
%    min      0.5*x'*H*x+f*x or f*x
%    st.      Aineq*x      <= bineq
%             Aeq*x         = beq
%             l*x + x'*Q*x <= r
%             lb <= x <= ub
%             x belongs to BICSN
%
% f, bineq, beq, l, r, lb and ub are column vectors.
% H, Aineq, Aeq, and Q are matrices.
% x is a BICSN vector -- that is, its individual entries are each required
% to be binary, general integer, continuous, semi-continuous or
% semi-integer.
%
% x = cplexmiqcp(H,f,Aineq,bineq) solves the mixed integer programming
% problem min 1/2*x'*H*x + f*x subject to Aineq*x <= bineq. If no
% quadratic objective term exists, set H=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq) solves the preceding problem
% while additionally satisfying the equality constraints Aeq*x = beq. If no
% inequalities exist, set Aineq=[] and bineq=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r) solves the preceding
% problem while additionally satisfying the quadratic inequality
% constraints l*x + x'*Q*x <= r. If no equalities exist, set Aeq=[] and beq=[].
%
% x = cplexmiqcp(f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt) solves
% the preceding problem with the additional requirement that the SOS
% constraints are satisfied. If no quadratic inequalities exist, set l=[],
% Q=[] and r=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,ub)
% defines a set of lower and upper bounds on the design variables, x, so
% that the solution is in the range lb <= x <= ub. If no SOS constraints
% exist, set sostype=[],sosind=[] and soswt=[].
%
% x =
% cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,ub,ctype
% ) defines the types for each of the design variables. If no bounds exist,
% set lb=[] and ub=[].
%
% x =
% cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,ub,x0)
% sets the starting point to x0. If all design variables are continuous,
% set ctype=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,lb,ub,ctype,x0,options) minimizes
% with the optimization options specified in the structure options, which
% can be created using the function cplexoptimset If you do not wish to
% give an initial point, set x0=[].
%
% x = cplexmiqcp(problem) where problem is a structure.
%
% [x,fval] = cplexmiqcp(...) returns the value of the objective function at
% the solution x: fval = 0.5*x'*H*x + f*x.
%
% [x,fval,exitflag] = cplexmiqcp(...) returns a value exitflag that
% describes the exit condition of cplexmiqcp.
%
% [x,fval,exitflag,output] = cplexmiqcp(...) returns a structure output
% that contains information about the optimization.
%
% Input Arguments
% H         Double matrix for objective function
% f         Double column vector for objective function
% Aineq     Double matrix for linear inequality constraints
% bineq     Double column vector for linear inequality constraints
% Aeq       Double matrix for linear equality constraints
% beq       Double column vector for linear equality constraints
% l         Double column vector or matrix
%           Linear part of quadratic constraints
% Q         Double matrix or double matrix cell for quadratic
%           constraints
% r         Double or double row vector
%           Righthand side of quadratic inequality constraints
% sostype   String with possible char values  '1', '2'
% sosind    Double column vector or column vector cell of indices for
%           the SOSs to be added
% soswt     Double column vector or column vector cell of weights for
%           the SOSs to be added
% lb        Double column vector of lower bounds
% ub        Double column vector of upper bounds
% ctype     String with possible char values 'B','I','C','S','N'
%           ctype(j) to 'B', 'I','C', 'S', or 'N' to indicate
%           that x(j) should be binary, general integer,
%           continuous, semi-continuous or semi-integer
%           (respectively).
% x0        Double column vector for initial point for x
% options   Options structure created with cplexoptimset
%
% problem   Structure containing the following fields:
%           H         Double matrix for objective function
%           f         Double column vector for objective function
%           Aineq     Double matrix for linear inequality constraints
%           bineq     Double column vector for linear inequality constraints
%           Aeq       Double matrix for linear equality constraints
%           beq       Double column vector for linear equality constraints
%      	    qc 	      Struct vector
%  	        qc(i).a 	 Double column vector for linear part of the
%                        quadratic constraint
%  	        qc(i).rhs    Double for righthand side for quadratic constraint
%  	        qc(i).Q 	 Double matrix for quadratic part of the
%                        quadratic constraint
%        	sos 	  Struct vector representing the SOSs
%  	        sos(i).type  String with possible char values  '1', '2'
%       	sos(i).ind   Double column vector of indices for the SOSs
%                        to be added
%  	        sos(i).wt    Double column vector of weights for the SOSs
%                        to be added
%           lb        Double column vector of lower bounds
%           ub        Double column vector of upper bounds
%           ctype     String with possible char values 'B','I','C','S','N'
%                     ctype(j) to 'B', 'I','C', 'S', or 'N' to indicate
%                     that x(j) should be binary, general integer,
%                     continuous, semi-continuous or semi-integer
%                     (respectively).
%           x0        Double column vector for initial point for x
%           options   Options structure created with cplexoptimset
%
% Output Arguments
% x         Solution found by the optimization function. If exitflag > 0,
%           then x is a solution; otherwise, x is the value of the
%           optimization routine when it terminated prematurely.
% fval      Value of the objective function at the solution x
% exitflag  Integer identifying the reason the optimization algorithm
%           terminated
% output    Structure containing information about the optimization. The
%           fields of the structure are:
%           iterations         Number of iterations
%           algorithm          Optimization algorithm used
%           message            Exit message
%           time               Execution time of the algorithm
%           cplexstatus        Status code of the solution
%           cplexstatusstring  Status string of the solution
%
%
%  See also cplexoptimset
%

% ---------------------------------------------------------------------------
% File: cplexmiqcp.m
% ---------------------------------------------------------------------------
% Licensed Materials - Property of IBM
% 5725-A06 5725-A29 5724-Y48 5724-Y49 5724-Y54 5724-Y55
% Copyright IBM Corporation 2008, 2010. All Rights Reserved.
%
% US Government Users Restricted Rights - Use, duplication or
% disclosure restricted by GSA ADP Schedule Contract with
% IBM Corp.
% ---------------------------------------------------------------------------


% dftfilt() - discrete Fourier filter
%
% Usage:
%   >> b = dftfilt(n,W,c,k,q)
%
% Inputs:
%   n - number of input samples
%   W - maximum angular freq. relative to n, 0 < W <= .5
%   c - cycles
%   k - oversampling
%   q - [0;1] 0->fft, 1->c cycles
%
% Authors: Sigurd Enghoff, Arnaud Delorme & Scott Makeig,
%          SCCN/INC/UCSD, La Jolla, 8/1/98

% Copyright (C) 8/1/98 Sigurd Enghoff & Scott Makei, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% 01-25-02 reformated help & license -ad

% future developments
% -------------------
% input into dftfilt:
% - lowfreq and maxfreq (of interest)
% - lowcycle and maxcyle (ex: 3 cycles at low freq and 10 cycles at maxfreq)
% - the delta in frequency: ex 0.5 Hz
% The function should: compute the number of points (len) automatically
% Warning with FFT compatibility
% Still, something has to be done about the masking so that it would be comaptible

function b = dftfilt(len,maxfreq,cycle,oversmp,wavfact)

count = 1;
for index = 1:1/oversmp:maxfreq*len/cycle % scan frequencies
	w(:,count) = j * index * cycle * linspace(-pi+2*pi/len, pi-2*pi/len, len)'; % exp(-w) is a sinus curve
	count = count+1; % -2*pi/len ensures that we really scan from -pi to pi without redundance (-pi=+pi)
end;
b = exp(-w);

%srate = 2*pi/len;						    % Angular increment.
%w = j * cycle * [0:srate:2*pi-srate/2]';	% Column.
%x = 1:1/oversmp:maxfreq*len/cycle;		    % Row.
%b = exp(-w*x);					            % Exponentiation of outer product.

for i = 1:size(b,2),
	m  = round(wavfact*len*(i-1)/(i+oversmp-1));	% Number of elements to discard.
	mu = round(m/2);				                % Number of upper elemnts.
	ml = m-round(m/2);				                % Number of lower elemnts.
	b(:,i) = b(:,i) .* [zeros(mu,1) ; hanning(len-m) ; zeros(ml,1)];
end

% syemtric hanning function
function w = hanning(n)
if ~rem(n,2)
   w = .5*(1 - cos(2*pi*(1:n/2)'/(n+1)));
   w = [w; w(end:-1:1)];
else
   w = .5*(1 - cos(2*pi*(1:(n+1)/2)'/(n+1)));
   w = [w; w(end-1:-1:1)];
end


function u_est = CG_MultiScale_LAP3D(I1, I2, FilterSizes, PreFilt, MedFilt, uin)
% The function implements a multi-scale framework for the 3D version of the
% LAP optical flow algorithm. Instead of downsampling the images, the
% framework changes the size of the all-pass filters used in the LAP
% algorithm. The filter basis used in the LAP algorithm spans the
% derivatives of a Gaussian filter.
% Note that this implementation is for greyscale images only.
%
% Input parameters:
%       I1 and I2   -> Input images of size M by N (Greyscale)
%       FilterSizes -> Controls the size of the filters (vector with
%                      values for R
%                      i.e. filter size is [2R + 1] by [2R + 1]
%       PreFilt     -> Optional parameter to decide whether to highpass
%                       filter the images. (Default = 1 (Yes), 0 = (No))
%       MedFilt     -> Optional parameter to decide whether to median
%                       filter the flow at fine scales. (Default = 1 (Yes), 0 = (No))
%
% Outputs:
%       u_est       -> Estimate of the optical flow
%
%
% Date: 08/07/2015, Author: Chris Gilliam
%
% modifications for MRI application:
% - different input parameter: Filter size instead of upper parameter R
% - initialization in single precision (except filter kernel)
% 26.08.2015, Verena Neumann/Thomas Kuestner

if nargin <= 3,
    PreFilt = 1;
    MedFilt = 1;
elseif nargin <= 4,
    MedFilt = 1;
end

% Obtain the dimensions of the images:
[M, N, P] = size(I1);

% Initialise filter functions:
funs = Filter_Functions;

% Local Gaussian filter (used in high pass filtering)
h = exp(-(-2:2).^2/2);
h = h./sum(h(:));

% Initial optical flow estimate
if(nargin < 6)
    u_holder = repmat({zeros(M,N,P,'single')}, 1, 3);
else
    u_holder = uin;
end
% u_holder = repmat({zeros(M,N,P)}, 1, 3);

% define half support of filters (i.e. R)
amp_array = FilterSizes;    % 2.^(Level_Max:-1:Level_Min);

% Initialise local counter:
num_level = 0;

% Local I1 variable:
if PreFilt == 1,
    im1 = I1 - funs.Filter_General(I1, h); % High pass filtering using gaussian filter h
%       im1 = funs.Filter_Laplacian(im1);  % high-pass using generalised Laplacian filter
else
    im1 = I1;
end

% Start estimating the optical flow
for l = 1:length(amp_array),
    num_level = num_level + 1;
    disp(['Level ', int2str(num_level), '/', int2str(size(amp_array,2))]);

    % Define filter parameter R at each iteration
    amp_size = amp_array(l);

    % Load Filter Basis:
    Basis_Set = loadbasis(3,amp_size);

    if l == 1,
        % No warping for first iteration:
        if PreFilt == 1,
            I2_shift = I2 - funs.Filter_General(I2, h); % High pass filtering using gaussian filter h
%             I2_shift = funs.Filter_Laplacian(I2);       % high-pass using generalised Laplacian filter
        else
            I2_shift = I2;
        end
    else
        % Warp I1 closer to I2 using current optical flow estimate
        if amp_size > 2,
            I2_shift = imshift_3D(I2,{-u_holder{1}, -u_holder{2}, -u_holder{3}}, 'shiftedlinear');
        else
            I2_shift = imshift_3D(I2,{-u_holder{1}, -u_holder{2}, -u_holder{3}});
        end
        if PreFilt == 1,
            I2_shift = I2_shift - funs.Filter_General(I2_shift, h); % High pass filtering using gaussian filter h
%           I2_shift = funs.Filter_Laplacian(I2_shift); % high-pass using generalised Laplacian filter
        end
    end

    % Using basis functions estimate optical flow on a local scale:
    [uest_Orig, ~] = optiflowFilter3D(im1, I2_shift, Basis_Set);

    % Clean optical flow
    % Stage 1: Remove nan's in the optical flow using inpainting:
    if sum(isnan(uest_Orig{1}(:))) >= M*N*P,
        error('All NaN. Suggest reduce Level_Num');
    end
    [uest_Clean, ~] = cleanOF3D(uest_Orig);

    % Stage 2: Remove flow elements that corresponding to large warping
    % errors.
    R = round(2*amp_size);
    k1 = -R:R;
    uest_Clean = cellfun(@funs.Filter_Gauss, uest_Clean, {R,R,R}, {k1,k1,k1}, 'uni',false);

    % Rescale optical flow and add to estimate
    u_holder = cellfun(@plus,u_holder,uest_Clean,'uniformoutput',false);

    % Refinement of OF at highest level based on errors in shifted
    %           images
    if amp_size <= 2 && MedFilt == 1,
        u_holder = Cleaning_Procedure(u_holder);
    end

end

u_est = u_holder;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%% Embedded functions %%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u_out = Cleaning_Procedure(u_in)
% function cleans the estimate of the optical flow. The cleaning process
% comprises a robust smoothing stage using two Median filters (of different
% sizes)
%

% Define size of median filters
B1 = 11;
B2 = 3;

% Two part median filtering:
% fine scale
u_out = cellfun(@medfilt3, u_in, {B2,B2,B2}, {'symmetric','symmetric','symmetric'},'uni',false);
% coarse scale
u_out = cellfun(@medfilt3, u_out, {B1,B1,B1}, {'symmetric','symmetric','symmetric'},'uni',false);
end


function pass = test_multipleOutputs(pref)
% This test ensures that the STRINGPARSER class is doing the correct thing when
% called with multiple outputs.

%% BVPS

% Strings we want to test.
str = { 'u(-1) = 1'
    'u(0)=1, u(2) + x = 3'
    'u = 1,v = 0,w = 3'
    'x + diff(sin(v))'
    'sum(u,0,.5) = 0'
    'fred(sin(x-y),u) = fred(cos(x-z),u)'};

% Store the number of strings
n = numel(str);
anFun = cell(n, 1);
indVarNames = anFun; varNames = anFun; pdeVarNames = anFun;
eigVarNames = anFun;
commaSeparated = zeros(n, 1);

for k = 1:n
    [anFun{k}, indVarNames{k}, varNames{k}, pdeVarNames{k}, eigVarNames{k}, ...
        commaSeparated(k)] = stringParser.str2anon(str{k}, 'bvp');
end

correctAnFun = {
    'feval(u,-1)-1'
    'feval(u,0)-1;feval(u,2)+x-3'
    'u-1;v;w-3'
    'x+diff(sin(v))'
    'sum(u,0,.5)'
    'fred(@(x,y)sin(x-y),u)-fred(@(x,z)cos(x-z),u)'};

correctIndVar = {
    {'', ''}
    {'x', ''}
    {'', ''}
    {'x', ''}
    {'', ''}
    {'x', ''}
    };

correctVarNam = {
    {'u'}
    {'u'}
    {'u'; 'v'; 'w'}
    {'v'}
    {'u'}
    {'u'}
    };

correctCommaSep = [0; 1; 1; 0; 0; 0];

passBVP = zeros(n, 1);
for k = 1:n
    passBVP(k) = ( strcmp(anFun{k}, correctAnFun{k}) && ...
        all( strcmp(indVarNames{k}, correctIndVar{k}) ) && ...
        all( strcmp(varNames{k}, correctVarNam{k}) ) && ...
        isempty(pdeVarNames{k}) && isempty(eigVarNames{k}) && ...
        ( commaSeparated(k) == correctCommaSep (k) ) );
end

%% EIGS

str = {'v''''-lambda*v = 0'
    'u''''+u'' = lam*(u + u'') + x*u'};
n2 = numel(str);
anFun = cell(n2, 1);
indVarNames = anFun; varNames = anFun; pdeVarNames = anFun;
eigVarNames = anFun;
commaSeparated = zeros(n2, 1);

for k = 1:n2
    [anFun{k}, indVarNames{k}, varNames{k}, pdeVarNames{k}, eigVarNames{k}, ...
        commaSeparated(k)] = stringParser.str2anon(str{k}, 'eig');
end

correctAnFun = {{'diff(v,2)'
    'v'}
    {'diff(u,2)+diff(u)-x.*u'
    'u+diff(u)'}};

correctIndVar = {
    {'', ''}
    {'x', ''}
    };

correctVarNam = {
    {'v'}
    {'u'}
    };

correctEigNam = {
    {'lambda'}
    {'lam'}
    };

correctCommaSep = [0; 0];
passEIG = zeros(n2, 1);
for k = 1:n2
    passEIG(k) = ( strcmp(anFun{k}{1}, correctAnFun{k}{1}) && ...
        strcmp(anFun{k}{2}, correctAnFun{k}{2}) && ...
        all( strcmp(indVarNames{k}, correctIndVar{k}) ) && ...
        all( strcmp(varNames{k}, correctVarNam{k}) ) && ...
        isempty(pdeVarNames{k}) && ...
        strcmp(eigVarNames{k}, correctEigNam{k}) && ...
        ( commaSeparated(k) == correctCommaSep (k) ) );
end

%% PDES

str = {'u_t+x*u'''' = u'''};
n3 = numel(str);

anFun = cell(n3, 1);
indVarNames = anFun; varNames = anFun; pdeVarNames = anFun;
eigVarNames = anFun;
commaSeparated = zeros(n2, 1);

for k = 1:n3
    [anFun{k}, indVarNames{k}, varNames{k}, pdeVarNames{k}, eigVarNames{k}, ...
        commaSeparated(k)] = stringParser.str2anon(str{k}, 'pde');
end
%%
correctAnFun = {'-x.*diff(u,2)+diff(u)'};
correctIndVar = {{'x', 't'}};
correctVarNam = {{'u'}};
correctPdeNam = {{'u_t'}};
correctCommaSep = 0;

passPDE = zeros(n3, 1);
for k = 1:n3
    passPDE(k) = ( strcmp(anFun{k}, correctAnFun{k}) && ...
        all( strcmp(indVarNames{k}, correctIndVar{k}) ) && ...
        all( strcmp(varNames{k}, correctVarNam{k}) ) && ...
        strcmp(pdeVarNames{k}, correctPdeNam{k}) && ...
        isempty(eigVarNames{k}) && ...
        ( commaSeparated(k) == correctCommaSep (k) ) );
end

%% Concatenate all types
pass = [passBVP; passEIG; passPDE];

end


function wulff()
    % wulff -- Program for plotting a Wulff net
    % to plot points, first calculate theta = pi*(90-azimuth)/180
    % then rho = tan(pi*(90-dip)/360), and finally the components
    % xp = rho*cos(theta) and yp = rho*cos(theta)
    % turned into function by Celso G Reyes 2017

    N = 50;
    cx = cos(0:pi/N:2*pi);                           % points on circle
    cy = sin(0:pi/N:2*pi);
    xh = [-1 1];                                     % horizontal axis
    yh = [0 0];
    xv = [0 0];                                      % vertical axis
    yv = [-1 1];
    axis([-1 1 -1 1]);
    axis('square');
    plot(xh,yh,'-k',xv,yv,'-k');                     %plot green axes
    axis off;
    set(gca,'NextPlot','add');
    plot(cx,cy,'-k');                                %plot white circle
    psi = 0:pi/N:pi;
    for i = 1:8                                      %plot great circles
        rdip = i*(pi/18);                             %at 10 deg intervals
        radip = atan(tan(rdip)*sin(psi));
        rproj = tan((pi/2 - radip)/2);
        x1 = rproj .* sin(psi);
        x2 = rproj .* (-sin(psi));
        y = rproj .* cos(psi);
        plot(x1,y,':k',x2,y,':k');
    end
    for i = 1:8                                     %plot small circles
        alpha = i*(pi/18);
        xlim = sin(alpha);
        % ylim = cos(alpha);
        x = -xlim:0.01:xlim;
        d = 1/cos(alpha);
        rd = d*sin(alpha);
        y0 = sqrt(rd*rd - (x .* x));
        y1 = d - y0;
        y2 = - d + y0;
        plot(x,y1,':k',x,y2,':k');
    end
    axis('square');
    set(gcf,'color','w');
end


%This computes the hand-eye calibration using the method described in
%"Hand-Eye Calibration Using Dual Quaternions" from
%Konstantinos Daniilidis
%
%Input:
%Hmarker2world      a 4x4xNumber_of_Views Matrix of the form
%                   Hmarker2world(:,:,i) = [Ri_3x3 ti_3x1;[ 0 0 0 1]]
%                   with
%                   i = number of the view,
%                   Ri_3x3 the rotation matrix
%                   ti_3x1 the translation vector.
%                   Defining the transformation of the robot hand / marker
%                   to the robot base / external tracking device
%Hgrid2cam          a 4x4xNumber_of_Views Matrix (like above)
%                   Defining the transformation of the grid to the camera
%
%Output:
%Hcam2marker_       The transformation from the camera to the marker /
%                   robot arm
%err                The residuals from the least square processes
%
%Christian Wengert
%Computer Vision Laboratory
%ETH Zurich
%Sternwartstrasse 7
%CH-8092 Zurich
%www.vision.ee.ethz.ch/cwengert
%wengert@vision.ee.ethz.ch
function [Hcam2marker_, err] = hand_eye_dual_quaternion(Hmarker2world, Hgrid2cam)
%Our quaternions are like this (q1 q2 q3,s )
    %Get n
    n = size(Hmarker2world,3);
    %Make movements (a,B) which are interposition transformations
    %(marker2wordl and cam2grid)
    %transform A,B into dual quaternions
    for i=1:n-1
        A = inv(Hmarker2world(:,:,i+1))*Hmarker2world(:,:,i);
        B = Hgrid2cam(:,:,i+1)*inv(Hgrid2cam(:,:,i));
        [q,qprime] = getDualQuaternion(A(1:3,1:3),A(1:3,4));
        Qa(i).q = q;
        Qa(i).qprime = qprime;

        [q,qprime] = getDualQuaternion(B(1:3,1:3),B(1:3,4));
        Qb(i).q = q;
        Qb(i).qprime = qprime;
    end

    %The dual quaternion is (Q.q + epsilon*Q.prime)
    %a = Qa.q, a' = Qa.prime  idem for b
    S = [];
    for i=1:n-1
        S(:,:,i) = [Qa(i).q(1:3)-Qb(i).q(1:3)   crossprod(Qa(i).q(1:3)+Qb(i).q(1:3)) zeros(3,1) zeros(3,3);...
                    Qa(i).qprime(1:3)-Qb(i).qprime(1:3)   crossprod(Qa(i).qprime(1:3)+Qb(i).qprime(1:3)) Qa(i).q(1:3)-Qb(i).q(1:3)   crossprod(Qa(i).q(1:3)+Qb(i).q(1:3))];
    end

    %Construct T
    T = [];
    for i=1:n-1
        T = [T  S(:,:,i)'];
    end

    T = T';
    %SVD
    [U,S,V] = svd(T);

    %Solution, right null vectors of T
    v7 = V(:,7);
    v8 = V(:,8);

    u1 = v7(1:4);
    v1 = v7(5:8);

    u2 = v8(1:4);
    v2 = v8(5:8);
    %Now lambda1*v7+lambda2*v8 = [q;qprime]
    %
    %or other:
    %
    %lambda1^2*u1'*u1+2*lambda1*lambda2*u1'*u2+lambda2^2*u2'*u2 = 1
    %and
    %lambda1^2*u1'*v1 + lambda1*lambda2*(u1'*v2+u2'*v1)+lambda2*u2'*v1 = 0
    %Setting lambda1/lambda2 = s
    %lambda1^2/lambda2^2*u1'*v1 + lambda1*lambda2/lambda2^2*(u1'*v2+u2'*v1)+lambda2^2/lambda2^2*u2'*v1 = 0
    %s^2*u1'*v1 + s*(u1'*v2+u2'*v1)+u2'*v1 = 0
    %s^2*u1'*v1 + s*(u1'*v2+u2'*v1)+u2'*v1 = 0
    a = u1'*v1;
    b = (u1'*v2+u2'*v1);
%     c = u2'*v1;
    c = u2'*v2 ;
    s = roots([a b c]);

    %insert into equation
    val1 = s(1)^2*u1'*u1+2*s(1)*u1'*u2+u2'*u2;
    val2 = s(2)^2*u1'*u1+2*s(2)*u1'*u2+u2'*u2;
    %Take bigger value
    if(val1>val2)
        s = s(1);
        val = val1;
    else
        s = s(2);
        val = val2;
    end
    %Get lambdas
    lambda2 = sqrt(1/val);
    lambda1 = s*lambda2;

    %This algorithm gives quaternion with the form of (s, q1 q2
    %q3)->contrary to the notation we used above (q1 q2 q3,s )
    %Therefore we must rearrange the elements!
    qfinal = lambda1*v7+lambda2*v8;
    q = [qfinal(2:4);qfinal(1)];
    qprime = [qfinal(6:8);qfinal(5)];

    %Extract transformation
    R = q2dcm(q);
    t = 2*qmult(qprime,qconj(q));
    t = t(1:3);

    %Assign output arguments
    Hcam2marker_ = [R -R*t;[0 0 0 1]]^-1;
    err=[];

%Creates a dual quaternion from a rotation matrix and a translation vector
function [q,qprime] = getDualQuaternion(R,t)
    %Conversion from R,t to the screw representation [d,theta,l,m]

    r = rodrigues(R);
    theta = norm(r);
    l = r/norm(theta);
    %Pitch d
    d = l'*t;
    %Make point c
    c = .5*(t-d*l)+cot(theta/2)*cross(l,t);
    %moment vector
    m = cross(c,l);
    %Rotation quaternion
    %(q1 q2 q3,s )
    q = [sin(theta/2)*l; cos(theta/2)];
    %Get dual
    qprime = [.5*(q(4)*t+cross(t,q(1:3)));-.5*q(1:3)'*t];


function y = geo_mean( varargin )

%GEO_MEAN   Geometric mean.
%   Y=GEO_MEAN(X), where X is a vector, computes the geometrix mean of X. If any
%   of the elements of X are negative, then Y=-Inf. Otherwise, it is equivalent
%   to Y=PROD(X).^(1/LENGTH(X)). All elements must be real.
%
%   For matrices, GEO_MEAN(X) is a row vector containing the geometric means of
%   the columns. For N-D arrays, GEO_MEAN(X) is an array of the geometric means
%   taken along the first non-singleton dimension of X.
%
%   GEO_MEAN(X,DIM) takes the geometric mean along the dimension DIM of X.
%
%   GEO_MEAN(X,DIM,W), where W is a vector of nonnegative integers, computes a
%   weighted geometric mean Y = PROD(X.^W)^(1/SUM(W)). This is more efficient
%   than replicating the values of X W times. Note that W must be a vector,
%   even if X is a matrix, and its length must be the same as SIZE(X,DIM).
%
%   Disciplined convex programming information:
%       GEO_MEAN is concave  and nondecreasing; therefore, when used in CVX
%       specifications, its argument must be concave.

%
% Check arguments
%

persistent P
if isempty( P ),
    P.map = cvx_remap( { 'real' ; 'concave' ; 'l_convex' ; 'l_concave' } );
    P.map = bsxfun( @and, P.map, ~cvx_remap( 'negative' ) );
    P.funcs = { @geo_mean_1, @geo_mean_2, @geo_mean_2, @geo_mean_2 };
    P.zero = 1;
    P.constant = 1;
    P.reduce = true;
    P.reverse = false;
    P.name = 'geo_mean';
    P.errargs = [];
    P.dimarg = 2;
end
[ sx, x, dim, w ] = cvx_get_dimension( varargin, 2 );
if ~isempty( w ),
    if ~( numel(w)==length(w) && isnumeric(w) && isreal(w) && all(w>=0) ),
        cvx_throw( 'Third argument must be a vector of nonnegative numbers.' );
    elseif ~any( w ),
        cvx_throw( 'The weight vector cannot be all zeros.')
    elseif numel( w ) ~= sx(dim),
        cvx_throw( 'Third argument must be a vector of length %d', sx(dim) );
    else
        w = w(:) / sum(w);
    end
end
y = cvx_reduce_op( P, x, dim, w );

function y = geo_mean_1( x, w )
[ nx, nv ] = size( x );
if isempty( w ),
    w = 1 / nx;
else
    w = repmat( w, [ 1, nv ] );
end
y = prod( x .^ w, 1 );

function y = geo_mean_2( x, w ) %#ok
[ nx, nv ] = size( x );
cvx_begin
    hypograph variable y(1,nv);
    { linearize(x), y } == geo_mean_cone( [nx,nv], 1, w, 'func' ); %#ok
    cvx_setnneg(y);
cvx_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.% 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.