function [x_best, fit_best, x_pop, fit_pop stats]=complexmethod(fcn_name,bounds,gen_max,x_start,fit_start,fcn_opts,complex_opts)
%[x_best, fit_best, x_pop, fit_pop stats]=complexmethod(fcn_name,bounds,gen_max,x_start,fit_start,fcn_opts,complex_opts)
%
% Implements the Complex Method of Contrained Optimization, as proposed by Box (1965),
% improved by Guin (1968) and Krus (1992), and following the method in Andresson (2001).
%
% Takes inputs of:
% fcn_name: string including a function to maximize.  This function must give outputs of
%   [fitness x] and take inputs of (x,fcn_opts)
% bounds: 2 by n_params matrix of the minimum and maximum bounds for each parameter
% gen_max; number of times to calculate fitness before stopping (this includes the
%    initialization calculations if fit_start is not given
% x_start: a n_population by n_params matrix of initial parameters
% fit_start: a n_params vector of fitnesses of x_start params
% fcn_opts: a variable (or structure) that may be passed to the fcn_name function
% complex_opts: a variable including some options for the optimization (you probably won't need this)
%
% References
%
%Andersson J, "Multiobjective Optimization in Engineering Design - Application to fluid Power %Systems." Doctoral thesis, Division of Fluid and Mechanical Engineering Systems, Department %of Mechanical Engineering, Linkping University, 2001 %http://citeseer.ist.psu.edu/562279.html
%
%Box, M.J., "A new method of constrained optimization and a comparison with other method," %Computer Journal, Vol. 8, No. 1, pp. 42-52, 1965.
%
%Guin J. A., "Modification of the Complex method of constraint optimization," Computer Jornal, %vol. 10, pp. 416-417, 1968. 32(34)
%
%KRUS P., JANSON A., PALMBERG J.-O., “Optimization Based on Simulation for Design of Fluid %Power Systems, in Proceedings of ASME Winter Annual Meeting, Anaheim, USA, 1992.
%
%    Copyright Travis Wiens 2008
%

if nargin<2
	error('Insufficient arguements')
end
if nargin<3
	gen_max=1000;
elseif isempty(gen_max)
    gen_max=1000;
end

if nargin<4
	x_start=[];
end
if nargin<5
	fit_start=[];
end
if nargin<6
	fcn_opts=[];
end
if nargin<7
	complex_opts.alpha=1.3;%constant determines how far to overshoot centroid
	complex_opts.n_r=4;%constant for adjusting to repeated unimprovements
elseif isempty(complex_opts)
    complex_opts.alpha=1.3;%constant determines how far to overshoot centroid
	complex_opts.n_r=4;%constant for adjusting to repeated unimprovements
end

fcn_str=['[fitness, x]=' fcn_name '(x,fcn_opts);'];%string to determine fitness
%output of x allows function to change x if desired

fit_best=-inf;%initial best fitness

n_params=size(bounds,2);

gen=1;%initialize number of generations

alpha=complex_opts.alpha;%copy values
n_r=complex_opts.n_r;

if isempty(x_start)
	f_excess=1.5;%factor to increase population over minimum (must be >=1)
	n_pop=round((n_params+1)*f_excess);
	x_pop=ones(n_pop,1)*bounds(1,:) +(ones(n_pop,1)*(bounds(2,:)-bounds(1,:))).*rand(n_pop,n_params);
else
	x_pop=x_start;
	n_pop=size(x_pop,1);

end

%initialize fitness
if isempty(fit_start)
	fit_pop=zeros(1,n_pop);
	for i=1:n_pop
		x=x_pop(i,:);
		eval(fcn_str); %get fitness for each x
		x_pop(i,:)=x;
		fit_pop(i)=fitness;
		gen=gen+1;
	end
else
	fit_pop=fit_start;
	if numel(fit_pop)~=n_pop
		error('Incorrect size of fit_pop');
	end
end

stats.trace_fitness=zeros(1,gen_max);
while (gen<gen_max)

	[fit_best idx_best]=max(fit_pop);%find best fitness
	x_best=x_pop(idx_best,:);%find best params
	[fit_worst idx_worst]=min(fit_pop);%find worst
	x_worst=x_pop(idx_worst,:);%find best params

	x_centroid=(sum(x_pop)-x_worst)/(n_pop-1);%calc centroid of all but worst x

	x_new=x_centroid+alpha*(x_centroid-x_worst);%"mirror" worst point through centroid

	idx=find(x_new<bounds(1,:));%find params out of bounds
	x_new(idx)=bounds(1,idx);
	idx=find(x_new>bounds(2,:));
	x_new(idx)=bounds(2,idx);

	x=x_new;
	eval(fcn_str); %get fitness for new x
	x_new=x;
	fit_new=fitness;
	stats.trace_fitness(gen)=fitness;
	gen=gen+1;%increment generation

	if fit_new<fit_worst %check if the new point is still worst
		k_r=1;%reset number of iterations
		while (fit_new<fit_worst&gen<gen_max)
			epsilon=(n_r/(n_r+k_r-1))^((n_r+k_r-1)/n_r);
			k_r=k_r+1;%increment number of iterations
			R=rand;
			x_store=x_new;%store previous x_new
			x_new=(x_new+epsilon*x_centroid+(1-epsilon)*x_best)/2+(x_centroid-x_best)*(1-epsilon)*(2*R-1);

			idx=find(x_new<bounds(1,:));%find params out of bounds
			x_new(idx)=bounds(1,idx);
			idx=find(x_new>bounds(2,:));
			x_new(idx)=bounds(2,idx);

			x=x_new;
			eval(fcn_str); %get fitness for new x
			x_new=x;
			fit_new=fitness;
			stats.trace_fitness(gen)=fitness;
			gen=gen+1;%increment generation
		end
	end
	x_pop(idx_worst,:)=x_new;%replace worst params with new params
	fit_pop(idx_worst)=fit_new;
end

[fit_best idx_best]=max(fit_pop);%find best fitness
x_best=x_pop(idx_best,:);%find best params


function test_ft_freqanalysis

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_freqanalysis

ntrl = 1000;
nchan = 32;
fs = 500;
start_time = -1; % seconds
end_time = 2.5; % seconds
nsamples = (end_time - start_time) * fs + 1;

data = [];
data.label = cellstr(num2str((1:nchan).'));
for i=1:ntrl
  data.time{i} = linspace(start_time, end_time, nsamples);
  data.trial{i} = randn(nchan,nsamples);
end

cfg = [];
cfg.method = 'mtmfft';
cfg.taper = 'hanning';
cfg.foilim = [4 30];
freq = ft_freqanalysis(cfg, data);


%[ P, inls ] = ht_simple_ransac_p3p( u, X, rthr, maxiter )
%u: 3 x n image points
%X: 3 x n 3D points
%rthr: inlier threshold
%maxiter: default=1000

function [ P, inls ] = ht_lo_ransac_p3p( u, X, rthr, max_iter )
if nargin < 4
    max_iter = 1000;
end

%initialization
u = bsxfun(@rdivide, u, sqrt(sum(u.^2, 1)));
Npts = size(u, 2);
rthr = cos(rthr);
max_inlsnum = 3;
no_iter = 0;
P = [];
inls = false(1, Npts);
%ransac
while no_iter < max_iter
    no_iter = no_iter + 1;

    idx = randperm(Npts, 3);
    P_cand = P3PSolver([u(:, idx); X(:, idx)]);
    [inls_cand, inls_cand_num] = calculate_inls_angular(P_cand, u, X, rthr);
    if length(P_cand) == 0
%         no_iter = no_iter - 1;
        continue;
    elseif length(P_cand) > 1
        [inls_cand_num, inls_cand_idx] = max(inls_cand_num);
        inls_cand = inls_cand{inls_cand_idx};
        P_cand = P_cand{inls_cand_idx};
    else
        inls_cand_num = inls_cand_num(1);
        inls_cand = inls_cand{1};
        P_cand = P_cand{1};
    end

    %nonlinuear local optimization
    if inls_cand_num > 3
        lo_cnt = 0;
        while lo_cnt < 10
            lo_cnt = lo_cnt + 1;
            [lo_P, lo_inls, lo_inls_num] = ht_PnPnonlin(P_cand, u, X, inls_cand, rthr);
            if lo_inls_num >= inls_cand_num
                inls_cand_num = lo_inls_num;
                inls_cand = lo_inls;
                P_cand = lo_P;
            else
                break;
            end
        end
    end

    if inls_cand_num >= max_inlsnum
        max_inlsnum = inls_cand_num;
        P = P_cand;
        inls = inls_cand;
        max_iter = min([max_iter, nsamples(max_inlsnum, Npts, 3, 0.95)]);
    end

end

end

function [SampleCnt, q] = nsamples(ni, ptNum, pf, conf)
    q  = prod (((ni-pf+1) : ni) ./ ((ptNum-pf+1) : ptNum));
    if q < eps
      SampleCnt = Inf;
    else
      %   SampleCnt  = log(1 - conf) / log(1 - q);
      if q > conf
        SampleCnt = 1;
      else
        SampleCnt  = log(1 - conf) / log(1 - q);
      end
    end
end

function [inls, inls_num] = calculate_inls_angular(Pcand, u, X, rthr)
    inls = cell(1, length(Pcand));
    inls_num = zeros(1, length(Pcand));
    for ii = 1:1:length(Pcand)
        X_reproj = Pcand{ii} * [X; ones(1, size(X, 2))];
        X_reproj = bsxfun(@rdivide, X_reproj, sqrt(sum(X_reproj.^2, 1)));

        res = sum(u .* X_reproj, 1);
        inls{ii} = res > rthr;
        inls_num(ii) = sum(inls{ii});
    end

end

function [Poptim, inls, inls_num] = ht_PnPnonlin(Pcand, u, X, idx, rthr)
    Poptim = PnP_mex_wrapper( u(:, idx), X(:, idx), Pcand );

    X_reproj = Poptim * [X; ones(1, size(X, 2))];
    X_reproj = bsxfun(@rdivide, X_reproj, sqrt(sum(X_reproj.^2, 1)));
    res = sum(u .* X_reproj, 1);
    inls = res > rthr;
    inls_num = sum(inls);
end


function [y, cost] = sig_manifWB(Fopt, FRF, FBB)
[Nt, NRF] = size(FRF);
K = size(FBB,3);

manifold = complexcirclefactory(Nt*NRF);
problem.M = manifold;

for k = 1:K
    temp = Fopt(:,:,k);
    A = kron(FBB(:,:,k).', eye(Nt));
    C1(:,:,k) = temp(:)'*A;
    C2(:,k) = A'*temp(:);
    C3(:,:,k) = A'*A;
    C4(k) = norm(temp,'fro')^2;
end
B1 = sum(C1,3);
B2 = sum(C2,2);
B3 = sum(C3,3);
B4 = sum(C4);

problem.cost = @(x) -B1*x - x'*B2 + trace(B3*x*x') + B4;
problem.egrad = @(x) -2*B2 + 2*B3*x;

% checkgradient(problem);
warning('off', 'manopt:getHessian:approx');

[x,cost] = conjugategradient(problem,FRF(:));
% [x,cost,info,options] = trustregions(problem, FRF(:));
y = reshape(x,Nt,NRF);

end

% problem.cost = @(x) mycost(Fopt, FBB, x);
% function g = mycost(Fopt, FBB, x)
%     g = 0;
%     for k = 1:K
%         temp = Fopt(:,:,k);
%         A = kron(FBB(:,:,k).', eye(Nt));
%         g = g + ( temp(:) -  A*x )' * ( temp(:) - A*x );
%     end
% end
%
% problem.egrad = @(x) mygrad(Fopt, FBB, x);
% function g = mygrad(Fopt, FBB, x)
%     g = 0;
%     for k = 1:K
%         temp = Fopt(:,:,k);
%         A = kron(FBB(:,:,k).', eye(Nt));
%         g = g -2*A'*temp(:) + 2*A'*A*x;
%     end
% end

% problem.costgrad = @(x) mycostgrad(Fopt, FBB, x);
% function [h, g] = mycostgrad(Fopt, FBB, x)
%     h = 0;
%     g = 0;
%     for k = 1:K
%         temp = Fopt(:,:,k);
%         A = kron(FBB(:,:,k).', eye(Nt));
%         g = g -2*A'*temp(:) + 2*A'*A*x;
%         h = h + ( temp(:) -  A*x )' * ( temp(:) - A*x );
%     end
% end


function [u,Du,eqn,info] = PoissonQ1old(node,elem,pde,bdFlag, option)
%% POISSONQ1 Poisson equation: Q1 bilinear element.
%
%   u = PoissonQ1(node,elem,pde,bdFlag) produces the bilinear finite element
%   approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The quad mesh is given by node and elem and the boundary edge is given by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
%   The function Poisson assembes the matrix equation AD*u = b and solves
%   it by the direct solver.
%
%   The diffusion coefficient d is a scalar function or a column array with
%   the same length as the elem array.
%
%   u = PoissonQ1(node,elem,pde,bdFlag,option) specifies the options.
%    - option.dquadorder: quadrature order for diffusion coefficients
%    - option.fquadorder: quadrature order for computing right hand side f
%
%   When only one type of boundary condition is imposed, the input argument
%   bdFlag can be skipped. The boundary condition is implicitly given in
%   the pde structure by specifying g_D or g_N only. See examples below.
%
%   [u,Du] = PoissonQ1(node,elem,pde,bdFlag) returns also the
%   non-modified stiffness matrix A, which is semi-definite. The kernel of
%   A consists of constant vectors. The matrix A can be used to evulate the
%   bilinear form A(u,v) = u'*A*v, especially the enery norm of a finite
%   element function u by sqrt(u'*A*u).
%
%   [u,Du,eqn] = PoissonQ1(node,elem,pde,bdFlag) returns also the equation
%   structure eqn, which includes:
%     - eqn.AD:  modified stiffness matrix AD;
%     - eqn.b:   the right hand side.
%   The solution u = AD\b. The output eqn can be used to test other solvers.
%
%
%   Example
%     clear all
%     node=  [0, 0; 1, 0; 1, 1; 0,1];
%     elem = [1,2,3,4];
%     for k = 1:4
%         [node,elem] = uniformrefinequad(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = PoissonQ1(node,elem,pde);
%     figure(1);
%     showsolution(node,elem,u);
%     % Non-homogenous Dirichlet boundary condition
%     pde.f = inline('-4*ones(size(p,1),1)','p');
%     pde.g_D = inline('p(:,1).^2 + p(:,2).^2','p');
%     u = PoissonQ1(node,elem,pde);
%     figure(2);
%     showsolution(node,elem,u);
%     % Homogenous Neumann boundary condition
%     clear pde
%     pde.f = inline('pi^2*cos(pi*p(:,1)).*cos(pi*p(:,2))','p');
%     u = PoissonQ1(node,elem,pde);
%     figure(3);
%     showsolution(node,elem,u);
%
% See also: Poisson3Q1
%
% Author: Huayi Wei < huayiwei1984@gmail.com>.
%
% Modified by Long Chen. Change the assembling process. Add mg solver and
% update boundary condition.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end
[N,Dim] = size(node);
[NT,NV] = size(elem);
Ndof = N;

tic;
%% Assemble stiffness matrix
% generate sparse pattern
ii = zeros(10*NT,1); jj = zeros(10*NT,1);
index = 0;
for i = 1:4
    for j = i:4
        ii(index+1:index+NT) = double(elem(:,i));
        jj(index+1:index+NT) = double(elem(:,j));
        index = index + NT;
    end
end
% quadrature points
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder')
    option.dquadorder = 2;        % default order is exact for quadratic function
end
[pts, w] = quadptsquad(option.dquadorder);
nQuad = size(pts,1);
% compute non-zeros
sA = zeros(10*NT,nQuad);
for p = 1:nQuad
    % Dphi at quadrature points
   [phi, Dphip, J] = quadbasis(node,elem,pts(p,:));
    index = 0;
    for i = 1:4
        for j = i:4
            Aij = 0;
            if isempty(pde.d) || isnumeric(pde.d)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = zeros(NT, Dim);
                for ip = 1:Dim
                    xi = node(:,ip);
                    pxy(:,ip) = xi(elem)*phi;
                end
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
            if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
                Aij = pde.d.*Aij;
            end
            Aij = Aij.*J;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
clear Aij ii jj Dphip

%% Assemble the right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end

if ~isempty(pde.f)
    [pts, weight] = quadptsquad(option.fquadorder);
    nQuad = size(pts,1);
    bt = zeros(NT,NV);
    for p = 1:nQuad
		% quadrature points in the x-y coordinate
        [phi, tempvar, J] = quadbasis(node,elem, pts(p,:));
        pxy = zeros(NT, Dim);
        for i = 1:Dim
            xi = node(:,i);
            pxy(:,i) = xi(elem)*phi; % ? questionable
        end
		fp = pde.f(pxy);
        bt = bt + (weight(p)*fp.*J)*phi';
    end
    b = accumarray(elem(:),bt(:),[N 1]);
end
clear pxy bt

%% Set up boundary conditions
if ~exist('bdFlag','var'), bdFlag = []; end
[AD,b,u,freeNode,isPureNeumann] = getbd(b);

%% Record assembling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
%         option.x0 = u;
        option.solver = 'CG';
        [u,info] = mg(AD,b,elem,option);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option);
end
% post-process for pure Neumann problem
if isPureNeumann
    patchArea = accumarray(elem(:),[J;J;J;J]/4, [N 1]);
    uc = sum(u.*patchArea)/sum(J);
    u = u - uc;   % int u = 0
end

%% Output information
eqn = struct('A',AD,'b',b,'freeNode',freeNode);
info.assembleTime = assembleTime;

%% Compute Du
Du = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeNode,isPureNeumann] = getbd(b)
    %% Set up of boundary conditions.
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedNode) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedNode.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.
    %
    % Reference: Long Chen. Finite Element Methods and its Programming. Lecture
    % Notes.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    isRobin = (bdFlag(:) == 3);
    if any(isRobin)
        allEdge = [elem(:,[1,2]); elem(:,[2,3]); elem(:,[3,4]); elem(:,[4 1])];    % quad
        Robin = allEdge(isRobin,:);
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        ve = node(Robin(:,1),:) - node(Robin(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        mid = (node(Robin(:,1),:) + node(Robin(:,2),:))/2;
        % int g_R phi_iphi_j ds
        ii = [Robin(:,1),Robin(:,1),Robin(:,2),Robin(:,2)];
        jj = [Robin(:,1),Robin(:,2),Robin(:,1),Robin(:,2)];
        temp = pde.g_R(mid).*edgeLength;
        ss = [1/3*temp, 1/6*temp, 1/6*temp, 1/3*temp];
%         ss = [1/4*temp, 1/4*temp, 1/4*temp, 1/4*temp];
        A = A + sparse(ii,jj,ss,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedNode
    fixedNode = []; freeNode = [];
    if ~isempty(bdFlag) % find boundary edges and boundary nodes
        [fixedNode,bdEdge,isBdNode] = findboundary(elem,bdFlag);
        freeNode = find(~isBdNode);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given
        [fixedNode,bdEdge,isBdNode] = findboundary(elem);
        freeNode = find(~isBdNode);
    end
    isPureNeumann = false;
    if isempty(fixedNode) && isempty(Robin) % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedNode = 1;
        freeNode = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    if ~isempty(fixedNode)
        bdidx = zeros(Ndof,1);
        bdidx(fixedNode) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the right hand side b
    % Find boundary edges: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        Neumann = bdEdge;
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        [tempvar,Neumann] = findboundary(elem); %#ok<ASGLU>
    end

    % Neumann boundary condition
    if  isnumeric(pde.g_N) && all(pde.g_N == 0)
        pde.g_N = [];
    end
    if ~isempty(Neumann) && ~isempty(pde.g_N)
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        phigN = lambdagN;                 % linear bases
        nQuadgN = size(lambdagN,1);
        ge = zeros(size(Neumann,1),2);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            for igN = 1:2
                ge(:,igN) = ge(:,igN) + weightgN(pp)*phigN(pp,igN)*gNp;
            end
        end
        ge = ge.*repmat(el,1,2);
        b = b + accumarray(Neumann(:), ge(:),[Ndof,1]);
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if isnumeric(pde.g_D) && all(pde.g_D == 0)   % zero g_D
        pde.g_D = [];
    end
    if ~isPureNeumann && ~isempty(fixedNode) && ~isempty(pde.g_D)
        if isnumeric(pde.g_D)  % pde.g_D could be a numerical array
            u(fixedNode) = pde.g_D(fixedNode);
        else % pde.g_D is a function handle
            u(fixedNode) = pde.g_D(node(fixedNode,:));
        end
        b = b - A*u;
    end
    if ~isPureNeumann % non-empty Dirichlet boundary condition
        b(fixedNode) = u(fixedNode);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b);   % compatilbe condition: sum(b) = 0
    end
    end % end of getbd
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end % end of PoissonQ1


% Fig. 9.43   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

%script to plot the phase plane for a bang-bang example
figure(1)
hold off
clf
N=-1;
x = -100;
xdot=  sqrt(200);
sim('bang')
plot(xbang(:,2),xdotbang(:,2));
hold on
N=1;
x = 100;
xdot=-sqrt(200);
sim('bang');
plot(xbang(:,2),xdotbang(:,2));
xlabel('x_1');
ylabel('x_2');
hold on
title(' Switching curve for 1/s^2 plant')
grid on
%script to plot the phase plane for a bang-bang example
figure(1)
hold off
clf
N=-1;
x = -100;
xdot=  sqrt(200);
sim('bang')
plot(xbang(:,2),xdotbang(:,2));
hold on
N=1;
x = 100;
xdot=-sqrt(200);
sim('bang');
plot(xbang(:,2),xdotbang(:,2));
xlabel('x_1');
ylabel('x_2');
hold on
title('Switching curve for 1/s^2 plant');
grid on


function pass = test_real(pref)
% Test REAL

if ( nargin == 0 )
    pref = chebfunpref;
end
tol = 100*pref.cheb3Prefs.chebfun3eps;

f = chebfun3v(@(x,y,z) cos(x.*y.*z), @(x,y,z) cos(x.*y.*z));
g = real(f);
pass(1) = norm(g - f) < tol;

f = chebfun3v(@(x,y,z) cos(x.*y.*z), @(x,y,z) cos(x.*y.*z));
g = real(1i*f);
pass(2) = norm(g) < tol;

f1 = chebfun3v(@(x,y,z) cos(x.*y.*z), @(x,y,z) cos(x.*y.*z));
f2 = chebfun3v(@(x,y,z) sin(x + y.^2+z.^3), @(x,y,z) sin(x + y.^2+z.^3));
g = real(f1 + 1i*f2);
pass(3) = norm(g - f1) < tol;

end


% IndexToAssignment Convert index to variable assignment.
%
%   A = IndexToAssignment(I, D) converts an index, I, into the .val vector
%   into an assignment over variables with cardinality D. If I is a vector,
%   then the function produces a matrix of assignments, one assignment
%   per row.
%
%   See also AssignmentToIndex.m and SampleFactors.m
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function A = IndexToAssignment(I, D)

D = D(:)'; % ensure that D is a row vector
A = mod(floor(repmat(I(:) - 1, 1, length(D)) ./ repmat(cumprod([1, D(1:end - 1)]), length(I), 1)), ...
        repmat(D, length(I), 1)) + 1;

end


function [normalized_speed, actual_speed] = normalize_speed(speed, failures, skipping, tracker, sequence)
% normalize_speed Normalizes tracker speed estimate
%
% This function normalizes speed estimates based on performance profile and some information about
% the way the measurement was obtained (sequence, number of failures, frame skipping).
%
% Input:
% - speed (double): The initial speed estimate.
% - failures (double): Number of failures of the tracker.
% - skipping (integer): Number of skipped frames after each failure.
% - tracker (structure): A valid tracker descriptor.
% - sequence (structure): A valid sequence descriptor.
%
% Output:
% - normalized_speed (double): Normalized speed estimate.
% - actual_speed (double): Corrected raw speed based on supplied information,
%

if ~isfield(tracker, 'performance')
    error('Tracker %s has no performance profile, unable to normalize speed.', tracker.identifier);
end;

performance = tracker.performance;

factor = performance.nonlinear_native;
startup = 0;

if strcmpi(tracker.interpreter, 'matlab')
    if isfield(performance, 'matlab_startup')
        startup = performance.matlab_startup;
    else
        model = get_global_variable('matlab_startup_model', []);
		if ~isempty(model)
			startup = model(1) * performance.reading + model(2);
		end;
    end;
end

failure_count = cellfun(@(x) numel(x), failures, 'UniformOutput', true);

if tracker.trax
	actual_length = sequence.length - (skipping - 1) * failure_count;
	full_length = sequence.length;
	startup_time = startup * (1 + failure_count);
else
	full_length = cellfun(@(x) sum(sequence.length - x - (skipping - 1)), failures, 'UniformOutput', true) + sequence.length;
	actual_length = full_length;
	startup_time = startup * (1 + failure_count);
end;

actual_speed = (((speed .* full_length) - startup_time) ./ actual_length);
normalized_speed = actual_speed / factor;


function [k, sk, n2] = ratquadKernCompute(kern, x, x2)

% RATQUADKERNCOMPUTE Compute the RATQUAD kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the rational quadratic
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the rational quadratic
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : ratquadKernParamInit, kernCompute, kernCreate, ratquadKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2006, 2009

% KERN

wi2 = .5/(kern.lengthScale*kern.lengthScale*kern.alpha);
if nargin < 3
  n2 = dist2(x, x);
else
  n2 = dist2(x, x2);
end
sk = (1+n2*wi2).^-kern.alpha;
k = kern.variance*sk;


function SO3F = conj(SO3F)
% Construct the complex conjugate function $\overline{f}$ of an SO3Fun $f$.
%
% Syntax
%   SO3F = conj(F)
%
% Input
%  F - @SO3Fun
%
% Output
%  SO3F - @SO3FunHarmonic
%

SO3F = SO3FunHandle(@(rot) conj(SO3F.eval(rot)),SO3F.SRight,SO3F.SLeft);
% SO3F = SO3FunHarmonic(SO3F);
% SO3F = conj(SO3F);

end


function [H11, H12, H21, H22] = d2Abr_dV2(d2F_dV2, dF_dV1, dF_dV2, F, V, mu)
%D2ABR_DV2   Computes 2nd derivatives of |branch flow|^2 w.r.t. V.
%
%   The derivatives can be take with respect to polar or cartesian coordinates
%   of voltage, depending on the first 3 arguments. Flows could be complex
%   current or complex or real power. Notation below is based on complex power.
%
%   [H11, H12, H21, H22] = D2ABR_DV2(D2F_DV2, DF_DV1, DF_DV2, F, V, MU)
%
%   Returns 4 matrices containing the partial derivatives w.r.t. voltage
%   components (angle, magnitude or real, imaginary) of the product of a
%   vector MU with the 1st partial derivatives of the square of the magnitude
%   of branch flows.
%
%   Takes as inputs a handle to a function that evaluates the 2nd derivatives
%   of the flows (with args V and mu only), sparse first derivative matrices
%   of flow, flow vector, voltage vector V and nl x 1 vector of multipliers
%   MU. Output matrices are sparse.
%
%   Example:
%       f = branch(:, F_BUS);
%       Cf =  sparse(1:nl, f, ones(nl, 1), nl, nb);
%       [Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch);
%       [dSf_dV1, dSf_dV2, dSt_dV1, dSt_dV2, Sf, St] = ...
%               dSbr_dV(branch, Yf, Yt, V);
%       dF_dV1 = dSf_dV1;
%       dF_dV2 = dSf_dV2;
%       F = Sf;
%       d2F_dV2 = @(V, mu)d2Sbr_dV2(Cf, Yf, V, mu, 0);
%       [H11, H12, H21, H22] = ...
%             d2Abr_dV2(d2F_dV2, dF_dV1, dF_dV2, F, V, mu);
%
%   Here the output matrices correspond to:
%     H11 = d/dV1 (dAF_dV1.' * mu)
%     H12 = d/dV2 (dAF_dV1.' * mu)
%     H21 = d/dV1 (dAF_dV2.' * mu)
%     H22 = d/dV2 (dAF_dV2.' * mu)
%
%   See also DABR_DV, DIBR_DV, DSBR_DV.
%
%   For more details on the derivations behind the derivative code used
%   in MATPOWER information, see:
%
%   [TN2]  R. D. Zimmerman, "AC Power Flows, Generalized OPF Costs and
%          their Derivatives using Complex Matrix Notation", MATPOWER
%          Technical Note 2, February 2010. [Online]. Available:
%          https://matpower.org/docs/TN2-OPF-Derivatives.pdf
%          doi: 10.5281/zenodo.3237866

%   MATPOWER
%   Copyright (c) 2008-2019, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% define
nl = length(mu);

diagmu = sparse(1:nl, 1:nl, mu, nl, nl);

[F11, F12, F21, F22] = d2F_dV2(V, conj(F) .* mu);
H11 = 2 * real( F11 + dF_dV1.' * diagmu * conj(dF_dV1) );
H21 = 2 * real( F21 + dF_dV2.' * diagmu * conj(dF_dV1) );
H12 = 2 * real( F12 + dF_dV1.' * diagmu * conj(dF_dV2) );
H22 = 2 * real( F22 + dF_dV2.' * diagmu * conj(dF_dV2) );


function imgGlare = ComputeGlareImage( img, PSF, hot_pixels_pos, C)
%
%       imgGlare = ComputeGlareImage( img, PSF )
%
%       This function computes the glare image of an input HDR image given
%       a point spread function (PSF) as a kernel.
%
%        Input:
%           -img: an HDR image
%           -PSF: a point spread function stored as a kernel
%           -hot_pixels_pos: hot pixels' coordinates
%
%        Output:
%           -imgGlare: the estimated glare in img.
%
%     Copyright (C) 2014  Francesco Banterle
%
%     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/>.
%
%

m = size(hot_pixels_pos, 2);

imgGlare = zeros(size(img));

[r,c,col] = size(img);

[X, Y] = meshgrid(1:c, 1:r);

for i=1:m
    x_p = hot_pixels_pos(1, i);
    y_p = hot_pixels_pos(2, i);

    r = max(sqrt((X-x_p).^2 + (Y-y_p).^2), 2);
    value = C(1) + C(2)./r + C(3)./(r.^2) + C(4)./(r.^3);

    tmp_glare = zeros(size(img));

    for j=1:col
        tmp_glare(:,:,j) = value * img(y_p, x_p, j);
    end

    imgGlare = imgGlare + tmp_glare;

end

%
% hot_pixels_col = zeros(m, size(img, 3));
% for i=1:m
%     hot_pixels_col(i, :) = img(hot_pixels_pos(2, i), hot_pixels_pos(1, i), :);
% end
%
%
% PSF_col = zeros(size(PSF,1), size(PSF, 2), size(img, 3));
% for i=1:size(img, 3)
%     PSF_col(:,:,i) = PSF;
% end
%[imgGlare, ~] = imSplat(size(img, 1), size(img, 2), PSF_col, hot_pixels_pos, hot_pixels_col);

%compensation
while(1)
    ind = find(imgGlare > img);

    if(isempty(ind))
        break
    end

    scale = img(ind(1)) / imgGlare(ind(1));

    imgGlare = imgGlare * scale;
end

end


function [w,run] = train_is(x,y,w,lambda)
% Iterative scaling

% Written by Thomas P Minka

if any(x(:) < 0)
  error('Iterative scaling must have x > 0')
  %x = x - min(x(:));
end

if nargin < 4
  lambda = 0;
end
[d,n] = size(x);
flops(0);
step = 1/max(sum(x,1));
i1 = (y > 0)';
% sum of all data
xs = row_sum(x);
% sum of data in class 1
x1 = x*i1;
% sum of data in class 2
x2 = x*(1-i1);
% ratio
r12 = x1./x2;
flops(flops + n + flops_row_sum(x));
if nargout > 1
  run.w = [];
  run.flops = [];
  run.e = [];
end
for iter = 1:10000
  old_w = w;
  s1 = 1./(1+exp(-(w'*x)))';
  xs1 = x*s1;
  %delta1 = x1./xs1;
  %w = w + step*log(delta1);
  %delta2 = (xs-xs1)./x2;
  %w = w + step*log(delta2);
  delta = xs./xs1 - 1;
  r12 = (x1 - lambda*w)./(x2 - lambda*w);
  w = w + step*log(r12.*delta);
  if iter == 1
    fl = flops_mul(w',x)+n*(flops_exp+2) + ...
	flops_mul(x,s1)+2*d + ...
	d*(1+flops_exp+2);
  end
  flops(flops + fl);

  if nargout > 1 & rem(iter,100) == 1
    run.w(:,end+1) = w;
    run.flops(end+1) = flops;
    run.e(end+1) = logProb(scale_cols(x,y),w) -0.5*lambda*w'*w;
  end
  if rem(iter,1000) == 0
    fprintf('IS iter %d\n', iter)
  end

  if max(abs(w - old_w)) < 1e-5
    break
  end
end
figure(2)
plot(run.e)
if iter == 10000
  warning('not enough iters')
end


function [mcD mcN smcN] = depthCuesHelper(pc, pcf, rr, sigmaSpace, qzc, nori, sigmaDisparity)
%function [mcD mcN smcN] = depthCuesHelper(pc, pcf, rr, sigmaSpace, qzc, nori, sigmaDisparity)
% Output:
% 	mcD oriented depth gradient based on the distance between the planes at the point being considered
%	mcN is th eangle betweent he 2 planes
%	smcN is the sign of the N gradient
% Input:
%	pc is the point cloud,
%	pcf is the filled in point cloud,
% 	rr determines the offset at which we want to look up the normals to compute the angle between them
%	sigmaSpace is the sigma for the gaussian to estimate the normals
% 	qzc is used to convert the Z value at a pixel into disparity, and to estimate the error at a particular depth.
%	nori is the number of orientations
%	sigmaDisparity is the value sigma for disparity in plane fitting.

	X = pc(:,:,1); Y = pc(:,:,2); Z = pc(:,:,3);
	Xf = pcf(:,:,1); Yf = pcf(:,:,2); Zf = pcf(:,:,3);

	[h w] = size(Z);
	mcN = zeros([size(Z) nori]);
	mcD = zeros([size(Z) nori]);

	thetaQ = -([0:1:(nori-1)]./nori*pi - pi/2);

	N{1} = NaN([size(Z) 3]);
	N{2} = NaN([size(Z) 3]);
	D{1} = NaN([size(Z) 3]);
	D{2} = NaN([size(Z) 3]);
	cntr{1} = NaN([h w 3]);
	cntr{2} = NaN([h w 3]);
	xyz = cat(3, Xf, Yf, Zf);

	% qZ1 = qzc*ordfilt2(Zf,1,ones(2*rr+1)).^2;
	se = strel(ones(2*rr+1));
	qZ2 = qzc*((-imdilate(-Zf, se)).^2);
	% assert(isequal(qZ1(rr+1:end-rr, rr+1:end-rr), qZ2(rr+1:end-rr, rr+1:end-rr)), 'Oops something is wrong with dilation as a substitue for max filter.');
	qZ = qZ2;

	for k = 1:nori,

		% For computing the signal at different orientations
		[pos(:,:,1) pos(:,:,2)] = ndgrid(1:size(Z,1), 1:size(Z,2));
		[theta, r] = cart2pol(pos(:,:,1), pos(:,:,2));
		theta = theta-thetaQ(k);
		[pos(:,:,1) pos(:,:,2)] = pol2cart(theta, r);

		% Point offsets at which we want to compare the features at
		anchorI = [-(rr+1)/2, (rr+1)/2];
		anchorJ = [0 0];
		[theta, r] = cart2pol(anchorI, anchorJ);
		theta = theta+thetaQ(k);
		[anchorI, anchorJ] = pol2cart(theta, r);
		Rpad = ceil(max(abs([anchorI, anchorJ])))+1;

		pos(:,:,2) = pos(:,:,2)/2; %sqrt(2);

		one_Z = 1./Z;
		X_Z = X./Z;
		Y_Z = Y./Z;
		one = Z; one(~isnan(one)) = 1;
		X_ZZ = X./(Z.*Z);
		Y_ZZ = Y./(Z.*Z);

		AtARaw = cat(3, X_Z.^2, X_Z.*Y_Z, X_Z, Y_Z.^2, Y_Z, one);
		AtbRaw = cat(3, X_ZZ, Y_ZZ, one_Z);
		AtARaw(isnan(AtARaw)) = 0;
		AtbRaw(isnan(AtbRaw)) = 0;

		AtA = filterItChopOffIS(cat(3, AtARaw, AtbRaw), pos, Zf, qzc, sigmaDisparity, sigmaSpace);
		Atb = AtA(:, :, (size(AtARaw,3)+1):end);
		AtA = AtA(:, :, 1:size(AtARaw,3));

		[AtA_1 detAtA] = invertIt(AtA);

		Nboth = mutiplyIt(AtA_1, Atb);
		filteredOne = AtA(:,:,end);

		% to ignore places with very few points.
		badPts = (filteredOne./max(filteredOne(:))) < 0.25;
		Nboth(repmat(badPts, [1 1 3])) = NaN;

		bboth = Nboth(:,:,1);
		bboth(:) = -detAtA;
		bboth = bsxfun(@rdivide, bboth, sqrt(sum(Nboth.^2,3)));
		Nboth = bsxfun(@rdivide, Nboth, sqrt(sum(Nboth.^2,3)));
		%Dboth  = xyz - bsxfun(@times, (sum(Nboth.*xyz, 3) + b), Nboth);

		Nboth = padarray(Nboth, [Rpad Rpad 0], 'replicate', 'both');
		bboth = padarray(bboth, [Rpad Rpad 0], 'replicate', 'both');
		XYZ = padarray(xyz, [Rpad Rpad 0], 'replicate', 'both');

		stI = round(Rpad+1+anchorI(1));
		enI = stI+(h-1);
		stJ = round(Rpad+1+anchorJ(1));
		enJ = stJ+(w-1);
		N{1} = Nboth(stI:enI, stJ:enJ, :);
		b{1} = bboth(stI:enI, stJ:enJ, :);
		D{1} = xyz - bsxfun(@times, (sum(N{1}.*xyz, 3) + b{1}), N{1});
		cntr{1} = XYZ(stI:enI, stJ:enJ, :);

		stI = round(Rpad+1+anchorI(2));
		enI = stI+(h-1);
		stJ = round(Rpad+1+anchorJ(2));
		enJ = stJ+(w-1);
		N{2} = Nboth(stI:enI, stJ:enJ, :);
		b{2} = bboth(stI:enI, stJ:enJ, :);
		D{2} = xyz - bsxfun(@times, (sum(N{2}.*xyz, 3) + b{2}), N{2});
		cntr{2} = XYZ(stI:enI, stJ:enJ, :);

		mcN(:,:,k) = 1-abs(sum(N{1}.*N{2}, 3));
		mcD(:,:,k) = sqrt(sum((D{1} - D{2}).^2, 3));
		fprintf('.');

		% Making the normals consistent.
		for i = 1:2,
			N{i} = N{i}.*repmat(sign(N{i}(:,:,3)),[1 1 3]);
			sn = sign(sum(N{i}.*cat(3, Xf, Yf, Zf),3));
			sn(isnan(sn)) = 1;
			N{i} = repmat(sn,[1 1 3]).*N{i};
		end

		% Computing the orientation for the normal discontinuity
		smcN(:,:,k) = sign(sum((N{1}-N{2}).*(cntr{1}-cntr{2}),3));
		mcD(:,:,k) = mcD(:,:,k).*(mcD(:,:,k) > qZ*1.05);

		fprintf('%d ',k);
	end

	fprintf('\n');
	mcN(isnan(mcN)) = 0;
	mcD(isnan(mcD)) = 0;
end

function fFilt = filterItChopOffIS(f, pos, Zf, qzc, sigmaDisparity, r)
	sp(:,:,1:2) = pos;
	sp(:,:,1:2) = sp(:,:,1:2)/r;
	sp(:,:,4) = (1./(qzc*Zf))/sigmaDisparity;
	fFilt = jointBilateral(sp, f, 1, 1000);
end

function x = mutiplyIt(AtA_1, Atb)
	a = @(k) AtA_1(:,:,k);
	b = @(k) Atb(:,:,k);
	x1 = a(1).*b(1) + a(2).*b(2) + a(3).*b(3);
	x2 = a(2).*b(1) + a(4).*b(2) + a(5).*b(3);
	x3 = a(3).*b(1) + a(5).*b(2) + a(6).*b(3);
	x = cat(3, x1, x2, x3);
end

function [AtA_1 detAtA]= invertIt(AtA)
	a = @(k) AtA(:,:,k);

	A = a(4).*a(6) - a(5).*a(5);
	D = -(a(2).*a(6 )- a(3).*a(5));
	G = a(2).*a(5) - a(3).*a(4);
	E = a(1).*a(6) - a(3).*a(3);
	H = -(a(1).*a(5) - a(2).*a(3));
	K = a(1).*a(4) - a(2).*a(2);

	detAtA = a(1).*A + a(2).*D + a(3).*G;

	AtA_1 = cat(3, A, D, G, E, H, K);
	%AtA_1 = bsxfun(@rdivide, AtA_1, detAtA);
end


% dftfilt2() - discrete complex wavelet filters
%
% Usage:
%   >> wavelet = dftfilt2( freqs, cycles, srate, cyclefact)
%
% Inputs:
%   freqs    - frequency array
%   cycles   - cycles array. If one value is given, all wavelets have
%              the same number of cycles. If two values are given, the
%              two values are used for the number of cycles at the lowest
%              frequency and at the highest frequency, with linear
%              interpolation between these values for intermediate
%              frequencies
%   srate    - sampling rate (in Hz)
%
%   cycleinc - ['linear'|'log'] increase mode if [min max] cycles is
%              provided in 'cycle' parameter. {default: 'linear'}
%   type     - ['sinus'|'morlet'] wavelet type is a sinusoid with
%              cosine (real) and sine (imaginary) parts tapered by
%              a Hanning or Morlet function. 'morlet' is a typical Morlet
%              wavelet (with p=2*pi and sigma=0.7) best matching the
%              'sinus' Hanning taper) {default: 'morlet'}
% Output:
%   wavelet - cell array of wavelet filters
%
% Note: The length of the window is automatically computed from the
%       number of cycles and is always made odd.
%
% Authors: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 3/28/2003

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

function wavelet = dftfilt2( freqs, cycles, srate, cycleinc, type);

    if nargin < 3
        error('3 arguments required');
    end;
    if nargin < 5
        type = 'morlet';
    end;

    % compute number of cycles at each frequency
    % ------------------------------------------
    if length(cycles) == 1
        cycles = cycles*ones(size(freqs));
    elseif length(cycles) == 2
        if nargin == 4 & strcmpi(cycleinc, 'log') % cycleinc
            cycles = linspace(log(cycles(1)), log(cycles(2)), length(freqs));
            cycles = exp(cycles);
        else
            cycles = linspace(cycles(1), cycles(2), length(freqs));
        end;
    end;

    % compute wavelet
    for index = 1:length(freqs)

        % number of cycles depend on window size
        % number of cycles automatically reduced if smaller window
        % note: as the number of cycle changes, the frequency shifts a little
        %       this has to be fixed

        winlen = cycles(index)*srate/freqs(index);
        winlenint = floor(winlen);
        if mod(winlenint,2) == 1, winlenint = winlenint+1; end;
        winval = linspace(winlenint/2, -winlenint/2, winlenint+1);

        if strcmpi(type, 'sinus') % Hanning
            win = exp(2i*pi*freqs(index)*winval/srate);
            wavelet{index} = win .* hanning(length(winval))';

        else % Morlet
            t = freqs(index)*winval/srate;
            p = 2*pi;
            s = cycles(index)/5;
            wavelet{index} = exp(j*t*p)/sqrt(2*pi) .* ...
                (exp(-t.^2/(2*s^2))-sqrt(2)*exp(-t.^2/(s^2)-p^2*s^2/4));
        end;
    end;

    return;

    % testing
    % -------
    wav1 = dftfilt2(5, 5, 256); wav1 = wav1{1};
    abs1 = linspace(-floor(length(wav1)),floor(length(wav1)), length(wav1));
    figure; plot(abs1, real(wav1), 'b');

    wav2 = dftfilt2(5, 3, 256); wav2 = wav2{1};
    abs2 = linspace(-floor(length(wav2)),floor(length(wav2)), length(wav2));
    hold on; plot(abs2, real(wav2), 'r');

    wav3 = dftfilt2(5, 1.4895990, 256); wav3 = wav3{1};
    abs3 = linspace(-floor(length(wav3)),floor(length(wav3)), length(wav3));
    hold on; plot(abs3, real(wav3), 'g');

    wav4 = dftfilt2(5, 8.73, 256); wav4 = wav4{1};
    abs4 = linspace(-floor(length(wav4)),floor(length(wav4)), length(wav4));
    hold on; plot(abs4, real(wav4), 'm');

    % more testing
    % ------------
    freqs = exp(linspace(0,log(10),10));
    win = dftfilt2(freqs, [3 3], 256, 'linear', 'sinus');
    win = dftfilt2(freqs, [3 3], 256, 'linear', 'morlet');

    freqs = [12.0008   13.2675   14.5341   15.8007   17.0674   18.3340   19.6007   20.8673   22.1339   23.4006   24.6672   25.9339   27.2005 28.4671   29.7338   31.0004   32.2670   33.5337   34.8003   36.0670   37.3336   38.6002   39.8669   41.1335   42.4002   43.6668 44.9334   46.2001   47.4667 ...
             48.7334   50.0000];

    win = dftfilt2(freqs, [3 12], 256, 'linear'); size(win)

    winsize = 0;
    for index = 1:length(win)
        winsize = max(winsize,length(win{index}));
    end;
    allwav = zeros(winsize, length(win));
    for index = 1:length(win)
        wav1 = win{index};
        abs1 = linspace(-(length(wav1)-1)/2,(length(wav1)-1)/2, length(wav1));
        allwav(abs1+(winsize+1)/2,index) = wav1(:);
    end;
    figure; imagesc(imag(allwav));

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


function [alphahat epshat etahat] = fastsmo_int(n, y, mis, anymis, allmis, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, cdyn, Hmat, Zmat, Tmat, Rmat, Qmat, cmat, a1, P1, tol)

%% Kalman filter %%
[d Fns v invF K L L1 QRt] = kalman_int(5, n, y, mis, anymis, allmis, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, cdyn, Hmat, Zmat, Tmat, Rmat, Qmat, cmat, a1, P1, tol);

%% Initialization %%
D       = (P1 == Inf);
P1(D)   = 0;
P1_inf  = double(D);
if ~Hdyn, H = Hmat; end
if ~Zdyn, Z = Zmat; end
if ~Tdyn, T = Tmat; end
if ~Rdyn, R = Rmat; end
if ~cdyn, c = cmat; end

%% Disturbance smoothing backwards recursion %%
m   = size(a1, 1);
r   = zeros(m, 1);
r1  = zeros(m, 1);
epshat  = zeros(size(y, 1), n);
etahat  = zeros(size(QRt{1}, 1), n);
for t = n : -1 : 1
    if Tdyn, T = Tmat{t}; end
    etahat(:, t) = QRt{t}*r;
    if allmis(t)
        %% Disturbance smoothing when all observations are missing %%
        epshat(:, t) = 0;
        r = T'*r;
        if t <= d, r1 = T'*r1; end
    else
        if Hdyn, H = Hmat{t}; end
        if Zdyn, Z = Zmat{t}; end
        if anymis(t), Z(mis(:, t),:)=[]; H(mis(:, t),:)=[]; H(:,mis(:, t))=[]; end
        if t > d || ~Fns(t)
            %% Normal disturbance smoothing or when F_inf is zero %%
            epshat(~mis(:, t), t) = H*(invF{t}*v{t} - K{t}'*r);
            r = Z'*invF{t}*v{t} + L{t}'*r;
            if t <= d, r1 = T'*r1; end
        else
            %% Exact initial disturbance smoothing when F_inf is nonsingular %%
            epshat(~mis(:, t), t) = -H*K{t}'*r;
            r1 = Z'*invF{t}*v{t} + L{t}'*r1 + L1{t}'*r;
            r = L{t}'*r;
        end
        if anymis(t), if ~Zdyn, Z = Zmat; end, if ~Hdyn, H = Hmat; end, end
    end
end

%% Fast state smoothing %%
alphahat        = zeros(m, n);
alphahat(:, 1)  = a1 + P1*r + P1_inf*r1;
for t = 1 : n-1
    if Tdyn, T = Tmat{t}; end
    if Rdyn, R = Rmat{t}; end
    if cdyn, c = cmat{t}; end
    alphahat(:, t+1) = c + T*alphahat(:, t) + R*etahat(:, t);
end


%compute normalization of the spinelength by the mean spinelength size in the whole video
function [data,units]=compute_normlength(trx,n)
larvae= trx.exp2flies{n};
numlarvae = numel(larvae);

meanlength=zeros(1,numlarvae);
numelements=zeros(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    meanlength(i)=nanmean(trx(larva).spinelength(1:end));
    numelements(i)=numel(trx(larva).spinelength(1:end));
end
normvalue=sum(meanlength.*numelements./(sum(numelements)));
normlength=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    normlength{1,i}=trx(larva).spinelength(1:end)/normvalue;
end

units=parseunits([]);
data=normlength;


function y = sfb2D_A(lo, hi, sf, d)

% 2D Synthesis Filter Bank
% (along single dimension only)
%
% y = sfb2D_A(lo, hi, sf, d);
% sf - synthesis filters
% d  - dimension of filtering
% see afb2D_A

lpf = sf(:, 1);     % lowpass filter
hpf = sf(:, 2);     % highpass filter

if d == 2
   lo = lo';
   hi = hi';
end

N = 2*size(lo,1);
L = length(sf);
y = upfirdn(lo, lpf, 2, 1) + upfirdn(hi, hpf, 2, 1);
y(1:L-2, :) = y(1:L-2, :) + y(N+[1:L-2], :);
y = y(1:N, :);
y = cshift2D(y, 1-L/2);

if d == 2
   y = y';
end


%% This function will calculate the l2 norm error between the discovered system and actual test data
% Last Updated: 2019/04/22
% Coded By: K

function [Score]=Get_Score(dData_test,Data_test,u,Control,tspan,state0,Shuffle)
% Get the ODE simulation result
Noise=0;

% Using ODEs to get the simulation data
%[dData_Es,Data_Es]=Get_Sim_Data(@(t,z)Sindy_ODE_RHS(t,z,u),state0,u,tspan,Noise,Control,Shuffle);

% Get the score of the result
%[n,m]=size(Data_Es);
%Score=sum(norm(dData_test(1:n,:)-dData_Es))+sum(norm(Data_test(1:n,:)-Data_Es));

%% Using one step prediction to get the simulation data
dData_Es=Sindy_ODE_RHS(0,Data_test',u)';

% Get the score of the result
Score=sum(norm(dData_test-dData_Es));


function B = biharmonic(f)
%BIHARMONIC   Biharmonic operator of a SEPARABLEAPPROX.
%   B = BIHARMONIC(F) returns a SEPARABLEAPPROX representing the biharmonic
%   operator applied to F.
%
% See also SEPARABLEAPPROX/BIHARM.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% biharmonic(f) = f_xxxx + f_yyyy + 2*f_xxyy:
B = diff(f, 4, 2) + diff(f, 4, 1) + 2*diff(diff(f, 2, 1), 2, 2);

end


function low_rank_tensor_completion_embedded()
% Given partial observation of a low rank tensor (possibly including noise),
% attempts to complete it.
%
% function low_rank_tensor_completion_embedded()
%
% NOTE: Tensor Toolbox version 2.6 or higher is required for this factory:
% see https://www.tensortoolbox.org/ or https://gitlab.com/tensors/tensor_toolbox
%
% This example demonstrates how to use the geometry factory for the
% embedded submanifold of fixed-rank tensors in Tucker format:
% fixedranktensorembeddedfactory.
%
% This geometry is described in the article
% "A Riemannian trust-region method for low-rank tensor completion"
% Gennadij Heidel and Volker Schulz, doi:10.1002/nla.2175.
%
% This can be a starting point for many optimization problems of the form:
%
% minimize f(X) such that rank(X) = [r1 ... rd], size(X) = [n1 ... nd].
%
% Important: to keep this example short, the code for the cost function,
% gradient and Hessian do not properly exploit sparsity of the
% observations, which leads to significant slow-downs for large tensors.
% This example file should be considered a starting point for more
% sophisticated implementations.
%
% Input:  None. This example file generates random data with noise.
%
% Output: None.
%
% Please cite the Manopt and Matlab Tensor Toolbox papers as well as the
% research paper:
%     @Article{heidel2018riemannian,
%       Title   = {A {R}iemannian trust-region method for low-rank tensor completion},
%       Author  = {G. Heidel and V. Schulz},
%       Journal = {Numerical Linear Algebra with Applications},
%       Year    = {2018},
%       Volume  = {23},
%       Number  = {6},
%       Pages   = {e1275},
%       Doi     = {10.1002/nla.2175}
%     }
%
% See also: fixedranktensorembeddedfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Gennadij Heidel, January 24, 2019.
% Contributors:
% Change log:

    if ~exist('tenrand', 'file')
        fprintf('Tensor Toolbox version 2.6 or higher is required.\n');
        return;
    end

    % Random data generation with pseudo-random numbers from a
    % uniform distribution on [0, 1].
    tensor_dims = [60 40 20];
    core_dims = [8 6 5];
    total_entries = prod(tensor_dims);
    d = length(tensor_dims);

    % Standard deviation of normally distributed noise.
    % Set sigma to 0 to test the noise-free case.
    sigma = 0.1;

    % Generate a random tensor A of size n1-by-...-by-nd of rank (r1, ..., rd).
    U = cell(1, d);
    R = cell(1, d);
    for i = 1:d
        [U{i}, R{i}] = qr(randn(tensor_dims(i), core_dims(i)), 0);
    end

    Z.U = R;
    Z.G = tenrand(core_dims);
    Core = ttm(Z.G, Z.U);

    Y.U = U;
    Y.G = Core;
    A = ttm(Core, Y.U);

    % Add noise to low-rank tensor
    A = A + sigma*tensor(randn(tensor_dims));

    % Generate a random mask P for observed entries:
    % P(i, j, k) = 1 if the entry (i, j, k) of A is observed,
    %            = 0 otherwise.
    fraction = 0.1; % Fraction of observed entries.
    nr = round(fraction * total_entries);
    ind = randperm(total_entries);
    ind = ind(1 : nr);
    P = false(tensor_dims);
    P(ind) = true;
    % Hence, we observe the nonzero entries in PA:
    P = tensor(P);
    PA = P.*A;
    % Note that an efficient implementation would require evaluating A as a
    % sparse tensor only at the indices of P.

    % Pick the submanifold of tensors of size n1-by-...-by-nd of
    % multilinear rank (r1, ..., rd).
    problem.M = fixedranktensorembeddedfactory(tensor_dims, core_dims);

    % Define the problem cost function.
    % The store structure is used to reduce full tensor evaluations.
    % Again: proper handling of sparse tensors would dramatically reduce
    % the computation time for large tensors. This file only serves as a
    % simple starting point. See help for the Tensor Toolbox regarding
    % sparse tensors. Same comment for gradient and Hessian below.
    problem.cost = @cost;
    function [f, store] = cost(X, store)
        if ~isfield(store, 'PXmPA')
            Xfull = full(X.X);
            store.PXmPA = P.*Xfull - PA;
        end
        f = .5*norm(store.PXmPA)^2;
    end

    % Define the Euclidean gradient of the cost function, that is, the
    % gradient of f(X) seen as a function of X without rank restrictions.
    problem.egrad =  @egrad;
    function [g, store] = egrad(X, store)
        if ~isfield(store, 'PXmPA')
            Xfull = full(X.X);
            store.PXmPA = P.*Xfull - PA;
        end
        g = store.PXmPA;
    end

    % Define the Euclidean Hessian of the cost at X along a vector eta.
    problem.ehess = @ehess;
    function H = ehess(X, eta)
        ambient_H = problem.M.tangent2ambient(X, eta);
        H = P.*ambient_H;
    end

    % Options
    X0 = problem.M.rand();
    options.maxiter = 3000;
    options.maxinner = 100;
    options.maxtime = inf;
    options.storedepth = 3;
    % Target gradient norm
    options.tolgradnorm = 1e-8*problem.M.norm(X0, getGradient(problem, X0));

    % Minimize the cost function using Riemannian trust-regions
    Xtr = trustregions(problem, X0, options);

    % Display some quality metrics for the computed solution
    Xtrfull = full(Xtr.X);
    fprintf('||X-A||_F / ||A||_F = %g\n', norm(Xtrfull - A)/norm(A));
    fprintf('||PX-PA||_F / ||PA||_F = %g\n', norm(P.*Xtrfull - PA)/norm(PA));

end


% compute X*Y on GPU in batches to avoid memory errors
function out = gpuBlockXY(X, Y)

% this code is for a very tall X, and small Y

g = gpuDevice;
bytesGPU = g.AvailableMemory;
infoX = whos('X');
infoY = whos('Y');
[hX, wX] = size(X);
[hY, wY] = size(Y); % hY should be equal to wX
% the dimensions of out will be hX-by-wY
bytesPerRowX = infoX.bytes/hX;
bytesPerRowY = infoY.bytes/hY;
% theoretically, to keep the tmp result, X, and Y in GPU memory
% bytesRequired = bytesPerRowY*batchSize + bytesPerRowX*batchSize + infoY.bytes;
batchSize = floor((bytesGPU-infoY.bytes)/(bytesPerRowX+bytesPerRowY));
batchSize = floor(batchSize/8); % just to be on the safe side
% also, interestingly with smaller batches the code runs slightly faster
nBatches = ceil(hX/batchSize);

startIdx = 1:batchSize:hX;
endIdx = startIdx + batchSize-1;
endIdx = min(endIdx, hX);

out = zeros([hX, wY], 'like', X);
gpuY = gpuArray(Y);
for iBatch = 1:nBatches
%     fprintf('Batch %d/%d\n', iBatch, nBatches);
    batchX = gpuArray(X(startIdx(iBatch):endIdx(iBatch), :));
    tmp = batchX*gpuY;

% MK - this is an annoying line of code, but it helps Matlab to do memory
% management on the GPU properly. Otherwise I get OUT OF MEMORY errors
% maybe it is just my GPU is a bit unstable
    wait(g);
    out(startIdx(iBatch):endIdx(iBatch), :) = gather(tmp);
end

return


function lse = mylogsumexp(b)
% does logsumexp across columns
B = max(b,[],2);
if issparse(b)
    lse = log(sum(exp(b-repmat(B,[1 size(b,2)])),2))+B;
else
    lse = log(sum(exp(b-repmatC(B,[1 size(b,2)])),2))+B;
end
end


function truncated_normal_b_pdf_values_test ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_PDF_VALUES_TEST tests TRUNCATED_NORMAL_B_PDF_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_B_PDF_VALUES_TEST:\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_B_PDF_VALUES stores values of\n' );
  fprintf ( 1, '  the PDF of the Normal Distribution truncated to (-oo,B].\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '            MU         SIGMA             B             X               FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, b, x, fx ] = ...
      truncated_normal_b_pdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %12f  %12f  %24.16f\n', ...
      mu, sigma, b, x, fx );

  end

  return
end


%NU_SVR Support Vector Classifier: NU algorithm
%
% 	[W,J,C] = NU_SVR(A,TYPE,PAR,C,SVR_TYPE,NU_EPS,MC,PD)
%
% INPUT
%   A	    Dataset
%   TYPE  Type of the kernel (optional; default: 'p')
%   PAR   Kernel parameter (optional; default: 1)
%   C     Regularization parameter (0 < C < 1): expected fraction of SV
%         (optional; default: 0.25)
%   SVR_TYPE  This type can be 'nu' or 'epsilon'
%   NU_EPS The corresponding value for NU or epsilon
%   MC    Do or do not data mean-centering (optional; default: 1 (to do))
%   PD    Do or do not the check of the positive definiteness (optional;
%         default: 1 (to do))
%
% OUTPUT
%   W     Mapping: Support Vector Classifier
%   J     Object identifiers of support objects
%   C     Equivalent C regularization parameter of SVM-C algorithm
%
% DESCRIPTION
% Optimizes a support vector classifier for the dataset A by
% quadratic programming. The classifier can be of one of the types
% as defined by PROXM. Default is linear (TYPE = 'p', PAR = 1). In J
% the identifiers of the support objects in A are returned.
%
% C belogs to the interval (0,1). C close to 1 allows for more class
% overlap.  Default C = 0.25.
%
% C is bounded from above by NU_MAX = (1 - ABS(Lp-Lm)/(Lp+Lm)), where
% Lp (Lm) is the number of positive (negative) samples. If NU > NU_MAX
% is supplied to the routine it will be changed to the NU_MAX.
%
% If C is less than some NU_MIN which depends on the overlap between
% classes algorithm will typically take long time to converge (if at
% all).  So, it is advisable to set NU larger than expected overlap.
%
% Output is rescaled in a such manner as if it were returned by SVC with
% the parameter C.
%
%
% SEE ALSO (PRTools Guide)
% NU_SVRO, SVO, SVC, MAPPINGS, DATASETS, PROXM

% Copyright: S.Verzakov, s.verzakov@ewi.tudelft.nl
% Based on SVC.M by D.M.J. Tax, D. de Ridder, R.P.W. Duin
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: nu_svr.m,v 1.2 2009/01/31 18:43:11 duin Exp $

function [W, J, epsilon_or_nu] = nu_svcr(a,type,par,C,svr_type,nu_or_epsilon,mc,pd)
if nargin < 2 | ~isa(type,'prmapping')
	if nargin < 8
		pd = 1;
	end
	if nargin < 7
		mc = 1;
	end
	if nargin < 6
		nu_or_epsilon = [];
	end
	if nargin < 5 | isempty(svr_type)
		svr_type = 'epsilon';
	end

	switch svr_type
	case 'nu'
		if isempty(nu_or_epsilon)
			prwarning(3,'nu is not specified, assuming 0.25.');
			nu_or_epsilon = 0.25;
		end
	%nu = nu_or_epsilon;
	case {'eps', 'epsilon'}
		svr_type = 'epsilon';
		if isempty(nu_or_epsilon)
			prwarning(3,'epsilon is not specified, assuming 1e-2.');
			nu_or_epsilon = 1e-2;
		end
	%epsilon = nu_or_epsilon;
	end

	if nargin < 4 | isempty(C)
		prwarning(3,'C set to 1\n');
		C = 1;
	end

	if nargin < 3 | isempty(par)
		par = 1;
		prwarning(3,'Kernel parameter par set to 1\n');
	end
	if nargin < 2 | isempty(type)
		type = 'p';
		prwarning(3,'Polynomial kernel type is used\n');
	end
	if nargin < 1 | isempty(a)
		W = prmapping(mfilename,{type,par,C,svr_type,nu_or_epsilon,mc,pd});
		W = setname(W,['Support Vector Regression (' svr_type ' algorithm)']);
		return;
	end

	islabtype(a,'targets');
	[m,k] = getsize(a);
	y = gettargets(a);
	% The 1-dim SVR
	if size(y,2) == 1   % 1-dim regression
		uy = mean(y);
		y  = y - uy;
		if mc
			u  = mean(a);
			a  = a - ones(m,1)*u;
		else
			u  = [];
		end

		K = a*proxm(a,type,par);
		% Perform the optimization:
		[v,J,epsilon_or_nu] = nu_svro(+K,y,C,svr_type,nu_or_epsilon,pd);
		% Store the results:
		v(end) = v(end)+uy;
		W = prmapping(mfilename,'trained',{u,a(J,:),v,type,par},getlablist(a),k,1);
		W = setname(W,['Support Vector Regression (' svr_type ' algorithm)']);
		%W = setcost(W,a);
		J = getident(a,J);
		%J = a.ident(J);

	else
		error('multivariate SVR is not supported');
	end

else % execution
	w = +type;
	m = size(a,1);

	% The first parameter w{1} stores the mean of the dataset. When it
	% is supplied, remove it from the dataset to improve the numerical
	% precision. Then compute the kernel matrix using proxm.

	if isempty(w{1})
		d = a*proxm(w{2},w{4},w{5});
	else
		d = (a-ones(m,1)*w{1})*proxm(w{2},w{4},w{5});
	end

	% When Data is mapped by the kernel, now we just have a linear
	% regression  w*x+b:
	d = [d ones(m,1)] * w{3};
	W = setdat(a,d,type);
end

return;


function judge = NearZero(near)
% *** BASIC HELPER FUNCTIONS ***
% Takes a scalar.
% Checks if the scalar is small enough to be neglected.
% Example Input:
%
% clear; clc;
% near = -1e-7;
% judge = NearZero(near)
%
% Output:
% judge =
%     1

judge = norm(near) < 1e-6;
end


function [R, scale]=ARFIT_arqr(v, p, mcor)
%ARQR	QR factorization for least squares estimation of AR model.
%
%  [R, SCALE]=ARQR(v,p,mcor) computes the QR factorization needed in
%  the least squares estimation of parameters of an AR(p) model. If
%  the input flag mcor equals one, a vector of intercept terms is
%  being fitted. If mcor equals zero, the process v is assumed to have
%  mean zero. The output argument R is the upper triangular matrix
%  appearing in the QR factorization of the AR model, and SCALE is a
%  vector of scaling factors used to regularize the QR factorization.
%
%  ARQR is called by ARFIT.
%
%  See also ARFIT.

%  Modified 29-Dec-99
%           24-Oct-10 Tim Mullen (added support for multiple realizatons)
%
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

  % n:   number of time steps (per realization)
  % m:   number of variables (dimension of state vectors)
  % ntr: number of realizations (trials)
  [n,m,ntr] = size(v);

  ne    = ntr*(n-p);            % number of block equations of size m
  np    = m*p+mcor;             % number of parameter vectors of size m

  % If the intercept vector w is to be fitted, least squares (LS)
  % estimation proceeds by solving the normal equations for the linear
  % regression model
  %
  %                  v(k,:)' = Aaug*u(k,:)' + noise(C)        (1)
  %
  % with Aaug=[w A] and `predictors'
  %
  %              u(k,:) = [1 v(k-1,:) ...  v(k-p,:)].         (2a)
  %
  % If the process mean is taken to be zero, the augmented coefficient
  % matrix is Aaug=A, and the regression model
  %
  %                u(k,:) = [v(k-1,:) ...  v(k-p,:)]          (2b)
  %
  % is fitted.
  % The number np is the dimension of the `predictors' u(k).
  %
  % If multiple realizations are given (ntr > 1), they are appended
  % as additional ntr-1 blocks of rows in the normal equations (1), and
  % the 'predictors' (2) correspondingly acquire additional row blocks.

  % Initialize the data matrix K (of which a QR factorization will be computed)
  K = zeros(ne,np+m);                 % initialize K
  if (mcor == 1)
    % first column of K consists of ones for estimation of intercept vector w
    K(:,1) = ones(ne,1);
  end

  % Assemble `predictors' u in K
  for itr=1:ntr
    for j=1:p
      K((n-p)*(itr-1) + 1 : (n-p)*itr, mcor+m*(j-1)+1 : mcor+m*j) = ...
          squeeze(v(p-j+1:n-j, :, itr));
    end
    % Add `observations' v (left hand side of regression model) to K
    K((n-p)*(itr-1) + 1 : (n-p)*itr, np+1 : np+m) = squeeze(v(p+1:n, :, itr));
  end

  % Compute regularized QR factorization of K: The regularization
  % parameter delta is chosen according to Higham's (1996) Theorem
  % 10.7 on the stability of a Cholesky factorization. Replace the
  % regularization parameter delta below by a parameter that depends
  % on the observational error if the observational error dominates
  % the rounding error (cf. Neumaier, A. and T. Schneider, 2001:
  % "Estimation of parameters and eigenmodes of multivariate
  % autoregressive models", ACM Trans. Math. Softw., 27, 27--57.).
  q     = np + m;             % number of columns of K
  delta = (q^2 + q + 1)*eps;  % Higham's choice for a Cholesky factorization
  scale = sqrt(delta)*sqrt(sum(K.^2));
  R     = triu(qr([K; diag(scale)]));


function [mask]= mrAnatClassifyCleanMask(mask,minClusterSize)
% mrAnatClassifyCleanMask - remove small clusters
% [mask]= mrAnatClassifyCleanMask(mask,minClusterSize);

% 16-Jun-2005 SOD wrote it

if nargin < 1, help(mfilename); return; end;

if ieNotDefined('minClusterSize'), minClusterSize = 9; end;

% keep input
orgmask = mask;

% clean mask forground (1)
[LabMask remove]=myClipCluster(orgmask,minClusterSize);
fprintf('[%s]:Removing clusters (1):',mfilename);
for n=1:length(remove),
    mask(LabMask==remove(n))=0;
    fprintf('.');drawnow;
end
fprintf('Done.\n');drawnow;

% clean mask background (0)
[LabMask remove]=myClipCluster(-1.*orgmask+1,minClusterSize);
fprintf('[%s]:Removing clusters (0):',mfilename);
for n=1:length(remove),
    mask(LabMask==remove(n))=1;
    fprintf('.');drawnow;
end
fprintf('Done.\n');drawnow;

%-------------------------
function [imgLabel, remove]=myClipCluster(m,t)

% connectivity
%        6     three-dimensional six-connected neighborhood
%        18    three-dimensional 18-connected neighborhood
%        26    three-dimensional 26-connected neighborhood
conn = 6;

% get clusters
[imgLabel,numObjects] = bwlabeln(m, 6);

% get sizes
[imgHist,labelNum] = hist(imgLabel(:),0:numObjects);

% clean mask
remove = labelNum(find(imgHist(2:end)<t))+1;

return;


%computes the novelty measure used by Hainsworth
%> called by ::ComputeNoveltyFunction
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data (unused)
%>
%> @retval d_hai novelty measure
% ======================================================================
function [d_hai] = NoveltyHainsworth (X, f_s)

    epsilon     = 1e-5;

    % difference spectrum
    X           = [X(:,1), X];
    X(X<=0)     = epsilon;

    % flux
    d_hai       = sum(log2(X(:,2:end)./X(:,1:end-1)))/size(X,1);
end


function [y] = mci_approach_gen (P,M,U)
% Approach to limit model
% FORMAT [y] = mci_approach_gen (P,M,U)
%
% P         parameters
% M,U       as usual
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_approach_gen.m 6548 2015-09-11 12:39:47Z will $

V=exp(P(1));
tau=exp(P(2));
t=U.X;

y=-60+V*(1-exp(-t/tau));


   function y = tang(x,y)
    % to find the exact angle in degrees
           y1 = atan2(y,x);
             if y1 >= 0
               y = y1*180.0/pi;
                 elseif  y1 < 0
                    y = y1*180.0/pi + 360.0;
                end;


function monomial_test08 ( )

%*****************************************************************************80
%
%% MONOMIAL_TEST08 tests MONO_UNRANK_GRLEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 December 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONOMIAL_TEST08\n' );
  fprintf ( 1, ...
    '  MONO_UNRANK_GRLEX is given a rank, and returns the corresponding\n' );
  fprintf ( 1, '  monomial in the sequence of all monomials in M dimensions\n' );
  fprintf ( 1, '  in grlex order.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For reference, print a monomial sequence with ranks.\n' );

  n = 4;
  m = 3;
  rank_max = mono_upto_enum ( m, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Let M = %d\n', m );
  fprintf ( 1, '      N = %d\n', n );
  fprintf ( 1, '\n' );

  x = [ 0, 0, 0 ];
  i = 1;

  while ( 1 )

    fprintf ( 1, '  %3d:', i );
    for j = 1 : m
      fprintf ( 1, '  %1d', x(j) );
    end
    fprintf ( 1, '\n' );

    if ( x(1) == n )
      break
    end

    x = mono_upto_next_grlex ( m, n, x );
    i = i + 1;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now choose random ranks between 1 and %d:\n', rank_max )
  fprintf ( 1, '\n' );

  seed = 123456789;
  test_num = 5;

  for test = 1 : test_num

    [ rank, seed ] = i4_uniform_ab ( 1, rank_max, seed );
    x = mono_unrank_grlex ( m, rank );

    fprintf ( 1, '  %3d:', rank );
    for j = 1 : m
      fprintf ( 1, '  %1d', x(j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [ok,msg] = checkOptiSol(optObj,tol)
%CHECKOPTISOL  Check the solution to an optimization for errors
%
%   Called by OPTI / checkSol

%   Copyright (C) 2011 Jonathan Currie (I2C2)

ok = 1;
msg = sprintf('Solver Status:');

if(nargin < 2), tol = 1e-6; end

lintol = tol;
quadtol = tol;
nllintol = tol;
sdtol = tol;
inttol = 1e-5;

%Check Exit Flag
switch(optObj.ef)
    case 1
        msg = sprintf('%s Solved\n',msg);
    case 0
        msg = sprintf('%s Iterations / Time / Nodes Exceeded\n',msg);
        ok = 0;
    case -1
        msg = sprintf('%s Infeasible\n',msg);
        ok = 0;
    otherwise
        msg = sprintf('%s Solver Error\n',msg);
        ok = 0;
end

%Get Problem Properties
prob = optObj.prob;
x = optObj.sol; [r,c] = size(x);
if(c > r), x = x'; end

%Convert SeDuMi problem to OPTI if specified
if(~isempty(prob.sdcone) && isstruct(prob.sdcone))
    prob = sedumi2opti(prob);
end

%Check Linear Constraints
if(~isempty(prob.rl))
    %Check Row Bounds
    if(~isempty(prob.A))
        cval = prob.A*x;
        [msg,ok] = checkRowCon(cval,prob.rl,prob.ru,'Linear',lintol,msg,ok);
    end
else
    %Check Inequality
    if(~isempty(prob.A))
        err = prob.A*x - prob.b;
        chk = err > lintol;
        if(any(chk))
            ok = 0;
            msg = sprintf('%s\nLinear <= Inequality Constraint(s) Broken: [tol %g]\n',msg,lintol);
            msg = solErrMsg(msg,true(size(chk)),chk,err);
        end
    end
    %Check Equality
    if(~isempty(prob.Aeq))
        err = abs(prob.Aeq*x - prob.beq);
        chk = err > lintol;
        if(any(chk))
            ok = 0;
            msg = sprintf('%s\nLinear == Equality Constraint(s) Broken: [tol %g]\n',msg,lintol);
            msg = solErrMsg(msg,true(size(chk)),chk,err);
        end
    end
end
%Check Bounds
if(~isempty(prob.lb))
    err = abs(x - prob.lb);
    chk = x < (prob.lb-lintol);
    if(any(chk))
        ok = 0;
        msg = sprintf('%s\nDecision Variable Lower Bound(s) Broken: [tol %g]\n',msg,lintol);
        msg = solErrMsg(msg,true(size(chk)),chk,err);
    end
end
if(~isempty(prob.ub))
    err = abs(x - prob.ub);
    chk = x > (prob.ub+lintol);
    if(any(chk))
        ok = 0;
        msg = sprintf('%s\nDecision Variable Upper Bound(s) Broken: [tol %g]\n',msg,lintol);
        msg = solErrMsg(msg,true(size(chk)),chk,err);
    end
end

%Check Quadratic Constraints
if(~isempty(prob.Q))
    if(iscell(prob.Q)) %multiple
        %Evaluate each constraint
        cval = zeros(size(prob.qrl));
        for i = 1:length(prob.qrl)
           cval(i) = x'*prob.Q{i}*x + prob.l(:,i)'*x;
        end
        %Check all constraints
        [msg,ok] = checkRowCon(cval,prob.qrl,prob.qru,'Quadratic',quadtol,msg,ok);
    else %single
        cval = x'*prob.Q*x + prob.l'*x;
        [msg,ok] = checkRowCon(cval,prob.qrl,prob.qru,'Quadratic',quadtol,msg,ok);
    end
end

%Check Integer Constraints
if(any(prob.int.str == 'I'))
    idx = prob.int.str == 'I';
    err = abs(x - round(x));
    chk = err > inttol;
    if(any(chk(idx)))
        ok = 0;
        msg = sprintf('%s\nDecision Variable Integer Constraint(s) Broken: [tol %g]\n',msg,inttol);
        msg = solErrMsg(msg,idx,chk,err);
    end
end
%Check Binary Constraints
if(any(prob.int.str == 'B'))
    idx = prob.int.str == 'B';
    err = abs(max([x - round(x), x - 1, 0 - x],[],2));
    chk = err > inttol;
    if(any(chk(idx)))
        ok = 0;
        msg = sprintf('%s\nDecision Variable Binary Constraint(s) Broken: [tol %g]\n',msg,inttol);
        msg = solErrMsg(msg,idx,chk,err);
    end
end

%Check Semidefinite Constraints
if(~isempty(prob.sdcone))
    if(iscell(prob.sdcone))
        err = zeros(length(prob.sdcone),1);
        chk = zeros(length(prob.sdcone),1);
        for i = 1:length(prob.sdcone)
            cval = evalSDCone(prob.sdcone{i},x);
            err(i) = -min(eig(cval));
            chk(i) = err(i) > sdtol;
        end
    else
        cval = evalSDCone(prob.sdcone,x);
        err = -min(eig(cval));
        chk = err > sdtol;
    end
    if(any(chk))
        ok = 0;
        msg = sprintf('%s\nSemidefinite Constraint(s) Broken: [tol %g]\n',msg,sdtol);
        msg = solErrMsg(msg,true(size(chk)),chk,err);
    end
end

%Check Nonlinear Constraints
if(~isempty(prob.nlcon))
    %Convert general constraints to row (if not already)
    if(~isempty(prob.nlrhs)), prob = nmix2row(prob); end
    %Get cval
    cval = prob.nlcon(x);
    %Run check
    [msg,ok] = checkRowCon(cval,prob.cl,prob.cu,'Nonlinear',nllintol,msg,ok);
end

function [msg,ok] = checkRowCon(cval,rl,ru,str,tol,msg,ok)
%Indices
eq = rl == ru; neq = ~eq;
ile = isfinite(ru) & neq;
ige = isfinite(rl) & neq;
if(any(ile))
    err = (cval - ru);
    chk = err > tol;
    if(any(chk(ile)))
        ok = 0;
        msg = sprintf('%s\n%s <= Inequality Constraint(s) Broken: [tol %g]\n',msg,str,tol);
        msg = solErrMsg(msg,ile,chk,err);
    end
end
if(any(ige))
    err = (rl - cval);
    chk = err > tol;
    if(any(chk(ige)))
        ok = 0;
        msg = sprintf('%s\n%s >= Inequality Constraint(s) Broken: [tol %g]\n',msg,str,tol);
        msg = solErrMsg(msg,ige,chk,err);
    end
end
if(any(eq))
    err = abs(cval - rl);
    chk = err > tol;
    if(any(chk(eq)))
        ok = 0;
        msg = sprintf('%s\n%s == Equality Constraint(s) Broken: [tol %g]\n',msg,str,tol);
        msg = solErrMsg(msg,eq,chk,err);
    end
end

function msg = solErrMsg(msg,indx,chk,err)
for i = 1:length(chk)
    if(indx(i) == true && chk(i) == true)
        msg = sprintf('%s #%-5d Error: %-10g\n',msg,i,err(i));
    end
end

function cval = evalSDCone(cone,x)
dim = sqrt(size(cone,1));
cval = -cone(:,1);
for i = 1:length(x)
    cval = cval + x(i).*cone(:,i+1);
end
cval = reshape(cval,dim,dim);


function [wavespec] = bz_WaveSpec(lfp,varargin)
%[wavespec] = bz_WaveSpec(lfp) calculates the
%wavelet transform of a signal with nfreqs frequencies in the range frange
%[fmin fmax]. Spacing between frequencies can be 'lin' or 'log'.
%Time-frequency resolution is defined by ncyc, the number of cycles in each
%wavelet. Uses Morlet (Gabor) wavelet.
%
%
%INPUT
%    lfp            a buzcode structure with fields lfp.data,
%                                                   lfp.timestamps
%                                                   lfp.samplingRate
%                   -lfp can also be a [t x 1] timeseries signal. in which
%                   case you need to input 'samplingRate'
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%       'frange'    [low frequency, high frequency]     (default: [1 128])
%       'nfreqs'    number of frequencies               (default: 100
%       'roundfreqs' round freqs to unique integer vals (default: false
%                       *Note this may decrease number of freqs
%       'nfreqs'    number of frequencies               (default: 100
%       'ncyc'      number of cycles in your wavelet    (default: 5)
%       'fvector'   predefined vector of frequencies
%       'space'     'log' or 'lin'  spacing of f's      (default: 'log')
%       'samplingRate' (only if input is not a buzcode structure)
%       'intervals'  restrict your spectrogram to timestamps in specific
%                   intervals
%       'chanID'    if lfp structure has multiple channels, which one would
%                   you like to calcaulte the wavelet transform of?
%                   (note: requires field lfp.channels)
%       'showprogress' true/false (default:false)
%       'saveMat '   put the basePath to save an LFP file
%       'MatNameExtraText'   text(X) to add to name as in: 'basename.wavespec(text).lfp.mat'
%       'downsampleout' factor by which to downsample output (default: 1)
%    =========================================================================
%
%OUTPUT
%   wavespec            buzcode-style structure
%       .data           [t x nfreqs] your spectrogram
%       .timestamps     [t x 1] timestamps
%       .freqs          frequencies of each column
%       .samplingRate   (Hz)
%       .channels       Channel indices of channels filtered... taken from lfp input
%       .filterparms    a structure that holds the parameters used for
%                       filtering, for future reference
%

%TO DO:
%   -Matricise For loop?
%   -Don't need to FFT every freq...
%   -update t output for if LFP is a cell array
%
%
%Dependencies
%   WaveFilt
%   MorletWavelet
%   FConv
%
%
%Last Updated: 10/9/15
%DLevenstein
%Modified by Antonio FR, 7/18/18

%% Parse the inputs

%Parameters
parms = inputParser;
addParameter(parms,'frange',[1 128],@isnumeric);
addParameter(parms,'nfreqs',100,@isnumeric);
addParameter(parms,'ncyc',5,@isnumeric);
addParameter(parms,'space','log');
addParameter(parms,'samplingRate',[]);
addParameter(parms,'showprogress',false,@islogical);
addParameter(parms,'roundfreqs',false,@islogical);
addParameter(parms,'saveMatPath',[]);
addParameter(parms,'MatNameExtraText',[]);
addParameter(parms,'fvector',[]);
addParameter(parms,'intervals',[-Inf Inf])
addParameter(parms,'chanID',[])
addParameter(parms,'downsampleout',1)

parse(parms,varargin{:})
frange = parms.Results.frange;
nfreqs = parms.Results.nfreqs;
ncyc = parms.Results.ncyc;
space = parms.Results.space;
samplingRate = parms.Results.samplingRate;
showprogress = parms.Results.showprogress;
roundfreqs = parms.Results.roundfreqs;
saveMatPath = parms.Results.saveMatPath;
MatNameExtraText = parms.Results.MatNameExtraText;
fvector = parms.Results.fvector;
intervals = parms.Results.intervals;
chanID = parms.Results.chanID;
downsampleout = parms.Results.downsampleout;

%Channel restrict
if ~isempty(chanID)
    usechannel = ismember(lfp.channels,chanID);
    lfp.data = lfp.data(:,usechannel);
    lfp.channels = lfp.channels(usechannel);
end

%lfp input
if isstruct(lfp)
    samplingRate = lfp.samplingRate;
elseif isempty(lfp)
    wavespec = lfp;
    return
elseif isnumeric(lfp)
    data_temp = lfp;
    clear lfp
    lfp.data = data_temp;
    lfp.timestamps = [1:length(lfp.data)]'./samplingRate;
end

si = 1./samplingRate;

%Restrict to intervals, with overhang to remove edge effects at transitions
%(then remove later)
overhang = (ncyc)./frange(1);
overint = bsxfun(@(X,Y) X+Y,intervals,overhang.*[-1 1]);
keepIDX = InIntervals(lfp.timestamps,overint);
lfp.data = lfp.data(keepIDX,:);
lfp.timestamps = lfp.timestamps(keepIDX);

%%
if ~isa(lfp.data,'single') || ~isa(lfp.data,'double')
    lfp.data = single(lfp.data);
end

%Frequencies
if ~isempty(fvector)
    freqs = fvector;
else
    fmin = frange(1);
    fmax = frange(2);
    if strcmp(space,'log')
        assert(fmin~=0,'Log-spaced frequencies cannot have min of 0')
        freqs = logspace(log10(fmin),log10(fmax),nfreqs);
    elseif strcmp(space,'lin')
        freqs = linspace(fmin,fmax,nfreqs);
    else
        display('Frequency spacing must be "lin" or "log".')
    end
end

if roundfreqs
    freqs = unique(round(freqs));
end

%Filter with wavelets
nfreqs = size(freqs,2);
nchan = size(lfp.data,2);
ntime = ceil(size(lfp.data,1)./downsampleout);
wavespec.data = nan(ntime,nfreqs,nchan);
wavespec.timestamps = downsample(lfp.timestamps,downsampleout);
for cidx = 1:nchan
    for f_i = 1:nfreqs
        if showprogress
            bz_Counter(f_i,nfreqs,'Wavelet Frequency')
        end
        wavelet = MorletWavelet(freqs(f_i),ncyc,si);
         wavespec.data(:,f_i,cidx) = ...
             downsample(FConv(wavelet',lfp.data(:,cidx)),downsampleout);
    end
end

%% Output in buzcode format
%Remove the overhang from intervals
keepIDX = InIntervals(wavespec.timestamps,intervals);
wavespec.data = wavespec.data(keepIDX,:);
wavespec.timestamps = wavespec.timestamps(keepIDX);

wavespec.freqs = freqs;
wavespec.nfreqs = nfreqs;
wavespec.samplingRate = samplingRate./downsampleout;
if isstruct(lfp) && isfield(lfp,'channels')
    wavespec.channels = lfp.channels;
end
wavespec.filterparms.ncyc = ncyc;
wavespec.filterparms.nfreqs = nfreqs;
wavespec.filterparms.frange = frange;
wavespec.filterparms.space = space;

clear lfp

if saveMatPath
    baseName = bz_BasenameFromBasepath(saveMatPath);
    if ~isempty(MatNameExtraText)
        lfpfilename = fullfile(saveMatPath,[baseName,'.wavespec' MatNameExtraText '.lfp.mat']);
    else
        lfpfilename = fullfile(saveMatPath,[baseName,'.wavespec.lfp.mat']);
    end

    s = whos('wavespec');
    if s.bytes>=1073741824%if greater than 2GB
        disp('wavespec variable greater than 2GB, saving as v7.3 .mat file')
        save(lfpfilename,'wavespec','-v7.3')
    else
        save(lfpfilename,'wavespec')
    end
end

end


% -------------------------------------------------------------------------------------------------------------------------
function y = max_score_err(x, y_gt)
% x is [m1, m2, 1, b]
% numel(y_gt) is b
% The dimensions m1 and m2 are odd numbers.
%
%   Luca Bertinetto, Jack Valmadre, Joao F. Henriques, 2016
% -------------------------------------------------------------------------------------------------------------------------

[m1, m2, k, b] = size(x);
assert(mod(m1, 2) == 1);
assert(mod(m2, 2) == 1);
assert(k == 1);

assert(numel(y_gt) == b);

y_gt = reshape(y_gt, [1, 1, 1, b]);
pos = y_gt > 0;
neg = y_gt < 0;

x = gather(x);

% % Express error e as a linear function of scores x.
% h_center = center_mask(m1, m2, b);
% h_max = max_mask(x);
% h = bsxfun(@times, pos, h_center) - bsxfun(@times, neg, h_max);
% % e is positive if classified correctly, negative if incorrectly.
% e = sum(sum(h .* x, 1), 2);

x_center = center_score(x);
x_max = max_score(x);
e = zeros(b, 1);
e(pos) = x_center(pos);
e(neg) = -x_max(neg);

y = sum(e <= 0);

end

function v = center_score(x)
    [m1, m2, ~, b] = size(x);
    c1 = (m1+1) / 2;
    c2 = (m2+1) / 2;
    v = x(c1, c2, :, :);
end

function h = center_mask(m1, m2, b)
% This should satisfy x .* center_mask(m1, m2, b) == center_score(x).
    c1 = (m1+1) / 2;
    c2 = (m2+1) / 2;
    h = zeros(m1, m2, 1, b, 'single');
    h(c1, c2, :, :) = 1;
end

function v = max_score(x)
    [m1, m2, ~, b] = size(x);
    v = max(max(x, [], 1), [], 2);
end

function h = max_mask(x)
% This should satisfy x .* max_mask(x) == max_score(x).
    [m1, m2, ~, b] = size(x);
    x = reshape(x, [m1*m2, b]);
    [~, u] = max(x);
    assert(numel(u) == b);
    h = zeros(m1*m2, b, 'single');
    h(sub2ind(size(x), u, 1:8)) = 1;
    h = reshape(h, [m1, m2, 1, b]);
end


function [y, details] = newton(functionf, dfunctionf, yest, yold, ...
                               tolabs, tolrel, maxit, printConsole)
%newton Newton iteration with Armijo step control for the nonlinear function
% and Jacobi. Starting from an estimate yest the function computes with a newton
% iteration the solution matrix y of f(y)=0 up to
% tol = tolrel*||f(y)||+tolabs.
%
% Call:   [matrix scalar scalar] =
%                newton(matrix, matrix, scalar, scalar, scalar, structure)
% Input:  matrix yest, size(yest) = (M,N) - estimate of the solution
% Input:  matrix yold, size(yold) = size(y) - parameter of fideq and dfideq
%         (state at previous time level)
% Input:  scalar tolabs>0 - absolute tolerance
% Input:  scalar tolrel>0 - relative tolerance
% Input:  scalar maxit>=1 - maximal number of iterations
% Input:  printConsole - > 0: print iteration information on console output
% Output: matrix y, size(y) = (M,N) - solution
% Output: scalar res, residuum ||f(y)||
% Output: scalar iter, number of iterations
%
% Author: Raimund Wegener, Stefan Schießl
% Date: 12.10.2011

if (nargin < 8)
    printConsole = 1;
end

%%
% initialization
sigma = 1e-2; alpha = 0.5; lambda = 1;

y = yest; f = functionf(y,yold); psi = 0.5*f(:)'*f(:); res = sqrt(2*psi);
tol= tolrel * res + tolabs;

%%
%iteration
[M,N] = size(y);
iter = 0;
% Flag for the message text
armijoMessage = '';
while (res > tol) && (iter < maxit)
    [i,j,S] = dfunctionf(y);
    dfdy = sparse(i,j,S,M*N,M*N);
    % '\' only gives a warning for singular matrices, generate an error
    % for that.
    lastwarn('');
    % bandden:  backslash uses band solver if band density is > bandden.
    % if bandden = 1.0, never use band solver, if bandden = 0.0, always use
    % band solver.
    spparms('bandden',0);
    del = -dfdy\f(:);
    assert(~strcmp(lastwarn, 'Matrix is singular to working precision.'), ...
        'Step:NotSolvable', 'Matrix is singular to working precision.');
    assert(~strcmp(lastwarn, 'Matrix is close to singular or badly scaled.'), ...
        'Step:NotSolvable', 'Matrix is close to singular or badly scaled.');

    y_ = y + del;
    f_ = functionf(y_,yold);
    psi_ = 0.5*f_(:)'*f_(:);
    if (psi_ > (1-2*sigma)*psi)
        lambda = min(lambda/alpha,1);
        y_ = y + lambda*del; f_ = functionf(y_,yold); psi_ = 0.5*f_(:)'*f_(:);
        while psi_ > (1-2*sigma*lambda)*psi
            lambda = lambda*alpha;
            % step control with Armijo rule
            y_ = y + lambda*del; f_ = functionf(y_,yold); psi_ = 0.5*f_(:)'*f_(:);

        end
        armijoMessage = ['newton/armijo: iter=' num2str(iter) ',\t lambda=' num2str(lambda) '\n'];
        if (printConsole > 1)
            fprintf(armijoMessage)
        end
    end

    y = y_; f = f_; psi = psi_; res = sqrt(2*psi);
    iter = iter + 1;
end
message = ['newton: res=' num2str(res) ', iter=' num2str(iter) '\n' armijoMessage];
if (printConsole > 0)
    fprintf(message)
end

if (isnan(res))
   error('Newton:NoConvergence', 'No solution found.');
end

if (res > tol)
   error('Newton:AccuracyNotReached', ['newton/armijo: Results are inaccurate (Res: ' num2str(res) ').']);
end

details.res = res;
details.iter = iter;
details.message = message;

end


function  [W, U, mu, UtU] = update_params(mu, W, U, dWUtot, nspikes)

[Nchan, Nfilt, Nrank] = size(U);

dWUtotCPU = gather_try(dWUtot);
ntot = sum(nspikes,2);

for k = 1:Nfilt
    if ntot(k)>5

        [Uall, Sv, Vall] = svd(gather_try(dWUtotCPU(:,:,k)), 0);
        Sv = diag(Sv);
        sumSv2 = sum(Sv(1:Nrank).^2).^.5;
        for irank = 1:Nrank
            [~, imax] = max(abs(Uall(:,irank)), [], 1);
            W(:,k,irank) = - Uall(:,irank) * sign(Uall(imax,irank)) * Sv(irank)/sumSv2;
            U(:,k,irank) = - Vall(:,irank) * sign(Uall(imax,irank));
        end
        mmax = max(abs(U(:,k,1)));
        Usize = squeeze(abs(U(:,k,:)));
        Usize = Usize .* repmat(Sv(1:Nrank)'/Sv(1), Nchan, 1);
        ibad = max(Usize, [], 2) < .1 * mmax;

        U(ibad,k,:) = 0;
    end
end

% mu = zeros(Nfilt,1, 'single');
for k = 1:Nfilt
    if ntot(k)>5
        wu = squeeze(W(:,k,:)) * squeeze(U(:,k,:))';
        mu(k) = sum(sum(wu.*squeeze(dWUtotCPU(:,:,k))));
    end
end

for k = 1:Nfilt
    if ntot(k)>5
        wu = squeeze(W(:,k,:)) * squeeze(U(:,k,:))';
        newnorm = sum(wu(:).^2).^.5;
        W(:,k,:) = W(:,k,:)/newnorm;
    end
end

% compute adjacency matrix UtU
U(isnan(U)) = 0;
U0 = gpuArray(U);
utu = gpuArray.zeros(Nfilt, 'single');
for irank = 1:Nrank
    utu = utu + (U0(:,:,irank)' * U0(:,:,irank));
end

UtU = logical(utu);


function [ i, j ] = ij_next ( i, j, n )

%*****************************************************************************80
%
%% IJ_NEXT returns the next matrix index.
%
%  Discussion:
%
%    For N = 3, the sequence of indices returned is:
%
%      (1,1), (1,2), (1,3), (2,1), (2,2), (2,3), (3,1), (3,2), (3,3), (0,0).
%
%    Note that once the value (N,N) is returned, the next value returned
%    will be (0,0).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the current pair of indices.
%
%    Input, integer N, the maximum value for I and J.
%
%    Output, integer I, J, the next pair of indices.  If either index is
%    illegal on input, the output value of (I,J) will be (1,1).
%
  if ( n < 1 )
    i = 0;
    j = 0;
    return
  end

  if ( i < 1 | n < i | j < 1 | n < j )
    i = 1;
    j = 1;
    return
  end

  if ( j < n )
    j = j + 1;
  elseif ( i < n )
    i = i + 1;
    j = 1;
  else
    i = 0;
    j = 0;
  end

  return
end


function AxB = Cross(A,B)
%This is a custom implementation of the Cross product operator;
%A and B must be (2x1) symbolic matricies, and AxB is a symbolic variable.
%It assumes that A and B are planar (with components i and j) and that AxB
%is the value of the number out of plane (in the k direction)

AxB = A(1)*B(2) - A(2)*B(1);

end


function [ y ] = vl_nneuclideanloss( x,r,vis,p )
% Euclidean Loss Layer
if nargin<4
    %forward
    %delta = x - r ;
    %y = sum(delta(:).^2) ;

    %vis=ones(1,1,21,1,'single');

    delta = (x - r).^2 ;
    temp(1,1,:,:)=vis;
    temp(1,2,:,:)=vis;
    delta = delta.*temp;
    y = sum(delta(:));

else
    %backward
    %y = 2 * p * (x - r) ;

    %vis=ones(1,1,21,1,'single');
    dx = 2 * (x - r) ;
    dx(:,1,:,:)=dx(:,1,:,:).*vis;
    dx(:,2,:,:)=dx(:,2,:,:).*vis;
    y = p*dx;

end

end


% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
Q3 = -pi:0.1:pi;
for j=1:numcols(Q3);
    M = p560.inertia([0 0 Q3(j) 0 0 0]);
    M22(j) = M(2,2);
end
plot(Q3, M22)
xlabel('q_3 (rad)');
ylabel('M_{22}');


function x = hatmap(u)
%HATMAP Summary of this function goes here
%   return skew symmetric matrix from a vector
x = [0, -u(3), u(2);
    u(3), 0, -u(1);
    -u(2), u(1), 0];
end


function smaps =  msCreateMultipleSegmentations(pE, adjlist, nsp, nsegall)
% 1) Randomly select superpixel s1, then randomly selects different superpixel
% s2 within same segment (if one exists); remove s1,s2 from s
% 2) Then, for randomly ordered i:
%     if si is adjacent to s1, assign si to s1 with probability pE(si, s1)
%     if si is adjacent to s2, assign si to s2 with probability pE(si, s2)
%     remove si from s
% 3) Repeat (2) until s is empty
%
% Note: this differs from the published version of the algorithm in that
% duplicate segments are removed (number is replaced with 0 in smaps)

% randomly split each segment further into connected components
%adjmat2 = curradjmat;
nmaps = numel(nsegall);

smaps = zeros(nsp, nmaps);

adjmat = zeros(nsp, nsp);
for k = 1:size(adjlist, 1)
    s1 = adjlist(k, 1);
    s2 = adjlist(k, 2);
    adjmat(s1, s2) = k;
    adjmat(s2, s1) = k;
end

nadj = zeros(nsp, 1);
for k = 1:nsp
    adj{k} = find(adjmat(k, :));
    nadj(k) = numel(adj{k});
end
for k = 1:nsp
    normalization = 1;%2./(nadj(k)+nadj(adj{k}));
    pOffAll{k} = (1-pE(adjmat(k, adj{k}))).^normalization + 1E-10;
    pOnAll{k} = pE(adjmat(k, adj{k})).^normalization + 1E-10;
end

for m = 1:nmaps
    rind = randperm(nsp);
    smap = zeros(nsp, 1);
    nseg = nsegall(m);

    nseg = min(nseg, nsp);
    smap(rind(1:nseg)) = (1:nseg);

    if nseg==nsp % more segments than superpixels: return on sp per segment
        smaps(:, m) = smap;
        continue;
    end

    rind(1:nseg) = [];

    nleft = nsp;
    %tic
    while sum(nadj(rind))>0 % do until all possible sp are assigned
        for r = rind
            %p = -Inf*ones(nseg, 1);

            if all(smap(adj{r})>0)
                p = -Inf*ones(nseg, 1);
                for k = unique(smap(adj{r}))'
                    %if any(smap(adj{r})==k)
                        sOn = (smap(adj{r})==k);
                        sOff = ~sOn; %setdiff(find(smap(adj{r})>0), sOn);
                        pOn = prod(pOnAll{r}(sOn));
                        pOff = 1;
                        if ~isempty(sOff)
                            pOff = prod(pOffAll{r}(sOff));
                        end
                        p(k) = log(pOn)+log(pOff);
                    %end
                end
                [kval, kmax] = max(p);
                smap(r) = kmax;
            else
                for k = unique(smap(adj{r}))' %1:nseg
                    if k > 0
                        kadj = (smap(adj{r})==k);
                    %if sum(kadj)>0
                        pOn = prod(pOnAll{r}(kadj));
                        pOff = prod(pOffAll{r}(kadj));
                        if rand(1) < pOn / (pOn+pOff) + 0.05
                            smap(r) = k;
                        end
                    %end
                    end
                end
            end
        end

        rind(smap(rind)>0) = [];

        % break if it seems stuck (can happen when pE is very low,
        % nsegments is small)
        if numel(nleft)>50 && all(nleft(end-49:end)==numel(rind))
            %disp(['break init early: ' num2str(numel(rind)) ' left'])
            %disp(sort(rind))
            break;
        end
        nleft(end+1) = numel(rind);

    end

    count = zeros(nseg, 1);
    for k = 1:nseg
        count(k) = sum(smap==k);
    end
    %toc
    %disp(['0: ' num2str(evaluateEdgeProb(adjlist, pE, smap))])
    %tic
    for t = 1:10
        lastmap = smap;
        rind = randperm(nsp);
        for r = rind
            if smap(r) > 0 && count(smap(r)) > 1
                p = -Inf*ones(nseg, 1);

                for k = unique(smap(adj{r}))' %1:nseg
                    if k > 0
                    %if sum(smap(adj{r})==k)>0
                        sOn = smap(adj{r})==k;
                        sOff = ~sOn; %setdiff([1:nadj(r)], sOn);
                        pOn = sum(log(pOnAll{r}(sOn)));
                        pOff = sum(log(pOffAll{r}(sOff)));
                        p(k) = pOn + pOff;
                    end
                    %end
                end
                [kval, kmax] = max(p);
                if kmax~=smap(r)
                    count(smap(r)) = count(smap(r))-1;
                    smap(r) = kmax;
                end

            end
        end
        if all(lastmap==smap)
            break;
        end
    end
    %disp(['1: ' num2str(evaluateEdgeProb(adjlist, pE, smap))])
    %toc
    %disp(num2str(t))
    smaps(:, m) = smap;
end

smaps = msPruneSegments(smaps);

%%

function p = evaluateEdgeProb(adjlist, pE, smap)
hasedge = zeros(size(adjlist, 1), 1);
for k = 1:size(adjlist, 1)
    if smap(adjlist(k, 1))==smap(adjlist(k, 2))
        hasedge(k)=1;
    end
end
p = sum(log(pE(hasedge==1)));
p = p+sum(log(1-pE(hasedge==0)));


function GBPD = calcGBPD(gB,ebsd,varargin)
% compute the grain boundary plane distribution
%
% Syntax
%
%   GBPD = calcGBPD(gB,ebsd)
%
%   % use a specific halfwidth
%   GBPD = calcGBPD(gB,ebsd,'halfwidth',10*degree)
%
% Input
%  gB   - @grainBoundary
%  ebsd - @EBSD
%
% Output
%
%  GBPD - @S2FunHarmonic
%
% See also
%

%% step 1: extract data

% grain boundary directions
d = gB.direction;

% rotations that rotate the x vector towards the trace normals
omega = 90*degree + angle(d,vector3d.X,vector3d.Z);
rot = rotation.byAxisAngle(zvector,omega);

% the orientations that align the crystallographic x-axis with the trace
% normals
ori = [rot .* ebsd('id',gB.ebsdId(:,1)).orientations;...
  rot .* ebsd('id',gB.ebsdId(:,2)).orientations];

%% step 2: define kernel function

% define a kernel function that is a fibre through the crystallograhic
% z-axis and the crystallographic x-axis
psi = S2FunHarmonic(S2DeLaValleePoussinKernel('halfwidth',5*degree,varargin{:}));

psi = psi.radon;

bw = min(getMTEXpref('maxS2Bandwidth'),psi.bandwidth);
rot = rotation.byAxisAngle(xvector,90*degree);

% multiply this kernel function with the sin of the polar angle
fun = @(v) pi/2*psi.eval(rot*v) .* sin(angle(v,zvector));

% the final kernel function as S2Harmonic
psi = S2FunHarmonicSym.quadrature(fun, 'bandwidth', bw, ori.CS);

%% testing only

%n = 100000;
%cs = crystalSymmetry('3','x||b');

%d = vector3d.byPolar(90*degree,-30*degree)
%omega = 90*degree+angle(d,vector3d.X,vector3d.Z);
%ori = orientation.rand(n,cs);
%omega = rand(n,1);

%rot = rotation.byAxisAngle(zvector,omega);

%ori = rot .* orientation.id(cs);

%% step 3: compute orientation density

% compute the orientation density of the modified boundary orientations
odf = calcDensity(ori,'kernel',SO3DirichletKernel(bw),'harmonic');

%% step 4: convolution
GBPD = conv(odf,psi);

%plot(GBPD)

end


clear all;

f = @(p)[p(1) p(1)^2]';

% sampling intervals for each parameter
domain = [-2 2];
% grid size: number of grid points for each parameter
gridsize = 19;

% sampling
D = sampling_vec(f, domain, gridsize);

% hosvd
[S U] = hosvd(D, [1 0]);
U = U{1};

[W V] = genhull(U,'box');

Uhat = -ones(size(W));
Uhat(1,1) = 0;
Uhat(8,1) = 0.4;
Uhat(10,1) = 0.6;
Uhat(19,1) = 0;
Uhat(5,2) = 0.6;
Uhat(1,3) = 0;
Uhat(19,3) = 1;
Uhat(1,4) = 1;
Uhat(19,4) = 0;
M = hull_manip(W,Uhat);
Wmod = W*M;
Vmod = M\V;
[i,j] = find(Uhat > -1);
uhat = Uhat(Uhat > -1);
n = length(i);
%for k=1:n
%    [uhat(k) Wmod(i(k),j(k))] %#ok<NOPTS>
%end

close all
set(0, 'DefaultFigureWindowStyle', 'docked')
plothull(U);
plothull(W);
plothull(Wmod);
hold on
plot(i,uhat,'kx')
hold off

figure
hold on
plot(D(:,1), D(:,2))
plot(V*S(:,1), V*S(:,2), 'go')
plot(Vmod*S(:,1), Vmod*S(:,2), 'mo')
hold off