function pde = DataDivCurlLinear(varargin)
%%
%           curl u = g    in \Omega,
%           div (eps* u) = f    in \Omega,
%           n \cdot (eps * u) = g_N  on \Gamma_0
% Reference: a linear vector field with modifiable curl and div

if isempty(varargin)
    alpha = 1; % strength of irrotational part (x,y,z)
    beta = 1; % strength of solenoidal part (z,x,y)
else
    alpha = varargin{1};
    beta = varargin{2};
end

pde.Eps = @(p) [1+p(:,1)*0 p(:,3)*0 p(:,3)*0; ...
    p(:,3)*0 1+p(:,2)*0 p(:,3)*0; ...
    0*p(:,1) 0*p(:,2) 1+p(:,3)*0];

pde.exactu = @(p) alpha*[p(:,1), p(:,2), p(:,3)] + ...
                   beta*[p(:,3), p(:,1), p(:,2)];
pde.f = @(p) 3*alpha*ones(size(p,1),1);
pde.g = @(p) beta*[ones(size(p,1),1),...
                  ones(size(p,1),1),...
                  ones(size(p,1),1)];

end


function w = perform_vf_reorientation(v, options)

% perform_vf_reorientation - try to reorient the vf.
%
%   w = perform_vf_reorientation(v, options);
%
%   Flip the orientation of some vectors to ensure a coherent vector field.
%   v and w are (n,n,2) 2D vector field matrices.
%
%   options.method can be 'xproj', 'yproj', 'circproj', 'localproj',
%   'custproj', 'randomized', 'laplacian'.
%
%   'randomized' uses a slow simulated annealing method to optimize
%   an Ising energy.
%   'laplacian' solves a big linear system and is the best method (but slow).
%
%   For 'custproj' you have to provide an additional vector options.w.
%
%   See also: perform_vf_normalization.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
w = getoptions(options, 'w', [1 0]);
method = getoptions(options, 'method', 'xproj');

if size(v,3)~=2
    error('Works only for 2D vector fields.');
end

n = size(v,1);
p = size(v,2);

if strcmp(method, 'laplacian')

    % Try to solve the following linear system for some s(k) at each
    % location k:
    %   s(k) = 1/|V_k| * sum_{i \in V_k} s(k)*cos(theta(k)-theta(i))
    % where V_k are the neighbors of point k and theta(k) is the angle
    % theta(k)=atan2(v_y(k),v_x(k));

    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);

    vx = v(:,:,1); vy = v(:,:,2);
    theta = atan2(vy,vx);

    i = []; j = []; s = [];
    sdiag = zeros(n^2,1);   % weights along the diagonal
    [Y,X] = meshgrid(1:n,1:n);
    I = X(:)+(Y(:)-1)*n;

    deplx = [1 -1 0 0];
    deply = [0 0 1 -1];
    for k=1:length(deplx)
        dx = deplx(k); dy = deply(k);
        J = find( X+dx<=n & X+dx>0 & Y+dy<=n & Y+dy>0);
        I1 = I(J);
        J1 = X(J)+dx+(Y(J)+dy-1)*n;
        i = [i; I1]; j = [j; J1];
        s = [s; cos( theta(I1)-theta(J1) )];
        sdiag(J) = sdiag(J)+1;
    end
    % add diagonal terms
    i = [i; I]; j = [j; I]; s = [s; -sdiag];
    % enforce constraint
    I = find(i>1);
    i = i(I); j = j(I); s = s(I);
    i = [i; 1]; j = [j; 1]; s = [s; 1];
    L = sparse(i,j,s);
    % solve
    y = zeros(n^2,1); y(1)=1;
    use_cg = getoptions(options, 'use_cg', 0);
    % use_cg = 0;
    if use_cg
        if not(isfield(options, 'niter_max'))
            options.niter_max = 500;
        end
        options.x = y*0+1;
        [s,err] = perform_conjugate_gradient(L'*L,L'*y,options);
    else
        s = L\y;
    end

    s = reshape(s,n,n);
    s = sign(s);
    w = v .* repmat(d, [1 1 2]) .* repmat(s, [1 1 2]);

    return;
end

if strcmp(method, 'laplacian1')
    %%%%%% OLD %%%%%%%%%
    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);
    % padd with zeros
    v0 = zeros(n+2,n+2,2);
    v0(2:end-1,2:end-1,:) = v; v = v0;

    [dY,dX] = meshgrid(-1:1, -1:1); dX = dX(:); dY = dY(:);
    dX((end+1)/2) = []; dY((end+1)/2) = [];

    i = [n^2+1]; j = [n/2+(n/2-1)*n]; z = [1];
    u = zeros(n^2,1); % diagonal term
    for t=1:length(dX)
        dx = dX(t); dy = dY(t);
        % interaction
        a = sum( v(2:end-1,2:end-1,:) .* v(2+dx:end-1+dx,2+dy:end-1+dy,:), 3);
        [y,x] = meshgrid(1:n,1:n);
        I = find(x+dx>=1 & x+dx<=n & y+dy>=1 & y+dy<=n);
        x = x(I); y = y(I);
        w = a( max(1-dx,1):min(end-dx,end), max(1-dy,1):min(end-dy,end) );
        w(abs(w)<1e-3) = 1e-3;
        i = [i; x+(y-1)*n ];
        j = [j; x+dx+(y+dy-1)*n ];
        z = [ z; w(:) ];
        % add to diagonal
        u(x+(y-1)*n) = u(x+(y-1)*n) + 1;
    end
    % add diagonal term
    [y,x] = meshgrid(1:n,1:n); x = x(:); y = y(:);
    i = [i; x+(y-1)*n ];
    j = [j; x+(y-1)*n ];
    z = [ z; -u(:) ];

    A = sparse(i,j,z);
    b = zeros(n^2+1, 1); b(end)=1;
    use_cg = getoptions(options, 'use_cg', 1);
    if use_cg
        if not(isfield(options, 'niter_max'))
            options.niter_max = 500;
        end
        y = b;
        A1 = A'*A; y = A'*b;
        options.x = y*0+1;
        [s,err] = perform_conjugate_gradient(A1,y,options);
    else
        s = A\b;
    end
    s = reshape(s,n,n);
    s = sign(s);

    w = v(2:end-1,2:end-1,:);
    w = w .* repmat(d, [1 1 2]) .* repmat(s, [1 1 2]);
    return;

end

if strcmp(method, 'propagation')

    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);
    % padd with zeros
    v0 = zeros(n+2,n+2,2);
    v0(2:end-1,2:end-1,:) = v; v = v0;

    neigh = {[1 0] [-1 0] [0 1] [0 -1]};
    % state
    n1 = n+2;
    S = zeros(n1);
    S(1,:) = -1; S(:,1) = -1; S(end,:) = -1; S(:,end) = -1;
    % initial point
    list = getoptions(options, 'start_points', floor(rand(2,1)*n+1));
    list = list+1;
    Prio = zeros(n1,n1)+Inf;
    for k=1:size(list,2)
        Prio(list(1,k),list(2,k))=0;
    end
    iter = 0;
    while not(isempty(list))
        iter = iter+1;
        progressbar(iter,n^2);
        % clf; imageplot(S); drawnow;
        I = list(1,:) + (list(2,:)-1)*n1;
        prio = Prio(I);
        % extract best match
        [tmp,I] = min(prio);
        i = list(1,I); j = list(2,I);
        prio(I) = []; list(:,I) = [];
        % set to dead
        S(i,j) = -1;
        % compute average vector
        tau = [S(i+1,j); S(i-1,j); S(i,j+1); S(i,j-1)]; tau = tau==-1;
        mtau = sum(tau);
        if mtau==0 && Prio(i,j)~=0 % iter>1
            error('Problem with computations.');
        end
        if mtau>0
            tau = repmat(tau, [1 1 2]);
            av = v(i+1,j,:).*tau(1,1,:) + v(i-1,j,:).*tau(2,1,:) + v(i,j+1,:).*tau(3,1,:) + v(i,j-1,:).*tau(4,1,:);
            av = av / mtau;
            if sum(av.*v(i,j,:))<0
                v(i,j,:) = -v(i,j,:);
            end
        end
        for s=1:length(neigh)
            is = i+neigh{s}(1); js = j+neigh{s}(2);
            w = squeeze(v(i,j,:));
            Prio(is,js) = min( Prio(is,js), Prio(i,j) + 1-abs( sum(w.*neigh{s}')) );
            if S(is,js)==0
                    % add to the front
                    list(:,end+1) = [is;js];
                    S(is,js) = 1; % the front
            end
        end
    end
    progressbar(iter,iter);

    w = v(2:end-1,2:end-1,:);
    w = w .* repmat(d, [1 1 2]);
    return;
end

if strcmp(method, 'localproj')
    % special case.
    for i=1:n
        for j=1:n
            m = zeros(1,1,2);
            if i>1
                m = m + v(i-1,j,:);
            end
            if j>1
                m = m + v(i,j-1,:);
            end
            if i>1 && j>1
                m = m + v(i-1,j-1,:);
            end
            s = dot( m, v(i,j,:) );
            if s<0
                v(i,j,:) = -v(i,j,:);
            end

        end
    end

    w = v;
    return;
end

if strcmp(method, 'randomized')

    % normalize
    d = sqrt(sum(v.^2,3)); d(d<eps) = 1;
    v = v ./ repmat(d,[1 1 2]);

    % padd with zeros
    v0 = zeros(n+2,n+2,2);
    v0(2:end-1,2:end-1,:) = v; v = v0;
    vx = v(:,:,1);
    vy = v(:,:,2);

    % number of iterations
    if isfield(options, 'niter_reorient')
        niter = options.niter_reorient;
    else
        niter = 2000;
    end
    % make 2 sub-grids
    [Y,X] = meshgrid(1:n,1:n);
    I = find( mod(X(:)+Y(:),2)==0 );
    [i1,j1] = ind2sub([n n], I);
    I = find( mod(X(:)+Y(:),2)==1 );
    [i2,j2] = ind2sub([n n], I);

    delta1 = 0.9; delta2 = 0;
    tlist = linspace(0,1,niter);
%    tlist = tlist.^3;

    err = [];
    for k=1:niter
        progressbar(k,niter);
        if mod(k,2)==1
            i = i1+1; j=j1+1;
        else
            i = i2+1; j=j2+1;
        end

        wx = vx( i + (j-1)*(n+2) );
        wy = vy( i + (j-1)*(n+2) );

        zx =    vx( i+1 + (j-1)*(n+2) ) + ...
            vx( i   + (j  )*(n+2) ) + ...
            vx( i-1 + (j-1)*(n+2) ) + ...
            vx( i   + (j-2)*(n+2) );
        zy =    vy( i+1 + (j-1)*(n+2) ) + ...
            vy( i   + (j  )*(n+2) ) + ...
            vy( i-1 + (j-1)*(n+2) ) + ...
            vy( i   + (j-2)*(n+2) );
        % normalize
        dd = sqrt(zx.^2+zy.^2); dd(dd<eps) = 1;
        zx = zx./dd; zy = zy./dd;

        delta = (1-tlist(k))*delta1 + tlist(k)*delta2;

        s = wx.*zx + wy.*zy;
        s = sign(s-delta);
        vx( i + (j-1)*(n+2) ) = wx.*s;
        vy( i + (j-1)*(n+2) ) = wy.*s;
        err(end+1) = sum(s<0);
    end
    v = cat(3,vx,vy);
    w = v(2:end-1,2:end-1,:);
    w = w .* repmat(d, [1 1 2]);
    return;
end

switch lower(method)
    case 'xproj'
        s = v(:,:,1);
    case 'yproj'
        s = v(:,:,2);
    case 'circproj'
        n = size(v,1);
        p = size(v,2);
        [Y,X] = meshgrid(0:p-1, 0:n-1);
        s = v(:,:,1).*X + v(:,:,2).*Y;
    case 'custproj'
        s = v(:,:,1)*w(1) + v(:,:,2)*w(2);
    otherwise
        error('Unknown method');
end

s = sign(s);
I = find(s==0); s(I) = 1;
w = v;
w(:,:,1) = v(:,:,1).*s;
w(:,:,2) = v(:,:,2).*s;


% Add the ImageGraph to path (in a folder named 'ImageGraphs'). Find it here:
% http://www.mathworks.com/matlabcentral/fileexchange/53614-image-graphs
addpath(fullfile(fileparts(mfilename('fullpath')), 'ImageGraphs'));

%% Read the image and features from the sample file
image = im2double(imread('docia.png'));
features = image(:, size(image, 2) / 2 + 1 : end, :);
image = image(:, 1 : size(image, 2) / 2, :);

% The eigendecomposition uses a lot of memory and may render the computer
% unresponsive, so better to test it first with a small image.
image = imresize(image, 0.5);
features = imresize(features, 0.5);

%% Semantic soft segmentation
% This function outputs many intermediate variables, if needed.
% The results may vary a bit from run to run, as there are 2 stages that use
% k-means for intialization & grouping.
sss = SemanticSoftSegmentation(image, features);

% To use the features generated using our network implementation,
% just feed them as the 'features' variable to the function. It will do
% the prepocessing described in the paper and give the processed
% features as an output.
% If you are dealing with many images, storing the features after
% preprocessing is recommended as raw hyperdimensional features
% take a lot of space. Check the 'preprocessFeatures.m' file.

% Visualize
figure; imshow([image features visualizeSoftSegments(sss)]);
title('Semantic soft segments');

% There's also an implementation of Spectral Matting included
sm = SpectralMatting(image);
% You can group the soft segments from Spectral Matting using
% semantic features, the way we presented our comparisons in the paper.
sm_gr = groupSegments(sm, features);
figure; imshow([image visualizeSoftSegments(sm) visualizeSoftSegments(sm_gr)]);
title('Matting components');


function [eg] = dag_to_essential_graph(dag)
cpdag = dag_to_cpdag(dag);
eg = dag + dag .* (cpdag + cpdag');

return;

% Coverts a DAG into Essential Graph where edges are coded by 2 and 3, 2 is
% directed edge and 3 is bidirected edge and is at one (the same as the original DAG) of the two
% symetrical places.

% Is implemented by the algorithm of Max Chickering in D.M.Chickering (1995).
% A transformational characterization of equivalent Bayesian network structures.
% In Proceedings of Eleventh Conference on Uncertainty in Artificial Intelligence, Montreal, QU,
% pages 87-98. Morgan Kaufmann
% http://research.microsoft.com/~dmax/publications/uai95.pdf

% Implemented by Tomas Kocka, AAU.

function [eg] = dag_to_essential_graph(dagx)

%print_dag(dagx); % Just checking input

order = topological_sort(dagx); % get the topological order of nodes and their number

% fprintf('the topological order is: %d',order);
% fprintf('\n');

[nx,ny] = size(dagx); % gets the number of nodes, note that nx == ny
[I,J] = find(dagx); % finds all nonzero elements in the adjacency matrix, i.e. arcs in the DAG - however we will overwrite it in a special order
% we will sort the arcs from lowest possible y and highest possible x, arcs are x->y
e = 1;
for y = 1:ny
    for x = nx:-1:1
        %fprintf('x %d ',order(x)); fprintf('y %d ',order(y));
        if dagx(order(x),order(y)) == 1
            I(e) = order(x);
            J(e) = order(y);
            e = e + 1;
            %fprintf('x order %d',x);
            %fprintf('y order %d',y);
            %fprintf('\n');
        end
    end
end

% fprintf('the arcs are: %d',I);
% fprintf('\n');
% fprintf('the arcs are: %d',J);
% fprintf('\n');

% Now we have to decide which arcs are part of the essential graph and
% which are undirected edges in the essential graph.
% Undecided arc in the DAG are 1, directed in EG are 2 and undirected in EG
% are 3.

for e = 1:length(I)
    if dagx(I(e),J(e)) == 1
        cont = true;
        for w = 1:nx
            if dagx(w,I(e)) == 2
                if dagx(w,J(e)) ~= 0
                    dagx(w,J(e)) = 2;
                else
                    for ww = 1:nx
                        if dagx(ww,J(e)) ~= 0
                           dagx(ww,J(e)) = 2;
                        end
                    end % and now skip the rest and start with another arc from the list
                    w = nx;
                    cont = false;
                end
            end
        end
        if cont
           exists = false;
           for z = 1:nx
               %fprintf('test %d',dagx(z,J(e)));
               if dagx(z,J(e)) ~= 0 & z ~= I(e) & dagx(z,I(e)) == 0
                  exists = true;
                  for ww = 1:nx
                        if dagx(ww,J(e)) == 1
                           dagx(ww,J(e)) = 2;
                        end
                  end
               end
           end
           if ~ exists
               for ww = 1:nx
                   if dagx(ww,J(e)) == 1
                      dagx(ww,J(e)) = 3;
                   end
               end
           end
        end
    end
end

%print_dag(dagx); % Just checking output


function [segmentsM] = mask2scan(maskM, optS, sliceNum)
%function [segmentsM] = mask2scan(maskM, optS, sliceNum)
%segmentsM holds the mask in 'scan segment' format:
%each row of this 2-D matrix has the elements:
%yValue, xStart, xStop, delta_x (all in AAPM coordinates).
%xStart and xStop are the points at which the filled-in segment
%begins and ends; yValue is the y-value of that row.  delta_x is the
%interval between x values.
%
%Get scan format information:
%
%Starting points for filled segments will be those after diff = 1; stopping points will
%those where diff = -1.  This also works for jutting points (one point
%in the segment).
%
%J.O.Deasy, deasy@radonc.wustl.edu.
%LM:  18 Mar 02, JOD;
%Major bug fix: 28 dec 02: Scan row was shifted up one row;
%scan column was shifted to the right one column.  Fixed prior to release
%of version 2 beta, JOD.

imageSizeV = optS.ROIImageSize;

%First convert to pixel-based coords:
xOffset = optS.xCTOffset;
yOffset = optS.yCTOffset;

zerosV = zeros(size(maskM,1),1);  %padding vector

delta_x = optS.ROIxVoxelWidth;
delta_y = optS.ROIyVoxelWidth;

mask2M = [zerosV, maskM, zerosV];

diffM = diff(mask2M');  %note: mask is rotated here!
startM = [diffM == 1];
startM = startM(:,2:end-1);
stopM  = [diffM == -1];
stopM = stopM(:,2:end-1);

[i1V, j1V] = find(startM);
[xStartV, yStartV] = mtoaapm(j1V + 1, i1V, size(maskM));
xStartV = xStartV * delta_x;
yStartV = yStartV * delta_y;

xStartV = xStartV + optS.xCTOffset;
yStartV = yStartV + optS.yCTOffset;

[i2V, j2V] = find(stopM);
[xStopV, yStopV] = mtoaapm(j2V + 1, i2V - 1 , size(maskM));
xStopV = xStopV * delta_x;
yStopV = yStopV * delta_y;

xStopV = xStopV + optS.xCTOffset;
yStopV = yStopV + optS.yCTOffset;

if any(yStartV ~= yStopV)
    error('Ooops! Problem in converting a mask to scan segments.  Check options.')
end

%Store: row y, start x, stop x, delta x, CT mask row, CT start col, CT end col.
z1V = ones(length(j1V),1) * sliceNum;
segmentsM = [yStartV(:), xStartV(:), xStopV(:), ones(length(xStopV),1) * delta_x, z1V, j1V(:) + 1, i1V(:), i2V(:) - 1];


%% OPTI Toolbox Differentiation Demo
%
% This file demonstrates each of the different differentiation algorithms
% supplied with the OPTI Toolbox. Note you should be familiar with the
% operation of OPTI Toolbox by reading the accompanying examples, as well
% as you should have completed the NLP demo.
%
%   Copyright (C) 2011 Jonathan Currie (I2C2)

%% Gradient vs Jacobian
% The terms Gradient and Jacobian are associated with differentiation
% functions, and for the purposes of this toolbox they are basically
% interchangeable:
%
% Gradient - The first derivative of the objective function (1 x n)
% Jacobian - The first derivative of the constraint function (m x n)
%
% Where you will note the main difference is the gradient is a vector, and
% the Jacobian is a matrix. However this is just a rule of thumb and will
% not hold in all instances (constraint functions with one row for
% example!). All functions are named as returning a Jacobian, which could
% also be a Gradient Vector.

%% Problem 1
% For the following examples we are going to use this nonlinear objective
% function:

fun = @(x) sin(pi*x(1)/12) * cos(pi*x(2)/16);

x = [0.75 0.25]';

%% Example 1 - Automatic Differentiation (AD)
% Automatic differentiation is one of the most powerful differentiation
% strategies which can provide error free gradients of a function by
% applying the chain rule to each operation.
%
% The AD routines implemented in OPTI are supplied by adiff, a Matlab
% project by William McIlhagga. While most Matlab functions are overloaded,
% you cannot use external code (i.e. via MEX) or any toolbox or class
% functions.

dx = autoJac(fun,x)

%% Example 2 - Numerical Differentiation (ND)
% Numerical differentiation using finite differences is a computationally
% expensive procedure which can result in an inaccurate gradient if the
% internal perturbations are not chosen correctly.
%
% The ND routine implemented in OPTI is the Intel MKL djacobi function
% which approximates the derivative using central differences. This is
% implemented via a MEX function which repeatedly calls the function in
% order to close in on the gradient.

dx = mklJac(fun,x)

%% Example 3 - Symbolic Differentiation (SD)
% Symbolic differentiation analytically differentiates the function as a
% symbolic expression, resulting in a single expression for the gradient.
% Complications occur if the function cannot be analytically differentiated
% or the symbolic routine cannot find a derivative.
%
% The SD routine implemented in OPTI uses the Matlab Symbolic Toolbox as
% well as two wrapper functions in order to generate the gradient. The
% wrapper functions convert the function handle to a symbolic expression
% and vice-versa.

grad = symJac(fun)

if(~isempty(grad)) %don't run if Symbolic Toolbox not installed
    dx = grad(x)
end

%% Problem 2
% For the next few examples we will be solving this NLP:

obj = @(x) log(1+x(1)^2) - x(2);

lb = [-2 -2]';
ub = [2 2]';

%% Example 4 - Applying AD to NLP Solving
% To use AD to generate the objective gradient for IPOPT create a function
% handle which calls autoJac, and then pass this to the optiprob function:

grad = @(x) autoJac(obj,x);

opts = optiset('solver','ipopt');
Opt = opti('obj',obj,'grad',grad,'bounds',lb,ub,'options',opts)

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

%% Example 5 - Applying ND to NLP Solving
% To use ND to generate the objective gradient for IPOPT just replace the
% above with mklJac:

grad = @(x) mklJac(obj,x);

Opt = opti('obj',obj,'grad',grad,'bounds',lb,ub,'options',opts)

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

%% Example 6 - Applying SD to NLP Solving
% To use SD we can generate the analytical gradient and use this as our
% gradient function:

grad = symJac(obj)

if(~isempty(grad))
    Opt = opti('obj',obj,'grad',grad,'bounds',lb,ub,'options',opts)

    [x,fval,exitflag,info] = solve(Opt,[0;0]);
    fval
    info
else
    display('Cannot run example without Symbolic Toolbox!');
end

%% Conclusion
% By default OPTI Toolbox will always use Numerical Differentiation
% (mklJac) for gradients / Jacobians if one has not been provided. This
% provides the best combination of flexibility with complex objective /
% constraint functions and performance. However if you can use SD to
% generate a gradient this would be a preferred method.
%
% If you find the optimizer is failing with ND and AD is a suitable
% candidate you can try it to ensure your gradients are accurate.


function layer_vec = get_bit_layer(N)
layer_vec = zeros(N, 1);
for phi = 0 : N - 1
    psi = floor(phi/2);
    layer = 0;
    while(mod(psi, 2) == 1)
        psi = floor(psi/2);
        layer = layer + 1;
    end
    layer_vec(phi + 1) = layer;
end
end


function [t,u,v,idx,xnode]=raysurf(p0,v0,node,face)
%
% [t,u,v,idx,xnode]=raysurf(p,v,node,face)
%
% perform a Havel-styled ray tracing for a triangular surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   p0: list of starting points of the rays
%   v0: directional vector of the rays,
%   node: a list of node coordinates (nn x 3)
%   face: a surface mesh triangle list (ne x 3)
%
% output:
%   t: distance from p0 to the intersection point for each surface
%      triangle, if t(i)=NaN, no intersection was found for that ray
%   u: bary-centric coordinate 1 of all intersection points
%   v: bary-centric coordinate 2 of all intersection points
%      the final bary-centric triplet is [u,v,1-u-v]
%   idx: idx lists the IDs of the face elements that intersects
%      each ray
%   xnode: optional output, if requested, xnode gives the intersection
%      point coordinates; to compute manually, xnode=p0+repmat(t,1,3).*v0
%
% Reference:
%  [1] J. Havel and A. Herout, "Yet faster ray-triangle intersection (using
%          SSE4)," IEEE Trans. on Visualization and Computer Graphics,
%          16(3):434-438 (2010)
%  [2] Q. Fang, "Comment on 'A study on tetrahedron-based inhomogeneous
%          Monte-Carlo optical simulation'," Biomed. Opt. Express, (in
%          press)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

len=size(p0,1);
if(len==0)
   error('p0 can not be empty');
end
if(size(node,2)<3)
   error('node must contain at least 3 columns');
end
if(size(face,2)<3)
   error('face must contain at least 3 columns');
end

if(size(v0,1)==1 || size(v0,2)==1 && len>1)
   v0=repmat(v0(:)',len,1);
end

t=zeros(len,1)*nan;
u=t;
v=t;
idx=t;

for i=1:len
   [ti,ui,vi,id]=raytrace(p0(i,:),v0(i,:),node,face);
   if(isempty(id)) continue; end
   ti=ti(id);
   tpid=find(ti>=0);
   if(isempty(tpid)) continue; end
   [tmin,tloc]=min(ti(find(ti>=0)));
   t(i)=tmin;
   u(i)=ui(id(tpid(tloc)));
   v(i)=vi(id(tpid(tloc)));
   idx(i)=id(tpid(tloc));
end

if(nargout>=5)
   xnode=p0+repmat(t,1,3).*v0;
end


function [block] = spm_vb_lambda(Y,block)
% Variational Bayes for GLM-AR models - Update lambda
% FORMAT [block] = spm_vb_lambda(Y,block)
%
% Y      - [T x N] time series
% block  - data structure (see spm_vb_glmar)
%__________________________________________________________________________
% Copyright (C) 2005-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny and Nelson Trujillo-Barreto
% $Id: spm_vb_lambda.m 6079 2014-06-30 18:25:37Z spm $

if block.verbose
    disp('Updating lambda');
end

p = block.p;
k = block.k;
N = block.N;

for n=1:N
    if p > 0
        % Equation 77 in paper VB1
        Gn          = spm_vb_get_Gn (Y,block,n);
    else
        subblock_n  = [(n-1)*k+1:n*k];
        en          = Y(:,n) - block.X*block.w_mean(subblock_n,1);
        Gn          = trace(block.w_cov{n}*block.XTX) + en'*en;
    end
    % Equation 75 in paper VB1
    block.b_lambda(n,1)    = 1./(Gn./2 + 1./block.b_lambda_prior(n));
    block.mean_lambda(n,1) = block.c_lambda(n)*block.b_lambda(n);
end


%% =======================================================================
%  ARDrone Simulation Example: Simulation Baseline Model
%  =======================================================================
%
%  The simulation is used to see the reposnse of the ARDrone to different
%  commands and  inputs.
%  Authors:
%       David Escobar Sanabria -> descobar@aem.umn.edu
%       Pieter J. Mosterman -> pieter.mosterman@mathworks.com
%  =======================================================================

%%
%  Cleaning workspace
bdclose all;
clear all;
clc

%%
% Adding ARDrone library path
addpath ../lib;
%% Simulation parameters

% Flight management system sample time. This is the sample time at which
% the control law is executed.
FMS.Ts = 0.065;

% Time delay due to communication between drone and host computer
timeDelay = FMS.Ts*4;

%% Vehicle model based on linear dynamics

% Loading state space representation of vehicle dynamics
setupARModel;

%%
% Loading list of waypoints
waypoints = getWaypoints() ;

%%
% Simulation time
simDT = 0.005 ;

%%
% Loading Simulink model of ARDrone
ARDroneBaseSim ;


function lines = month_cal_store_common ( y, m )

%*****************************************************************************80
%
%% MONTH_CAL_STORE_COMMON stores a Common month calendar.
%
%  Discussion:
%
%    The "common" calendar is meant to be the calendar which is Julian before
%    the transition date, and Gregorian afterwards, with the transition date
%    best specified as as JED = 2299160.
%
%  Format:
%
%           1  2  3  4  5
%     6  7  8  9 10 11 12
%    13 14 15 16 17 18 19
%    20 21 22 23 24 25 26
%    27 28 29 30
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y,  M, the YM date.
%
%    Output, string LINES(6,20), the lines of the calendar.
%
  lines = [];
%
%  Make local copies of the input.
%
  m2 = m;
  y2 = y;
%
%  Check the month and year.  After this call, month is
%  guaranteed to be between 1 and 12.
%
  [ y2, m2, ierror ] = ym_check_common ( y2, m2 );

  if ( ierror ~= 0 )
    return
  end
%
%  Find the day of the week for Y M 1.
%
  d2 = 1;
  f2 = 0.0;

  w = ymdf_to_weekday_common ( y2, m2, d2, f2 );

  days = month_length_common ( y2, m2 );
%
%  Find the appropriate label for the first box in the calendar.
%
  iday = 2 - w;
%
%  Print out a line of day numbers.
%  IDAY keeps track of the numerical day,
%  JDAY keeps track of the label for the day, which differed in October 1582.
%
  d2 = iday;
  f2 = 0.0;

  n_line = 0;

  while ( n_line < 6 )
% while ( iday <= days )

    n_line = n_line + 1;
    s = '                    ';

    for w = 1 : 7

      i1 = 3 * ( w - 1 ) + 1;
      i2 = i1 + 1;

      if ( 1 <= iday && iday <= days )
        s(i1:i2) = sprintf ( '%2d', d2 );
      else
        s(i1:i2) = '  ';
      end

      iday = iday + 1;

      [ y2, m2, d2, f2 ] = ymdf_next_common ( y2, m2, d2, f2 );

    end

    lines = [ lines; s ];

  end

  return
end


% PURPOSE: Gets the halfamp cutoff frequency value using filtfilt.m
%
% WARNING: For working with filtfilt.m function!
%
% filtfilt result has the following characteristics:
%       a) Zero-phase distortion
%       b) A filter transfer function, which equals the squared magnitude of the original filter transfer function
%           WARNING by JLC: THIS IMPLIES THAT THE CUTOFF FREQUENCY IS NOT AT -3dB ANYMORE, IT IS AT -6dB INSTEAD!!!
%       c) A filter order that is double the order of the filter specified by b and a
%
% *** This function is part of ERPLAB Toolbox ***
% Author: Javier Lopez-Calderon
% Center for Mind and Brain
% University of California, Davis,
% Davis, CA
% 2009

function frec3dB = halfamp(b, a, fs)
try
        [hf,f1] = freqz(b,a,10000,fs);
        hf2 = hf^2; % filtfilt has a transfer function, which equals the squared magnitude of the original filter transfer function.
        [v loc] = min(abs(0.707-abs(hf2))); % frequency at gain 70.7%
        frec3dB = f1(loc);
catch
        frec3dB = [];
end


function [respWave] = Wfm(data, up)
%WFM extracts a respiratory signal corresponding to FM using the continuous
%wavelet transform (Morlet wavelet).
%
%	[respWave] = Wfm(data, up)
%
%	Inputs:
%       data            raw signal data
%       up              universal parameters structure
%
%	Outputs:
%       respWave        a respiratory signal
%

%% Setup
data.t = (1/data.fs)*(1:length(data.v));
data.v = data.v;

%% Downsample to make processing possible
up.paramSet.filt_resample_fs = 25;   % temporarily change downsample freq (perhaps to 50) to increase resolution?
d_s = downsample_data(data, up);
up.paramSet.filt_resample_fs = 25;

%% CWT
% Specify characteristics
s0  = 6/d_s.fs;  % smallest scale
ds = 0.001; % spacing between scales
NbSc = 3000; % number of scales
SCA = {s0,ds,NbSc, 'lin'}; % specify scales
cwtstruct = cwtft({d_s.v, 1/d_s.fs},'scales',SCA);
scales = cwtstruct.scales;
F = scal2frq(scales,cwtstruct.wav,1/d_s.fs); F = d_s.fs./F;
%contour(d_s.t,F,real(cwtstruct.cfs));
%xlabel('Seconds'); ylabel('Pseudo-frequency');
%hold on
mag_mat = cwtstruct.cfs;
rel_rows = F >= up.paramSet.hr_range(1)/60 & F <= up.paramSet.hr_range(2)/60;
rel_mag_mat = mag_mat(rel_rows, :);
[rel_mags, rel_els] = max(rel_mag_mat);
%plot(d_s.t,F(rel_els), 'r', 'LineWidth', 3)
[fm_sig, am_sig] = deal(d_s);
fm_sig.v = F(rel_els);
am_sig.v = abs(rel_mags);

%% Downsample result to usual downsample freq
fm_sig = downsample_data(fm_sig, up);
am_sig = downsample_data(am_sig, up);

%% Store
respWave = fm_sig;

end


function [X_out] = etrf2000itrf2008(X_in, date)

% SYNTAX:
%   [X_out] = etrf2000itrf2008(X_in, date);
%
% INPUT:
%   X_in = input coordinates (XYZ)
%   date = reference date
%
% OUTPUT:
%   X_out = output coordinates (XYZ)
%
% DESCRIPTION:
%   ETRF2000 to ITRF2008 coordinate converter.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%decimal year (YYYY.DDD)
[~, frac] = date2doy(datenum(date));
t = date(1) + frac;

%ITRF2008 - ETRF2000 parameters
% (http://etrs89.ensg.ign.fr/memo-V8.pdf)

%translation [mm]
T1 =  52.1; T1dot =  0.1;
T2 =  49.3; T2dot =  0.1;
T3 = -58.5; T3dot = -1.8;

%scale factor
D = 1.34e-9; Ddot = 0.08e-9;

%rotation [mas]
R1 =  0.891; R1dot =  0.081;
R2 =  5.390; R2dot =  0.490;
R3 = -8.712; R3dot = -0.792;

%propagate parameters to epoch t
T1 = T1 + T1dot*(t - 2000.0);
T2 = T2 + T2dot*(t - 2000.0);
T3 = T3 + T3dot*(t - 2000.0);
D  = D  + Ddot *(t - 2000.0);
R1 = R1 + R1dot*(t - 2000.0);
R2 = R2 + R2dot*(t - 2000.0);
R3 = R3 + R3dot*(t - 2000.0);

%convert translation parameters to [m]
T1 = T1 * 1e-3;
T2 = T2 * 1e-3;
T3 = T3 * 1e-3;

%convert rotation parameters to [rad]
R1 = R1 * 4.848136e-9;
R2 = R2 * 4.848136e-9;
R3 = R3 * 4.848136e-9;

%translation vector
T = [T1; T2; T3];

%rotation/scale matrix
R = [1 -R3 R2; R3 1 -R1; -R2 R1 1];

%7-parameters Helmert inverse transformation
X_out = inv(R)/(1+D)*(X_in - T);


function [ha] = yd22ha(yd2)
% Convert area from square yards to hectares.
% Chad A. Greene 2012
ha = yd2*0.000083612736;


function MS = createMeshCylindrical3D(varargin)
% MeshStructure = createMesh3D(Nr, Ntheta, Nz, Radius, theta, height)
% MeshStructure = createMesh3D(facelocationR, facelocationTheta, facelocationZ)
% creates a uniform 3D mesh:
% Nx is the number of cells in x (horizontal) direction
% Ny is the number of cells in y (vertical) direction
% Nz is the number of cells in z (perpendicular) direction
% Lx is the domain length in x direction
% Ly is the domain length in y direction
% Lz is the domain length in z direction
%
% SYNOPSIS:
%   MeshStructure = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz)
%
% PARAMETERS:
%   Nx: number of cells in the x direction
%   Lx: domain length in x direction
%   Ny: number of cells in the y direction
%   Ly: domain length in y direction
%   Nz: number of cells in the z direction
%   Lz: domain length in z direction
%
% RETURNS:
%   MeshStructure.
%                 dimensions=3 (3D problem)
%                 numbering: shows the indexes of cellsn from left to right
%                 and top to bottom and back to front
%                 cellsize: x, y, and z elements of the cell size =[Lx/Nx,
%                 Ly/Ny, Lz/Nz]
%                 cellcenters.x: location of each cell in the x direction
%                 cellcenters.y: location of each cell in the y direction
%                 cellcenters.z: location of each cell in the z direction
%                 facecenters.x: location of interface between cells in the
%                 x direction
%                 facecenters.y: location of interface between cells in the
%                 y direction
%                 facecenters.z: location of interface between cells in the
%                 z direction
%                 numberofcells: [Nx, Ny, Nz]
%
%
% EXAMPLE:
%   Nx = 2;
%   Lx = 1.0;
%   Ny = 3;
%   Ly = 2.0;
%   Nz = 4;
%   Lz = 3.0;
%   m = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz);
%   [X, Y, Z] = ndgrid(m.cellcenters.x, m.cellcenters.y, m.cellcenters.z);
%   [Xf, Yf, Zf] = ndgrid(m.facecenters.x, m.facecenters.y, m.facecenters.z);
%   plot3(X(:), Y(:), Z(:), 'or')
%   hold on;
%   plot3(Xf(:), Yf(:), Zf(:), '+b')
%   legend('cell centers', 'cell corners');
%
% SEE ALSO:
%     createMesh1D, createMesh2D, createMeshCylindrical1D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==6
  % uniform 1D mesh
  Nx=varargin{1};
  Ny=varargin{2};
  Nz=varargin{3};
  Width=varargin{4};
  Depth=varargin{6};
  Tetta=varargin{5};
  if Tetta>2*pi
      warning('Tetta is higher than 2*pi. It is scaled to 2*pi');
      Tetta = 2*pi;
  end
  MS=createMesh3D(Nx,Ny,Nz,Width, Tetta, Depth);
elseif nargin==3
  % nonuniform 1D mesh
  facelocationX=varargin{1};
  facelocationTheta=varargin{2};
  facelocationZ=varargin{3};
  if facelocationTheta(end)>2*pi
      facelocationTheta = facelocationTheta/facelocationTheta(end)*2.0*pi;
      warning('The domain size adjusted to match a maximum of 2*pi.')
  end
  MS=createMesh3D(facelocationX,facelocationTheta,facelocationZ);
end
MS.dimension=3.2;


function [y2,ntrimmed,spikes, yy] = splinetrim(y,varargin)
% :Usage:
% ::
%
%     function [y,ntrimmed,spikes, yfit] = splinetrim(y,[iqrmult],[knotrate],[X],['p'])
%
% Uses a robust measure of deviations in a timeseries gradient
% to find high-velocity 'spikes', presumed to be artifacts
%
% Uses spline interpolation to replace spikes with reasonable values.
%
% :Input:
%
%   **y:**
%        a timeseries
%
% :Optional Inputs:
%
%   **iqrmult:**
%        how many times the interquartile range above which velocities are
%           outliers, default is 1.5
%
%   **knotrate:**
%        sets knot points every k observations, default is 3
%
%   **X:**
%        matrix of session means or other linear regressors to remove
%
%   **p:**
%        plot the results
%
%   X and 'p' can be entered in any order, but after iqrmult and knotrate
%
% :Examples:
% ::
%
%    [y2,nt] = splinetrim(trialdat,3,5,'p'); nt
%
% ..
%    3/29/05, Tor Wager
% ..

% -------------------------------------------------
% * setup
% -------------------------------------------------

iqrmult = 3;  % how many times the interquartile range above which pts are outliers
knotrate = 5;   % in images
X = [];
plotme = 0;

if length(varargin) > 0, iqrmult = varargin{1};, end
if length(varargin) > 1, knotrate = varargin{2};, end

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
        case 'p', plotme = 1;
        otherwise error('Unrecognized argument: acceptable is ''p'' for plot.')
        end
    else
        if ndims(varargin{i}) == 2 & sum(size(varargin{i}))>2
            X = varargin{i};
        end
    end
end
% filter y using X matrix; yf is residuals
if ~isempty(X),
    mfit = X * pinv(X) * y;
    y = y - mfit;
end

% ---------------------------------------------------------
% find spike regions
% ---------------------------------------------------------
% EXCLUDE FROM CONSIDERING AS KNOT POINTS BASED ON VELOCITY
veloc = gradient(y); veloc(end) = 0;    % clamp last value to 0
tmp = abs(veloc - median(veloc));

% threshold is mad + 1.5 times the interquartile range of abs. deviations
thr = median(tmp) + iqrmult * (prctile(tmp,75) - prctile(tmp,25));

spikes = tmp>thr;

% EXCLUDE FROM CONSIDERING AS KNOT POINTS BASED ON OUTLIER STATUS
d = abs(y - median(y));    % distance from mean
thr = median(d) + iqrmult * (prctile(d,75) - prctile(d,25));
sptmp = d>thr;
spikes = spikes + sptmp;

% smooth this some, so that low-deviation regions in the middle of spikes
% do not get counted
spikes2 = smooth_timeseries(spikes,4);  % excluded from being knot points

spikes = find(spikes>0);        % which points to interpolate in the end
spikes2 = find(spikes2>0);      % larger set of pts to not include as knot pts

% -------------------------------------------------
% * get spline fit
% -------------------------------------------------
bp = zeros(size(y));
bp(1:knotrate:length(y)) = 1;
bp(isnan(y)) = 0;                % ignore NaNs
%bp(end) = 1;                     % clamp last point to be an endpt
bp(spikes2) = 0;                 % do not put knot points on spikes

bp = find(bp);

nbp = length(bp);
bpy = zeros(nbp, 1);

% figure out medians for each segment -
% this is the interp knot point.
ytmp = y;
ytmp(spikes2) = NaN;  % get rid of spikes

for i = 1:nbp

    if i == 1
        st = 1;
    else
        st = bp(i) - round((bp(i) - bp(i-1)) ./ 2);
    end

    if i == nbp
        en = length(y);
    else
        en = bp(i) + round((bp(i+1) - bp(i)) ./ 2);
    end
    data = ytmp(st:en);
    bpy(i) = median(data);

end

% xx = 1:length(y);
yy = spline(bp, bpy, 1:length(y));
if ~iscol(yy), yy = yy'; end

% -------------------------------------------------
% * find pts that are really far from spline fit
% -------------------------------------------------
d = abs(y - yy);    % distance from spline fit
thr = median(d) + iqrmult * (prctile(d,75) - prctile(d,25));

spikes3 = d>thr;

spikes = unique([spikes; find(spikes3>0)]);        % final answer
% replace y data with spline fits where spikes occur
y2 = y;
y2(spikes) = yy(spikes);

ntrimmed = length(spikes);

if plotme
    figure; hold on;
    plot(y,'k','LineWidth',2);
    plot(y2,'g');
    plot(yy,'b');
    plot(bp, yy(bp), 'b.','MarkerSize', 10);
    legend({'Original' 'Adjusted' 'Spline fit'})
    plot(spikes,y2(spikes),'ro','MarkerFaceColor','r','MarkerSize',6);
end

return


% AssignmentToIndex Convert assignment to index.
%
%   I = AssignmentToIndex(A, D) converts an assignment, A, over variables
%   with cardinality D to an index into the .val vector for a factor.
%   If A is a matrix then the function converts each row of A to an index.
%
%   See also IndexToAssignment.m and FactorTutorial.m

function I = AssignmentToIndex(A, D)

D = D(:)'; % ensure that D is a row vector
if (any(size(A) == 1)),
    I = cumprod([1, D(1:end - 1)]) * (A(:) - 1) + 1;
else
    I = sum(repmat(cumprod([1, D(1:end - 1)]), size(A, 1), 1) .* (A - 1), 2) + 1;
end;

end


function [f]=ref_idwiltii_1(coef,g,a,M)
%REF_IDWILTII_1  Reference IDWILTII by IDGT type II
%

%   Author : Peter L. Søndergaard

L=size(g,1);
N=L/a;
W=size(coef,2);

coef=reshape(coef,M*2,N/2,W);

coef2=zeros(2*M,N,W);

if 0

  % --- loop version ---
  for n=0:N/2-1

    % m=0
    coef2(1,2*n+1,:) = coef(1,n+1,:);

    % m odd
    for m=1:2:M-1
      coef2(m+1,2*n+1,:)     = -i/sqrt(2)*coef(m+1,n+1,:);
      coef2(2*M-m+1,2*n+1,:) = -i/sqrt(2)*coef(m+1,n+1,:);

      coef2(m+1,2*n+2,:)     =  1/sqrt(2)*coef(M+m+1,n+1,:);
      coef2(2*M-m+1,2*n+2,:) = -1/sqrt(2)*coef(M+m+1,n+1,:);
    end;

    % m even
    for m=2:2:M-1
      coef2(m+1,2*n+1,:)     =  1/sqrt(2)*coef(m+1,n+1,:);
      coef2(2*M-m+1,2*n+1,:) = -1/sqrt(2)*coef(m+1,n+1,:);

      coef2(m+1,2*n+2,:)     = -i/sqrt(2)*coef(M+m+1,n+1,:);
      coef2(2*M-m+1,2*n+2,:) = -i/sqrt(2)*coef(M+m+1,n+1,:);
    end;

    % m=nyquest
    if mod(M,2)==0
      coef2(M+1,2*n+2,:) = -i*coef(M+1,n+1,:);
    else
      coef2(M+1,2*n+1,:) = -i*coef(M+1,n+1,:);
    end;

  end;

else

  % --- Vector version ---
  % First and middle modulation are transferred unchanged.
  coef2(1,1:2:N,:) = coef(1,:,:);

  coef2(2:2:M,1:2:N,:)        = -i/sqrt(2)*coef(2:2:M,:,:);
  coef2(2*M:-2:M+2,1:2:N,:)   = -i/sqrt(2)*coef(2:2:M,:,:);

  coef2(2:2:M,2:2:N,:)        =  1/sqrt(2)*coef(M+2:2:2*M,:,:);
  coef2(2*M:-2:M+2,2:2:N,:)   = -1/sqrt(2)*coef(M+2:2:2*M,:,:);

  if M>2
    coef2(3:2:M,1:2:N,:)        = 1/sqrt(2)*coef(3:2:M,:,:);
    coef2(2*M-1:-2:M+2,1:2:N,:) = -1/sqrt(2)*coef(3:2:M,:,:);

    coef2(3:2:M,2:2:N,:)        = -i/sqrt(2)*coef(M+3:2:2*M,:,:);
    coef2(2*M-1:-2:M+2,2:2:N,:) = -i/sqrt(2)*coef(M+3:2:2*M,:,:);
  end;

  if mod(M,2)==0
    coef2(M+1,2:2:N,:) = -i*coef(M+1,:,:);
  else
    coef2(M+1,1:2:N,:) = -i*coef(M+1,:,:);
  end;

end;

f=ref_igdgt(reshape(coef2,2*M*N,W),g,a,2*M,.5,0,0);

%if norm(imag(f(:)))<1e-10
%  f=real(f);
%end;


function s = y2s(y)
% S = y2s(Y)
%
% Admittance to Scattering transformation
% for square matrices at multiple frequencies
%
% s = inv(I+y) * (I-y)
% ver 0.0 original	31.03.1998
% ver 0.1 +freq		27.09.2002
% ver 0.2 +octave supp.	01.06.2005

  if size(size(y),2) > 2
      nF = size(y,3);
    else nF = 1;
    end;

I = diag(ones(1, size(y,2)));

  for i=1:nF
    s(:,:,i) = inv(I+y(:,:,i)) * (I-y(:,:,i));
  end;


function [Ft3]=tri6_subtri3(Ft6,Vt6)

% function [Ft3]=tri6_subtri3(Ft6,Vt6)
% ------------------------------------------------------------------------
% The input 6-node triangles are either oriented such that the first node
% is a corner node or that the first node is a mid-edge node. The two cases
% are highlighted below.
%
% Case I
%                     1
%                    / \
%                   2   6
%                  /     \
%                 3___4___5
% Case II
%                     6
%                    / \
%                   5   1
%                  /     \
%                 4___3___2
%
% The output will follow one of these rules and maintains face normals
% directions:
%
% Case I
%                     1
%                    / \
%                   2___6
%                  / \ / \
%                 3___4___5
%
% Case II
%                     6
%                    / \
%                   5___1
%                  / \ / \
%                 4___3___2
%
% ------------------------------------------------------------------------
%%
%Check face order based on first
a=Vt6(Ft6(1,2),:)-Vt6(Ft6(1,1),:); %2 1 edge
b=Vt6(Ft6(1,6),:)-Vt6(Ft6(1,1),:); %6 1 edge
c=Vt6(Ft6(1,3),:)-Vt6(Ft6(1,2),:); %3 2 edge

am=sqrt(sum(a.^2));
bm=sqrt(sum(b.^2));
cm=sqrt(sum(c.^2));

phi=abs(acos(dot(a,b)./(am*bm)));
phi=mod(phi,pi); %Angle between 2 1 and 6 1 edge
theta=abs(acos(dot(a,c)./(am*cm)));
theta=mod(theta,pi); %Angle between 2 1 and 3 2 edge

if phi>theta
    Ft3=[Ft6(:,[1 2 6]); Ft6(:,[2 3 4]); Ft6(:,[4 5 6]); Ft6(:,[2 4 6])]; %Clockwise
else
    Ft3=[Ft6(:,[1 2 3]); Ft6(:,[3 4 5]); Ft6(:,[5 6 1]); Ft6(:,[1 3 5])]; %Anti-clockwise
end

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


function output = F_inner_product_normalized(input_layers)

input1 = input_layers{1}.a;
input2 = input_layers{2}.a;

output = sum(input1 .* input2);

output = output / size(input1,1);

end


%QMINCON Resolve redundancy in robots by avoiding joint limits
%
% A popular way to resolve redundant robots is to keep joints away from
% their mechanical limits to allow freer motion. This function will do
% that process. Requires fmincon from the optimization toolbox.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% This file requires file(s) from The Robotics Toolbox for MATLAB (RTB)
% by Peter Corke (www.petercorke.com), see file for statement
%
% Syntax:
%  (1) [qstar, error, exitflag] = robot.qmincon(q)
%
% Outputs:
%  qstar    : Optimised joint angles, mxrobot.n where m = size(q,2)
%  error    : The error measurement (value of objective function)
%  exitflag : The exitflag direct from fmincon
%
% Inputs:
%  q : Joint configuration(s), may be an mxrobot.n matrix of m poses
%
% See also fmincon ikcon ikunc

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 1993-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license

function [qstar, error, exitflag] = qmincon(robot, q)

M = size(q,1);
n = robot.n;

qstar = zeros(M,n);
error = zeros(M,1);
exitflag = zeros(M,1);

opt = optimoptions('fmincon', ...
    'Algorithm', 'active-set', ...
    'Display', 'off');

lb = robot.qlim(:,1);
ub = robot.qlim(:,2);

x_m = 0; % Little trick for setting x0 in first iteration of loop

for m = 1:M
    q_m = q(m,:);

    J = robot.jacobn(q(m,:));
    N = null(J);

    if isempty(N)
        error(pHRIWARE('error', 'Robot is not redundant'));
    end

    f = @(x) sumsqr((2*(N*x + q_m') - ub - lb)./(ub-lb));

    x0 = zeros(size(N,2), 1) + x_m;

    A = [N; -N];
    b = [ub-q_m'; q_m'-lb];

    [x_m, err_m, ef_m] = fmincon(f,x0,A,b,[],[],[],[],[],opt);

    qstar(m,:) = q(m,:) + (N*x_m)';
    error(m) = err_m;
    exitflag(m) = ef_m;
end

end

function s = sumsqr(A)
    s = sum(A.^2);
end


%BCSFRONTIER BlueChipStock rolling efficient frontiers

addpath ./source

load BlueChipBacktest0
load BlueChipBacktest

% Plot 3D efficient frontiers

figure(1);
surf(X0,Y0,Z0,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
ylabel('\bfStd.Dev. of Returns');
zlabel('\bfMean of Returns');
title('\bfRolling Efficient Frontiers (Absolute Total Return)');
camlight right;
view(30,30);

i = input('Continue >');

figure(1);
surf(X,Y,Z,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
ylabel('\bfStd.Dev. of Returns');
zlabel('\bfMean of Returns');
title('\bfRolling Efficient Frontiers (Relative Total Return vs DJIA)');
camlight right;
view(30,30);


function pass = test_BartelsStewart( pref )
% Test generalized Sylvester matrix equation solver

if ( nargin < 1 )
    pref = chebfunpref();
end
tol = 1e4*pref.cheb2Prefs.chebfun2eps;

n = 10;
rng(0)
A = rand(n);
B = rand(n);
C = rand(n);
D = rand(n);
X = rand(n);

E = A * X * B.' + C * X * D.';

Y = chebop2.bartelsStewart(A, B, C, D, E, 0, 0);
pass(1) = norm( Y - X ) < tol;

A = rand(n) + 1i*rand(n);
B = rand(n) + 1i*rand(n);
C = rand(n) + 1i*rand(n);
D = rand(n) + 1i*rand(n);
X = rand(n) + 1i*rand(n);

E = A * X * B.' + C * X * D.';

Y = chebop2.bartelsStewart(A, B, C, D, E, 0, 0);
pass(2) = norm( Y - X ) < 10*tol;

tol = 1000*tol;
n = 100;
rng(0)
A = rand(n);
B = rand(n);
C = rand(n);
D = rand(n);
X = rand(n);

E = A * X * B.' + C * X * D.';

Y = chebop2.bartelsStewart(A, B, C, D, E, 0, 0);
pass(3) = norm( Y - X ) < 10*tol;

A = rand(n) + 1i*rand(n);
B = rand(n) + 1i*rand(n);
C = rand(n) + 1i*rand(n);
D = rand(n) + 1i*rand(n);
X = rand(n) + 1i*rand(n);

E = A * X * B.' + C * X * D.';

Y = chebop2.bartelsStewart(A, B, C, D, E, 0, 0);
pass(4) = norm( Y - X ) < 10*tol;

end


function DEM_FEP_Least_Action
%--------------------------------------------------------------------------
% This routine uses a Lorenz system to show that the most likely autonomous
% path (or deterministic path) is uniquely identified by its initial and
% final states. In other words, if we knew the end state of an autonomous
% trajectory, then we would implicitly know the path taken from the initial
% particular state, even if we did not know the external states. This point
% is demonstrated using numerical analyses of the Lorenz system; treating
% the first state and an active state and the third as an external state.
% In this example, 1024 solutions are obtained from the same initial
% particular (i.e., sensory and active) states but sampling from a Gaussian
% distribution over external states. The ensuing trajectories over 128 time
% bins of 1/128 seconds are shown in the left panels. The sample
% distribution over active states is shown as a (scaled) histogram along
% the x-axis. Paths that end within 1/8 of an arbitrary active state (here,
% ?? = -4) are shown in red. The corresponding autonomous (i.e., active)
% paths are shown as a function of time in the right panels. one can repeat
% this analysis for different levels of random fluctuations; e.g.,log
% precisions of 2 and 16. The key thing to observe is that as the amplitude
% of random fluctuations decreases (i.e., its precision increases) the
% paths that begin and end in the same place collapse to a single
% trajectory of least action. This is the most likely or deterministic
% path. Clearly, this behaviour rests upon a diffeomorphic mapping between
% the initial and final states: for example, a final active state of -8 has
% the least two paths of least action (xT in the code below).
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_FEP_Least_Action.m 7512 2019-01-05 21:15:16Z karl $

% generative model
%==========================================================================                       % switch for demo
spm_figure('GetWin','DEM'); clf

% flow
%--------------------------------------------------------------------------
dt    = 1/128;
G.f   = @(x,v,P,G) v(:) + [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]*x*dt;

% set up
%--------------------------------------------------------------------------
T     = 128;                        % length of trajectory
N     = 1024;                       % number of paths
g     = exp(-8);                    % amplitude of random fluctuations
s     = exp(2);                     % amplitude deviations
P     = [10; -8/3; 28];             % Rayleigh parameter
x0    = [1; 1; 25];
for k = 1:N

    % random fluctuations (and action)
    %----------------------------------------------------------------------
    U.u      = randn(T,3)*g;
    A(k)     = (1/2)*sum(U.u(:).^2)/(g^2)/T;

    % random deviations in external states (and probability)
    %----------------------------------------------------------------------
    dx       = randn*s;
    A(k)     = A(k) + (1/2)*sum(dx.^2)/(s^2);

    % integrate timeseries with random initial hidden state
    %----------------------------------------------------------------------
    G.x      = x0;
    G.x(3)   = x0(3) + dx;
    x(:,:,k) = spm_int_L(P,G,U);

end

% plot ensemble densities
%--------------------------------------------------------------------------
subplot(2,2,1),cla, hold on
td    = fix(linspace(1,N,32));
for t = td
    plot(x(:,1,t),x(:,3,t),':','MarkerSize',1)
    plot(x(T,1,t),x(T,3,t),'r.','MarkerSize',8)
end
axis square, axis([-20 20 0 60])
title('Paths from initial state','FontSize',16)
xlabel('active state'),ylabel('external state'),drawnow

% find trajectories that start and end at the same place
%==========================================================================

% get surprisal of final (active) state using sample density
%--------------------------------------------------------------------------
nb    = 32;
xN    = squeeze(x(end,1,:));
[n,a] = hist(xN,nb);
n     = 2*nb*n/sum(n);
bar(a,n,1)

% identify and plot trajectories with the same endpoints
%--------------------------------------------------------------------------
xT    = -4;
k     = find(abs(xN - xT) < 1/8);
for t = k(:)'
    plot(x(:,1,t),x(:,3,t),'r')
end
plot([x0(1),x0(1)],[0 48],'r-.')
plot([xT   ,xT   ],[0 48],'r-.')

% paths as a function of time
%--------------------------------------------------------------------------
subplot(2,2,2), hold on
pst   = (1:T)*dt;
for t = k(:)'
    plot(pst,x(:,1,t),'r')
end
title('Autonomous paths','FontSize',16)
xlabel('time (seconds)'),ylabel('active state'),axis square

return

% NB: numerical analysis of the action and surprisal
%--------------------------------------------------------------------------
for i = 1:N
    [d,j] = min(abs(xn - xN(i)));
    L(i)  = -log(n(j));
end
L      = log(spm_softmax(L(:)));
A      = log(spm_softmax(A(:)));

subplot(2,2,2)
plot(L,A,'.',L,L,':')
title('Action and surprisal','FontSize',16)
xlabel('Surprisal of final state'),ylabel('Action of path'),axis square


% Fig. 6.47   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=1;
den=[1 0 0];
w=logspace(-2,2,100);
[m,p]=bode(num,den,w);
loglog(w,m);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.47 Spacecraft frequency-response magnitude');
bodegrid;


% block_fatrix_test.m
% Test the block_fatrix object

if 1 || ~isvar('A5'), printm 'setup'
	rng(0)
%	A1 = Gsparse(sparse(rand(10,20)));
	A1 = rand(10,20);
	A2 = magic(20);
	A3 = rand(10,30);
	A4 = Gdft('mask', true(5,4), 'samp', rand(5,4) > 0.5); % for gram
	A5 = rand(size(A4));

	Ac = block_fatrix({A1, A2}, 'type', 'col');
	Ad = block_fatrix({A1, A4}); % diag
	Ak = block_fatrix({A1}, 'type', 'kron', 'Mkron', 2);
	Ar = block_fatrix({A1, A3}, 'type', 'row');
	As = block_fatrix({A4, A5}, 'type', 'sum');
end

if 1, printm 'basic Fatrix tests'
	tester = @(A, mask, name) ...
		Fatrix_test_basic(A, mask, 'complex', 1, 'name', name)
	tester(Ac, true(20,1), 'A:col')
	tester(Ad, true(40,1), 'A:diag')
	tester(Ak, true(40,1), 'A:kron')
	tester(Ar, true(50,1), 'A:row')
	tester(As, true(20,1), 'A:sum')
end

if 1, printm 'adjoint tests'
	tester = @(A) test_adjoint(A, 'complex', 1);
	tester(Ac);
	tester(Ad);
	tester(Ak);
	tester(Ar);
	tester(As);
end

if 1 % test with a "tomo" object
	ig = image_geom('nx', 8, 'ny', 7, 'dx', 1);
	ig.mask = ig.circ > 0;
	sg0 = sino_geom('par', 'nb', 9, 'na', 10, 'dr', 1);
	sg1 = sg0; sg1.na = 6; sg1.orbit = sg0.orbit / sg0.na * sg1.na;
	sg2 = sg1; sg2.na = sg0.na - sg1.na; sg2.orbit_start = sg1.orbit;
		sg2.orbit = sg0.orbit - sg1.orbit;
	tmp = {'square/strip', 'Ltab', [1000], 'strip_width', sg0.dr};
	At0 = Gtomo2_table(sg0, ig, tmp);
	At1 = Gtomo2_table(sg1, ig, tmp);
	At2 = Gtomo2_table(sg2, ig, tmp);
	Atb = block_fatrix({At1, At2}, 'type', 'col', 'tomo', true);

	if 1
		xx = ig.circ;
		yt = cat(2, At1 * xx, At2 * xx);
		yb = Atb * xx;
		jf_equal(yb, yt)
		jf_equal(yb, At0 * xx)
	end

	if 1
		yy = Atb * ig.circ;
		yc = mat2cell(yy, [sg1.nb], [sg1.na sg2.na]);
		xx = At1' * yc{1} + At2' * yc{2};
		x0 = At0' * yy;
		equivs(x0, xx)
		xb = Atb' * yy;
		jf_equal(xb, xx)
	end

%	equivs(full(At0), full(Atb))
	tester_tomo2(Atb, ig.mask, 'A2', At0, 'halt', 0, 'nblock', 0)
end

if 1 % test 'col'
	x = [1:ncol(A1)]';
	y1 = A1 * x;
	y2 = A2 * x;
	yy = Ac * x;
%	printm('col forw error %g', max_percent_diff([y1; y2], yy))
	jf_equal([y1; y2], yy)

	x1 = A1' * y1;
	x2 = A2' * y2;
	xx = Ac' * [y1; y2];
%	printm('col back error %g', max_percent_diff(x1+x2, xx))
	jf_equal(x1+x2, xx)
end

if 1 % test 'diag'
	x1 = [1:ncol(A1)]';
	x2 = [1:ncol(A4)]';
	x = [x1; x2];
	y1 = A1 * x1;
	y2 = A4 * x2;
	yy = Ad * x;
%	printm('diag forw error %g', max_percent_diff([y1; y2], yy))
	jf_equal([y1; y2], yy)

	x1 = A1' * y1;
	x2 = A4' * y2;
	xx = Ad' * yy;
%	printm('diag back error %g', max_percent_diff([x1; x2], xx))
	jf_equal([x1; x2], xx)

	Td = build_gram(Ad, [], 0);
	y1 = Td * xx;
	y2 = [A1' * A1 * x1; A4' * (A4 * x2)];
%	printm('diag gram error %g%%', max_percent_diff(y1, y2))
	jf_equal(y1, y2)
end

if 1 % test 'kron'
	xx = rand(ncol(A1), Ak.arg.Mkron);
	clear xt yt
	for ic=1:Ak.arg.Mkron
		yt(:,ic) = A1 * xx(:,ic);
	end
	yy = Ak * xx(:);
%	printm('kron forw error %g', max_percent_diff(yt(:), yy))
	jf_equal(yt(:), yy)

	yy = reshapee(yy, [], Ak.arg.Mkron);
	for ic=1:Ak.arg.Mkron
		xt(:,ic) = A1' * yy(:,ic);
	end
	xx = Ak' * yy(:);
%	printm('kron back error %g', max_percent_diff(xt(:), xx))
	jf_equal(xt(:), xx)
end

if 1 % test 'row'
	x1 = [1:ncol(A1)]';
	x2 = [1:ncol(A3)]';
	y1 = A1 * x1;
	y2 = A3 * x2;
	yy = Ar * [x1; x2];
%	printm('row forw error %g', max_percent_diff(y1+y2, yy))
	equivs(y1+y2, yy)

	x1 = A1' * yy;
	x2 = A3' * yy;
	xx = Ar' * yy;
%	printm('row back error %g', max_percent_diff([x1; x2], xx))
	equivs([x1; x2], xx)
end

if 1 % test 'sum'
	x = [1:ncol(A4)]';
	y1 = A4 * x;
	y2 = A5 * x;
	yy = As * x;
%	printm('sum forw error %g', max_percent_diff(y1+y2, yy))
	jf_equal(y1+y2, yy)
	x1 = A4' * yy;
	x2 = A5' * yy;
	xx = As' * yy;
%	printm('sum back error %g', max_percent_diff(x1+x2, xx))
	jf_equal(x1+x2, xx)
end


function [obj, x_eig, c_eig] = rotate_to_pca(obj)
% Rotate design matrix columns within all conditions to principal component projection.

eigval_cutoff = .1; % 100*eps;  % or 1

meth = 'nuis only';

switch meth
    case 'all'

        c = obj.xX.cond_assignments;

        % add column for all nuisance covs
        c(:, end + 1) = (~any(c'))';

        % here, if startval = size(c, 2), then
        % it will do only nuisance covariates.  This is the default behavior.
        % if startval were 1, it would do ALL conditions.
        startval = 1;

    case 'nuis only'

        % this for nuisance only instead
        c = false(size(obj.xX.X, 1), 1);
        c(obj.xX.iC) = true;

        startval = size(c, 2);

end

[c_eig, x_eig] = deal(cell(1, size(c, 2)));

for j = startval:size(c, 2)
    % for each condition

    % columns for condition j
    x = obj.xX.X(:, c(:, j));

    % for determining which to save and which are empty
    x2 = scale(x);

    [v, sc, lam] = princomp(x2, 'econ');
    wh_empty = lam < eigval_cutoff;

    v = princomp(x, 'econ');

    % eliminate empty columns
    v(:, wh_empty) = [];
    x = x * v;

    % re-build c matrix
    c_eig{j} = ones(size(x, 2), 1);

    x_eig{j} = x;

end

x_eig = cat(2, x_eig{:});
c_eig = blkdiag(c_eig{:});

switch meth
    case 'all'

    case 'nuis only'
        wh_to_replace = obj.xX.iC(1:size(x_eig, 2));
        wh_to_delete = obj.xX.iC(size(x_eig, 2)+1:end);
        num_to_delete = length(wh_to_delete);

        obj.xX.X(:, wh_to_replace) = x_eig;
        obj.xX.X(:, wh_to_delete) = [];
        obj.xX.cond_assignments(wh_to_delete, :) = [];

        obj.xX.iB = obj.xX.iB - num_to_delete;
        obj.xX.iC = wh_to_replace;

end

end


function [x] = epp1(v, rho)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Element-wise Soft Thesholding
%
% x= sign(v) max( |v|- rho, 0)
%
% Authors: Yin Li @ PAMI Lab SJTU
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function weights_new = importance_weights(particle_system, new_rho)
% IMPORTANCE_WEIGHTS: Function updates the importance weights for each
% binary model based on the geometric bridge model and the empirical
% distribution

% Extract inputs to function
models     = particle_system.models;
model_val  = particle_system.model_val;
old_wts    = particle_system.weights;
old_rho    = particle_system.rho;

% Determine the number of models and trials
[n_models, ~] = size(models);

% Declare a vector to store weights
weights_new = zeros(n_models,1);

for i=1:n_models

    % Evaluate posterior ratios: pi(M) \propto exp(rho*f(M))
    post_new_rho = new_rho*model_val(i);
    post_old_rho = old_rho*model_val(i);

    % Find the weights corresponding to each model
    weights_new(i) = old_wts(i)*exp(post_new_rho - post_old_rho);

end

% Check for NaNs and set weight to zero
nan_weight = isnan(weights_new);
weights_new(nan_weight) = 0;

% Renormalize weights
weights_new = weights_new/sum(weights_new);

end


function v = lexicmp(a,b, tol)

% compare using lexicographical ordering the arrays
%
%   v = lexicmp(a,b, tol);
%
%   tol (default 1e-9) is a tolerance of equality.
%
%   return -1 if a<b, +1 if a>b, 0 if a=b.
%
%   Copyright (c) 2008 Gabriel Peyre

tol = 1e-9;
a = a(:);
b = b(:);

n = max(length(a),length(b));
a(n+1:end) = -Inf;
b(n+1:end) = -Inf;

for i=1:n
    if a(i)<b(i)-tol
        v = -1; return;
    end
    if a(i)>b(i)+tol
        v = +1; return;
    end
end
v = 0;


function [ nodes_per_layer, n ] = hex_grid_approximate_n ( box, n_goal )

%*****************************************************************************80
%
%% HEX_GRID_APPROXIMATE_N seeks a hex grid of about N nodes.
%
%  Discussion:
%
%    The parameter NODES_PER_LAYER controls the number of nodes, but
%    in a somewhat obscure way.  This routine experiments with various
%    values until it is convinced it has the value of NODES_PER_LAYER
%    that comes as close as possible to producing N nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real BOX(2,2), the lower and upper corners
%    of the rectangular region.
%
%    Input, integer N_GOAL, the desired number of nodes.
%
%    Output, integer NODES_PER_LAYER, the number of nodes per layer
%    which produces a mesh with about N_GOAL nodes.
%
%    Output, integer N, the number of nodes in the mesh.
%
  m = 2;

  if ( n_goal <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HEX_GRID_APPROXIMATE_N - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of N_GOAL = %d\n', n_goal );
    error ( 'HEX_GRID_APPROXIMATE_N - Fatal error!' );
  end

  nodes_per_layer_low = 0;
  n_low = 0;

  nodes_per_layer = round ( sqrt ( n_goal ) );

  nodes_per_layer_high = n_goal;
  n_high = n_goal * n_goal;

  while ( 1 )

    n = hex_grid_n ( nodes_per_layer, box );

    if ( n == n_goal )
      break
    end

    if ( n < n_goal )
      nodes_per_layer_low = nodes_per_layer;
      n_low = n;
    else
      nodes_per_layer_high = nodes_per_layer;
      n_high = n;
    end

    if ( nodes_per_layer_low + 1 == nodes_per_layer_high )
      if ( n - n_low <= n_high - n )
        nodes_per_layer = nodes_per_layer_high;
        n = n_high;
      else
        nodes_per_layer = nodes_per_layer_low;
        n = n_low;
      end
      break
    end

    nodes_per_layer = ...
      round ( ( nodes_per_layer_low + nodes_per_layer_high ) / 2 );

  end

  return
end


function [y, dzdx2] = vl_nncosineloss(x1, x2, c, varargin)
%VL_NNCOSINELOSS Compute cosine embedding loss
%   Y = VL_NNCOSINELOSS(X1, X2, C) computes the contrastive loss incurred by
%   the similar and dissimilar pairs in X1 and X2 labelled by C, making use
%   of the cosine similarity function to assess the distance between embeddings
%   (rather than euclidean metric used in the original contrastive loss
%   formulation of [1]).
%
%   The variables X1 nd X2 are of size H x W x D x N. The distance between
%   two pairs is computed between vectorised chunks of size HWD x N,
%   keeping the spatial and channel arrangement.
%
%   C has dimension 1 x 1 x 1 x N and specifies dissimilar pairs when
%   equal 0 and similar pairs otherwise.
%
%   The loss between two vectors X1 and X2 with cosine similarity
%   D = 1 - X1'X2 / (||X1||_2 *||X2||_2) and label C is computed as:
%   L(D, L) = sum(C * (1 - COS_SIM(X1,X2)) + (1-C) * max(COS_SIM(X1,X2) - M, 0)).
%
%   The term "cosine distance" is often used to denote the quantity
%   D = 1 - cos_sim(X1,X2), where cos_sim refers to the cosine similarity
%   between X1 and X2.  However, it is worth noting that this is not, strictly
%   speaking a distance function (it does not obey Cauchy-Schwarz). The
%   VL_NNCOSINELOSS seeks to minimise the cosine distance between similar pairs
%   and minimise cosine similarity between dissimilar pairs, up to a margin.
%
%   [DZDX1, DZDX2] = VL_NNCOSINELOSS(X1, X2, C, DZDY) computes the
%   derivative of the block projected onto the output derivative DZDY.
%   DZDX1, DZDX2 and DZDY have the same dimensions as X1, X2 and Y
%   respectively.
%
%   VL_NNCOSINELOSS(.., 'option', value, ...) accepts the following options:
%
%   `margin`:: 0.5
%    The maximum margin to be enforced between dissimilar pairs.
%
%    See also: VL_NNLOSS().
%
%    Based on the VL_NNCONTRLOSS() by Karel Lenc.
%
%    [1] Hadsell, Raia, Sumit Chopra, and Yann LeCun. "Dimensionality
%    reduction by learning an invariant mapping." CVPR 2006

% Copyright (C) 2018 Samuel Albanie
% Licensed under The MIT License [see LICENSE.md for details]

	opts.margin = 0.5 ;
  [opts, dzdy] = vl_argparsepos(opts, varargin) ;

	sx1 = size(x1) ; sx2 = size(x2) ; bsize = size(x1, 4) ;
	assert(numel(sx1) == numel(sx2), 'input sizes must match') ;
	assert(all(sx1 == sx2), 'Invalid input sizes.') ;
	assert(numel(c) == bsize, 'Invalid number of labels.') ;

  % allow element-specific margins
	if numel(opts.margin) > 1
		assert(numel(opts.margin) == bsize, 'Invalid margin.');
		opts.margin = reshape(opts.margin, [], bsize);
	end
	c = reshape(c, [], bsize);

  sims = vl_nncosinesim(x1, x2) ;
  diff = 1 - sims ;
	mdist = sims - opts.margin ;

	if isempty(dzdy)
		sims(c == 0) = max(mdist(c == 0), 0) ;
		y = sum(sims);
	else
    % outcome is invariant to order of projection
		[dcos1, dcos2] = vl_nncosinesim(x1, x2, dzdy{1}) ;
		keyboard
		one = ones(1, 'like', x1);
		mdist = squeeze(mdist);
		y1 = diff * (dzdy * 2);
		nf = mdist ./ (dist + 1e-4*one);
		neg_sel = mdist >  0 & c == 0;
		y1(:, neg_sel) = bsxfun(@times, -y1(:, neg_sel), nf(neg_sel));
		y1(:, mdist <= 0 & c == 0) = 0;
		y2 = -y1;
		y1 = reshape(y1, sx1);
		y2 = reshape(y2, sx2);
	end


function distance = compute_interfiber_distances_multires(fgFile, maxDistanceOfInterest,  range1start, range1end, range2start,  range2end)
%Multiresolution approach: first approximation with mass_center distances,
%for the closest fibers pointwise average distance is computed.

%First we compute center-of-mass interfiber distances for fg.fibers structures in infile.mat
%Then we threshold these distances to retain only those that are smaller
%than 20mm. For those interfiber parwise_dist distances are computed.
%save distance matrix into outfile. Range1 ([Nfrst Nlasst]) and range2 specify groups of fibers among which the distances should be computed.

%ER 12/2007 Range1/2start/end: supply as a string
%(e.g., '5') -- needed for later to pass bash string arguments, when
%working on the cluster.

%02/2008 Revision:
%1. Omit last 4 parameters if want to have the wholefile processed.
%2. Resampling is no longer performed -- use
%ResampledFiberGroup=resampleFiberGroup(fg, numNodes) and save the
%resampled dataset as save('ResampledDataFile', 'fg', 'versionNum',
%'coordinateSpace')
%3. Added parameter maxDistanceOfInterest: fibers whos mass-center distance
%in mm exceeds this parameters will be considered "infinitely remote" (for the
%purpose off sparse matrix production). A typical value is 40 (in mm) for the first step, 500 for the last step.

epsilon=.000000001;

%Strip off the .mat extension if provided
if fgFile(length(fgFile)-3:length(fgFile))=='.mat'
    fgFile=fgFile(1:length(fgFile)-4);
end
load(fgFile);

method='multiresolution';

%Checking number of arguments: if 1, full range in file is used; if 5, arbitrary ranges for first and second fiber groups are allowed.
if nargin==6
range1start=min(max(str2num(range1start), 1), size(fg.fibers, 1));
range2start=min(max(str2num(range2start), 1), size(fg.fibers, 1)) ;

range1end=max(min(str2num(range1end), size(fg.fibers, 1)), range1start);
range2end=max(min(str2num(range2end), size(fg.fibers, 1)), range2start) ;
samegroupflag=0;

else
    if nargin==2
    samegroupflag=1;
    range1start=1; range2start =1; %By default process full range.
    range1end=size(fg.fibers, 1);
    range2end=size(fg.fibers, 1);
    else
        display('Wrong number of argument supplied');
    end
end

display(['Data: ' fgFile ]);

display(['Distance metric: ' method ]);
display(['Fibergroups analyzed: ' num2str(range1start) ' to ' num2str(range1end) ' and ' num2str(range2start) ' to ' num2str(range2end)]);

fibergroup1=fg.fibers(range1start:range1end);
fibergroup2=fg.fibers(range2start:range2end);
clear fg;

if (range1start==range2start&range1end==range2end)
    display('One fiber group found');

    samegroupflag=1;
    tic; distanceSquared = InterfiberMassCenterDistances(fibergroup1);  toc;

else
    display('Two distinct fiber groups found');
    tic; distanceSquared =  InterfiberMassCenterDistances(fibergroup1, fibergroup2);  toc;

end
display('Mass center distances computed');

%Thresholding distmsr and transform it into the sparse matrix:
distanceSquared(distanceSquared(:)==0)=epsilon; %This is to keep actual zeros in the matrix, as the matrix entries corresponding to "larger-than-threshold" distances will be turned to zeros in the sparse matrix;
distanceSquared(distanceSquared>maxDistanceOfInterest^2)=0;  %Note that maxDistanceOfInterest has to be squared because distanceSquared returned by InterfiberMassCenterDistances is squared!
distanceSquared=sparse(double(distanceSquared));

%Form curve arrays from fg structure
nfibers1=size(fibergroup1, 1);
nfibers2=size(fibergroup2, 1);
npoints=  size(fibergroup1{1}, 2);

curves1=zeros(3, npoints, nfibers1);
%<s>Resample fibers in Fiberr Group 1 using splines</s>
%02/2008: Resampling no longer performed. Will complain if you have not
%resampled data apriori.

for i=1:nfibers1
%   curves1(:, :, i)=dtiFiberResample(fibergroup1{i}, npoints);
   curves1(:, :, i)=fibergroup1{i};
end
if (samegroupflag==1)
    curves2=curves1;
else
     curves2=zeros(3, npoints, nfibers1);
    %<s>Resample fibers in Fiberr Group 2 using splines</s>
    %02/2008: Resampling no longer performed.

    for i=1:nfibers2
       %curves2(:, :, i)=dtiFiberResample(fibergroup2{i}, npoints);
       curves2(:, :, i)=fibergroup2{i};
    end

end
%display('Fibers resampled');

if(samegroupflag==1)
    distanceSquared=tril(distanceSquared); %If the matrix is symmetric, avoid extra computations
end

%Distances for the following pairs of fibers will be fine-tuned:
[i1 j1]=find(distanceSquared~=0);
finetunedElementIndices=find(distanceSquared~=0);

%Computing interfiber distances (pointwise)
%%%%%%%%%%%%%%%%%%%%%%%%%%%
distance=sparse(zeros(size(distanceSquared)));

tic;
for pair=1:size(i1)

distance(i1(pair), j1(pair))=intercurve_dist_pointwise(curves1(:, :, i1(pair)), curves2(:, :, j1(pair)), samegroupflag);
%The new, more "precise" values replace those not precise.
end
toc; %Wow, for 20% of fibers pairs still takes 5 times longer.
display(['Precise distances for pairs of fibers whose mass-center distance did not exceed ' num2str(maxDistanceOfInterest) 'mm computed']);

if(samegroupflag==1)
    distance=distance+distance'-triu(distance); %Since the matrix for "samegroup" case was symmetric, we need to repopulate the upper triangle.
end

% Remember: we have a sparse matrix representation where zeros mean
% "infinite distance" actually (and are not stored). So actual true zero distances should be  made a
% very small number.
distance(nonzeros(finetunedElementIndices.*(distance(finetunedElementIndices)==0)))=epsilon;

outfile=[fgFile 'distances_multires' num2str(range1start) 'to' num2str(range1end) 'vs' num2str(range2start) 'to' num2str(range2end) '.mat'];
[pathstr, name, ext, versn] = fileparts(outfile) ;
if isempty(pathstr)
    pathstr=['.' filesep ];
end

mkdir(pathstr, 'multiresDistances');
outfilefull=fullfile(pathstr, 'multiresDistances', name);
save(outfilefull, 'distance', 'fibergroup1', 'fibergroup2', 'fgFile', 'range1start', 'range2start', 'range1end', 'range2end', 'method');


function [Gu,Gn,w, dt] = spm_csd_fmri_gu(P,dt)
% spectra of neuronal fluctuations and noise
% FORMAT [Gu,Gn,Hz,dt] = spm_csd_fmri_gu(P,dt)
%
% P  - model parameters
% dt - sampling interval
%
% This routine returns the spectra of neuronal fluctuations and noise for a
% standard frequency range specified by the sampling interval
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_csd_fmri_gu.m 7270 2018-03-04 13:08:10Z karl $

% compute log-spectral density
%==========================================================================

% frequencies of interest
%--------------------------------------------------------------------------
Hz1  = 1/128;
Hz2  = 1/(2*dt);
nw   = 32;
w    = linspace(Hz1,Hz2,nw)';

% number of nodes and endogenous (neuronal) fluctuations
%--------------------------------------------------------------------------
nn   = size(P.A,1);
nu   = nn;
form = '1/f';

% spectrum of neuronal fluctuations (Gu) and observation noise (Gn)
%==========================================================================

% experimental inputs
%--------------------------------------------------------------------------
Gu    = zeros(nw,nu,nu);
Gn    = zeros(nw,nn,nn);

% neuronal fluctuations (Gu) (1/f or AR(1) form)
%--------------------------------------------------------------------------
for i = 1:nu
    if strcmp(form,'1/f')
        G     = w.^(-exp(P.a(2,1)));
    else
        G     = spm_mar2csd(exp(P.a(2,1)),w);
    end
    Gu(:,i,i) = Gu(:,i,i) + exp(P.a(1,1))*G/sum(G);
end

% region specific observation noise (1/f or AR(1) form)
%--------------------------------------------------------------------------
for i = 1:nn
    if strcmp(form,'1/f')
        G     = w.^(-exp(P.b(2,1))/2);
    else
        G     = spm_mar2csd(exp(P.b(2,1))/2,w);
    end
    Gn(:,i,i) = Gn(:,i,i) + exp(P.c(1,i))*G/sum(G);
end

% global components
%--------------------------------------------------------------------------
if strcmp(form,'1/f')
    G = w.^(-exp(P.b(2,1))/2);
else
    G = spm_mar2csd(exp(P.b(2,1))/2,w);
end
for i = 1:nn
    for j = i:nn
        Gn(:,i,j) = Gn(:,i,j) + exp(P.b(1,1))*G/sum(G);
        Gn(:,j,i) = Gn(:,i,j);
    end
end


function im = fftc(d)
% Function performs a centered fft
im = fftshift(fft(fftshift(d)));


function [ly] = nm2ly(nm)
% Convert length from nanometers to light years.
% Chad A. Greene 2012
ly = nm*1.057000834025e-25;


function iclust = initialize_clusters(Ucell, Nk, type, Lx, Ly)

switch type
    case 'random'
        vs = randn(size(Ucell,1), Nk);
        vs = bsxfun(@rdivide, vs, sum(vs.^2,1).^.5 + 1e-8);% normalize activity vectors
        vs = single(vs);
        xs          = vs' * Ucell;
        [~, iclust] = max(xs,[],1);
    case 'Voronoi'

        xs = repmat(1:Lx, Ly, 1);
        ys = repmat((1:Ly)', 1, Lx);

        randx = rand(1, Nk) * Lx;
        randy = rand(1, Nk) * Ly;

        dx = repmat(xs(:), 1, Nk) - repmat(randx, numel(xs(:)), 1);
        dy = repmat(ys(:), 1, Nk) - repmat(randy, numel(ys(:)), 1);

        dxy = dx.^2 + dy.^2;
        [~, iclust] = min(dxy, [], 2);
    case 'squares'
        nsqrt = round(sqrt(Nk));

        xs = repmat(round(linspace(1, nsqrt, Lx)), Ly, 1);
        ys = repmat(round(linspace(1, nsqrt, Ly))', 1, Lx);
        iclust = xs + (ys-1) * nsqrt;

end

iclust = iclust(:)';


clear all;
rand('seed',0);
randn('seed',0);
fprintf('test mexFistaPathCoding\n');
p=100;
n=1000;
% generate a DAG
G=sprand(p,p,0.05);
G=mexRemoveCyclesGraph(G);
fprintf('\n');

% generate a data matrix
X=randn(n,p);
X=X-repmat(mean(X),[size(X,1) 1]);
X=mexNormalize(X);
Y=randn(n,2);
Y=Y-repmat(mean(Y),[size(Y,1) 1]);
Y=mexNormalize(Y);
W0=zeros(size(X,2),size(Y,2));

% input graph
graph.weights=G;
graph.stop_weights=zeros(1,p);
graph.start_weights=10*ones(1,p);

% FISTA parameters
param.num_threads=-1; % all cores (-1 by default)
param.verbose=true;   % verbosity, false by default
param.lambda=0.005; % regularization parameter
param.it0=1;      % frequency for duality gap computations
param.max_it=100; % maximum number of iterations
param.L0=0.01;
param.tol=1e-4;
param.precision=10000000;
param.pos=false;

fprintf('Square Loss + convex path penalty\n');
param.loss='square';
param.regul='graph-path-conv';
tic
[W1 optim_info]=mexFistaPathCoding(Y,X,W0,graph,param);
t=toc;
fprintf('mean loss: %f, mean relative duality_gap: %f, time: %f, number of iterations: %f\n',mean(optim_info(1,:)),mean(optim_info(3,:)),t,mean(optim_info(4,:)));
num=mexCountConnexComponents(graph.weights,W1(:,1));
fprintf('Num of connected components: %d\n',num);

fprintf('\n');
fprintf('Square Loss + non-convex path penalty\n');
param.loss='square';
param.regul='graph-path-l0';
param.lambda=0.0001; % regularization parameter
param.ista=true;
tic
[W2 optim_info]=mexFistaPathCoding(Y,X,W0,graph,param);
t=toc;
num=mexCountConnexComponents(graph.weights,W2(:,1));
fprintf('Num of connected components: %d\n',num);

fprintf('\n');
fprintf('Note that for non-convex penalties, continuation strategies sometimes perform better:\n');
tablambda=param.lambda*sqrt(sqrt(sqrt(2))).^(20:-1:0);
lambda_orig=param.lambda;
tic
W2=W0;
for ii = 1:length(tablambda)
   param.lambda=tablambda(ii);
   param.verbose=false;
   [W2]=mexFistaPathCoding(Y,X,W2,graph,param);
end
param.verbose=true;
param.lambda=lambda_orig;
[W2 optim_info]=mexFistaPathCoding(Y,X,W2,graph,param);
t=toc;
num=mexCountConnexComponents(graph.weights,W2(:,1));
param.ista=false;
fprintf('Num of connected components: %d\n',num);


function [M] = ft_connectivity_mim(inputdata, varargin)

% FT_CONNECTIVITY_MIM computes the multivariate interaction measure from a
% data-matrix containing the cross-spectral density. This implements the method
% described in Ewald et al., Estimating true brain connectivity from EEG/MEG data
% invariant to linear and static trasformations in sensor space. Neuroimage, 2012;
% 476:488.
%
% Use as
%   [m] = hcp_connectivity_mim(inputdata, ...)
%
% The input data should be an array organized as
%   Channel x Channel x Frequency
%
% The output m contains the newChannel x newChannel x Frequency connectivity measure,
% with newChannel equal to max(indices).
%
% Additional optional input arguments come as key-value pairs:
%   'indices' = 1xN vector with indices of the groups to which the channels belong,
%               e.g. [1 1 2 2] for a 2-by-2 connectivity between 2 planar MEG channels.
%
%
% See also CONNECTIVITY, FT_CONNECTIVITYANALYSIS

% Copyright (C) 2011-2014 by the Human Connectome Project, WU-Minn Consortium (1U54MH091657)
% Copyright (C) 2021 Jan-Mathijs Schoffelen, DCCN
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

indices = ft_getopt(varargin, 'indices');

if isempty(indices) && isequal(size(inputdata(:,:,1)), [2 2])
  % simply assume two channels
  indices = [1 1 2 2];
end

sizein  = size(inputdata);
sizeout = [sizein 1];
sizeout(1:2) = max(indices);

% compute the inverse of the auto terms only once for speed up
invC = cell(sizeout(1),sizeout(3));
for kk = 1:sizeout(3)
  for k = 1:sizeout(1)
    invC{k,kk} = pinv(real(inputdata(indices==k,indices==k,kk)));
  end
end

M = zeros(sizeout);
for kk = 1:sizeout(3)
  for k = 1:sizeout(1)
    for m = 1:sizeout(1)
      indx1 = indices==k;
      indx2 = indices==m;
      %cs_aa_re = real(input(indx1,indx1));
      %cs_bb_re = real(input(indx2,indx2));
      cs_ab_im = imag(inputdata(indx1,indx2,kk));

      %inv_cs_bb_re = pinv(cs_bb_re);
      %inv_cs_aa_re = pinv(cs_aa_re);
      inv_cs_bb_re = invC{m,kk};
      inv_cs_aa_re = invC{k,kk};
      transp_cs_ab_im = transpose(cs_ab_im);
      M(k,m,kk) = trace(inv_cs_aa_re*cs_ab_im*inv_cs_bb_re*transp_cs_ab_im); % try to speed up by dividing calculation in steps
    end
  end
end

% taking the mldivide and mrdivide operators doesn't change the results, but speeds up by a factor of 4 over 1000 iterations (on LM Notebook)
% m = trace(cs_aa_re\cs_ab_im*inv_cs_bb_re*transp_cs_ab_im);

% % % block_a=cs_aa_re\cs_ab_im;
% % % block_b=cs_bb_re\transpose(cs_ab_im);
% % % m = trace(block_a*block_b);

% m = trace(cs_aa_re\cs_ab_im*(cs_bb_re\transpose(cs_ab_im)));


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

% M=1 scattering

run_name = 'DSS_Table2_GTZAN_m1';

N=5*2^17;

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

fparam.filter_type = {'gabor_1d','morlet_1d'};
fparam.Q = [8 2];
fparam.J = T_to_J(8192,fparam);

options.M = 1;

Wop = wavelet_factory_1d(N, fparam, options);

feature_fun = {@(x)(format_scat(log_scat(renorm_scat(scat(x,Wop)))))};

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

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

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

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

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

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

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

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

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


function out = PeubSource(k, p, d, L, accuracy)
%upper bound for the error probability of the binary source
%achievability via sphere covering
%k - source blocklength
%p - source bias
%d - distortion threshold
%L = log M, where M is the number of representation points
%accuracy = accuracy of Csum: 1 - more accurate, 0 - faster

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

Kbig = 80;

if accuracy < 0
    %normal approximation
    Rd = h(p) - h(d);
    Vs = p*(1-p)*(log2((1-p)/p))^2;
    if p ~= 1/2 && k >= Kbig
        out = Q( (L - k*Rd - log2(k))/sqrt(k*Vs) );
    else
        out = PeubSource(L, k, p, d);
    end
    return;
end

if d == 0
    %lossless
    out = PeubSourceLossless(L, k, p);
elseif p == 1/2
    %FAIR coin flip source
    out = PeubSourceFair(L, k, d);
else
    %arbitrary p source
    out = PeubSourceBiased(L, k, p, d);
end
%        fprintf('SphereCoveringA: L = %i, k = %i, Peub = %f\n', L, k, out);

    function out = PeubSourceLossless(L, k, p)
        csum = 0;
        kstar = k;
        for i = 0:k
            csum = csum + C(k, i, 'ub');
            if log2(csum) > L
                kstar = i - 1;
                break;
            end
        end
        out =  1 - binocdf(kstar,k,p);
    end

    function out = PeubSourceFair(L, k, d)
        if k <= Kbig
            out =  2^L *log(1 - Csum(k, floor(k*d), 'lb', accuracy) /(2^k));
        else
            out = -2^(L-k)*Csum(k, floor(k*d), 'lb', accuracy);
        end
        out = exp(out);
    end

    function out = PeubSourceBiased(L, k, p, d)
        q = max ( 0, (p-d)/(1-2*d));
        if q == 0
            q = p;
        end
        out = 0;
        for t = 0:k
            out = out + binopdfbound(t,k,p, 'ub')*e(k,t);
            if isnan(out)
                break;
            end
        end

        function out = e(k,t)
            out = 0;
            if 1
                for T = 0:k
                    out = out + binopdfbound(T, k, q, 'ub')*Pwithin(k, t, T);
                end
            else
                T = round(q*k); %#ones
                out = Pwithin(k, t, T);
            end

            if k <= Kbig
                out = 2^L*log( 1 - out);
                out = exp(out);
            else
                out = exp(-2^L*out);
            end

        end

        function out = Pwithin(k,t, T)
            %y of type T is within a given x of type t - almost exact lower bound
            t0 = max ( 0, ceil( (t+T - k*d)/2) ); %# ones  in 11111 zone (T ones in codewords)

            out = logC(T, t0, 'lb') + logC(k-T, t - t0, 'lb') - logC(k, t, 'ub');
            out = 2^out;
        end

    end
end


function ksvddenoisedemo
%KSVDDENOISEDEMO K-SVD denoising demonstration.
%  KSVDDENISEDEMO reads an image, adds random white noise and denoises it
%  using K-SVD denoising. The input and output PSNR are compared, and the
%  trained dictionary is displayed.
%
%  To run the demo, type KSVDDENISEDEMO from the Matlab prompt.
%
%  See also KSVDDEMO.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2009

disp(' ');
disp('  **********  K-SVD Denoising Demo  **********');
disp(' ');
disp('  This demo reads an image, adds random Gaussian noise, and denoises the image');
disp('  using K-SVD denoising. The function displays the original, noisy, and denoised');
disp('  images, and shows the resulting trained dictionary.');
disp(' ');

%% prompt user for image %%

pathstr = fileparts(which('ksvddenoisedemo'));
dirname = fullfile(pathstr, 'images', '*.png');
imglist = dir(dirname);

disp('  Available test images:');
disp(' ');
for k = 1:length(imglist)
  printf('  %d. %s', k, imglist(k).name);
end
disp(' ');

imnum = 0;
while (~isnumeric(imnum) || ~iswhole(imnum) || imnum<1 || imnum>length(imglist))
  imnum = input(sprintf('  Image to denoise (%d-%d): ', 1, length(imglist)), 's');
  imnum = sscanf(imnum, '%d');
end

imgname = fullfile(pathstr, 'images', imglist(imnum).name);

%% generate noisy image %%

sigma = 20;

disp(' ');
disp('Generating noisy image...');

im = imread(imgname);
im = double(im);

n = randn(size(im)) * sigma;
imnoise = im + n;

%% set parameters %%

params.x = imnoise;
params.blocksize = 8;
params.dictsize = 256;
params.sigma = sigma;
params.maxval = 255;
params.trainnum = 40000;
params.iternum = 20;
params.memusage = 'high';

% denoise!
disp('Performing K-SVD denoising...');
[imout, dict] = ksvddenoise(params);

% show results %

dictimg = showdict(dict,[1 1]*params.blocksize,round(sqrt(params.dictsize)),round(sqrt(params.dictsize)),'lines','highcontrast');
figure; imshow(imresize(dictimg,2,'nearest'));
title('Trained dictionary');

figure; imshow(im/params.maxval);
title('Original image');

figure; imshow(imnoise/params.maxval);
title(sprintf('Noisy image, PSNR = %.2fdB', 20*log10(params.maxval * sqrt(numel(im)) / norm(im(:)-imnoise(:))) ));

figure; imshow(imout/params.maxval);
title(sprintf('Denoised image, PSNR: %.2fdB', 20*log10(params.maxval * sqrt(numel(im)) / norm(im(:)-imout(:))) ));


function [pass, u1, u2, info1, info2] = test_scalarODE_sign(pref)
% A nonlinear CHEBOP test. This test tests a scalar ODE, where there is a
% breakpoint in the domain of the CHEBOP. Furthermore, the operator has a
% discontinuous coefficient, which will induce a further breakpoint in the
% solution. The problem is solved using chebcolloc1, chebcolloc2 and ultraS
% discretizations. The problem solved does not require damping for the Newton
% iteration to converge.
%
% Asgeir Birkisson, May 2014.

%% Setup
if ( nargin == 0 )
    pref = cheboppref;
end
dom = [-1 .5 1];

N = chebop(@(x,u) diff(u,2) + sign(x).*sin(u), dom);
N.lbc = @(u) u - 2;
N.rbc = @(u) u - 2;
rhs = 0;

%% Try different discretizations
% Start with chebcolloc2
pref.discretization = @chebcolloc2;
[u1, info1] = solvebvp(N, rhs, pref);

%% Change to ultraS
pref.discretization = @ultraS;
[u2, info2] = solvebvp(N, rhs, pref);

%% Change to chebcolloc1
pref.discretization = @chebcolloc1;
[u3, info3] = solvebvp(N, rhs, pref);

%% Did we pass?
% To pass, both residuals have to be small, but we should not expect u1 and u2
% to be identical!

% TODO: This used to be pref.bvpTol. Once we tune the algorithms better, should
% try to restore it.
tol = 1e3*pref.bvpTol;
pass(1) = norm(N(u1)) < tol;
pass(2) = norm(N(u2)) < tol;
pass(3) = norm(N(u3)) < tol;
pass(4) = ( (norm(u1 - u2) ~= 0) && (norm(u2 - u3) ~= 0) && ...
    (norm(u1 - u3) ~= 0));

end


function y = BF_PreProcess(y,preProcessHow)
% BF_PreProcess   Preprocess a time series, y

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if ~isempty(preProcessHow)
    switch preProcessHow
    case 'diff1'
        % Takes incremental differences of the input time series
        y = diff(y);
    case 'rescale_tau'
        % Coarse-graining at a given scale, as in multiscale entropy approaches
        % Find first zero of the autocorrelation function
        tau = CO_FirstCrossing(y,'ac',0,'discrete');
        % Buffer the time series into nonoverlapping windows of length tau
        y_buffer = BF_MakeBuffer(y,tau);
        % Mean each window to get a coarse-grained time series
        y = mean(y_buffer,2);
    otherwise
        error('Unknown preprocessing setting: ''%s''',preProcessHow);
    end
end

end


function value = year_length_roman ( y )

%*****************************************************************************80
%
%% YEAR_LENGTH_ROMAN returns the number of days in a Roman year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, integer VALUE, the number of days
%    in the year.
%
  if ( year_is_leap_roman ( y ) )
    value = 366;
  else
    value = 365;
  end

  return
end


%TEST_DESCI_CASSI_BIRD Test decompress snapshot compressive imaging (DeSCI)
%for simulated coded aperture snapshot spectral imaging (CASSI) `bird`
%dataset.
% Reference
%   [1] Y. Liu, X. Yuan, J. Suo, D.J. Brady, and Q. Dai, Rank Minimization
%       for Snapshot Compressive Imaging, IEEE Trans. Pattern Anal. Mach.
%       Intell. (TPAMI), DOI:10.1109/TPAMI.2018.2873587, 2018.
%   [2] X. Yuan, Generalized alternating projection based total variation
%       minimization for compressive sensing, in Proc. IEEE Int. Conf.
%       Image Process. (ICIP), pp. 2539-2543, 2016.
% Dataset
%   `toy` from the CAVE multispectral image database
%     (http://www1.cs.columbia.edu/CAVE/databases/multispectral/).
%   `bird` from the multiframe CASSI system [3] captured in [4].
%     [3] D. Kittle, K. Choi, A. Wagadarikar, and D. J. Brady, Multiframe
%         image estimation for coded aperture snapshot spectral imagers,
%         Appl. Opt., vol. 49, no. 36, pp. 6824-6833, 2010.
%     [4] A. Rajwade, D. Kittle, T.-H. Tsai, D. Brady, and L. Carin, Coded
%         Hyperspectral Imaging and Blind Compressive Sensing, SIAM J. on
%         Imag. Sci., vol. 6, no. 2, pp. 782-812, 2013.
% Contact
%   Xin Yuan, Bell Labs, xyuan@bell-labs.com, initial version Jul 2, 2015.
%   Yang Liu, Tsinghua University, y-liu16@mails.tsinghua.edu.cn, last
%     update Dec 17, 2018.
%   See also GAPDENOISE_CACTI, GAPDENOISE.
clear; clc;
% [0] environment configuration
addpath(genpath('../algorithms')); % algorithms
addpath(genpath('../packages')); % packages
addpath(genpath('../utils')); % utilities

datasetdir = '../dataset'; % dataset
resultdir  = '../results'; % results

% [1] load dataset
para.type   = 'cassi'; % type of dataset, cassi or cacti
para.name   = 'bird'; % name of dataset
para.number = 24; % number of frames in the dataset

datapath = sprintf('%s/%s%d_%s.mat',datasetdir,para.name,...
    para.number,para.type);

if exist(datapath,'file')
    load(datapath); % mask, meas, orig (and para)
else
    error('File %s does not exist, please check dataset directory!',...
        datapath);
end

para.nframe = 1; % number of coded frames in this test
para.MAXB   = 255;

[nrow,ncol,nmask] = size(mask);
nframe = para.nframe; % number of coded frames in this test
MAXB = para.MAXB;

% [2] apply GAP-Denoise for reconstruction
% yall = meas/MAXB;

para.Mfunc  = @(z) A_xy(z,mask);
para.Mtfunc = @(z) At_xy_nonorm(z,mask);

para.Phisum = sum(mask.^2,3);
para.Phisum(para.Phisum==0) = 1;
% common parameters
para.lambda   =     1; % correction coefficiency
para.acc      =     1; % enable GAP-acceleration
para.flag_iqa = false; % disable image quality assessments in iterations

%% [2.1] GAP-TV, ICIP'16
para.denoiser = 'tv'; % TV denoising
  para.maxiter  = 250; % maximum iteration
  para.tvweight = 5; % weight for TV denoising
  para.tviter   = 5; % number of iteration for TV denoising

[vgaptv,psnr_gaptv,ssim_gaptv,tgaptv] = ...
    gapdenoise_cacti(mask,meas,orig,[],para);

fprintf('GAP-%s mean PSNR %2.2f dB, mean SSIM %.4f, total time % 4.1f s.\n',...
    upper(para.denoiser),mean(psnr_gaptv),mean(ssim_gaptv),tgaptv);

%% [2.2] DeSCI (with GAP-TV for initialization), TPAMI'18
para.denoiser = 'wnnm'; % WNNM denoising
  para.wnnm_int_fwise = true; % enable GAP-WNNM integrated (with frame-wise denoising)
    para.blockmatch_period = 20; % period of block matching
  para.sigma   = [12]/MAXB; % noise deviation (to be estimated and adapted)
  para.vrange  = 1; % value range
  para.maxiter = [40];
  para.patchsize = 32; % patch size
  para.iternum = 1; % iteration number in WNNM
  para.enparfor = true; % enable parfor
  if para.enparfor % if parfor is enabled, start parpool in advance
      delete(gcp('nocreate')); % delete current parpool
      mycluster = parcluster('local');
      div = 1;
      while nmask/div > mycluster.NumWorkers
          div = div+1;
      end
      parnum = max(min(ceil(nmask/div),mycluster.NumWorkers),1);
      parpool(mycluster,parnum);
  end

[vdesci,psnr_desci,ssim_desci,tdesci,psnrall] = ...
    gapdenoise_cacti(mask,meas,orig,vgaptv,para); % vgaptv as initialization

  tdesci = tdesci + tgaptv;
  delete(gcp('nocreate')); % delete parpool

fprintf('DeSCI mean PSNR %2.2f dB, mean SSIM %.4f, total time % 4.1f s.\n',...
    mean(psnr_desci),mean(ssim_desci),tdesci);

%% [3] save results as mat file
matdir = [resultdir '/savedmat'];
if ~exist(matdir,'dir')
    mkdir(matdir);
end
save([matdir '/desci_' para.type '_' para.name '.mat']);