function [X, A] = PCA_stochastic(A, k)
% Example of stochastic gradient algorithm in Manopt on a PCA problem.
%
% PCA (principal component analysis) on a dataset A of size nxd consists
% in solving
%
%   minimize_X  f(X) = -.5*norm(A*X, 'fro')^2 / n,
%
% where X is a matrix of dimension dxk with orthonormal columns. This
% is equivalent to finding k dominant singular vectors of A, or k top
% eigenvectors of A'*A.
%
% If n is large, this computation can be expensive. Thus,  stochastic
% gradient algorithms take the point of view that f(X) is a sum of many (n)
% terms: each term involves only one of the n rows of A.
%
% To make progress, it may be sufficient to optimize with respect to a
% subset of the terms at each iteration. This way, each individual
% iteration can be very cheap. In particular, individual operations have
% cost independent of n, because f or its gradient need never be evaluated
% completely (or at all in the case of f.)
%
% Stochastic gradient algorithms (this implementation in particular) are
% sensitive to proper parameter tuning. See in code.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Bamdev Mishra and Nicolas Boumal, Sept. 6, 2017
% Contributors:
%
% Change log:
%

    % If none is given, generate a random data set: n samples in R^d
    if ~exist('A', 'var') || isempty(A)
        d = 1000;
        n = 100000;
        fprintf('Generating data...');
        A = randn(n, d)*diag([[15 10 5], ones(1, d-3)]);
        fprintf(' done (size: %d x %d).\n', size(A));
    else
        [n, d] = size(A);
    end

    % Pick a number of component to compute
    if ~exist('k', 'var') || isempty(k)
        k = 3;
    end

    % We are looking for k orthonormal vectors in R^d: Stiefel manifold.
    problem.M = stiefelfactory(d, k);

    % The cost function to minimize is a sum of n terms. This parameter
    % must be set for stochastic algorithms.
    problem.ncostterms = n;

    % We do not need to specify how to compute the value of the cost
    % function (stochastic algorithms never use this). All we need is to
    % specify how to compute the gradient of the cost function, where the
    % sum is restricted to a subset of the terms (a sample). Notice that we
    % specify a partial Euclidean gradient (hence the 'e' in partialegrad).
    % This way, Manopt will automatically convert the Euclidean vector into
    % a proper Riemannian partial gradient, in the tangent space at X.
    % In particular, if sample = 1:n, then the partial gradient corresponds
    % to the actual (complete) gradient.
    problem.partialegrad = @partialegrad;
    function G = partialegrad(X, sample)

        % X is an orthonormal matrix of size dxk
        % sample is a vector if indices between 1 and n: a subset
        % Extract a subset of the dataset
        Asample = A(sample, :);

        % Compute the gradient of f restricted to that sample
        G = -Asample'*(Asample*X);
        G = G / n;

    end

    % If one wants to use checkgradient to verify one's work, then it is
    % necessary to specify the cost function as well, as below.
    % problem.cost = @(X) -.5*norm(A*X, 'fro')^2 / n;
    % checkgradient(problem); pause;

    % To have the solver record statistics every x iterations, set
    % options.checkperiod to x. This will record simple quantities which
    % are almost free to compute (namely, elapsed time and step size of the
    % last step.) To record more sophisticated quantities, you can use
    % options.statsfun as usual. Time spent computing these statistics is
    % not counted in times reported in the info structure returned by the
    % solver.
    options.checkperiod = 10;
    options.statsfun = statsfunhelper('metric', @(X) norm(A*X, 'fro'));

    % Set the parameters for the solver: stochastic gradient algorithms
    % tend to be quite sensitive to proper tuning, especially regarding
    % step size selection. See the solver's documentation for details.
    options.maxiter = 200;
    options.batchsize = 10;
    % options.stepsize_type = 'decay';
    options.stepsize_init = 1e2;
    options.stepsize_lambda = 1e-3;
    options.verbosity = 2;

    % Run the solver
    [X, info] = stochasticgradient(problem, [], options);

    % Plot the special metric recorded by options.statsfun
    plot([info.iter], [info.metric], '.-');
    xlabel('Iteration #');
    ylabel('Frobenius norm of A*X');
    title('Convergence of stochasticgradient on stiefelfactory for PCA');

    % Add to that plot a reference: the globally optimal value attained if
    % the true dominant singular vectors are computed.
    fprintf('Running svds... ');
    t = tic();
    [V, ~] = svds(A', k);
    fprintf('done: %g [s] (note: svd may be faster)\n', toc(t));
    hold all;
    bound = norm(A*V, 'fro');
    plot([info.iter], bound*ones(size([info.iter])), '--');
    hold off;

    legend('Algorithm', 'SVD bound', 'Location', 'SouthEast');

end


function [Tx,Ty,Tz,nr,A] = surfcv(x,y,z,f,const);
%SURFCV 3-D surface of constant value plot.
%   SURFCV(X,Y,Z,F,CONST) draws a surface of constant value
%   for a function of three variables:  F(X,Y,Z) = CONST .
%   The arrays X,Y,Z define the coordinates for F and must
%   be monotonic and 3-D plaid (as if produced by meshgrid).
%   The surface is calculated by linear interpolation of the
%   values of F and is drawn with triangular patches using
%   FILL3.  The color of each patch is determined by its
%   orientation with respect to a specified direction.
%   F must be an M-by-N-by-P volume array.
%
%   SURFCV(F,CONST) assumes X=1:N, Y=1:M, Z=1:P.
%
%   H = SURFCV(...) returns a column vector of handles
%   to PATCH objects, one handle per patch.
%
%   [TX,TY,TZ] = SURFCV(X,Y,Z,F,CONST); returns 3-by-K
%   arrays specifying x,y,z coordinates of the vertices
%   of the triangular patches, where K is the number
%   of patches.  FILL3(TX,TY,TZ,C) can be used to draw
%   the surface.
%
%   [TX,TY,TZ,NR] = SURFCV(...) also returns 3-by-K
%   arrays of normal vectors for each patch which
%   can be used to compute lighting conditions of the
%   surface.  Vectors are normalized to unity and
%   point in the direction of increasing F.
%
%   [TX,TY,TZ,NR,A] = SURFCV(...) returns the area
%   of each patch.
%
%   Example 1: To visualize the surface
%              1./(X.^2+Y.^2) +  1./(Y.^2+Z.^2) + 1./(Z.^2+X.^2) = 2
%              over the range -3 < X < 3, -3 < Y < 3, -3 < Z < 3:
%
%      [x,y,z] = meshgrid(-3:.4:3, -3:.4:3, -3:.4:3);
%      f = 1./(x.^2+y.^2) +  1./(y.^2+z.^2) + 1./(z.^2+x.^2);
%      const = 2;
%      surfcv(x,y,z,f,const);
%
%   Example 2: The routine does not require the grid to be Cartesian
%              and can handle a more general problem of the form:
%                 F(U(X,Y,Z),V(X,Y,Z),W(X,Y,Z)) = CONST
%              This allows to change surface appearance not only
%              by modifying the function F, but also by deforming
%              the grid:
%
%      [x,y,z] = meshgrid(-3:.4:3, -3:.4:3, -3:.4:3);
%      f = 1./(x.^2+y.^2) +  1./(y.^2+z.^2) + 1./(z.^2+x.^2);
%      const = 2;
%      z = z + 0.2.*(x.^2 - y.^2);      %  Grid deformation
%      surfcv(x,y,z,f,const);
%

%  Copyright (c) 1997 Ruslan L. Davidchack
%                     University of Kansas, Lawrence, KS
%                     e-mail: ruslan@ukans.edu
%                     URL:    http://www.ukans.edu/home/ruslan
%      created:  Sep 29, 1997;
%      modified: Dec 23, 1998;

%Short description of the algorithm.
%   Function F is defined on the M-by-N-by-P 3-D grid which
%   divides the space into (M-1)*(N-1)*(P-1) cubes.
%   F specifies values of the function at the vertices.
%   The surface of constant value cuts a patch through a
%   cube if F < CONST for some vertices of the cube and
%   F > CONST for others.
%     Note: F is slightly modified on the input to assure
%           that it does not exactly equal CONST on a vertex,
%           and thus a vertex of a surface element belongs to
%           only one edge (see below).
%   Every vertex of the patch lays on the edge of the
%   cube and its coordinates can be determined by linear
%   interpolation between the values of the function on the
%   ends of the edge. Two vertices of a patch are connected
%   if they lay on the edges that belong to the same face of
%   a cube. If a patch has more than three vertices, it is
%   subdivided into triangular elements by additional
%   connections between vertices that do not belong to the
%   same face.
%   The idea of the algorithm is to group the edges which
%   contain vertices of the surface patches into pairs according
%   to their affiliation with the faces (there can be only 2 or 4
%   patch vertices on a face).  Then the edge pairs are grouped
%   according to their affiliation with the cubes.  The "cube-edge"
%   array, thus created, is then decomposed into triplets of patch
%   vertices.  There can be more than one patch per cube and
%   the algorithm handles this case as well.
%Naming convensions
%   ve - vertex, ed - edge, fa - face, cu - cube
%   nv - number of vertices, ne - number of edges, etc.
%   ce - coordinates of edges, cf - coordinates of faces, etc.
%   ie - indices of edges, ia - indices of faces, etc.

%Simple case to follow the algorithm
%      [x,y,z] = meshgrid(1:4, 1:3, 1:2); const = 15;
%      f = x.^2 + y.^2 + z.^2;
%Different cool surfaces:
% 1.   [x,y,z] = meshgrid(-3:.2:3, -3:.2:3, -3:.2:1.6); const = 7;
%      f = 1./(x.^2+y.^2)+1./(y.^2+z.^2)+1./(z.^2+x.^2)+x.^2+y.^2+z.^2;
%
% 2.   [x,y,z] = meshgrid(-2:.2:2, -2:.2:2, -2:.2:2);  const = 0;
%      f = (x.^2 + y.^2 + z.^2).^2 - 4*(x.^2 - y.^2 - z.^2);
%
% 3.   [x,y,z] = meshgrid(-2:.2:2, -2:.2:2, -2:.2:2);  const = 0;
%      f = x.^2 + y.^2 - z.^2.*(2 - z)./(2 + z);

  if nargin == 2,
    f = x; const = y;
    [x,y,z] = meshgrid(1:size(f,2),1:size(f,1),1:size(f,3));
  elseif nargin ~=5
    disp('Wrong number of input arguments'); return;
  end,

%Make sure F == CONST is never true on the grid
  f1 = f; rngf = max(max(max(f))) - min(min(min(f)));
  if ~isfinite(rngf), rngf = 1; end,
  f1(find(f == const)) = const + 1e-12*rngf;

%Number of vertices in each direction:  nv = [M N P]
%   Total number of vertices = prod(nv)
  nv = size(f1);  iv = reshape(1:prod(nv),nv);
  [ix,iy,iz] = meshgrid(1:nv(2),1:nv(1),1:nv(3));

%Number of edges of each orientation:
%			ne = [(M-1)*N*P  M*(N-1)*P  M*N*(P-1)]
%   Total number of edges = sum(ne)
  ne = prod(nv) - prod(nv([2 1 1;3 3 2]));

%Determine indices of vertices that belong to each edge
%   Indexing of vertices is the same as in f(:)
  ed = zeros(sum(ne),2);
%   Edges parallel to y-axis
  ed((1:ne(1)),1) = reshape(iv(1:nv(1)-1,:,:),ne(1),1);
  ed((1:ne(1)),2) = ed((1:ne(1)),1) + 1;
%   Edges parallel to x-axis
  ie = (1:ne(2)) + ne(1);
  ed(ie,1) = reshape(iv(:,1:nv(2)-1,:),ne(2),1);
  ed(ie,2) = ed(ie,1) + nv(1);
%   Edges parallel to z-axis
  ie = (1:ne(3)) + ne(1) + ne(2);
  ed(ie,1) = (1:ne(3))';
  ed(ie,2) = ed(ie,1) + nv(1)*nv(2);

%Select edges crossed by the surface
  se = find(prod(f1(ed)' - const) < 0)';
  if isempty(se),
    disp('There is no surface in the specified range');
    if nargout > 0, Tx = []; end;
    if nargout > 2, Ty = []; Tz = []; end;
    if nargout > 3, nr = []; end;
    if nargout > 4, A = []; end;
    return; end;
  ed = ed(se,:);

%Restore coordinates of the selected edges
  ce = zeros(length(se),4);
  ce(:,1:3) = [ix(ed(:,1)) iy(ed(:,1)) iz(ed(:,1))];
  ce(:,4) = (se > ne(1)) + (se > ne(1)+ne(2)) + 1;

%Determine coordinates of the patch vertices
  dd = diff(f1(ed)')';  i1 = find(dd < 0);
  dd = (const - f1(ed(:,1)))./dd;
  np = zeros(3,length(se));  np(1,:) = diff(x(ed)');
  np(2,:) = diff(y(ed)');    np(3,:) = diff(z(ed)');
  xv = np(1,:)'.*dd + x(ed(:,1));
  yv = np(2,:)'.*dd + y(ed(:,1));
  zv = np(3,:)'.*dd + z(ed(:,1));
  np(:,i1) = -np(:,i1);

  ie1 = find(ce(:,4) == 1); ie2 = find(ce(:,4) == 2);
  ie3 = find(ce(:,4) == 3);
  ie = cumsum([length(ie1); length(ie2); length(ie3)]);

%Find faces containing the selected edges (maximum of 4 faces per edge)
  cf = repmat([(1:length(se))' ce],1,4);
%Edge-to-face coordinate transformation matrix
  etof = [0 0 0 0 0   0 -1  0  0 0   0 0 0 0  2   0  0  0 -1  2;...
	  0 0 0 0 0   0  0  0 -1 0   0 0 0 0 -1   0  0 -1  0 -1;...
          0 0 0 0 0   0  0 -1  0 0   0 0 0 0 -1   0 -1  0  0 -1];
  ie = diff([0;ie]);
  cf = cf + [repmat(etof(1,:),ie(1),1);repmat(etof(2,:),ie(2),1);...
             repmat(etof(3,:),ie(3),1)];
  cf = reshape(cf',5,4*length(se));

%Discard faces that are outside the range of x,y,z
  cf(:,find(prod(cf) == 0)) = [];  cf = cf';
  i1 = find(cf(:,5) ~= 2);  cf(i1(find(cf(i1,3) > nv(1)-1)),:) = [];
  i2 = find(cf(:,5) < 3);   cf(i2(find(cf(i2,2) > nv(2)-1)),:) = [];
  i3 = find(cf(:,5) > 1);   cf(i3(find(cf(i3,4) > nv(3)-1)),:) = [];

%Find indices of the selected faces
  i1 = find(cf(:,5)==1); i2 = find(cf(:,5)==2); i3 = find(cf(:,5)==3);
%Number of faces of each orientation:
%                ne = [(M-1)*(N-1)*P  M*(N-1)*(P-1)  (M-1)*N*(P-1)]
  nf = [nv(3)*(nv(2)-1)*(nv(1)-1) nv(1)*(nv(2)-1)*(nv(3)-1)...
        nv(2)*(nv(3)-1)*(nv(1)-1)];
  fa = zeros(size(cf,1),1);
  fa(i1) = cf(i1,3)+((cf(i1,2)-1)+(cf(i1,4)-1)*(nv(2)-1))*(nv(1)-1);
  fa(i2) = cf(i2,3)+((cf(i2,2)-1)+(cf(i2,4)-1)*(nv(2)-1))*nv(1)+nf(1);
  fa(i3) = cf(i3,3)+((cf(i3,2)-1)+(cf(i3,4)-1)*nv(2))*(nv(1)-1)+...
	   nf(1) + nf(2);
  [fa ia] = sort(fa);

%!!!!!!!! Check whether fac values come in pairs.
%   Each face can have only 2 or 4 selected edges.
%   The check below can be removed after debugging process
%   is completed
if mod(length(fa),2) ~= 0,
  disp('Odd number!'); return,
else
  if sum(diff(reshape(fa,2,length(fa)/2))) > 0,
  disp('Odd number!'); return, end, end;
%!!!!!!!!

%Combine face coord. with indices of edges that belong to the faces
  cf = [fa(1:2:end) reshape(cf(ia,1),2,length(fa)/2)' ...
        cf(ia(1:2:end),2:5)];

%Find cubes containing selected faces
  i1 = find(cf(:,7)==1); i2 = find(cf(:,7)==2); i3 = find(cf(:,7)==3);
  cc = repmat(cf(:,2:6),1,2);
  ftoc = [zeros(3,7) [0 0 -1;0 -1 0;-1 0 0]];
  cc = cc + [repmat(ftoc(1,:),length(i1),1);...
   repmat(ftoc(2,:),length(i2),1);repmat(ftoc(3,:),length(i3),1)];
  cc = reshape(cc',5,size(cf,1)*2);
  cc(:,find(prod(cc) == 0 | cc(4,:) > nv(1)-1 | cc(3,:) > nv(2)-1 |...
            cc(5,:) > nv(3)-1)) = [];  cc = cc';

  cu = cc(:,4) + ((cc(:,3)-1) + (cc(:,5)-1)*(nv(2)-1))*(nv(1)-1);
  [cu ic] = sort(cu);
%Combine indices of selected cubes with indices of edges that belong
%   to them into a cube-edge index array
  cc = [cu cc(ic,1:2)];

%Combine edges into triplets composing surface patches
  ip = [];   cc = sortrows(cc);
  while 1,
%Indices of patches
    ic = find(diff([0;cc(:,1);0]) ~= 0);
%Indices of triangular patches
    ic3 = ic(find(diff(ic) == 3));
%Indices of patches with more than 3 vertices
    icm = ic(find(diff(ic) > 3));

%Select triangular patches and take them out of the cube-edge array
    ic(end) = [];   ip = [ip [cc(ic,2:3)';cc(ic+1,3)']];
    if ~isempty(ic3),
      irm = repmat(ic3,1,3) + repmat(0:2,length(ic3),1);
    else irm = []; end,
    irm = [irm(:);icm];   cc(icm+1,2) = cc(icm,3);   cc(irm,:) = [];

%Sort new cube-edge index array or quit if it is empty
    if ~isempty(cc),  cc = sortrows(cc);  else,
      disp(['Number of patches = ' num2str(size(ip,2))]);
      break; end

%Find cases when there is more than one patch per cube
    irm = find(sum(abs(diff(cc)')) == 0);
    if ~isempty(irm),
      %disp(['More than one patch in ' int2str(length(irm)) ' cubes']);
      irm = repmat(irm,2,1)+repmat([0;1],1,length(irm)); irm = irm(:);
      cc(irm,:) = []; end,

  end;

  Tx = xv(ip);  Ty = yv(ip);  Tz = zv(ip);

%This is to compute area of each patch and the normal vector
  x2 = Tx(2,:)-Tx(1,:); y2 = Ty(2,:)-Ty(1,:); z2 = Tz(2,:)-Tz(1,:);
  x3 = Tx(3,:)-Tx(1,:); y3 = Ty(3,:)-Ty(1,:); z3 = Tz(3,:)-Tz(1,:);
  if nargout == 5
     A = sqrt((x2.*y3-x3.*y2).^2 + (y2.*z3-y3.*z2).^2 +...
              (z2.*x3-z3.*x2).^2)./2; end
  nr = [y2.*z3-y3.*z2; z2.*x3-z3.*x2; x2.*y3-x3.*y2];
  nr = nr./repmat(sqrt(sum(nr.^2)),3,1);
  i1 = find(sum(nr.*np(:,ip(1,:))) < 0);
  nr(:,i1) = -nr(:,i1);
  i2 = ip(2,i1); ip(2,i1) = ip(3,i1); ip(3,i1) = i2;
  Tx = xv(ip);  Ty = yv(ip);  Tz = zv(ip);
  if nargout < 3,
    va = [-2 3 1];
% Colored according to the patch orientation
    C = va*nr;
% Colored according to the patch distance from the origin
%    C = sqrt(Tx.^2 + Ty.^2 + Tz.^2);
    clf;  H = fill3(Tx,Ty,Tz,C);  axis image;
    set(gca,'xlim',[min(min(Tx)) max(max(Tx))],...
            'ylim',[min(min(Ty)) max(max(Ty))],...
            'zlim',[min(min(Tz)) max(max(Tz))],'box','on');
    view(va);
    Tx = H;
  end
  if nargout == 0, clear Tx, end
  if nargout < 4, clear nr, end


function u = u_function ( time )

%*****************************************************************************80
%
%% U_FUNCTION evaluates the time-dependent boundary values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real TIME, the current time.
%
%    Output, real U(2), the boundary values at the left and right endpoints.
%
  ua =   sin ( pi * time );
  ub = - sin ( pi * time );

  u = [ ua; ub ];

  return
end


function om_constants

% astrodynamic and utility constants

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global dtr rtd mu mmu smu omega req flat j2 aunit

dtr = pi / 180.0;

rtd = 180.0 / pi;

% earth gravitational constant (km**3/sec**2)

mu = 398600.436233;

% moon gravitational constant (km**3/sec**2)

mmu = 4902.800076;

% sun gravitational constant (km**3/sec**2)

smu = 132712440040.944;

% earth inertial rotation rate (radians/second)

omega = 7.292115486e-5;

% earth equatorial radius (kilometers)

req = 6378.1363;

% earth flattening factor (non-dimensional)

flat = 1.0 / 298.257;

% earth oblateness gravity coefficient (non-dimensional)

j2 = 0.00108263;

% astronomical unit (kilometers)

aunit = 149597870.691;


function featuresS = ngtdmToScalarFeatures(s,p,numVoxels)
% function featuresS = ngtdmToScalarFeatures(s,p,numVoxels)
%
% APA, 3/15/2017

% Coarseness
featuresS.coarseness = 1/(sum(s .* p) + 1e-6);

% Contrast
Ng = sum(p > 0);
numLevels = length(p);
indV = (1:numLevels)';
term1 = 0;
term2 = 0;
for lev = 1:numLevels
    term1 = term1 + ...
        sum(p .* circshift(p,lev) .* (indV-circshift(indV,lev)).^2);
    term2 = term2 + s(lev);
end
featuresS.contrast = 1/Ng/(Ng-1) * term1 * term2 / numVoxels;

% Busyness
denom = 0;
for lev = 1:numLevels
    pShiftV = circshift(p,lev);
    indShiftV = circshift(indV,lev);
    usePv = p > 0;
    usePshiftV = pShiftV > 0;
    denom = denom + ...
        sum(usePv .* usePshiftV .* abs(p .* indV - pShiftV .* indShiftV));
end
featuresS.busyness = sum(p .* s) / denom;

% Complexity
complxty = 0;
for lev = 1:numLevels
    pShiftV = circshift(p,lev);
    sShiftV = circshift(s,lev);
    indShiftV = circshift(indV,lev);
    usePv = p > 0;
    usePshiftV = pShiftV > 0;
    term1 = abs(indV - indShiftV);
    term2 = usePv .* usePshiftV .* (p .* s + pShiftV .* sShiftV)...
        ./ (p + pShiftV + eps);
    complxty = complxty + sum(term1 .* term2);
end
featuresS.complexity = complxty / numVoxels;

% Texture strength
strength = 0;
for lev = 1:numLevels
    pShiftV = circshift(p,lev);
    indShiftV = circshift(indV,lev);
    usePv = p > 0;
    usePshiftV = pShiftV > 0;
    term = sum(usePv .* usePshiftV .* (p + pShiftV) .* (indV - indShiftV).^2);
    strength = strength + term;
end
featuresS.strength = strength / sum(s);


function [B,twom]=multicatdir_f(A,gamma,omega)
%MULTICATDIR_F  returns multilayer Leicht-Newman modularity matrix for categorical directed layers, function handle version
%
% Version: 2.2.0
% Date: Thu 11 Jul 2019 12:25:42 CEST
%
%   Input: A: Cell array of NxN adjacency matrices for each layer of a
%          categorical directed multilayer network
%          gamma: intralayer resolution parameter
%          omega: interlayer coupling strength
%
%   Output: B: function handle where B(i) returns the ith column of
%          [NxT]x[NxT] flattened modularity tensor for the
%           multilayer network with uniform ordinal coupling (T is
%           the number of layers of the network)
%           twom: normalisation constant
%
%   Example of usage: [B,twom]=multicatdir_f(A,gamma,omega);
%          [S,Q]= genlouvain(B); % see iterated_genlouvain.m and
%          postprocess_categorical_multilayer.m for how to improve output
%          multilayer partition
%          Q=Q/twom;
%          S=reshape(S,N,T);
%
%   [B,twom] = MULTICATDIR_F(A,GAMMA, OMEGA) with A a cell array of square
%   matrices of equal size each representing an directed network "layer"
%   computes the Leicht-Newman multilayer modularity matrix B using the
%   quality function described in Mucha et al. 2010, with intralayer
%   resolution parameter GAMMA, and with interlayer coupling OMEGA
%   connecting all-to-all categorical layers. Once the mulilayer modularity
%   matrix is computed, optimization can be performed by the generalized
%   Louvain code GENLOUVAIN or ITERATED_GENLOUVAIN. The output B can be used
%   with other heuristics, provided the same mapping is used to go from the
%   multilayer tensor to the multilayer flattened matrix. That is, the
%   node-layer tuple (i,s) is mapped to i + (s-1)*N. [Note that we can
%   define a mapping between a multilayer partition S_m stored as an N by T
%   matrix and the corresponding flattened partition S stored as an NT by 1
%   vector. In particular S_m = reshape(S,N,T) and S = S_m(:).]
%
%   See also
%       genlouvain heuristics:      GENLOUVAIN, ITERATED_GENLOUVAIN
%       multilayer wrappers:        MULTICAT, MULTICAT_F, MULTIORD
%       other heuristics:           SPECTRAL23
%       Kernighan-Lin improvement:  KLNB
%
%   Notes:
%     The matrices in the cell array A are assumed to be square,
%     and of equal size.  These assumptions are not checked here.
%
%     For smaller systems, it is potentially more efficient (and easier) to
%     directly use the sparse quality/modularity matrix B in MULTICAT. For
%     large systems with undirected layer networks, use MULTICAT_F.
%
%     This code serves as a template and can be modified for situations
%     with other wrinkles (e.g., different intralayer null models,
%     different numbers of nodes from layer-to-layer, or systems which are
%     both multiplex and longitudinal).  That is, this code is only a
%     starting point; it is by no means exhaustive.
%
%     By using this code, the user implicitly acknowledges that the authors
%     accept no liability associated with that use.  (What are you doing
%     with it anyway that might cause there to be a potential liability?!?)
%
%   References:
%     Blondel, Vincent D., Jean-Loup Guillaume, Renaud Lambiotte, and
%     Etienne Lefebvre, "Fast unfolding of communities in large networks,"
%     Journal of Statistical Mechanics: Theory and Experiment, P10008
%     (2008).
%
%     Fortunato, Santo, "Community detection in graphs," Physics Reports
%     486, 75-174 (2010).
%
%     Good, Benjamin H., Yves-Alexandre de Montjoye, and Aaron Clauset,
%     "Performance of modularity maximization in practical contexts,"
%     Physical Review E 81, 046106 (2010).
%
%     Mucha, Peter J., Thomas Richardson, Kevin Macon, Mason A. Porter, and
%     Jukka-Pekka Onnela. "Community Structure in Time-Dependent,
%     Multiscale, and Multiplex Networks," Science 328, 876-878 (2010).
%
%     Elizabeth A. Leicht and Mark E. J. Newman. "Community structure in
%     Directed Networks", Physical Review Letters 100, 118703 (2008).
%
%     Porter, M. A., J. P. Onnela, and P. J. Mucha, "Communities in
%     networks," Notices of the American Mathematical Society 56, 1082-1097
%     & 1164-1166 (2009).
%
%   Acknowledgments:
%     Thank you to Dani Bassett, Jesse Blocher, Bruce Rogers, and Simi Wang
%     for their collaborative help which led to significant cleaning up
%     of earlier versions of our multilayer community detection codes.

if nargin<2||isempty(gamma)
    gamma=1;
end

if nargin<3||isempty(omega)
    omega=1;
end

N=length(A{1});
T=length(A);

if length(gamma)==1
    gamma=repmat(gamma,T,1);
end

m=zeros(T,1);
for i=1:T
    m(i)=sum(A{i}(:));
end
A=blkdiag(A{:});
kout=sum(A,1);
koutmat=sparse(1:(N*T),kron(1:T,ones(1,N)),kout);
kin=sum(A,2);
kinmat=sparse(1:(N*T),kron(1:T,ones(1,N)),kin);
A=(A+A')./2;
all2all = N*[(-T+1):-1,1:(T-1)];
A = A + omega*spdiags(ones(N*T,2*T-2),all2all,N*T,N*T);

B=@(i) A(:,i)-gamma(ceil(i./(N+eps))).*(kout(i).*kinmat(:,ceil(i./(N+eps)))+kin(i).*koutmat(:,ceil(i./(N+eps))))./(2*m(ceil(i./(N+eps))));

twom=sum(m)+omega*2*N*(T-1);
end


% VL_PEGASOS PEGASOS linear SVM solver
%   W = VL_PEGASOS(X, Y, LAMBDA) learns a linear SVM W given training
%   vectors X, their labels Y, and the regularization parameter LAMBDA
%   using the PEGASOS [1] solver. The algorithm finds a minimizer W of
%   the objective function
%
%     LAMBDA/2 |W|^2 + 1/N SUM_i LOSS(W, X(:,i), Y(i))
%
%   where LOSS(W,X,Y) = MAX(0, 1 - Y W'X) is the hinge loss and N is
%   the number of training vectors in X.
%
%   [W B INFO] = VL_SVMPEGASOS(X, Y, LAMBDA) learns a linear SVM W
%   and a bias B given training vectors X, their labels Y, and the
%   regularization parameter LAMBDA using the PEGASOS [1]
%   solver. INFO is a struct containing the input parameters plus
%   diagnostic informations:
%
%   energy::
%     SVM energy value.
%
%   iterations::
%     Number of iterations performed.
%
%   elapseTime::
%     Elapsed time since the start of the SVM learning.
%
%   regulizerTerm::
%     Value of the SVM regulizer term.
%
%   lossPos::
%     Value of loss function only for data points labeled positives.
%
%   lossNeg::
%     Value of loss function onlt for data points labeled negatives.
%
%   hardLossPos::
%     Number of mislabeled positive points.
%
%   hardLossNeg::
%     Number of mislabeled negative points.
%
%   ALGORITHM. PEGASOS is an implementation of stochastic subgradient
%   descent. At each iteration a data point is selected at random, the
%   subgradient of the cost function relative to that data point is
%   computed, and a step is taken in that direction. The step size is
%   inversely proportional to the iteration number. See [1] for
%   details.
%
%   VL_SVMPEGASOS() accepts the following options:
%
%   Epsilon:: [empty]
%     Specify the SVM stopping criterion threshold. If not
%     specified VL_SVMPEGASOS will finish when the maximum number
%     of iterations is reached. The stopping criterion is tested
%     after each ENERGYFREQ iteration.
%
%   MaxIterations:: [10 / LAMBDA]
%     Sets the maximum number of iterations.
%
%   BiasMultiplier:: [0]
%     Appends to the data X the specified scalar value B. This
%     approximates the training of a linear SVM with bias.
%
%   StartingModel:: [null vector]
%     Specify the initial value for the weight vector W.
%
%   StartingIteration:: [1]
%     Specify the iteration number to start from. The only effect
%     is to change the step size, as this is inversely proportional
%     to the iteration number.
%
%   StartingBias:: [0]
%     Specify the inital bias value.
%
%   BiasLearningRate:: [1]
%     Specify the frequency of the bias learning. The default
%     setting updates the bias at each iteration.
%
%   Permutation:: [empty]
%     Specify a permutation PERM to be used to sample the data (this
%     disables random sampling). Specifically, at the T-th iteration
%     the algorithm takes a step w.r.t. the PERM[T']-th data point,
%     where T' is T modulo the number of data samples
%     (i.e. MOD(T'-1,NUMSAMPLES)+1). PERM needs not to be
%     bijective. This allows specifying certain data points more or
%     less frequently, implicitly increasing their relative weight in
%     the error term. A common application is to balance an unbalanced
%     dataset.
%
%   DiagnosticFunction:: [empty]
%     Specify a function handle to be called every ENERGYFREQ iterations.
%
%   DiagnosticCallRef:: [empty]
%     Specify a paramater to be passed to the DIAGNOSTICFUNCTION handle.
%
%   EnergyFreq:: [100]
%     Specify how often the SVM energy is computed.
%
%   HOMKERMAP:: [empty]
%     Specify the use of an Homogeneus Kernel map for the training
%     data (See [2],[3]). The passed value N is such that a 2*N+1
%     dimensional approximated kernel map is computed. Each
%     training data point is expanded online into a vector of
%     dimension 2*N+1.
%
%   KChi2::
%     Compute the map for the Chi2 kernel.
%
%   KINTERS::
%     Compute the map for the intersection kernel.
%
%   KL1::
%     Same as KINTERS, but deprecated as the name is not fully
%     accurate.
%
%   KJS::
%     Compute the map for the JS (Jensen-Shannon) kernel.
%
%   Period:: [automatically tuned]
%     Set the period of the kernel specturm. The approximation is
%     based on periodicizing the kernel specturm. If not specified,
%     the period is automatically set based on the heuristic described
%     in [2].
%
%   Window:: [RECTANGULAR]
%     Set the window used to truncate the spectrum before The window
%     can be either RECTANGULAR or UNIFORM window. See [2] and the API
%     documentation for details.
%
%   Gamma:: [1]
%     Set the homogeneity degree of the kernel. The standard kernels
%     are 1-homogeneous, but sometimes smaller values perform better
%     in applications. See [2] for details.
%
%   Verbose::
%     Be verbose.
%
%   Example::
%     The options StartingModel and StartingIteration can be used
%     to continue training. I.e., the command
%
%       vl_twister('state',0) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',1000) ;
%
%     produces the same result as the sequence
%
%       vl_twister('state',0) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',500) ;
%       w = vl_pegasos(x,y,lambda,'NumIterations',500, ...
%                      'StartingIteration', 501, ...
%                      'StartingModel', w) ;
%
%   REFERENCES::
%     [1] S. Shalev-Shwartz, Y. Singer, N. Srebro, and
%     A. Cotter. Pegasos: Primal Estimated sub-GrAdient SOlver for
%     SVM. MBP, 2010.
%
%     [2] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     Proc. CVPR, 2010.
%
%     [3] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     PAMI, 2011 (submitted).
%
%   See also: VL_HOMKERMAP(), VL_HELP().

% AUTHORIGHTS


function a = i4mat_elim ( m, n, a )

%*****************************************************************************80
%
%% I4MAT_ELIM carries out exact Gauss elimination on an I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, integer A(M,N), the matrix to be Gauss eliminated.
%
%    Output, integer A(M,N), the Gauss-eliminated matrix.
%

%
%  Initialize the swap parity counter.
%
  iswap = 1;
%
%  For each column JCOL...
%
  for jcol = 1 : min ( m, n )
%
%  Find the maximum element in rows JCOL through M.
%
    amax = abs ( a(jcol,jcol) );
    imax = jcol;

    for i = jcol + 1 : m
      if ( amax < abs ( a(i,jcol) ) )
        amax = abs ( a(i,jcol) );
        imax = i;
      end
    end
%
%  If the maximum entry is nonzero, then...
%
    if ( amax ~= 0 )
%
%  If the maximum entry does not occur in row JCOL, then swap rows.
%
      if ( imax ~= jcol )
        iswap = -iswap;
        temp(1:n) = a(jcol,1:n);
        a(jcol,1:n) = a(imax,1:n);
        a(imax,1:n) = temp(1:n);
      end
%
%  Eliminate all nonzero entries in column JCOL, below the diagonal entry.
%
      for i = jcol+1 : m

        if ( a(i,jcol) ~= 0 )

          jmult = a(i,jcol);
          imult = a(jcol,jcol);
          ifact = i4_gcd ( imult, jmult );
          imult = imult / ifact;
          jmult = jmult / ifact;

          for j = jcol : n
            a(i,j) = jmult * a(jcol,j) - imult * a(i,j);
          end

        end

      end
%
%  Remove any row or column factors.
%
      [ a, irow, icol ] = i4mat_red ( m, n, a );

    end

  end

  return
end


%A 2D uniform PDF function.
%xrange and yrange are inclusive limits of region
%z is a 2D sample point.
function val = unifpdf_2d(xrange, yrange, z)
    minX = xrange(1);
    maxX = xrange(2);
    minY = yrange(1);
    maxY = yrange(2);
    evalX = z(1);
    evalY = z(2);
    if(evalX < minX)
        val = 0;
        return;
    elseif(evalX > maxX)
        val = 0;
        return
    elseif (evalY < minY)
        val = 0;
        return;
    elseif(evalY > maxY)
        val = 0;
        return;
    else
        val = 1 / ((maxX - minX) * (maxY - minY));
    end

end


function plane = createPlane(varargin)
%CREATEPLANE Create a plane in parametrized form.
%
%   PLANE = createPlane(P1, P2, P3)
%   creates a plane containing the 3 points
%
%   PLANE = createPlane(PTS)
%   The 3 points are packed into a single 3x3 array.
%
%   PLANE = createPlane(P0, N);
%   Creates a plane from a point and from a normal to the plane. The
%   parameter N is given either as a 3D vector (1-by-3 row vector), or as
%   [THETA PHI], where THETA is the colatitute (angle with the vertical
%   axis) and PHI is angle with Ox axis, counted counter-clockwise (both
%   given in radians).
%
%   PLANE = createPlane(P0, Dip, DipDir);
%   Creates a plane from a point and from a dip and dip direction angles
%   of the plane. Parameters Dip and DipDir angles are given as numbers.
%   Dip : maximum inclination to the horizontal.
%   DipDir : direction of the horizontal trace of the line of dip,
%            measured clockwise from north.
%
%   The created plane data has the following format:
%   PLANE = [X0 Y0 Z0  DX1 DY1 DZ1  DX2 DY2 DZ2], with
%   - (X0, Y0, Z0) is a point belonging to the plane
%   - (DX1, DY1, DZ1) is a first direction vector
%   - (DX2, DY2, DZ2) is a second direction vector
%   The 2 direction vectors are normalized and orthogonal.
%
%   See also
%   planes3d, medianPlane

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-02-18
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

if length(varargin) == 1
    var = varargin{1};

    if iscell(var)
        plane = zeros([length(var) 9]);
        for i=1:length(var)
            plane(i,:) = createPlane(var{i});
        end
    elseif size(var, 1) >= 3
        % 3 points in a single array
        p1 = var(1,:);
        p2 = var(2,:);
        p3 = var(3,:);

        % create direction vectors
        v1 = p2 - p1;
        v2 = p3 - p1;

        % create plane
        plane = normalizePlane([p1 v1 v2]);
        return;
    end

elseif length(varargin) == 2
    % plane origin
    p0 = varargin{1};

    % second parameter is either a 3D vector or a 3D angle (2 params)
    var = varargin{2};
    if size(var, 2) == 2
        % normal is given in spherical coordinates
        n = sph2cart2([var ones(size(var, 1))]);
    elseif size(var, 2)==3
        % normal is given by a 3D vector
        n = normalizeVector3d(var);
    else
        error ('wrong number of parameters in createPlane');
    end

    % ensure same dimension for parameters
    if size(p0, 1)==1
        p0 = repmat(p0, [size(n, 1) 1]);
    end
    if size(n, 1)==1
        n = repmat(n, [size(p0, 1) 1]);
    end

    % find a vector not colinear to the normal
    v0 = repmat([1 0 0], [size(p0, 1) 1]);
    inds = vectorNorm3d(cross(n, v0, 2))<1e-14;
    v0(inds, :) = repmat([0 1 0], [sum(inds) 1]);
%     if abs(cross(n, v0, 2))<1e-14
%         v0 = repmat([0 1 0], [size(p0, 1) 1]);
%     end

    % create direction vectors
    v1 = normalizeVector3d(cross(n, v0, 2));
    v2 = -normalizeVector3d(cross(v1, n, 2));

    % concatenate result in the array representing the plane
    plane = [p0 v1 v2];
    return;

elseif length(varargin)==3
    var1 = varargin{1};
    var2 = varargin{2};
    var3 = varargin{3};

    if size(var1, 2) == 3 && size(var2, 2) == 3 && size(var3, 2) == 3
        p1 = var1;
        p2 = var2;
        p3 = var3;

        % create direction vectors
        v1 = p2 - p1;
        v2 = p3 - p1;

        plane = normalizePlane([p1 v1 v2]);
        return;
    elseif size(var1, 2) == 3 && size(var2, 2) == 1 && size(var3, 2) == 1
        p0 = var1;
        n = [sin(var2)*sin(var3) sin(var2)*cos(var3) cos(var2)];

        % find a vector not colinear to the normal
        v0 = repmat([1 0 0], [size(p0, 1) 1]);
        inds = vectorNorm3d(cross(n, v0, 2))<1e-14;
        v0(inds, :) = repmat([0 1 0], [sum(inds) 1]);

        % create direction vectors
        v1 = normalizeVector3d(cross(n, v0, 2));
        v2 = -normalizeVector3d(cross(v1, n, 2));

        % concatenate result in the array representing the plane
        plane = [p0 v1 v2];
        return;
    else
        error('Wrong argument in "createPlane".');
    end
else
    error('Wrong number of arguments in "createPlane".');
end


function [ n_data, x, fx ] = normal_01_cdf_values ( n_data )

%*****************************************************************************80
%
%% NORMAL_01_CDF_VALUES returns some values of the Normal 01 CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = NormalDistribution [ 0, 1 ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 17;

  fx_vec = [ ...
     0.5000000000000000E+00, ...
     0.5398278372770290E+00, ...
     0.5792597094391030E+00, ...
     0.6179114221889526E+00, ...
     0.6554217416103242E+00, ...
     0.6914624612740131E+00, ...
     0.7257468822499270E+00, ...
     0.7580363477769270E+00, ...
     0.7881446014166033E+00, ...
     0.8159398746532405E+00, ...
     0.8413447460685429E+00, ...
     0.9331927987311419E+00, ...
     0.9772498680518208E+00, ...
     0.9937903346742239E+00, ...
     0.9986501019683699E+00, ...
     0.9997673709209645E+00, ...
     0.9999683287581669E+00 ];

  x_vec = [ ...
     0.0000000000000000E+00, ...
     0.1000000000000000E+00, ...
     0.2000000000000000E+00, ...
     0.3000000000000000E+00, ...
     0.4000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.6000000000000000E+00, ...
     0.7000000000000000E+00, ...
     0.8000000000000000E+00, ...
     0.9000000000000000E+00, ...
     0.1000000000000000E+01, ...
     0.1500000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2500000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3500000000000000E+01, ...
     0.4000000000000000E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


% effOFexpDesignsNoOverlap2
% ER-fMRI data analysis
% script:
% throughout we assume TR=2.

loadDataYes=1; % =1 -> loads existing data file

%cd /home/giedrius/giedrius/projects/matlab/erfmri
pluginCorrYes=0;
overlapYes=0;
nReps=10000; %
nVals=3;
n2=12; % assumed # TRs to cover HDR fn
pwRange=6:10;

daleYes=1; % 1-usual Dale efficiency. 0-Fisher
% event matrices are defined based on vectors with the average
% energy removed

cutEndPad=1; %cutting reduces efficiency!

if ~loadDataYes,
rndEffAll=[];
mEffAll=[];

tic
for pwr=pwRange,
% creating event vectors
ms=m2bin([mseq(2,pwr,0,1)])'; % fixing to the length of 2^n
n=length(ms)  % scan duration in TRs
if pluginCorrYes
  b=[0.406;0.8825]; % parameters from SPG fMRI data
  fittedACorr=autocorrFnct(b,1:n+(n2-1)*rem(cutEndPad+1,2));
  Cninv=inv(toeplitz(fittedACorr));
else
  Cninv=eye(n+(n2-1)*rem(cutEndPad+1,2));
end;

mRange=(2^pwRange(1)-2);
mEff=zeros(1,mRange);

%shift1=ceil(2^pwRange(1)/2);
%XXXXXXXXXXXXXXXXXXXXX
for shift=1:mRange,
	%shift=2^(pwr-1)+5;
	foo=[ms(shift+1:end) ms(1:shift)];%shifted version
	%foo1=[ms(shift1+1:end) ms(1:shift1)];%shifted version
	foo=ms+foo*2; %+foo1*4;

	mEvent=zeros(nVals,n);
	for k=1:nVals,
   	  ind=find(foo==k);
   	  mEvent(k,ind)=1;
	end;

	% defining event matrix as convolution matrix
	%eventMatrix=makeEventMtrx(mEvent-(ones(length(mEvent),1)*...
	%sum(mEvent'))'/length(mEvent),n2);

	eventMatrix=makeEventMtrx(mEvent,n2);

        if cutEndPad, eventMatrix=eventMatrix(1:n,:); end;
	eventMatrix=eventMatrix-ones(size(eventMatrix,1),1)* ...
	    sum(eventMatrix)/size(eventMatrix,1);

   if daleYes,
	  designEff=1/trace(inv(eventMatrix'*Cninv*eventMatrix));
	else
	  designEff=trace(eventMatrix'*Cninv*eventMatrix);
	end;

	mEff(shift)=designEff;
end;

%mProbab=sum(mEvent'); mProbab=mean(mProbab)/n;
plot(mEff,'k','LineWidth',2);
set(gca,'FontSize',12);
xlabel('Cyclical shift of event vector #3','FontSize',16)
ylabel('Efficiency','FontSize',16)
set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,5,4]);
%axis([0 n 0 .5])
drawnow;
%print -dpsc2 cyclingNonOverlapping
pause

mEffAll=[mEffAll, max(mEff)];

% for simply randomized designs:
nVals=size(mEvent,1);
%n=n+1
if pluginCorrYes
  b=[0.406;0.8825]; % parameters from SPG fMRI data
  fittedACorr=autocorrFnct(b,1:n+(n2-1)*rem(cutEndPad+1,2));
  Cninv=inv(toeplitz(fittedACorr));
else
  Cninv=eye(n+(n2-1)*rem(cutEndPad+1,2));
end;

if pwr<8, correction=0.02;
elseif pwr<9, correction=0.005;
elseif pwr==10, correction=0.001;
end;
correction=0;

pEv=nVals/(nVals+1)+correction; %

rndEff=zeros(1,nReps);
for k=1:nReps;
   rndEvent=balancedRnd(n,nVals,pEv,overlapYes);%sum(rndEvent')

   % defining event matrix as convolution matrix
   %eventMatrix=makeEventMtrx(rndEvent-(ones(length(rndEvent),1)*...
   %sum(rndEvent'))'/length(rndEvent),n2);

   eventMatrix=makeEventMtrx(rndEvent,n2);

   if cutEndPad, eventMatrix=eventMatrix(1:n,:); end;
   eventMatrix=eventMatrix-ones(size(eventMatrix,1),1)* ...
       sum(eventMatrix)/size(eventMatrix,1);

   if daleYes,
	  designEff=1/trace(inv(eventMatrix'*Cninv*eventMatrix));
   else
	  designEff=trace(eventMatrix'*Cninv*eventMatrix);
   end;
   rndEff(k)=designEff;
end;

rndEffAll=[rndEffAll;rndEff];
end;
toc;

else
  eval(['load dataEffNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);
end %fi loadDataYes

nn=2.^[pwRange]-1;

%[b,a]=hist(rndEff,20);
%bar(a,b/nReps); hold on;
%plot(max(mEff),0,'k*');

figure(1); clf;
loglog(nn,mEffAll,'r*');hold on;

% theoretical max -- only for one event type:
%theoMax=(nn+1)/n2/4;
%loglog(nn,theoMax,'g+');hold on;

p99=prctile(rndEffAll',99.9);
p00=prctile(rndEffAll',0.1);
med=median(rndEffAll');
loglog(nn,med,'k.-','LineWidth',2);
loglog([nn;nn],[p00;p99],'k')
loglog([nn*.93;nn*1.1],[p00;p00],'k')
loglog([nn*.93;nn*1.1],[p99;p99],'k')

axis([50 10^3*1.2 10^-2 10^1]);
%set(gca,'YTick',[0.003 .01 .1 .3 1 3 10 30]);
set(gca,'XTick',nn);
set(gca,'LineWidth',2,'FontSize',12);

xlabel('Sequence length','FontSize',12);
ylabel('Efficiency','FontSize',12);
title(['No Overlap: n_e=',num2str(nVals),' p=',num2str(pEv),' n_h=',num2str(n2)]);

set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,4,3]);
eval(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);
disp(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);

figure(2); clf;
semilogx(nn,mEffAll./max(rndEffAll'),'-ok','LineWidth',2); hold on;
semilogx(nn,mEffAll./med,':+k','LineWidth',2)
set(gca,'Position',[.2,.15,.7,.7]);
xlabel('Sequence length','FontSize',12);
ylabel('m-seq/random efficiency ratio','FontSize',12);
title(['No Overlap: n_e=',num2str(nVals),' p=',num2str(pEv),' n_h=',num2str(n2)]);
set(gca,'XTick',nn)
set(gca,'LineWidth',2,'FontSize',12);
set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,4,3]);
axis([50 10^3*1.2 .9 2.5]);
eval(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nhRATIO']);
disp(['print -dpsc2 effNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nhRATIO']);

eval(['save dataEffNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh rndEffAll p99 p00 med mEffAll']);
disp(['save dataEffNOV',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh rndEffAll p99 p00 med mEffAll']);


%% Lucas-Kanade Optical Flow
%
% In this demo, we will:
%
% * understand the concepts of optical flow and its estimation using
%   Lucas-Kanade method.
% * use functions like |cv.calcOpticalFlowPyrLK| to track feature points
%   in a video.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/d7/d8b/tutorial_py_lucas_kanade.html>
%

%% Optical Flow
%
% Optical flow is the pattern of apparent motion of image objects between two
% consecutive frames caused by the movemement of object or camera. It is 2D
% vector field where each vector is a displacement vector showing the movement
% of points from first frame to second. Consider the image below:
%
% <<https://docs.opencv.org/3.2.0/optical_flow_basic1.jpg>>
%
% It shows a ball moving in 5 consecutive frames. The arrow shows its
% displacement vector. Optical flow has many applications in areas like:
%
% * Structure from Motion
% * Video Compression
% * Video Stabilization
% * etc.
%
% Optical flow works on several assumptions:
%
% # The pixel intensities of an object do not change between consecutive frames
% # Neighbouring pixels have similar motion
%
% Consider a pixel $I(x,y,t)$ in first frame. It moves by distance $(dx,dy)$
% in next frame taken after $dt$ time. So since those pixels are the same and
% intensity does not change, we can say,
%
% $$I(x,y,t) = I(x+dx, y+dy, t+dt)$$
%
% Then take taylor series approximation of right-hand side, remove common
% terms and divide by $dt$ to get the following equation:
%
% $$f_x u + f_y v + f_t = 0$$
%
% where
%
% $$f_x = \frac{\partial f}{\partial x} \quad ; \quad
%   f_y = \frac{\partial f}{\partial y}$$
%
% $$u = \frac{dx}{dt} \quad ; \quad
%   v = \frac{dy}{dt}$$
%
% Above equation is called Optical Flow equation. In it, we can find $f_x$ and
% $f_y$, they are image gradients. Similarly $f_t$ is the gradient along time.
% But $(u,v)$ is unknown. We cannot solve this one equation with two unknown
% variables. So several methods are provided to solve this problem and one of
% them is Lucas-Kanade.
%

%% Lucas-Kanade method
%
% We have seen an assumption before, that all the neighbouring pixels will
% have similar motion. Lucas-Kanade method takes a 3x3 patch around the point.
% So all the 9 points have the same motion. We can find $(f_x, f_y, f_t)$ for
% these 9 points. So now our problem becomes solving 9 equations with two
% unknown variables which is over-determined. A better solution is obtained
% with least square fit method. Below is the final solution which is two
% equation-two unknown problem and solve to get the solution:
%
% $$
% \left[\matrix{u \cr v}\right] =
% \left[\matrix{
%   \sum_{i}{f_{x_i}}^2       & \sum_{i}{f_{x_i} f_{y_i}} \cr
%   \sum_{i}{f_{x_i} f_{y_i}} & \sum_{i}{f_{y_i}}^2
% }\right]^{-1}
% \left[\matrix{
%   - \sum_{i}{f_{x_i} f_{t_i}} \cr
%   - \sum_{i}{f_{y_i} f_{t_i}}
% }\right]
% $$
%
% (Note similarity of inverse matrix with Harris corner detector. It denotes
% that corners are better points to be tracked.)
%
% So from user point of view, idea is simple, we give some points to track,
% we receive the optical flow vectors of those points. But again there are
% some problems. Until now, we were dealing with small motions. So it fails
% when there is large motion. So again we go for pyramids. When we go up in
% the pyramid, small motions are removed and large motions becomes small
% motions. So applying Lucas-Kanade there, we get optical flow along with the
% scale.
%

%% Lucas-Kanade Optical Flow in OpenCV
%
% OpenCV provides all these in a single function, |cv.calcOpticalFlowPyrLK|.
% Here, we create a simple application which tracks some points in a video.
% To decide the points, we use |cv.goodFeaturesToTrack|. We take the first
% frame, detect some Shi-Tomasi corner points in it, then we iteratively track
% those points using Lucas-Kanade optical flow. For the function
% |cv.calcOpticalFlowPyrLK| we pass the previous frame, previous points and
% next frame. It returns next points along with some status numbers which has
% a value of 1 if next point is found, else zero. We iteratively pass these
% next points as previous points in next step. See the code below.
%
% This code doesn't check how correct are the next keypoints. So even if any
% feature point disappears in image, there is a chance that optical flow finds
% the next point which may look close to it. So actually for a robust tracking,
% corner points should be detected in particular intervals. OpenCV samples
% comes up with such a sample which finds the feature points at every 5 frames.
% It also run a backward-check of the optical flow points got to select only
% good ones.
%

%% Video
% Prepare video source
if mexopencv.require('vision')
    vid = fullfile(toolboxdir('vision'), 'visiondata', 'visiontraffic.avi');
    %cap.PosFrames = 80;  % skip first few seconds with no motion
elseif true
    vid = fullfile(mexopencv.root(), 'test', '768x576.avi');
else
    vid = fullfile(mexopencv.root(), 'test', 'sparse_optical_flow.avi');
    if exist(vid, 'file') ~= 2
        disp('Downloading video...')
        url = 'https://cdn.rawgit.com/opencv/opencv_extra/3.2.0/gpu_demos_pack/demos/sparse_optical_flow/data/sparse_optical_flow.avi';
        urlwrite(url, vid);
    end
end
if exist(vid, 'file') ~= 2, vid = 0; end
cap = cv.VideoCapture(vid);
assert(cap.isOpened(), 'Failed to initialize capturing');

%% First frame
% Grab first frame
frame = cap.read();
assert(~isempty(frame), 'Failed to read frame');
prev = cv.cvtColor(frame, 'RGB2GRAY');

%%
% Detect corners using Shi-Tomasi method (performed only once at the start)
pts0 = cv.goodFeaturesToTrack(prev, ...
    'MaxCorners',100, 'QualityLevel',0.3, 'MinDistance',7, 'BlockSize',7);
pts0 = cat(1, pts0{:});
assert(~isempty(pts0), 'No corners found');
fprintf('%d points\n', size(pts0,1));

%%
% Initialize a mask image for drawing purposes
% (on which point tracks are drawn and remembered)
mask = zeros(size(frame), class(frame));

%%
% Some random colors for plotting
N = 64;
clrs = uint8(hsv(N) * 255);  % randi([0 255], [N 3])
clrs(:,4) = 0;

%%
% Plot
hImg = imshow(frame);
title('PyrLK [Sparse]')

%% Main loop
while ishghandle(hImg)
    % Grab next frame
    frame = cap.read();
    if isempty(frame), break; end
    next = cv.cvtColor(frame, 'RGB2GRAY');

    % Calculate sparse optical flow using Lucas-Kanade method to track points
    [pts1, status] = cv.calcOpticalFlowPyrLK(prev, next, pts0, ...
        'WinSize',[15 15], 'MaxLevel',2, ...
        'Criteria',struct('type','Count+EPS', 'maxCount',10, 'epsilon',0.03));
    pts1 = cat(1, pts1{:});
    status = logical(status);

    % Keep good points (points for which the flow has been found)
    if ~any(status)
        break;
    elseif ~all(status)
        fprintf('%d points\n', nnz(status));
    end
    pts0 = pts0(status,:);
    pts1 = pts1(status,:);

    % Draw latest locations of tracked points
    clr = clrs(rem((1:size(pts1,1))-1,N)+1,:);  % cycle through colors
    frame = cv.circle(frame, pts1, 5, 'Colors',clr, 'Thickness','Filled');

    % Draw point tracks (comet-like plot)
    mask = cv.line(mask, pts1, pts0, 'Colors',clr, 'Thickness',2);
    frame = cv.addWeighted(frame, 0.5, mask, 0.5, 0.0);

    % Display result
    set(hImg, 'CData',frame);
    drawnow;

    % Next iteration: update the previous frame and previous points
    prev = next;
    pts0 = pts1;
end
cap.release();


%% OPTI Toolbox Global Nonlinear Program Demo
%
% This file contains a number of Global NLP problems and demonstrates how
% to solve them using the OPTI Toolbox. You should read and complete
% Basic_demo.m & LP_demo.m BEFORE running the below examples.
%
%   Copyright (C) 2012 Jonathan Currie (I2C2)

%% Determing which Solver to Use
% OPTI Toolbox comes with a number of NLP solvers, thus to determine which
% ones are available on your system you can type:

checkSolver('NLP')

% Note the columns DR and GL. A cross in DR indicates the solver requires
% 1st (and perhaps 2nd) derivatives, while a cross in GL indicates the
% solver can solve Global Optimization problems. For noisy problems unless
% you have exact derivatives, avoid solvers with a cross in DR.

%% Typical Global Optimization Problems
% Global optimization problems result from any of the following circumstances:
%
%       - Objectives containing noise / a stochastic element
%       - Non-convex functions (functions which are not a bowl / hill in 2D)
%       - Objectives that include periodic functions (sin, cos)
%       - Parameter estimation of ODEs solved with adaptive step integrators
%       - Any problem that contains multiple local minima.
%
% An extreme example from Wolfram is shown below:

%Objective
fun = @(x) norm([x(1) - sin(2*x(1) + 3*x(2)) - cos(3*x(1) - 5*x(2));
          x(2) - sin(x(1) - 2*x(2)) + cos(x(1) + 3*x(2))]);
lb = [-4;-4]; ub = [4;4];
x0 = [-4;-4];

%Plot
n = 1e2;
x = linspace(-4,4,n); y = linspace(-4,4,n); Z = zeros(n,n);
for i = 1:n
    for j = 1:n
        Z(j,i) = fun([x(i),y(j)]);
    end
end
surfc(x,y,Z)
colormap summer; shading interp; lighting phong; view(-38,58);
xlabel('x1'); ylabel('x2'); zlabel('obj'); title('Wolfram Global Optimization Problem');

%% Example 1 - Basic Setup
% The main difference when solving a Global NLP is that OPTI treats Global
% and Local NLP problems identically, and therefore you will have to specify
% a Global solver. For this example we will build 4 OPTI objects with 4
% different solvers:

%Build OPTI Problem
prob = optiprob('fun',fun,'bounds',lb,ub);
%Choose Global Solver
opts1 = optiset('solver','nomad');
opts2 = optiset('solver','pswarm');
opts3 = optiset('solver','nlopt','solverOpts',nloptset('algorithm','GN_DIRECT'));
opts4 = optiset('solver','ipopt','warnings','off');

%Pass to OPTI Constructor for Error Checking + Setup
Opt1 = opti(prob,opts1);
Opt2 = opti(prob,opts2);
Opt3 = opti(prob,opts3);
Opt4 = opti(prob,opts4);

%% Example 1 - Solving the Problem
% Call solve to solve the problem. Check the plot for a comparison of the
% solution points. Note NOMAD and NLOPT are deterministic with the current
% settings, PSWARM includes random elements, and IPOPT is for comparison of
% a local solution.

[x1,fval1] = solve(Opt1,x0);
[x2,fval2] = solve(Opt2,x0);
[x3,fval3] = solve(Opt3,x0);
[x4,fval4] = solve(Opt4,x0);

view(0,90); hold on;
plot3(x0(1),x0(2),10,'kx','markersize',10);
plot3(x1(1),x1(2),10,'ro'); text(x1(1)+0.1,x1(2)+0.1,10,sprintf('NOMAD: %f',fval1));
plot3(x2(1),x2(2),10,'ro'); text(x2(1)+0.1,x2(2)-0.1,10,sprintf('PSWARM: %f',fval2));
plot3(x3(1),x3(2),10,'ro'); text(x3(1)+0.1,x3(2)+0.2,10,sprintf('NLOPT: %f',fval3));
plot3(x4(1),x4(2),10,'ro'); text(x4(1)+0.1,x4(2)+0.1,10,sprintf('IPOPT: %f',fval4));
hold off;

%% Problem 2 - Quartic
% Includes Linear and Nonlinear Constraints

%Problem
fun = @(x) x(1)^4 - 14*x(1)^2 + 24*x(1) - x(2)^2;

%Linear Constraints
A = [-1 1]; b = 8;
%Nonlinear Constraints
nlcon = @(x) (-x(1)^2) - 2*x(1) + x(2);
nlrhs = -2;
nle = -1;
%Bounds + Starting Guess
lb = [-8;0];
ub = [10;10];
x0 = [0;0];

%% Example 2
% Solving a constrained global optimization problem. Note linear
% constraints will be converted to nonlinear ones for this solver

opts = optiset('solver','nomad','solverOpts',nomadset('direction_type','lt 2n'));
Opt = opti('fun',fun,'ineq',A,b,'nlmix',nlcon,nlrhs,nle,'bounds',lb,ub,'options',opts)

%% Example 2 - Problem Solved
% This will take 5-7 seconds...

[x,fval,exitflag,info] = solve(Opt,x0)

%% Problem 3 - Saddle Point
% Note x0 is on the local minima side of the saddle

%Problem
fun = @(x) -2*x(1)*x(2);

%Constraints
lb = [-0.5;-0.5];
ub = [1;1];
x0 = [-0.3;-0.3];

%Plot
n = 1e2;
x = linspace(-0.5,1,n); y = linspace(-0.5,1,n); Z = zeros(n,n);
for i = 1:n
    for j = 1:n
        Z(j,i) = fun([x(i),y(j)]);
    end
end
surfc(x,y,Z); hold on; plot3(x0(1),x0(2),fun(x0),'r.','markersize',20); hold off;
colormap winter; shading flat; lighting gouraud; view(18,28);
xlabel('x1'); ylabel('x2'); zlabel('obj'); title('Saddle Point Optimization Problem');

%% Example 3 - Solving with PSwarm
% PSwarm solves bounded and linearly constrained global problems

Opt = opti('fun',fun,'bounds',lb,ub,'options',optiset('solver','pswarm'))

%% Example 3 - Problem Solved.
% Note the PSwarm found the solution of the otherside of the saddle, check
% the OPTI solution plot

[x,fval,exitflag,info] = solve(Opt,x0)
plot(Opt,3)

%% Problem 4 - White Box Quartic
% The following problem is the same as problem 2, however this time we are
% going to solve it using a white box solver (SCIP). The SCIP interface
% will parse the following functions into an algebraic description,
% allowing SCIP to find a global solution to this problem.

%Problem
fun = @(x) x(1)^4 - 14*x(1)^2 + 24*x(1) - x(2)^2;

%Linear Constraints
A = [-1 1]; b = 8;
%Nonlinear Constraints
nlcon = @(x) (-x(1)^2) - 2*x(1) + x(2);
nlrhs = -2;
nle = -1;
%Bounds + Starting Guess
lb = [-8;0];
ub = [10;10];
x0 = [0;0];

%% Example 4 - Solving with SCIP
% SCIP solves a subset of nonlinear and mixed integer problems, provided
% the problem is deterministics and constains a subset of allowable functions.

Opt = opti('fun',fun,'ineq',A,b,'nlmix',nlcon,nlrhs,nle,'bounds',lb,ub,...
           'options',optiset('solver','scip'))

%% Example 4 - Problem Solved.
% Solution returned is guaranteed (within numerical tolerances) to be the
% global solution. Note you must have an academic version of OPTI to use
% SCIP.

[x,fval,exitflag,info] = solve(Opt,x0)

%% Summary
% While Global Optimization solvers may take longer, many real engineering
% problems result in noisy or non-convex objectives and local solvers will
% often struggle to return a result, or fall into the closest local
% optima. OPTI provides a range of competitive blackbox global optimization
% solvers which can return much better results, as well as a white box
% solver for academic users which guarantees a global solution.


function Wgrid = Fdirs2grid(W, aziRes, polarRes, CLOSED)
%FDIRS2GRID Replicate vector function values on a regular grid
%
%   Fdirs2grid takes a vector of values of function evaluated at a
%   spherical grid with the grid2dirs function, and convert it back to a
%   2D grid. Useful for plotting with functions such as surf, or for
%   numerical integration numerically spherical functions.
%
%   W:  column vector of function values evaluated at each grid direction,
%       with the direction ordering given by grid2dirs. If W is a matrix
%       then each column is considered as a separate function to be
%       converted
%   aziRes: azimuth resolution of the grid in degrees (should be the same
%           as the one used in the grid2dirs function
%   polarRes:   inclination resolution of the grid in degrees (should be
%               the same as the one used in the grid2dirs function
%   CLOSED: {0,1} if true then the returned matrix replicates the first
%           column of function values at 0deg azimuth also at 360deg,
%           useful for 3D plotting so that the shape does not have a
%           hole in the end (see plotSphFunction test script)
%
%   Wgrid:  if W is a vector then Wgrid is the 2D matrix of the function
%           values replicated on the grid points. If W is a matrix, then
%           Wgrid is a 3D matrix with one grid per column of W, on the 3rd
%           dimension.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Archontis Politis, 10/10/2013
%   archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if mod(360, aziRes) ~= 0 || mod(180, polarRes) ~= 0
    error('azimuth or elevation resolution should divide exactly 360 and 180deg')
end

if nargin<4
    CLOSED = 0;
end

Nphi = 360/aziRes;
Ntheta = 180/polarRes+1;

Nf = size(W, 2);
Wgrid = zeros(Nphi, Ntheta, Nf);
for i = 1:Nf

    Wgrid(:, 2:end-1, i) = reshape(W(2:end-1, i), Nphi, Ntheta-2);
    Wgrid(:, 1, i) = ones(Nphi, 1) * W(1, i);
    Wgrid(:, end, i) = ones(Nphi, 1) * W(end, i);
end

if Nf~=1
    Wgrid = permute(Wgrid, [2 1 3]);
else
    Wgrid = Wgrid.';
end

if CLOSED
    Wgrid = horzcat(Wgrid, Wgrid(:,1,:));
end


function [ a, ipvt, info ] = dgefa ( a, lda, n )

%*****************************************************************************80
%
%% DGEFA factors a real matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix A.
%
%    Output, real A(LDA,N), an upper triangular matrix and the multipliers
%    used to obtain it.  The factorization can be written A=L*U, where L is
%    a product of permutation and unit lower triangular matrices, and U is
%    upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO, singularity indicator.
%    0, normal value.
%    K, if U(K,K) == 0.  This is not an error condition for this subroutine,
%    but it does indicate that DGESL or DGEDI will divide by zero if called.
%    Use RCOND in DGECO for a reliable indication of singularity.
%

%
%  Gaussian elimination with partial pivoting.
%
  info = 0;

  for k = 1 : n - 1
%
%  Find L = pivot index.
%
    l = idamax ( n-k+1, a(k:n,k), 1 ) + k - 1;
    ipvt(k) = l;
%
%  Zero pivot implies this column already triangularized.
%
    if ( a(l,k) == 0.0 )
      info = k;
      continue
    end
%
%  Interchange if necessary.
%
    if ( l ~= k )
      [ a(l,k), a(k,k) ] = r8_swap ( a(l,k), a(k,k) );
    end
%
%  Compute multipliers.
%
    a(k+1:n,k) = - a(k+1:n,k) / a(k,k);
%
%  Row elimination with column indexing.
%
    for j = k+1 : n
      t = a(l,j);
      if ( l ~= k )
        a(l,j) = a(k,j);
        a(k,j) = t;
      end
      a(k+1:n,j) = daxpy ( n-k, t, a(k+1:n,k), 1, a(k+1:n,j), 1 );
    end

  end

  ipvt(n) = n;

  if ( a(n,n) == 0.0 )
    info = n;
  end

  return
end


function P = pu2_encode( L )
% Perceptually uniform luminance encoding using the CSF from HDR-VDP-2
%
% P = pu2_encode( L )
%
% Transforms absolute luminance values L into approximately perceptually
% uniform values P.
%
% This is meant to be used with display-referred quality metrics - the
% image values much correspond to the luminance emitted from the target
% HDR display.
%
% This is an improved encoding described in detail in the paper:
%
% Aydin, T. O., Mantiuk, R., & Seidel, H.-P. (2008). Extending quality
% metrics to full luminance range images. Proceedings of SPIE (p. 68060B–10).
% SPIE. doi:10.1117/12.765095
%
% Note that the P-values can be negative or greater than 255. Most metrics
% can deal with such values.
%
% Copyright (c) 2014, Rafal Mantiuk <mantiuk@gmail.com>

persistent P_lut;
persistent l_lut;

l_min = -5;
l_max = 10;

if( isempty( P_lut ) ) % caching for better performance

    metric_par.csf_sa = [30.162 4.0627 1.6596 0.2712];
    l_lut = linspace( l_min, l_max, 2^12 );
    S = hdrvdp_joint_rod_cone_sens( 10.^l_lut, metric_par );

    [~, P_lut] = build_jndspace_from_S(l_lut,S);
end

l = log10(max(min(L,10^l_max),10^l_min));

pu_l = 31.9270;
pu_h = 149.9244;

P = 255 * (interp1( l_lut, P_lut, l ) - pu_l) / (pu_h-pu_l);

end

function S = hdrvdp_joint_rod_cone_sens( la, metric_par )
% Copyright (c) 2011, Rafal Mantiuk <mantiuk@gmail.com>

% Permission to use, copy, modify, and/or distribute this software for any
% purpose with or without fee is hereby granted, provided that the above
% copyright notice and this permission notice appear in all copies.
%
% THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
% WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
% MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
% ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
% WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
% ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
% OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

cvi_sens_drop = metric_par.csf_sa(2); % in the paper - p6
cvi_trans_slope = metric_par.csf_sa(3); % in the paper - p7
cvi_low_slope = metric_par.csf_sa(4); % in the paper - p8

S = metric_par.csf_sa(1) * ( (cvi_sens_drop./la).^cvi_trans_slope+1).^-cvi_low_slope;

end

function [Y jnd] = build_jndspace_from_S(l,S)

L = 10.^l;
dL = zeros(size(L));

for k=1:length(L)
    thr = L(k)/S(k);

    % Different than in the paper because integration is done in the log
    % domain - requires substitution with a Jacobian determinant
    dL(k) = 1/thr * L(k) * log(10);
end

Y = l;
jnd = cumtrapz( l, dL );

end


function [fMc, fBvalue, fAvalue, mu, fSigma] = calc_McEMR(catalog, binInterval)
    % Determine Mc using Entire Magnitude Range (EMR)-method. Calculates also a- and b-value.
    % [fMc, fBvalue, fAvalue, mu, fSigma] = calc_McEMR(catalog, binInterval);
    % -----------------------------------------------------------------------------------------------------
    % Determine Mc using EMR-method. Calculates also a- and b-value.
    % Fitting non-cumulative frequency magnitude distribution above and below Mc:
    % below: Cumulative NORMAL distribution function
    % above: Gutenberg-Richter law
    %
    % Incoming variables:
    % catalog   : EQ catalog
    % binInterval   : Binning interval, usually 0.1
    %
    % Outgoing variables:
    % fMc        : Best estimated magnitude of completeness
    % fBvalue    : b-value
    % fAvalue    : a-value
    % mu        : mu-value of the normal CDF
    % fSigma     : sigma-values of the normal CDF
    %
    % J. Woessner: woessner@seismo.ifg.ethz.ch

    narginchk(2,2);

    % Initialize
    vProbability = [];
    vMc = [];
    vABValue =[];
    mFitRes = [];
    vDeltaBest = [];
    vX_res = [];
    vNmaxBest = [];
    mResult=[];

    % Determine exact time period
    timespan = years(max(catalog.Date) - min(catalog.Date)); % guessing it should be years

    % Set starting value for Mc loop and LSQ fitting procedure
    fMcTry= calc_Mc(catalog, McMethods.MaxCurvature);
    fSmu = abs(fMcTry / 2);
    fSSigma = abs(fMcTry / 4);
    if (fSmu > 1)
        fSmu = fMcTry / 10;
        fSSigma = fMcTry / 20;
    end
    fMcBound = fMcTry;

    % Calculate FMD for original catalog
    [vFMDorga, vNonCFMDorg, fmdbins] = calc_FMD(catalog.Magnitude);
    % convert answer back to this file's expectations...
    vFMDorg = [fmdbins'; vFMDorga']; % as rows
    vNonCFMDorg = [fmdbins'; vNonCFMDorg'];

    fMinMag = min(vNonCFMDorg(1,:));

    % %% Shift to positive values
    % if fMinMag ~= 0
    %     fMcBound = fMcTry-fMinMag;
    % end
    % Loop over Mc-values
    for fMc = round(fMcBound-0.4 : 0.1 : fMcBound+0.4, -1)
        vFMD = vFMDorg;
        vNonCFMD = vNonCFMDorg;
        vNonCFMD = fliplr(vNonCFMD);
        % Calculate a and b-value for GR-law and distribution vNCum
        [~, ~, vSel, ~] = fMagToFitBValue(catalog, vFMD, fMc);
        if (length(catalog.Longitude(vSel)) >= 20)
            %[ fBValue, fStdDev, fAValue] =  calc_bmemag(catalog.Magnitude(vSel), binInterval);
            [fBValue, ~, fAValue] =  calc_bmemag(catalog.Magnitude(vSel), binInterval);
            % Normalize to time period
            vFMD(2,:)       = vFMD(2,:)./timespan; % ceil taken out
            vNonCFMD(2,:)   = vNonCFMD(2,:)./timespan; % ceil removed
            % Compute quantity of earthquakes by power law
            fMaxMagFMD  = max(vNonCFMD(1,:));
            fMinMagFMD  = min(vNonCFMD(1,:));
            vMstep      = fMinMagFMD:0.1:fMaxMagFMD;
            vNCum       = 10.^(fAValue-fBValue.*vMstep); % Cumulative number

            % Compute non-cumulative numbers vN
            fNCumTmp    = 10^(fAValue - fBValue * (fMaxMagFMD + 0.1));
            vNCumTmp    = [vNCum, fNCumTmp ];
            vN          = abs(diff(vNCumTmp));

            % Normalize vN
            vN = vN./timespan;
            % Data selection
            % mData = Non-cumulative FMD values from GR-law and original data
            mData = [vN' vNonCFMD'];
            vSel = (mData(:,2) >= fMc);
            mDataTest = mData(~vSel,:);
            mDataTmp = mData(vSel,:);
            % Check for zeros in observed data
            vSelCheck = (mDataTest(:,3) == 0);
            mDataTest = mDataTest(~vSelCheck,:);
            % Choices of normalization
            fNmax = mDataTmp(1,3); % Frequency of events in Mc bin

            if (~isempty(isempty(fNmax)) &&  ~isnan(fNmax) & fNmax ~= 0 & length(mDataTest(:,1)) > 4)
                mDataTest(:,3) = mDataTest(:,3)/fNmax; % Normalize datavalues for fitting with CDF
                % Move to M=0 to fit with lsq-algorithm
                fMinMagTmp = min(mDataTest(:,2));
                mDataTest(:,2) = mDataTest(:,2)-fMinMagTmp;
                % Curve fitting: Non cumulative part below Mc
                options = optimset('Display','off',...
                    'Tolfun'        , 1e-5,...
                    'TolX'          , 0.001,...
                    'MaxFunEvals'   , 1000,...
                    'MaxIter'       , 1000);
                [vX, resnorm, resid, exitflag, output, lambda, jacobian] = lsqcurvefit(...
                    @calc_normalCDF,[fSmu  fSSigma], mDataTest(:,2), mDataTest(:,3),[],[], options);
                mDataTest(:,1) = normcdf(mDataTest(:,2), vX(1), vX(2))*fNmax;
                if (length(mDataTest(:,2)) > length(vX(1,:)))
                    %% Confidence interval determination
                    % vPred : Predicted values of lognormal function
                    % vPred+-delta : 95% confidence level of true values
                    [vPred,delta] = nlpredci(@calc_normalCDF, mDataTest(:,2), vX, resid, jacobian);
                else
                    vPred = NaN;
                    delta = NaN;
                end % END: This section is due for errors produced with datasets less long than amount of parameters in vX
                % Results of fitting procedure
                mFitRes = [mFitRes; vX resnorm exitflag];
                % Move back to original magnitudes
                mDataTest(:,2) = mDataTest(:,2)+fMinMagTmp;
                % Set data together
                mDataTest(:,3) = mDataTest(:,3)*fNmax;
                mDataPred = [mDataTest; mDataTmp];
                % Denormalize to calculate probabilities
                mDataPred(:,1) = round(mDataPred(:,1).*timespan);
                mDataPred(:,3) = mDataPred(:,3).*timespan;
                vProb_ = calc_log10poisspdf2(mDataPred(:,3), mDataPred(:,1)); % Non-cumulative

                % Sum the probabilities
                fProbability    = (-1) * sum(vProb_,'omitnans');
                vProbability    = [vProbability; fProbability];
                % Move magnitude back
                mDataPred(:,2)  = mDataPred(:,2)+fMinMag;
                vMc             = [vMc; fMc];
                vABValue        = [vABValue; fAValue fBValue];

                % Keep values
                vDeltaBest  = [vDeltaBest; delta];
                vX_res      = [vX_res; vX resnorm exitflag];
                vNmaxBest   = [vNmaxBest; fNmax];

                % Keep best fitting model
                if (fProbability == min(vProbability))
                    vDeltaBest      = delta;
                    vPredBest       = [mDataTest(:,2) vPred*fNmax*timespan delta*fNmax*timespan]; % Gives back uncertainty
                    mDatPredBest    = [mDataPred];
                end
            else
                %disp('Not enough data');
                % Setting values
                fProbability = NaN;
                fMc = NaN;
                vX(1) = NaN;
                vX(2) = NaN;
                resnorm = NaN;
                exitflag = NaN;
                delta = NaN;
                vPred = [NaN NaN NaN];
                fNmax = NaN;
                fAValue = NaN;
                fBValue = NaN;
                vProbability = [vProbability; fProbability];
                vMc = [vMc; fMc];
                vX_res = [vX_res; vX resnorm exitflag];
                %             vDeltaBest = [vDeltaBest; NaN];
                %             vPredBest = [vPredBest; NaN NaN NaN];
                vNmaxBest = [vNmaxBest; fNmax];
                vABValue = [vABValue; fAValue fBValue];
            end % END of IF fNmax
        end % END of IF length(catalog.Longitude(vSel))

    end % END of FOR fMc
    % Result matrix
    mResult = [mResult; vProbability vMc vX_res vNmaxBest vABValue];

    % Find best estimate, excluding the case of mResult all NAN
    if  ~isempty(min(mResult)) && ~isnan(min(mResult(:,1)))
        vSel = find(min(mResult(:,1)) == mResult(:,1));
        fMc = min(mResult(vSel,2));
        %fMls = min(mResult(vSel,1));
        mu = min(mResult(vSel,3));
        fSigma = min(mResult(vSel,4));
        fAvalue = min(mResult(vSel,8));
        fBvalue = min(mResult(vSel,9));
    else
        fMc = NaN;
        %fMls = NaN;
        mu = NaN;
        fSigma = NaN;
        fAvalue = NaN;
        fBvalue = NaN;
    end
end


function x = mono_next_grlex ( m, x )

%*****************************************************************************80
%
%% MONO_NEXT_GRLEX: grlex next monomial.
%
%  Discussion:
%
%    Example:
%
%    M = 3
%
%    #  X(1)  X(2)  X(3)  Degree
%      +------------------------
%    1 |  0     0     0        0
%      |
%    2 |  0     0     1        1
%    3 |  0     1     0        1
%    4 |  1     0     0        1
%      |
%    5 |  0     0     2        2
%    6 |  0     1     1        2
%    7 |  0     2     0        2
%    8 |  1     0     1        2
%    9 |  1     1     0        2
%   10 |  2     0     0        2
%      |
%   11 |  0     0     3        3
%   12 |  0     1     2        3
%   13 |  0     2     1        3
%   14 |  0     3     0        3
%   15 |  1     0     2        3
%   16 |  1     1     1        3
%   17 |  1     2     0        3
%   18 |  2     0     1        3
%   19 |  2     1     0        3
%   20 |  3     0     0        3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer X(M), the current monomial.
%    The first item is X = [ 0, 0, ..., 0, 0 ].
%
%    Output, integer X(M), the next monomial.
%

%
%  Ensure that 1 <= M.
%
  if ( m < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MONO_NEXT_GRLEX - Fatal error!' );
    fprintf ( 1, '  M < 1\n' );
    error ( 'MONO_NEXT_GRLEX - Fatal error!' );
  end
%
%  Ensure that 0 <= XC(I).
%
  for i = 1 : m
    if ( x(i) < 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'MONO_NEXT_GRLEX - Fatal error!' );
      fprintf ( 1, '  X(I) < 0\n' );
      error ( 'MONO_NEXT_GRLEX - Fatal error!' );
    end
  end
%
%  Find I, the index of the rightmost nonzero entry of X.
%
  i = 0;
  for j = m : -1 : 1
    if ( 0 < x(j) )
      i = j;
      break
    end
  end
%
%  set T = X(I)
%  set X(I) to zero,
%  increase X(I-1) by 1,
%  increment X(M) by T-1.
%
  if ( i == 0 )
    x(m) = 1;
    return
  elseif ( i == 1 )
    t = x(1) + 1;
    im1 = m;
  elseif ( 1 < i )
    t = x(i);
    im1 = i - 1;
  end

  x(i) = 0;
  x(im1) = x(im1) + 1;
  x(m) = x(m) + t - 1;

  return
end


%  Figure 10.45      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_45.m is a script to generate Fig. 10.45 the step response for the SRL
% design for the aircraft with state feedback augmenting the
% inner loop stabilization
clf;
%system matrices
ftq =[-0.0064    0.0263         0  -32.2000      0;
   -0.0941   -0.6240  761.1400 -196.2000         0;
   -0.0002   -0.0015   -4.4120  -12.4800         0;
         0         0    1.0000         0         0;
         0   -1.0000         0  830.0000         0];
g=[0; -32.7; -2.08; 0; 0];
h=[0 0 0 0 1];
j=0;
k=[-.0011 .0016 -.0833 -1.613 -.0010];
fc=ftq-g*k;
% dc gain
dcgain=-h*inv(fc)*g;
r=100/dcgain;
t=0:.3:30;
% step response
y=step(fc,g*r,h,j,1,t);
plot(t,y);
v=[0, 30, 0, 100];
axis(v);
xlabel('Time (sec)');
ylabel('Altitude, h (ft)');
grid;

title( 'Fig. 10.45 Step response of the altitude autopilot')


function [dynpcm2] = MPa2dynpcm2(MPa)
% Convert pressure from megapascals to dyn per sq-cm
% Chad Greene 2012
dynpcm2 = MPa*1.00000e+7;


function [distances,v,w] = convolutionalDistance(p0,p1,areaWeights,kernel,kernelTranspose,options)

% convolutionalDistance - compute entropy-OT ditance and rescaling factors
%   [distances,v,w] = convolutionalDistance(p0,p1,kernel,kernelTranspose, options)
%
% The normalized coupling reads
%       pi = diag(v)*K*diag(w)
% and it should satisfy
%       pi*a  = p1
%   and pi'*a = p0

n = size(p0,1);
options.null = 0;
niter = getoptions(options, 'niter', 100);
tol = getoptions(options, 'tol', 1e-6);
verb = getoptions(options, 'verb', 1);
displayFunction = getoptions(options, 'disp', @(x,y) disp(''));
disp_rate = getoptions(options, 'disp_rate', 10);
disp_time = getoptions(options, 'disp_time', 0);

if nargin<5 || isempty(kernelTranspose);
    kernelTranspose = kernel; % assume symmetry
end

if isempty(areaWeights)
    areaWeights = ones(n,1);
end

p0 = p0+eps;
p1 = p1+eps;

v = ones(size(p0));
w = ones(size(p1));

distances = zeros(size(v,2),1);

A = sum(areaWeights);
aw = bsxfun(@times,areaWeights,w);

for i=1:niter
    if disp_time
        tic
    end

    v = p1 ./ kernel(aw);
    av = bsxfun(@times,areaWeights,v);
    w = p0 ./ kernelTranspose(av);
    aw = bsxfun(@times,areaWeights,w);

    oldDistances = distances;

    ll = @(x) real(log(x));
    lv = av.*ll(v);
    lw = aw.*ll(w);
    distances = sum((log(A)*av+lv).*kernel(aw),2) + sum(av.*kernel(lw),2);

    change = norm(oldDistances-distances,'fro');

    if verb==1, fprintf('Iteration %d:  %g\n', i, change);
    elseif verb==2, progressbar(i,niter); end

    if isa(displayFunction,'function_handle') && mod(i,disp_rate)==1
        displayFunction(v,w); drawnow;
    end

    if change<tol && i > 2
        if verb==2, progressbar(niter,niter); end
        break;
    end

    if disp_time, toc, end
end

distances = sqrt(max(distances,0));
fprintf('\n');


function [i1_rect,i2_rect] = rect_pollefeys(F,i1,i2)

% function [i1_rect,i2_rect] = rect_pollefeys(i1,i2,F,P1,P2)
%
%	[i1_rect,i2_rect]=rect_pollefeys(i1,i2,F,P1,P2)
%
%
%Rectifies a pair of images related by fundamental F
%
%IN:
%	i1 - Matlab image
%	i2 - Matlab image
%	F - Fundamental matrix (p1'*F*p2=0). Assumes that image coordiantes
%		are 1..width where pixel centers are at integer locations.
%
%OUT:
%	fig - handle to the figure
[w,h,~] = size(i1);
line  = 0;

% C1 = null(P1);
% e2 = P2*C1;
e1 = null(F);
e2 = null(F');
e2 = e2/e2(3);
% C2 = null(P2);
% e1 = P1*C2;
e1 = e1/e1(3);
thetas = get_theta_bounds(e1,e2,F,[w,h]);
theta = thetas(1);
if 0,
    h0 = my_figure(gr1,gr2,F);
    ud = get(gcf, 'UserData');
end

while theta<thetas(2)
    line = line+1;
    l1 = l_from_theta_p(theta,e1);
    l2 = F_transfer_l(F',l1,e1);
    if 0,
        pts=get_line_points(l1,ud.sizes(1,:));
        axes(ud.ah(1));
        delete(ud.l1);
        delete(ud.l2);
        ud.l1=plot(pts(1,:), pts(2,:), [ud.color '-'], ...
            'LineWidth', ud.size, 'EraseMode','xor');
        pts=get_line_points(l2,ud.sizes(1,:));
        axes(ud.ah(2));
        ud.l2=plot(pts(1,:), pts(2,:), [ud.color '-'], ...
            'LineWidth', ud.size, 'EraseMode','xor');
    end
    [ps1{line},rr1(:,line)] = get_ps(l1,e1,[w,h]);
    [ps2{line},rr2(:,line)] = get_ps(l2,e2,[w,h]);
    dtheta(line) = min(1/rr1(1,line),1/rr2(1,line));
    theta = theta + dtheta(line);
end

%% project images onto new (r,theta) coordtinates
dim = nan(3,1);
dim(1) = size(ps1,2);
rr1_w = max(rr1(2,:))-min(rr1(1,:));
rr2_w = max(rr2(2,:))-min(rr2(1,:));
dim(2) = round(max(rr2_w,rr1_w));
dim(3) = 3;

i1_rect = im_project(i1,ps1,rr1,dim);
i2_rect = im_project(i2,ps2,rr2,dim);

function h0 = my_figure(i1,i2,F)

h0 = figure;
ah1 = axes('Parent', h0, 'Position',[0 0 .5 1]);
h1 = imshow(i1); hold on; title('Image 1');
set(h1, 'ButtonDownFcn','vgg_gui_F(''b1'');');

ah2 = axes('Parent',h0, ...
    'Position',[.5 0 .5 1], ...
    'Tag','Axes2');
h2=imshow(i2); hold on; title('Image 2');
set(h2, 'ButtonDownFcn','vgg_gui_F(''b2'');');

point=plot(-1000, -1000,'EraseMode','xor');
l1=plot([-1000, -1001], [-1000 -1000], 'r-','EraseMode','xor');
l2=plot([-1000, -1001], [-1000 -1000], 'r-','EraseMode','xor');

s1=size(i1); s2=size(i2);
t(:,:,1)=F';  t(:,:,2)=F;  F=t;

ud=struct('h0', h0, 'h',[h1 h2], 'ah', [ah1, ah2], ...
    'sizes', [s1(1:2); s2(1:2)], ...
    'color', 'k', 'size', 1, ...
    'F', F, 'l1', l1,'l2',l2 );

set(h0,'UserData',ud);

function pts=get_line_points(l,sz)
a=l(1); b=l(2);c=l(3);
h=sz(1); w=sz(2);

% This might cause 'divide by zero' warning:
ys=c/-b ;
yf=-(a*w+c)/b;
xs=c/-a;
xf=-(b*h+c)/a;

m1 = [[xs;1] [xf;h] [1;ys] [w;yf]];
w2 = [(xs<=w & xs>=1) (xf<=w & xf>=1) (ys<=h & ys>=1) (yf<=h & yf>=1)];
v = w2>0;
pts = [m1(:,v)];


function [ly] = pm2ly(pm)
% Convert length from picometers to light years.
% Never know when you might need this.
% Chad A. Greene 2012
ly = pm*1.05702341e-28;


% GET THE QUATERNION BETWEEN TWO VECTORS
function [q] = qArgument(u,v)
q = zeros(4,1);
% Normalise the quaternion
u = u/norm(u);
v = v/norm(v);
% Get the axis vector
q(2:4) = cross(u,v);
% Define the rotation about that vector
q(1) = sqrt((norm(u)^2)*(norm(v)^2)) + dot(u,v);
% Normalise the quaternion
[q] = unit(q);
end


function [s] = gsp_filter_synthesis(G, filters, c, param)
%GSP_FILTER_SYNTHESIS Synthesis operator of a gsp filterbank
%   Usage:  s = gsp_filter_synthesis(G, filters, c);
%           s = gsp_filter_synthesis(G, filters, c, param);
%
%   Input parameters:
%         G         : Graph structure.
%         filters   : Set of spectral graph filters.
%         c         : Transform coefficients
%         param     : Optional parameter
%   Output parameters:
%         signal    : sythesis signal
%
%   'gsp_filter_synthesis(G,filters,c)' computes the synthesis
%   operator for coefficient $c$, where the atoms of the transform
%   dictionary are generalized translations of each graph spectral filter
%   to each vertex on the graph.
%
%   .. f = D * c
%
%   .. math:: f =  D c
%
%   where the columns of $D$ are $g_{i,m}=T_i g_m$, and $T_i$ is a
%   generalized translation operator applied to each filter
%   $\hat{g}_m(\cdot)$.
%
%   Each column of *c* is the response of the signal to one filter.
%
%   Example:::
%
%         Nf = 4;
%         G = gsp_sensor(30);
%         G = gsp_estimate_lmax(G);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_mexican_hat(G, Nf);
%         f = zeros(G.N,1);
%         f(1) = 1;
%         f = G.L^2*f;
%         ff = gsp_filter_analysis(G,g,f);
%         f2 = gsp_filter_synthesis(G,g,ff);
%         paramplot.show_edges = 1;
%         figure()
%         subplot(211)
%         gsp_plot_filter(G,g)
%         subplot(223)
%         gsp_plot_signal(G,f,paramplot);
%         subplot(224)
%         gsp_plot_signal(G,f2,paramplot);
%
%   Additional parameters
%   ---------------------
%
%   * *param.method*  : Select the method to be used for the computation.
%     * 'exact'     : Exact method using the graph Fourier matrix
%     * 'cheby'     : Chebyshev polynomial approximation
%     * 'lanczos'   : Lanczos approximation
%     Default: if the Fourier matrix is present: 'exact' otherwise 'cheby'
%   * *param.order* : Degree of the Chebyshev approximation
%     (default=30).
%   * *param.verbose* : Verbosity level (0 no log - 1 display warnings)
%     (default 1).
%
%   See also: gsp_filter_analysis gsp_filter_inverse
%
%   References: hammond2011wavelets
%

% Author: Nathanael Perraudin
% Testing: test_filter
% Date: 19 March 2014

% TODO: Perfect

% Read input parameters
if nargin < 4
    param = struct;
end

if iscell(G)
    NG = numel(G);
    s = cell(NG,1);
    for ii = 1:NG
        warning('Check what happen here')
       s{ii} = gsp_filter_synthesis(G{ii}, filters{ii}, c{ii}, param);
%         if iscell(s)
%             c{ii} = gsp_filter_analysis(G{ii}, fi{ii}, s{ii}, param);
%         else
%             c{ii} = gsp_filter_analysis(G{ii}, fi{ii}, s, param);
%         end
    end
    return
end

if isnumeric(filters)
    Nf = size(filters,2);
else
    Nf = numel(filters);
end

if isfield(param, 'exact')
    warning('param.exact is not used anymore. Please use param.method instead');
    if param.exact
        param.method = 'exact';
    else
        param.method = 'cheby';
    end
end

if ~isfield(param,'method')
    if gsp_check_fourier(G)
        param.method = 'exact';
    else
        param.method = 'cheby';
    end
end

if ~isfield(param,'order'); param.order = 30; end
if ~isfield(param,'verbose'); param.verbose = 1; end

if isfield(param, 'cheb_order')
    param.order = param.cheb_order;
    warning('param.cheb_order is not used anymore. Please use param.order instead');
end

switch param.method
    case 'exact'

        if ~gsp_check_fourier(G)
            if param.verbose
                warning(['GSP_FILTER_SYNTHESIS: The Fourier matrix is not ',...
                    'available. The function will compute it for you. ',...
                    'However, if you apply many time this function, you ',...
                    'should precompute it using the function: ',...
                    'gsp_compute_fourier_basis']);
            end
            G=gsp_compute_fourier_basis(G);
        end
        if isnumeric(filters)
            fie = filters;
        else
            fie = gsp_filter_evaluate(filters,G.e);
        end
%         Nv = size(c,2);
%         s =zeros(G.N,size(c,2));
%         for ii=1:Nf
%             s = s + G.U * ...
%                 (repmat(fie(:,ii),1,Nv) ...
%                 .* (G.U' * c((1:G.N)+G.N * (ii-1),:)));
%         end

        chat = gsp_gft(G,gsp_vec2mat(c,numel(filters)));
        shat = squeeze(sum(bsxfun(@times, fie, chat), 2));
        s = gsp_igft(G, shat);

    case 'cheby'
        if ~isfield(G,'lmax');
            G = gsp_estimate_lmax(G);
            if param.verbose
                warning(['GSP_FILTER_ANALYSIS: The variable lmax is not ',...
                    'available. The function will compute it for you. ',...
                    'However, if you apply many time this function, you ',...
                    'should precompute it using the function: ',...
                    'gsp_estimate_lmax']);
            end
        end

        cheb_coeffs = gsp_cheby_coeff(G, filters,...
                param.order, param.order +1);

        s=zeros(G.N,size(c,2));

        for ii=1:Nf
            s = s + gsp_cheby_op(G,cheb_coeffs(:,ii),c((1:G.N)+G.N * (ii-1),:));
        end

    case 'lanczos'
        s=zeros(G.N,size(c,2));
        if ~iscell(filters)
            filters = {filters};
        end
        for ii=1:Nf
            s = s + gsp_lanczos_op(G, filters{ii}, c((1:G.N)+G.N * (ii-1),:), param);
        end

    otherwise
        error('Unknown method: please select exact, cheby or lanczos');
end

end


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

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

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

rng(3)
% Construct function handles to objective function (fx) and two constraint
% functions (fxc, fxc2)
fx = @(x) -log( (mvnpdf([x(:,1) x(:,2)],[3.5 2.5], [1 0.3; 0.3 1]) + 0.3*mvnpdf([x(:,1) x(:,2)],[7 8], [3 0.5; 0.5 4])).*...
    mvnpdf([x(:,1) x(:,2)],[5 5], [100 0; 0 100])) ./15 -1;
fxc = @(x) ((x(:,1)-5) .^2 + (x(:,2)-5).^2 -1)/30;
fxc2 = @(x) ( (x(:,1)-5) .^2)./30 - 0.5;

% The upper and lower limits for the constraints
const = [0 0.8 ; -10 0.1];

% Help variables for visualization
lb=0;
ub=10;
[X,Y] = meshgrid(linspace(lb,ub,100),linspace(lb,ub,100));
xl = [X(:) Y(:)];
Z = reshape(fx(xl),100,100);
Zc1 = fxc(xl); Zc1(Zc1<const(1,1) | Zc1>const(1,2)) = nan;
Zc1(~isnan(Zc1))=1; Zc1 = reshape(Zc1,100,100);
Zc2 = fxc2(xl); Zc2(Zc2<const(2,1) | Zc2>const(2,2)) = nan;
Zc2(~isnan(Zc2))=1; Zc2 = reshape(Zc2,100,100);

% ----- conduct Bayesian optimization -----

% construct GP models for the objective function and constraint functions
cfc = gpcf_constant('constSigma2',10,'constSigma2_prior', prior_fixed);
cfse = gpcf_sexp('lengthScale',[1 1]);
cfl = gpcf_linear('coeffSigma2', 10);
cfl2 = gpcf_squared('coeffSigma2', 10, 'interactions', 'on');
lik = lik_gaussian('sigma2', 0.001, 'sigma2_prior', prior_fixed);
% GP model for objective function
%gp1 = gp_set('cf', {cfc, cfl, cfl2, cfse}, 'lik', lik);cfl, cfl2,
gp1 = gp_set('cf', {cfc, cfse}, 'lik', lik);
% GP models for constraint functions
gpc1 = {gp_set('cf', {cfc, cfse}, 'lik', lik, 'jitterSigma2', 1e-6),...
    gp_set('cf', {cfc, cfse}, 'lik', lik, 'jitterSigma2', 1e-6)};

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

% draw initial points
% we assume that at first we don't have any observation from the objective
% function but only observations from constraint functions. Hence x and y
% are initialized to zero
x = [];
y = [];
xc1 = 10*rand(2,2);
yc1 = fxc(xc1);
xc2 = 10*rand(5,2);
yc2 = fxc2(xc2);

figure, % figure for visualization
i1 = 1;
maxiter = 25;
improv = inf;   % improvement between two successive query points
while i1 < maxiter && improv>1e-6
%while i1 < maxiter
    % Train the GP models and calculate variables that are needed when
    %   calculating the Expected improvement (Acquisition function)
    % Objective function
    if ~isempty(x)
        gp = gp_optim(gp1,x,y);
        [K, C] = gp_trcov(gp,x);
        invC = inv(C);
        a = C\y;
        fmin = min( fx(x) );
    else
        a=[];
        x=[];
        invC=[];
        fmin=[];
        gp=gp1;
    end
    % constrain function 1
    gpct = gp_optim(gpc1{1},xc1,yc1);
    [~, Cct] = gp_trcov(gpct,xc1);
    const1.gpc = gpct;
    const1.invCc = inv(Cct);
    const1.ac = Cct\yc1;
    const1.const = const(1,:);
    const1.xc = xc1;
    % constrain function 2
    gpct = gp_optim(gpc1{2},xc2,yc2);
    [~, Cct] = gp_trcov(gpct,xc2);
    const2.gpc = gpct;
    const2.invCc = inv(Cct);
    const2.ac = Cct\yc2;
    const2.const = const(2,:);
    const2.xc = xc2;

    % Calculate EI and the posterior of the functions for visualization
    if ~isempty(x)
        [Ef,Varf] = gp_pred(gp, x, y, xl);
        EI = expectedimprovement_eg(xl, gp, x, a, invC, fmin, const1, const2);
    else
        Ef = zeros(size(xl,1),1);
        Varf = zeros(size(xl,1),1);
        EI = zeros(size(xl,1),1);
    end
    [Efc1] = gp_pred(const1.gpc, xc1, yc1, xl);
    [Efc2] = gp_pred(const2.gpc, xc2, yc2, xl);

    % optimize acquisition function
    %  * Note! Opposite to the standard notation we minimize negative Expected
    %    Improvement since Matlab optimizers seek for functions minimum
    %  * Note! We alternate the acquisition function between Expected
    %    Improvement and expected variance. The latter helps the
    %    optimization so that it does not get stuck in local mode
    % Here we use multiple starting points for the optimization so that we
    % don't crash into suboptimal mode of acquisition function
%     if mod(i1,5)==0  %Do just exploration by finding the maimum variance location
%         fh_eg = @(x_new) expectedvariance_eg(x_new, gp, x, [], invC);
%     else
        fh_eg = @(x_new) expectedimprovement_eg(x_new, gp, x, a, invC, fmin, const1, const2);
%     end
    nstarts = 20;
    xstart = [repmat(lb,nstarts,1) + repmat(ub-lb,nstarts,1).*rand(nstarts,2) ]; %; repmat(x(indbest,:),2,1)+0.1*randn(2,size(x,2))
    for s1=1:nstarts
        x_new(s1,:) = optimf(fh_eg, xstart(s1,:), [], [], [], [], lb, ub, [], opt);
    end
    xnews = x_new;
    EIs = fh_eg(x_new);
    x_new = x_new( find(EIs==min(EIs),1), : );

    % New sample point
    x(end+1,:) = x_new;
    y(end+1,:) = fx(x(end,:));
    xc1(end+1,:) = x_new;
    yc1(end+1,:) = fxc(x(end,:));
    xc2(end+1,:) = x_new;
    yc2(end+1,:) = fxc2(x(end,:));

    % visualize
    clf
    % Plot the objective function
    subplot(2,4,1),hold on, title('Objective, query points')
    pcolor(X,Y,Z),shading flat
    clim = caxis;
    plot(x(1:end-1,1),x(1:end-1,2), 'rx', 'MarkerSize', 10),
    plot(x(end,1),x(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    % Plot the posterior mean of the GP model
    subplot(2,4,2),hold on, title(sprintf('GP prediction, mean, iter: %d',i1))
    pcolor(X,Y,reshape(Ef,100,100)),shading flat
    caxis(clim)
    % Plot the posterior variance of GP model
    subplot(2,4,6),hold on, title('GP prediction, variance')
    pcolor(X,Y,reshape(Varf,100,100)),shading flat
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(x(end,1),x(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    plot(x(1:end-1,1),x(1:end-1,2), 'rx', 'MarkerSize', 10),

    % The expected information
    subplot(2,4,5), hold on, title(sprintf('Expected improvement %.2e', min(EIs)))
    pcolor(X,Y,reshape(EI,100,100)),shading flat
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(x(end,1),x(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    plot(x(1:end-1,1),x(1:end-1,2), 'rx'),

    % constraint 1
    subplot(2,4,3), hold on, title(sprintf('constraint 1'))
    pcolor(X,Y,Zc1),shading flat
    plot(xc1(1:end-1,1),xc1(1:end-1,2), 'rx', 'MarkerSize', 10);
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10);
    plot(xc1(end,1),xc1(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3);
    % constraint 2
    subplot(2,4,4), hold on, title(sprintf('constraint 2'))
    pcolor(X,Y,Zc2),shading flat
    l1= plot(xc2(1:end-1,1),xc2(1:end-1,2), 'rx', 'MarkerSize', 10);
    l2=plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10);
    l3=plot(xc2(end,1),xc2(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3);
    legend([l1,l2,l3], {'function evaluation points','local modes of acquisition function','The next query point'})

    % prediction of constraint 1
    subplot(2,4,7), hold on, title(sprintf('prediction for const 1'))
    Efc1(Efc1<const(1,1) | Efc1>const(1,2)) = nan;
    Efc1(~isnan(Efc1))=1; Efc1 = reshape(Efc1,100,100);
    pcolor(X,Y,reshape(Efc1,100,100)),shading flat
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(xc1(end,1),xc1(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)
    plot(xc1(1:end-1,1),xc1(1:end-1,2), 'rx'),
    % prediction of constraint 2
    subplot(2,4,8), hold on, title(sprintf('prediction for const 2'))
    Efc2(Efc2<const(2,1) | Efc2>const(2,2)) = nan;
    Efc2(~isnan(Efc2))=1; Efc2 = reshape(Efc2,100,100);
    pcolor(X,Y,reshape(Efc2,100,100)),shading flat
    plot(xc2(1:end-1,1),xc2(1:end-1,2), 'rx', 'MarkerSize', 10),
    plot(xnews(:,1),xnews(:,2), 'ro', 'MarkerSize', 10)
    plot(xc2(end,1),xc2(end,2), 'ro', 'MarkerSize', 10, 'linewidth', 3)

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

    if test == 0
        pause
    end
end