function [varargout]=discQuadMesh(varargin)

% function [F,V,C,indEdge]=discQuadMesh(nElements,r,f)
% ------------------------------------------------------------------------
% This function meshes a circle using quadrilaterial elements.
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 10/05/2016 Updated for GIBBON
%------------------------------------------------------------------------

%% Parse input
switch nargin
    case 1
        nElements=varargin{1};
        r=1;
        f=0.5;
    case 2
        nElements=varargin{1};
        r=varargin{2};
        f=0.5;
    case 3
        nElements=varargin{1};
        r=varargin{2};
        f=varargin{3};
end

%% Creating central regular quad mesh

nElements=nElements+~iseven(nElements);%Force even

[X_centralMesh,Y_centralMesh]=meshgrid(linspace(-1,1,nElements+1));
[F_centralMesh,V_centralMesh] = surf2patch(X_centralMesh,Y_centralMesh,zeros(size(X_centralMesh)));
V_centralMesh=V_centralMesh(:,1:2);

%Edge of central mesh
logicCentralMeshEdge=(X_centralMesh==1)|(Y_centralMesh==1)|(X_centralMesh==-1)|(Y_centralMesh==-1);
nEdge=(nElements*4);

% Scaling radius
[ThetaMesh,RadiusMesh]=cart2pol(V_centralMesh(:,1),V_centralMesh(:,2));
RadiusMesh=f*(1/2)*sqrt(2)*RadiusMesh;
[V_centralMesh(:,1),V_centralMesh(:,2)]=pol2cart(ThetaMesh,RadiusMesh);

%% Creating outer mesh

RadiusOuterEdge=ones(1,nEdge);
ThetaOuterEdge=linspace(0,pi*2,nEdge+1);
ThetaOuterEdge=ThetaOuterEdge(2:end)-pi;

[xOuterEdge,yOuterEdge]=pol2cart(ThetaOuterEdge,RadiusOuterEdge);
V_outerEdge=[xOuterEdge(:) yOuterEdge(:)];

V_innerEdge=V_centralMesh(logicCentralMeshEdge,:);
[ThetaEdge,RadiusEdge]=cart2pol(V_innerEdge(:,1),V_innerEdge(:,2));
[ThetaEdge,sortInd]=sort(ThetaEdge);
RadiusEdge=RadiusEdge(sortInd);
[V_innerEdge(:,1),V_innerEdge(:,2)]=pol2cart(ThetaEdge,RadiusEdge);

[Xr]=linspacen(V_innerEdge(:,1),V_outerEdge(:,1),nElements/2+1); Xr(end+1,:)=Xr(1,:);
[Yr]=linspacen(V_innerEdge(:,2),V_outerEdge(:,2),nElements/2+1); Yr(end+1,:)=Yr(1,:);

[Fs2,Vs2] = surf2patch(Xr,Yr,zeros(size(Xr)));
Vs2=Vs2(:,1:2);

V=[V_centralMesh;Vs2];
F=[F_centralMesh;Fs2+size(V_centralMesh,1)];
C=[ones(size(F_centralMesh,1),1); 2*ones(size(Fs2,1),1); ];

indEdge=((size(V,1)-size(Xr,1))+1):size(V,1);

%% Removing double points

[F,V,~,IND_IND]=mergeVertices(F,V);
indEdge=IND_IND(indEdge(1:end-1));

%Scaling radius
[ThetaMesh,RadiusMesh]=cart2pol(V(:,1),V(:,2));
RadiusMesh=r*RadiusMesh;
[V(:,1),V(:,2)]=pol2cart(ThetaMesh,RadiusMesh);

varargout{1}=F;
varargout{2}=V;
varargout{3}=C;
varargout{4}=indEdge;

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


function [ fea, out ] = ex_navierstokes1( varargin )
%EX_NAVIERSTOKES1 2D Example for incompressible stationary flow in a channel.
%
%   [ FEA, OUT ] = EX_NAVIERSTOKES1( VARARGIN ) Sets up and solves stationary Poiseuille
%   flow in a rectangular channel. The inflow profile is constant and the outflow
%   should assume a parabolic profile ( u(y)=U_max*4/h^2*y*(h-y) ).
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       rho         scalar {1}             Density
%       miu         scalar {0.001}         Molecular/dynamic viscosity
%       umax        scalar {0.3}           Maximum magnitude of inlet velocity
%       h           scalar {0.5}           Channel height
%       l           scalar {2.5}           Channel length
%       igrid       scalar 1/{0}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {0.04}          Max grid cell size
%       sf_u        string {sflag1}        Shape function for velocity
%       sf_p        string {sflag1}        Shape function for pressure
%       iphys       scalar 0/{1}           Use physics mode to define problem (=1)
%       solver      string openfoam/su2/{} Use OpenFOAM, SU2, FEniCS, or default solver
%       ischeme     scalar {0}             Time stepping scheme (0 = stationary)
%       iplot       scalar 0/{1}           Plot solution and error (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct
%
%   See also EX_NAVIERSTOKES1B

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'rho',      1;
  'miu',      1e-3;
  'umax',     0.3;
  'h',        0.5;
  'l',        2.5;
  'igrid',    1;
  'hmax',     0.04;
  'sf_u',     'sflag1';
  'sf_p',     'sflag1';
  'iphys',    1;
  'solver',   '';
  'ischeme',  0;
  'iplot',    1;
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Model parameters.
rho       = opt.rho;     % Density.
miu       = opt.miu;     % Molecular/dynamic viscosity.
umax      = opt.umax;    % Maximum magnitude of inlet velocity.
% Geometry and grid parameters.
h         = opt.h;       % Height of rectangular domain.
l         = opt.l;       % Length of rectangular domain.
% Discretization parameters.
sf_u      = opt.sf_u;    % FEM shape function type for velocity.
sf_p      = opt.sf_p;    % FEM shape function type for pressure.

% Geometry definition.
gobj = gobj_rectangle( 0, l, 0, h );
fea.geom.objects = { gobj };
fea.sdim = { 'x' 'y' };   % Coordinate names.

% Grid generation.
if ( opt.igrid==1 )
  fea.grid = gridgen(fea,'hmax',opt.hmax,'fid',fid);
else
  fea.grid = rectgrid(round(l/opt.hmax),round(h/opt.hmax),[0 l;0 h]);
  if( opt.igrid<0 )
    fea.grid = quad2tri( fea.grid );
  end
end
n_bdr = max(fea.grid.b(3,:));           % Number of boundaries.

% Boundary conditions.
dtol      = opt.hmax;
i_inflow  = findbdr( fea, ['x<',num2str(dtol)] );     % Inflow boundary number.
i_outflow = findbdr( fea, ['x>',num2str(l-dtol)] );   % Outflow boundary number.
s_inflow  = ['2/3*',num2str(umax)];                                            % Definition of inflow profile.
s_refsol  = ['4*',num2str(umax),'*(y*(',num2str(h),'-y))/',num2str(h),'^2'];   % Definition of velocity profile.

% Problem definition.
if ( opt.iphys==1 )

  fea = addphys(fea,@navierstokes);     % Add Navier-Stokes equations physics mode.
  fea.phys.ns.eqn.coef{1,end} = { rho };
  fea.phys.ns.eqn.coef{2,end} = { miu };
  fea.phys.ns.eqn.coef{5,end} = { s_inflow };
  if( any(strcmp(opt.solver,{'openfoam','su2'})) )
    fea.phys.ns.sfun = { 'sflag1', 'sflag1', 'sflag1' };
  else
    fea.phys.ns.sfun = { sf_u sf_u sf_p };           % Set shape functions.
  end
  fea.phys.ns.bdr.sel(i_inflow)  = 2;
  fea.phys.ns.bdr.sel(i_outflow) = 4;
  fea.phys.ns.bdr.coef{2,end}{1,i_inflow} = s_inflow;         % Set inflow profile.
  fea = parsephys(fea);                 % Check and parse physics modes.

else

  fea.dvar  = { 'u'  'v'  'p'  };       % Dependent variable name.
  fea.sfun  = { sf_u sf_u sf_p };       % Shape function.

  % Define equation system.
  cvelx = [num2str(rho),'*',fea.dvar{1}];   % Convection velocity in x-direction.
  cvely = [num2str(rho),'*',fea.dvar{2}];   % Convection velocity in y-direction.
  fea.eqn.a.form = { [2 3 2 3;2 3 1 1]       [2;3]                   [1;2];
                     [3;2]                   [2 3 2 3;2 3 1 1]       [1;3];
                     [2;1]                   [3;1]                   []   };
  fea.eqn.a.coef = { {2*miu miu cvelx cvely}  miu                    -1;
                      miu                    {miu 2*miu cvelx cvely} -1;
                      1                       1                      [] };
  fea.eqn.f.form = { 1 1 1 };
  fea.eqn.f.coef = { 0 0 0 };

  % Define boundary conditions.
  fea.bdr.d = cell(3,n_bdr);
 [fea.bdr.d{1:2,:}]         = deal( 0 );

  fea.bdr.d{1,i_inflow}     = s_inflow;

 [fea.bdr.d{:,i_outflow  }] = deal([]);
  % fea.bdr.d{end,i_outflow}  = 0;   % Set pressure to zero on outflow boundary.

  fea.bdr.n = cell(3,n_bdr);
end

% Parse and solve problem.
fea = parseprob(fea);             % Check and parse problem struct.
if( opt.iphys==1 && strcmp(opt.solver,'fenics') )
  fea = fenics( fea, 'fid', fid, 'ischeme', opt.ischeme, 'tmax', 10 );
elseif( opt.iphys==1 && strcmp(opt.solver,'openfoam') )
  if( opt.ischeme==0 )
    dt = 1.0;
    tstop = 1000;
    ddtScheme = 'steadyState';
  elseif( opt.ischeme==1 )
    dt = 0.1;
    tstop = 100;
    ddtScheme = 'backward';
  elseif( opt.ischeme>=2 )
    dt = 0.1;
    tstop = 100;
    ddtScheme = 'CrankNicolson 0.9';
  end
  logfid = fid; if( ~got.fid ), fid = []; end
  fea.sol.u = openfoam( fea, 'fid', fid, 'logfid', logfid, 'ddtScheme', ddtScheme, 'deltaT', dt, 'endTime', tstop );
  fid = logfid;
elseif( opt.iphys==1 && strcmp(opt.solver,'su2') )
  logfid = fid; if( ~got.fid ), fid = []; end
  fea.sol.u = su2( fea, 'fid', fid, 'logfid', logfid, 'ischeme', opt.ischeme, 'tstep', 0.5, 'tmax', 20+30*(opt.ischeme==1) );
  fid = logfid;
else
  if( opt.ischeme==0 )
    jac.form  = {[1;1] [1;1] [];[1;1] [1;1] []; [] [] []};
    jac.coef  = {[num2str(rho),'*ux'] [num2str(rho),'*uy'] []; [num2str(rho),'*vx'] [num2str(rho),'*vy'] []; [] [] []};
    fea.sol.u = solvestat( fea, 'fid', fid, 'nsolve', 2, 'jac', jac );   % Call to stationary solver.
  else
    fea.sol.u = solvetime( fea, 'fid', fid, 'ischeme', opt.ischeme, 'tmax', 10 );
  end
end
fea.sol.u = fea.sol.u(:,end);

% Postprocessing.
s_velm = 'sqrt(u^2+v^2)';
s_err  = ['abs(sqrt((',s_refsol,')^2)-(',s_velm,'))'];
s_len  = ['(x>',num2str(3/4*l),')'];
if ( opt.iplot>0 )
  figure
  subplot(3,1,1)
  postplot(fea,'surfexpr',s_velm,'evaltype','exact')
  title('Velocity field')
  subplot(3,1,2)
  postplot(fea,'surfexpr','p','evaltype','exact')
  title('Pressure')
  subplot(3,1,3)
  postplot(fea,'surfexpr',[s_err,'*',s_len],'evaltype','exact')
  title('Error')
end

% Error checking.
if ( size(fea.grid.c,1)==4 )
  xi = [0;0];
else
  xi = [1/3;1/3;1/3];
end
c_ind = find(evalexpr0(s_len,xi,1,1:size(fea.grid.c,2),[],fea))';
err = evalexpr0(s_err,xi,1,c_ind,[],fea);
ref = evalexpr0(['sqrt((',s_refsol,')^2)'],xi,1,c_ind,[],fea);
err = sqrt(sum(err.^2)/sum(ref.^2));

if( ~isempty(fid) )
  fprintf(fid,'\nL2 Error: %f\n',err)
  fprintf(fid,'\n\n')
end

out.err  = err;
out.pass = err<0.06;
if ( nargout==0 )
  clear fea out
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%Fourier Transfrom properties

%	Time reversal

%x(t)=t*u(t)

%X(-w)
x=t*heaviside(t);
X=fourier(x,w) ;
Right=subs(X,w,-w)

%x(-t)
x_t=subs(x,t,-t);
Left=fourier(x_t,w)


% blob_unity1.m
% find blob parameters that make the best "partition of unity"
% jeff fessler

% 1d
% J m alpha	alpha/J	(max/min-1)%
% 2 2 2.502	1.251	0.0192		1 min
% 3 2 2.472	0.824	0.0021		2 min
% 3 2 7.464	2.488	0.0587776%	"
% 4 2 2.461	0.61525 0.000315658%	3 min
% 4 2 8.687	2.17175	0.0102284%	"
% 4 2 11.17	2.7925	0.00177428%	"

% 2d
% J m alpha	alpha/J	(max/min-1)%
% 4 2 7.888	1.972	0.0411645
% 4 2 10.83	2.7075	0.0180456%

% 3d
% J m alpha	alpha/J	(max/min-1)%
% 4 2 10.4	2.6	0.0290578	and mae and rmse < 0.01%.
% for CT this is a fraction of a HU!

if ~isvar('J')
	J = 4; % a = J/2

	alf_list = linspace(0.5*J, 3.0*J, 26+0*101);
	alf_list = linspace(7, 12, 5*10+1); % 2d
	alf_list = linspace(10.5, 11, 5*10+1); % 2d
	alf_list = linspace(2, 12, 10001); % 1d
	alf_list = linspace(7.0, 8.0, 1001); % 1d fine, J=3
alf_list = linspace(0.1, 20, 1001); % 1d coarse
	alf_list = linspace(8.0, 9.0, 1001); % 1d fine, J=4
%	alf_list = linspace(10.3, 10.5, 3); % 3d
%	alf_list = 10.4; % 3d
%	m_list = linspace(1.9, 2.1, 21);
	m_list = 2.0;
	%alf_list = 2.34 * J;
	%m_list = 2;
	[aa mm] = ndgrid(alf_list, m_list);

	x1 = linspace(0, J/2, 101)';
	x2 = linspace(0, J/2, 101)';
	x3 = linspace(0, J/2, 101)';
	x2 = 0; % 1d
	x3 = 0; % 2d
	[xx1 xx2 xx3] = ndgrid(x1, x2, x3);

	% get all j that affect x in [0,J/2]
	j1max = ceil(J-1);
	j1min = floor(-J/2+1);
	ndim = 1 + (length(x2) > 1);
	nj1 = j1max-j1min+1;

	if ndim >= 3
		nj3 = nj1;
		j3min = j1min;
		j3max = j1max;
	else
		nj3 = 1;
		j3min = 0;
		j3max = 0;
	end

	if ndim >= 2
		nj2 = nj1;
		j2min = j1min;
		j2max = j1max;
	else
		nj2 = 1;
		j2min = 0;
		j2max = 0;
	end
end

%
% precompute r samples for each j offset
%
if ~isvar('rr'), disp 'do rr'
	rr = zeros([length(x1) length(x2) length(x3) nj1 nj2 nj3]);
	for j3=j3min:j3max
		for j2=j2min:j2max
			for j1=j1min:j1max
				rr(:,:,:,j1-j1min+1,j2-j2min+1,j3-j3min+1) = ...
				sqrt((xx1-j1).^2 + (xx2-j2).^2 + (xx3-j3).^2);
			end
		end
	end
	%im(x1, x2, rr)
end

if ~isvar('bad'), disp 'do bad'
	bad = zeros(size(mm));
	bb = zeros(length(x1), length(x2), length(x3), nj1*nj2*nj3);

	for ii=1:numel(aa)
		ticker(mfilename, ii, numel(aa))
		kb_a = aa(ii);
		kb_m = mm(ii);

		bb = kaiser_bessel(rr, J, kb_a, kb_m);
		bsum = sum(sum(sum(bb, 3), 4), 5);
		bad(ii) = max(bsum(:)) / min(bsum(:));
	end
	bad = abs(bad-1);
end

if 1
	im clf, im(121, alf_list/J, m_list, bad), cbar
	axis normal
	hold on
	ibest = imin(bad, 2);
	plot(alf_list(ibest(1))/J, m_list(ibest(2)), '*')
	hold off
	xlabel '\alpha/J', ylabel 'm'
end

if ~isvar('bmean')
	kb_a = alf_list(ibest(1));
	kb_m = m_list(ibest(2));

	bb = kaiser_bessel(rr, J, kb_a, kb_m);
	bsum = sum(sum(sum(bb, 3), 4), 5);
	bmean = mean(bsum(:));
end

printf('J m alpha alpha/J ratio-1')
printf('%g %g %g %g %g%%', ...
	J, kb_m, kb_a, kb_a / J, (max(bsum(:)) / min(bsum(:))-1)*100)
printf('mea=%g%%', mean(abs(bsum(:)-bmean))/bmean * 100)
printf('rmse=%g%%', sqrt(mean(abs(bsum(:)-bmean).^2))/bmean * 100)

if 1
	subplot(122)
	if ndim == 1
		plot(x1, reshape(bb, length(x1), []), '--', x1, bsum, '-')
		clf, semilogy(alf_list, bad, '.-'), xlabel '\alpha'
	else
		im(x1, x2, bsum/bmean, 'bsum/bmean'), cbar
	end
end

if 0
	clf
	for j1=1:nj1
		for j2=1:nj2
			subplot(nj1, nj2, j1 + (j2-1)*nj1)
			if ndim == 1
				plot(x1, bb(:,1,j1,j2))
			else
				im(x1, x2, bb(:,:,j1,j2))
			end
		end
	end
end


%
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  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. This file 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.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function p = wrappednormcdf(x, m, s, N)

    % N=10 gives a log error smaller than 1e-12 for s=8
    % with s=8 distribution is "almost" uniform (std < 1e-12)
    if nargin<4; N=10; end

    p = 0;
    for k=-N:N
        p = p + normcdf(x+2.*pi.*k, m, s);
    end

    p = p - N;

return


classdef CoSaMP < handle
    % Implements CoSaMP-MMV algorithm for sparse recovery

    properties
        % These properties can be configured before running cosamp
        % Default threshold
        errorNormThreshold = 1e-6;
        % Maximum number of iterations for approximation
        MaxIters
        Verbose = false
        % Indicates if matching pursuit should be used for identification
        % UseMPIdentification = false
        % Indicates if least squares should be run on final support
        LSOnFinalSupport = false
        % The norm to be chosen for rows
        P
        % Indicates that residuals should be orthogonalized for rank awareness
        RankAwareResidual = false
    end

    properties(SetAccess=private)
        % The dictionary
        Dict
        % Ambient signal dimensions
        N
        % Number of atoms in dictionary
        D
        % Sparsity level of representations (may be negative)
        K
        % Result of a solver
        result
    end

    methods
        function self  = CoSaMP(Dict, K, P, options)
            if nargin < 3
                % By default we apply l_1 norm on rows
                P = 1;
            end
            if P ~= 1 &&  P ~= 2
                error('Only l_1 and l_2 norms are supported.');
            end
            self.P = P;
            % We assume that all the columns in dictionary are normalized.
            if isa(Dict, 'spx.dict.Operator')
                self.Dict = Dict;
            elseif ismatrix(Dict)
                self.Dict = spx.dict.MatrixOperator(Dict);
            else
                error('Unsupported operator.');
            end
            [self.N, self.D] = size(Dict);
            if nargin < 2
                % No sparsity level has been pre-specified.
                % We make an estimate of sparsity level
                % based on phase transition analysis by Donoho
                K = round((self.N / 2) * log (self.D));
            end
            self.K = K;
            % Maximum number of iterations
            maxIter = 30;
            self.MaxIters = maxIter;
            if nargin >= 4
                % options have been specified
                if isfield(options, 'RankAwareResidual')
                    self.RankAwareResidual = options.RankAwareResidual;
                end
            end
        end

        function result  = solve(self,Y)
            n = self.N;
            d = self.D;
            k = self.K;
            % The number of signals being approximated.
            s = size(Y, 2);
            dict = self.Dict;
            % Current estimate
            solution_nz_mat = zeros(k,s);
            % Current residual
            residual_mat = Y;
            % Number of iterations
            iterations = 0;
            y_norm = norm(Y, 'fro');
            old_residual_norm = 1;
            min_residual_norm = old_residual_norm;
            errorNormThreshold = self.errorNormThreshold;
            maxIterations = self.MaxIters;
            result.halted_on_max_iter = false;
            result.halted_on_residual_norm = false;
            result.halted_on_norm_change = false;
            result.halted_on_support_change = false;
            % K indices for current support
            current_support = [];
            while true
                iterations  = iterations+1;
                % We identify the support for largest 2K entries
                extra = 2*k;
                % if isempty(current_support)
                %     extra = k + extra;
                % end
                if self.RankAwareResidual
                    residual_mat = orth(residual_mat);
                end
                % Compute the proxy
                proxy_mat = dict.apply_ctranspose(residual_mat);
                % identify the largest entries
                largest_2k  = spx.pursuit.joint.CoSaMP.largest_k_l2(proxy_mat, extra);
                % We now compute our support
                % build an array holding up to 3 K largest indices
                support_3k = false(d, 1);
                % the older K columns
                support_3k(current_support) = true;
                % New 2K columns
                support_3k(largest_2k) = true; % T <= 3K
                % We pickup corresponding columns from Phi
                subdict = dict.columns(support_3k); % MxT
                % We compute signal estimate over these columns
                % B_subdict =  linsolve(subdict, Y); % TxM * MxS
                B_subdict = spx.pursuit.joint.CoSaMP.least_squares_on_support(subdict, Y);
                % sort them in descending row norm order
                pruned_largest_k =  spx.pursuit.joint.CoSaMP.largest_k_l2(B_subdict, k);
                % keep only first k rows as new solution
                solution_nz_mat = B_subdict(pruned_largest_k, :);
                old_support = current_support;
                % update current support by choosing largest k indices
                support_3k = find(support_3k == 1);
                current_support = support_3k(pruned_largest_k);
                % compute the measurement estimate
                y_estimate_matrix = dict.apply_columns(...
                    solution_nz_mat, current_support);
                % compute the new residual
                residual_mat = Y - y_estimate_matrix;
                % compute the residual norm
                residual_norm = norm(residual_mat, 'fro');
                residual_norm = residual_norm / y_norm;
                % how many indices have been added and/or removed
                support_change = length(union(old_support,current_support)) - length(intersect(old_support,current_support));
                % if residual_norm > 1.2*min_residual_norm
                %     % We are diverging
                %     break;
                % end
                if residual_norm < old_residual_norm
                    improvement = (old_residual_norm - residual_norm) / old_residual_norm;
                    % if improvement < 1e-8
                    %     % No improvement in this iteration
                    %     result.halted_on_norm_change = true;
                    %     break;
                    % end
                end
                if(residual_norm < errorNormThreshold)
                    result.halted_on_residual_norm = true;
                    break;
                end
                if iterations >= maxIterations
                    % Too many iterations we are going nowhere
                    result.halted_on_max_iter = true;
                    break;
                end
                if ~isempty(old_support) && support_change == 0
                    result.halted_on_support_change = true;
                    break;
                end
                % TODO add support for detection of no change
                % in support
                old_residual_norm = residual_norm;
                min_residual_norm = min(residual_norm, min_residual_norm);
            end
            % CoSaMP is done
            if  self.LSOnFinalSupport
                % This is used only in high coherence situations.
                % The least squares estimate is not good enough
                % as it allows more distribution of energy on to other 2K indices
                % solve another least squares problem.
                subdict = dict.columns(current_support);
                solution_nz_mat = linsolve(subdict, Y);
            end
            % copy the solution
            result.Z = zeros(d, s);
            result.Z(current_support, :) = solution_nz_mat;
            % copy the measurement residual
            result.R = residual_mat;
            result.iterations = iterations;
            result.support = current_support;
        end

    end

    methods(Static)
        function largest_indices  = largest_thresholding(dict, R, count)
            % We create the proxy of current residual
            E = dict.apply_ctranspose(R);
            tmp = abs(e);
            [~, indices] = sort(tmp, 'descend');
            % We identify the support for largest count entries
            largest_indices  = indices(1:count);
        end

        function largest_indices = largest_k_l1(data_matrix, K)
            sums = spx.norm.norms_l1_rw(data_matrix);
            [~, indices] = sort(sums, 'descend');
            % We identify the support for largest K entries
            largest_indices  = indices(1:K);
        end

        function largest_indices = largest_k_l2(data_matrix, K)
            sums = spx.norm.norms_l2_rw(data_matrix);
            [~, indices] = sort(sums, 'descend');
            % We identify the support for largest K entries
            largest_indices  = indices(1:K);
        end

        function largest_indices  = largest_mp(dict, r, count)
            % Uses matching pursuit to identify largest indices.
            % We create the proxy of current residual
            d = size(dict,2);
            indices = false(1, d);
            k = 0;
            while k < count
                products = dict.apply_ctranspose(r);
                abs_products = abs(products);
                % Find the highest inner product
                [~, index] = max(abs_products);
                % Add this index to support
                if ~indices(index)
                    % we have discovered a new atom.
                    k = k + 1;
                    indices(index) = true;
                end
                % pick up the coefficient of this inner product
                coeff = products(index);
                % update residual
                r = r - coeff * dict.column(index);
            end
            % We identify the support for largest 2K entries
            largest_indices  = find(indices);
        end

        function X = least_squares_on_support(subdict, Y)
            [U,s,V] = csvd(subdict);
            num_signals = size(Y, 2);
            num_cols = size(subdict, 2);
            X = zeros(num_cols, num_signals);
            x = zeros(num_cols, 1);
            for ns=1:num_signals
                y = Y(:, ns);
                normBound = 2*norm(y);
                [alpha2,lambda,maxIterReached] = lsqi(U,s,V,y,normBound);
                if maxIterReached
                    % lsqi did not converge; use cvx (slower) instead
                    [aa,bb] = size(subdict);
                    cvx_begin quiet
                      variable alpha3(bb) complex;
                      minimize norm(y - subdict*alpha3)
                      subject to
                      norm(alpha3) <= normBound;
                    cvx_end
                    x = alpha3;
                else
                    x = alpha2;
                end
                X(:, ns) = x;
            end
        end
    end
end


% DEMO
%
% Demonstrate the use of the Support Vector Machine toolbox to distinguish
% examples of Versicolour from Setosa and Virginica varieties of Iris, given
% petal width an length attributes.  The data is taken from the well known
% Iris benchmark [1], available from the UCI Repository of Machine Learning
% Databases (http://www.ics.uci.edu/~mlearn/MLRepository.html).
%
% [1] R. A. Fisher,
%     "The use of multiple measurements in taxonomic problems",
%     Annual Eugenics, 7(2), pp 179-188, 1936.

%
% File        : demo.m
%
% Date        : Saturday 16th September 2000
%
% Author      : Dr Gavin C. Cawley
%
% Description : Test harness for object oriented implementation of Vapnik's
%               linear support vector machine (SVM) [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 16/08/1999 - v1.00
%               13/09/2000 - v1.01 minor changes to comments etc
%               13/09/2000 - v1.02 updated to use gateway method svc/train
%               16/09/2000 - v1.10 added xi-alpha estimate of l-o-o error
%               24/11/2000 - v1.11 minor bug-fix for loading iris data under
%                                  all operating systems.
%
% Copyright   : (c) Dr Gavin C. Cawley, November 2000.
%
%    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
%

% start from a clean slate

clf;

clear classes all;

% load some data

fprintf(1,'loading training data...\n');

iris = load('data/iris.txt');

x = iris(:,3:4);
y = 2*(iris(:,5) == 2)-1;

% define useful constants

l = size(x,1);

% display data

fprintf(1,'displaying data...\n');

plot(x(find(y==1),1)',x(find(y==1),2)','bs',x(find(y==-1),1)',x(find(y==-1),2)','go')
axis(axis + [-0.1 +0.1 -0.1 +0.1])
legend('class one','class two',0);
drawnow;

% create tutor

fprintf(1,'creating tutor...\n');

kernel = rbf(0.5);
C      = 1.0;
tutor  = smosvctutor;

% train support vector machine

fprintf(1,'training support vector machine...\n');

net = train(svc, tutor, x, y, C, kernel);

net = fixduplicates(net, x, y);

net2 = strip(net);

% display support vectors

fprintf(1,'displaying support vectors...\n');

sv = getsv(net2);

hold on
plot(sv(:,1)',sv(:,2)','k+');
legend('class one','class two','support vector',0);
hold off

fprintf(1, 'there are %d support vectors\n', getnsv(net2));

% compute correctness

fprintf(1,'correctness = %4.1f%%\n', 100*sum(sign(fwd(net2,x))==y)/l);

% display decision boundary

fprintf(1,'displaying decision boundary...\n');

a     = axis;
[X,Y] = meshgrid(a(1):0.05:a(2),a(3):0.05:a(4));
X2    = [reshape(X,prod(size(X)),1) reshape(Y,prod(size(X)),1)];
z     = fwd(net2,X2);
z     = reshape(z,size(X));

hold on
contour(X,Y,z,[+1 +1],'b');
contour(X,Y,z,[+0 +0],'r');
contour(X,Y,z,[-1 -1],'g');
legend('class one','class two','support vector','class one margin','decision boundary','class two margin',0);
hold off

% highlight estimated leave-one-out errors

e = xialpha(net);

i = find(e);

fprintf(1,'l-o-o correctness >= %4.1f%%\n', 100*correctness(e));

hold on
plot(x(i,1)', x(i,2)', 'rx');
legend('class one','class two','support vector','class one margin','decision boundary','class two margin','l-o-o error',0);
hold off

% all done

fprintf(1,'bye bye...\n');

% bye bye...


function psi = data2psiX(data,srate,freqbins,permtest)
% calculates phase slope index (PSI) as formulated in the paper:
%    Nolte G, Ziehe A, Nikulin VV, Schlogl A, Kramer N, Brismar T, Muller KR.
%    Robustly estimating the flow direction of information in complex physical systems.
%    Physical Review Letters. To appear.
%    (for further information:    http://doc.ml.tu-berlin.de/causality/ )
%
% Usage:
%   psi = data2psiX(data,srate,freqbins,permtest);
%
% Input:
%     data:  MxNxT matrix for M channels, N timepoints, and T trials
%    srate:  data sampling rate in Hz
% freqbins:  KxQ matrix containing frequency boundaries in rows K.
% permtest:  Permutation test (boolean). If true, psi output values are in standardizes Z values
%               relative to a null hypothesis distribution
%
% Output:
%      psi:  phase-slope-index values. For M channels PSI is an MxM matrix if
%               one frequency bin, or MxMxK if freqbins has K rows (with K>1).
%               psi(i,j) is the directed connectivity from channel i to
%               channel j, (e.g., channel i is the sender if psi(i,j) is
%               positive; channel i is the receiver if negative)
%
%
%  (This function was modified from the original by Mike X Cohen)

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

% modified from the original dat2psi.m by mikexcohen@gmail.com

%% check inputs and initialize

if nargin<2
    help data2psiX
    error('Read help file.');
elseif nargin==2
    permtest = 0;
    freqbins = [];
elseif nargin==3
    permtest = 0;
end

n_permutes = 1000;

% data dimensions
[nchan,npnts,ntrials]=size(data);

% define frequencies
hz  = linspace(0,srate/2,floor(npnts/2)+1);
nhz = length(hz);

% initialize
cs  = zeros(nchan,nchan,nhz);
psi = zeros(nchan,nchan,size(freqbins,1));

%% compute cross-spectral density

datafft = fft(bsxfun(@times,data,hanning(npnts)'),[],2);

% trial-average cross-spectral density
for triali=1:ntrials
    for freqi=1:nhz

        tempfftdat = squeeze(datafft(:,freqi,triali))';
        cs(:,:,freqi) = cs(:,:,freqi) + tempfftdat'*conj(tempfftdat);
    end
end
cs = cs./triali;

%% compute PSI

for freqbini=1:size(freqbins,1)

    % find FFT indices of requested frequency bands
    freqidx = dsearchn(hz',freqbins(freqbini,1)) : dsearchn(hz',freqbins(freqbini,2));
    nfidx   = length(freqidx);

    if nfidx<4
        warning('There are fewer than four frequency bins. Consider using longer time windows or wider frequency bands.')
    end

    % temporary phase-frequency matrix from this frequency band
    pp = zeros(nchan,nchan,nfidx);

    for fi=1:nfidx
        pp(:,:,fi) = cs(:,:,freqidx(fi))./sqrt(diag(cs(:,:,freqidx(fi)))*diag(cs(:,:,freqidx(fi)))');
    end

    % average phase slope for each frequency band
    psi(:,:,freqbini) = sum(imag(conj(pp(:,:,1:end-1)).*pp(:,:,2:end)),3);

    %% optional permutation testing

    if permtest

        nulldist = zeros(nchan,nchan,n_permutes);
        for permi=1:n_permutes
            nulldist(:,:,permi) = sum(imag(conj(pp(:,:,randperm(nfidx))).*pp(:,:,randperm(nfidx))),3);
        end

        psi(:,:,freqbini) = ( psi(:,:,freqbini)-mean(nulldist,3) ) ./ std(nulldist,[],3);
    end

    % zero-out diagonals
    tmp = squeeze(psi(:,:,freqbini));
    tmp(logical(eye(nchan))) = 0;
    psi(:,:,freqbini) = tmp;

end

%% end


function [C,T,A]=naca4_3d(airfoil,y,nc)
% determine airfoil skin and camber coordinates from NACA designation
%% NacaCoord.m
% NACA 4-digit or 5-digit series airfoil
%
% outputs:  C = matrix of camber line coordinates, x,y,z
%               ordered trailing edge first
%           T = matrix of skin coordinates
%               ordered from trailing edge over top surface then
%               along bottom surface to trailing edge
%           A = area of airfoil cross section (normalized)
%
% NOTE: Chord of output airfoil is 1
%
% inputs:   airfoil = NACA 4- or 5-digit airfoil as string
%           y = spanwise location
%           nc = number of chordwise panels
%
% Ref:      http://www.aerospaceweb.org/question/airfoils/q0041.shtml

%           Ira H. Abbott and Albert E. Von Doenhoff, Theory of Wing
%           Sections, 1959, Dover Publications, NY.
%
% Coordinate system:
%   x: chord direction, origin at LE, positive is pointing toward TE
%   y: station along span, perpendicular to aircraft midplane
%   z: perpendicular to chord, positive is up

%This 'if' statement establishes the constants for either 4- or 5- digit
%airfoils.
if length(airfoil) == 4
    m = str2num(airfoil(1))/100;
    p = str2num(airfoil(2))/10;
    t = str2num(airfoil(3:4))/100;

elseif length(airfoil) == 5
    switch airfoil(1:3)
        case '210'
            m = 0.0580;
            k1 = 361.4;
        case '220'
            m = 0.1260;
            k1 = 51.64;
        case '230'
            m = 0.2025;
            k1 = 15.957;
        case '240'
            m = 0.2900;
            k1 = 6.643;
        case '250'
            m = 0.3910;
            k1 = 3.230;
        otherwise
    end
    t = str2num(airfoil(4:5))/100;
end

%% Constants
% nc          % number of line segments describing top surface
% (= number of line segments describing bottom surface)
npc = nc+1;      % number of points along mean camber line
nps = 2*nc+1;    % number of points along surface
dx=1/nc;        % x-direction increment (along chord)

%% Fill coordinate matrices with y coordinate
C = zeros(npc,3); % mean camber
C(:,2) = y;
T = zeros(nps,3); %skin
T(:,2) = y;

%% Mean Camber Line and Slope
% dzcdx is the derivative of the mean camber line
% theta is the local angle of the mean camber line
% x=1:-dx:0; % order x locations starting at trailing edge, x=1
x=0:dx:1; % order x locations starting at leading edge, x=0
if length(airfoil) == 4
    for i = 1:nc+1
        if x(i)<p    % leading edge
            zc(i) = m/(p^2)*(2*p*x(i)-x(i)^2);  %Eqn 6.4 Abbot
            dzcdx(i) = m/p^2*(2*p-2*x(i));
            theta(i) = atan(dzcdx(i));
        else            % trailing edge
            zc(i) = m/(1-p)^2*((1-2*p)+2*p*x(i)-x(i)^2);    %Eqn 6.4 Abbot
            dzcdx(i) = m/(1-p)^2*(2*p-2*x(i));
            theta(i) = atan(dzcdx(i));
        end
    end
elseif length(airfoil) == 5
    for i = 1:nc+1
        if x(i)<m    % leading edge
            zc(i) = 1/6*k1*(x(i)^3-3*m*x(i)^2+m^2*(3-m)*x(i));  %Eqn 6.6 Abbot
            dzcdx(i) = 1/6*k1*(3*x(i)^2-6*m*x(i)+m^2*(3-m));
            theta(i) = atan(dzcdx(i));
        else            % trailing edge
            zc(i) = 1/6*k1*m^3*(1-x(i));    %Eqn 6.6 Abbot
            dzcdx(i) = -1/6*k1*m^3;
            theta(i) = atan(dzcdx(i));
        end
    end
end

C(:,1)=-x;  % output mean camber line coordinates; negative x due to axis convention
C(:,3)=-zc; %negative z due to axis convention

%% Thickness distribution is the same for both 4- and 5-digit airfoils
for i=1:nc+1
    zt(i) = t/0.2*[0.2969*x(i)^0.5-0.1260*x(i)-0.3516*x(i)^2+0.2843*x(i)^3-0.1015*x(i)^4];  %Eqn 6.2 Abbott
end

%% Area
A=0;
for i=1:nc
    da=dx*(zt(i)+zt(i+1));  %Take the average of zt(i) and zt(i+1) and double it
    A=A+da;
end
A;

%% Upper skin
for i=1:nc+1
    xu(i) = x(i)-zt(i)*sin(theta(i));  %Eqn 6.1 Abbott
    zu(i) = zc(i)+zt(i)*cos(theta(i));  %Eqn 6.1 Abbott
    T(i,1) = -xu(i);  %negative x due to sign convention
    T(i,3) = -zu(i);  %negative z due to sign convention
end

%% lower skin
% exclude point nc+1 at leading edge
% because it is included as the last point of the upper surface

for i=1:nc
    xl(i) = x(i)+zt(i)*sin(theta(i));    %Eqn 6.1 Abbott
    zl(i) = zc(i)-zt(i)*cos(theta(i));    %Eqn 6.1 Abbott
end

% reverse order so LE to TE
for i=nc+2:nps
    T(i,1) = -xl(2*(nc+1)-i);%negative x due to sign convention
    T(i,3) = -zl(2*(nc+1)-i);%negative z due to sign convention
end
%figure
%plot(xu,zu,'o')
%hold on
%plot(xl,zl,'or')
%axis equal


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

t = 0;              % current time
T = 1;              % maturity

F = 80:.25:120;       % possible forwards in T
A = 0.01:0.01:0.4;  % possible volatilities in T

f = 100;            % current spot asset
alpha = 0.2;        % current spot volatility

beta = 0.5;         % SABR CEV coeff
nu = 0.2;           % Vol of vol
rho = 0;         % Correlation

d_base = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu,rho);   % approx density

xret = ((F-f)/f);   % possible relative changes

low = 0;            % limit integration low
up = 1;             % limit integration high

y_base = density1d(F,d_base,low,up);    % compute density
legend_base = 'Base';
title_plot = 'SABR Density';

y2_base = density2d(F,A,d_base);

%% Change alpha
d_low = @(x,y) psabr(t,T,f,x,alpha-0.1,y,beta,nu,rho); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha+0.1,y,beta,nu,rho);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \alpha low';
legend_high = 'Changing \alpha high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);

%% change beta
d_low = @(x,y) psabr(t,T,f,x,alpha,y,beta-.2,nu,rho); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha,y,beta+.2,nu,rho);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \beta low';
legend_high = 'Changing \beta high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);

y2_low = density2d(F,A,d_low);
y2_high = density2d(F,A,d_high);

create_density_2d(F',A,y2_low', y2_base', y2_high', legend_low, legend_base, legend_high, title_plot);

%% change nu
d_low = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu-.1,rho); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu+.1,rho);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \nu low';
legend_high = 'Changing \nu high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);

%% change rho
d_low = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu,rho-.6); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu,rho+.6);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \rho low';
legend_high = 'Changing \rho high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);


function [transformationMatrix] = ...
    optical_flow_linear_registration2d(moving, fixed, varargin)
% OPTICAL_FLOW_LINEAR_REGISTRATION2D Estimates a transformation matrix using optical flow
%
% INPUT ARGUMENTS
% moving                - Moving iamge
% fixed                 - Fixed image
%
% OPTIONAL INPUT ARGUMENTS
% 'transformationModel'     - Transformation model for estimating the
%                             displacement field
%                             'translation', 'affine' (default)
%
% 'multiModal'              - Set whether to perform multi-modal or
%                             uni-modal image registration
%                             false (default), true
%
% 'numberOfChannels'        - Number of channels to use in when computing
%                             the entropy (based on channel coding). This
%                             is only relevant if multiModal is set to
%                             true.
%                             Default value is 8
%
% OUTPUT ARGUMENTS
% transformationMatrix  - Estimated transformation matrix

% Copyright (c) 2012
% danne.forsberg@outlook.com
%
% 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/>.

%% Setup default parameters
% translation, affine
transformationModel = 'affine';

% multi-modal
multiModal = false;

% number of channels, only valid for multi-modal registration
numberOfChannels = 8;

% Overwrites default parameter
for k=1:2:length(varargin)
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

% Initialize transformation matrix
transformationMatrix = eye(3);

[b_moving(:,:,1) b_moving(:,:,2)] = gradient(moving);
[b_fixed(:,:,1) b_fixed(:,:,2)] = gradient(fixed);

if multiModal
    b = b_fixed(:,:,2);
    b(:,:,2) = b_fixed(:,:,1);

    [delta_c mask] = estimate_delta_c(fixed,moving,numberOfChannels);
    delta_c(mask ~= 1) = 0;
    mask = repmat(mask,[1 1 2]);
    b(mask ~= 1) = 0;
else
    b = (b_moving(:,:,2) + b_fixed(:,:,2))/2;
    b(:,:,2) = (b_moving(:,:,1) + b_fixed(:,:,1))/2;

    delta_c = fixed - moving;
end

[G, h] = build_G_h_linear2d(b, delta_c, transformationModel);

% Solve the equation system
switch transformationModel
    case 'translation'
        d = G \ h;
        transformationMatrix(1:2,3) = d;
    case {'rigid','affine'}
        p = G \ h;
        transformationMatrix(1:2,1:3) = [1+p(1) p(2) p(5);...
                                         p(3) 1+p(4) p(6)];
end


function [mssim, ssim_map, mcs, cs_map] = ssim_index_new(img1, img2, K, win, max_val)

if (nargin < 2 || nargin > 5)
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

if (size(img1) ~= size(img2))
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

[M N] = size(img1);

if (nargin == 2)
   if ((M < 11) || (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   win = fspecial('gaussian', 11, 1.5);	%
   K(1) = 0.01;										% default settings
   K(2) = 0.03;										%
end

if (nargin == 3)
   if ((M < 11) || (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   win = fspecial('gaussian', 11, 1.5);
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 4 || nargin == 5)
   [H W] = size(win);
   if ((H*W) < 4 || (H > M) || (W > N))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

C1 = (K(1)*max_val)^2;
C2 = (K(2)*max_val)^2;
win = win/sum(sum(win));

mu1   = filter2(win, img1, 'valid');
mu2   = filter2(win, img2, 'valid');
mu1_sq = mu1.*mu1;
mu2_sq = mu2.*mu2;
mu1_mu2 = mu1.*mu2;
sigma1_sq = filter2(win, img1.*img1, 'valid') - mu1_sq;
sigma2_sq = filter2(win, img2.*img2, 'valid') - mu2_sq;
sigma12 = filter2(win, img1.*img2, 'valid') - mu1_mu2;

if (C1 > 0 & C2 > 0)
   ssim_map = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2));
   cs_map = (2*sigma12 + C2)./(sigma1_sq + sigma2_sq + C2);
else
   numerator1 = 2*mu1_mu2 + C1;
   numerator2 = 2*sigma12 + C2;
	denominator1 = mu1_sq + mu2_sq + C1;
   denominator2 = sigma1_sq + sigma2_sq + C2;

   ssim_map = ones(size(mu1));
   index = (denominator1.*denominator2 > 0);
   ssim_map(index) = (numerator1(index).*numerator2(index))./(denominator1(index).*denominator2(index));
   index = (denominator1 ~= 0) & (denominator2 == 0);
   ssim_map(index) = numerator1(index)./denominator1(index);

   cs_map = ones(size(mu1));
   index = denominator2 > 0;
   cs_map(index) = numerator2(index)./denominator2(index);
end

mssim = mean2(ssim_map);
mcs = mean2(cs_map);

return


function [Y, problem, S] = elliptope_SDP_complex(A, p, Y0)
% Solver for complex semidefinite programs (SDP's) with unit diagonal.
%
% function [Y, problem, S] = elliptope_SDP_complex(A)
% function [Y, problem, S] = elliptope_SDP_complex(A, p)
% function [Y, problem, S] = elliptope_SDP_complex(A, p, Y0)
%
% A is a Hermitian matrix of size n.
%
% This function uses a local optimization method in Manopt to solve the SDP
%
%   min_X trace(A*X) s.t. diag(X) = 1, X is complex, positive semidefinite.
%
% In practice, the Hermitian matrix X of size n is parameterized as
% X = Y*Y', where Y has size n x p. By default, p is taken large enough
% (that is, sqrt(n)) to ensure that there exists an optimal X whose rank is
% smaller than p. This ensures that the SDP is equivalent to the new
% problem in Y:
%
%   min_Y  trace(Y'*A*Y)  s.t.  diag(Y*Y') = 1, Y complex
%
% The constraints on Y require each row of Y to have unit norm, which is
% why Manopt is appropriate software to solve this problem. An optional
% initial guess can be specified via the input Y0.
%
% See the paper below for theory, specifically, for a proof that, for
% almost all A, second-order critical points of the problem in Y are
% globally optimal. In other words: there are no local traps in Y, despite
% non-convexity.
%
% Outputs:
%
%       Y: is the best point found (an nxp matrix with unit norm rows.)
%          To find X, form Y*Y' (or, more efficiently, study X through Y.)
%
%       problem: is the Manopt problem structure used to produce Y.
%
%       S: is a dual optimality certificate (a Hermitian matrix of size n,
%          sparse if A is sparse). The optimality gap (in the cost
%          function) is at most n*min(eig(S)), for both Y and X = Y*Y'.
%          Hence, if min(eig(S)) is close to zero, Y is close to globally
%          optimal. This can be computed via eigs(S, 1, 'SR').
%
% Paper: https://arxiv.org/abs/1606.04970
%
% @inproceedings{boumal2016bmapproach,
%   author  = {Boumal, N. and Voroninski, V. and Bandeira, A.S.},
%   title   = {The non-convex {B}urer-{M}onteiro approach works on smooth semidefinite programs},
%   booktitle={Neural Information Processing Systems (NIPS 2016)},
%   year    = {2016}
% }
%
% See also: maxcut elliptope_SDP

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Oct. 21, 2016
% Contributors:
% Change log:
%
%    Xiaowen Jiang Aug. 20, 2021
%       Added AD to compute the egrad and the ehess

    % If no inputs are provided, since this is an example file, generate
    % a random complex matrix. This is for illustration purposes only.
    if ~exist('A', 'var') || isempty(A)
        n = 100;
        A = randn(n) + 1i*randn(n);
        A = (A+A')/sqrt(2*n);
    end

    n = size(A, 1);
    assert(n >= 2, 'A must be at least 2x2.');
    assert(size(A, 2) == n, 'A must be square.');

    % Force A to be Hermitian
    A = (A+A')/2;

    % By default, pick a sufficiently large p (number of columns of Y).
    if ~exist('p', 'var') || isempty(p)
        p = floor(sqrt(n)+1);
    end

    assert(p >= 1 && p == round(p), 'p must be an integer >= 1.');

    % Pick the manifold of complex n-by-p matrices with unit norm rows.
    manifold = obliquecomplexfactory(p, n, true);

    problem.M = manifold;

    % These three, quick commented lines of code are sufficient to define
    % the cost function and its derivatives. This is good code to write
    % when prototyping. Below, a more advanced use of Manopt is shown,
    % where the redundant computation A*Y is avoided between the gradient
    % and the cost evaluation.
    % % problem.cost  = @(Y) .5*sum(sum(real((A*Y).*conj(Y))));
    % % problem.egrad = @(Y) A*Y;
    % % problem.ehess = @(Y, Ydot) A*Ydot;

    % Products with A dominate the cost, hence we store the result.
    % This allows to share the results among cost, grad and hess.
    % This is completely optional.
    function store = prepare(Y, store)
        if ~isfield(store, 'AY')
            AY = A*Y;
            store.AY = AY;
            store.diagAYYt = sum(real(AY .* conj(Y)), 2);
        end
    end

    % Define the cost function to be /minimized/.
    problem.cost = @cost;
    function [f, store] = cost(Y, store)
        store = prepare(Y, store);
        f = .5*sum(store.diagAYYt);
    end

    % Define the Riemannian gradient.
    problem.grad = @grad;
    function [G, store] = grad(Y, store)
        store = prepare(Y, store);
        G = store.AY - bsxfun(@times, Y, store.diagAYYt);
    end

    % If you want to, you can specify the Riemannian Hessian as well.
    problem.hess = @hess;
    function [H, store] = hess(Y, Ydot, store)
        store = prepare(Y, store);
        SYdot = A*Ydot - bsxfun(@times, Ydot, store.diagAYYt);
        H = manifold.proj(Y, SYdot);
    end

    % An alternative way to compute the egrad and the ehess is to use
    % automatic differentiation provided in the deep learning toolbox
    % (slower). AD does not support complex numbers if the Matlab version
    % is R2021a or earlier. The cost function should be defined differently
    % In this case. See complex_example_AD.m and manoptADhelp.m for more
    % information.
    % problem.cost = @cost_AD;
    %    function f = cost_AD(Y)
    %        AY = cprod(A, Y);
    %        diagAYYt = csum(creal(cdottimes(AY, cconj(Y))), 2);
    %        f = .5*csum(diagAYYt);
    %    end
    % Call manoptAD to automatically obtain egrad and ehess:
    % problem = manoptAD(problem);

    % If the version of Matlab installed is R2021b or later, specify the
    % cost function in the normal way and call manoptAD.
    % problem.cost = @cost_AD;
    %    function f = cost_AD(Y)
    %        AY = A*Y;
    %        diagAYYt = sum(real(AY .* conj(Y)), 2);
    %        f = .5*sum(diagAYYt);
    %    end
    % problem = manoptAD(problem);

    % If no initial guess is available, tell Manopt to use a random one.
    if ~exist('Y0', 'var') || isempty(Y0)
        Y0 = [];
    end

    % Call your favorite solver.
    opts = struct();
    opts.verbosity = 0;      % Set to 0 for no output, 2 for normal output
    opts.maxinner = 500;     % maximum Hessian calls per iteration
    opts.tolgradnorm = 1e-6; % tolerance on gradient norm
    Y = trustregions(problem, Y0, opts);

    % If required, produce an optimality certificate.
    if nargout >= 3
        S = A - spdiags(sum(real((A*Y).*conj(Y)), 2), 0, n, n);
    end

end


function val = peval(x,coef,deg)
% function val = peval(x,coef,deg)
%
% DESCRIPTION
%   Evaluate a polynomial at x.
%
%   Note: The SUBS function should be used for polynomial evaluations.
%   The PEVAL function has no error checking for speed and it assumes
%   that the polynomial is a row or column vector.  It also assumes that
%   the rows of x are ordered to correspond to the columns in the degree
%   matrix deg.
%
% INPUTS
%   x: nvars-by-npts matrix each column of which specifies a point
%        at which to evaluate the polynomial.
%   coef: nterms-by-lp coefficient matrix where lp is the length
%        of the polynomial
%   deg: nterms-by-nvars degree matrix
%
% OUTPUTS
%   val: lp-by-npts matrix where each column specifies the value of
%        the polynomial evaluated at the corresponding column of x.
%
% SYNTAX
%   val = peval(x,coef,deg)

% 6/15/06  PJS  Initial Coding

% Compute dimensions
[nterms,nvars]=size(deg);
npts = size(x,2);
lp = size(coef,2);
coef = full(coef);
deg = full(deg);

% Compute monomials
xs = shiftdim(x,-1);            % xs is 1-by-nvars-by-npts
xrep = repmat(xs,[nterms 1 1]); % xrep is nterms-by-nvars-by-npts
drep = repmat(deg,[1 1 npts]);  % drep is nterms-by-nvars-by-npts

% npow = xrep;
% idx = find(drep~=1);
% npow(idx) = xrep(idx).^drep(idx);

npow = xrep.^drep;
nmonom = prod(npow,2);          % nmonom is nterms-by-1-by-npts

% Evalute polynomial--mulitply by coefs and sum monomials
nmonomrep = repmat(nmonom,[1 lp 1]); % nmonomrep is nterms-by-lp-by-npts
coefrep = repmat(coef,[1 1 npts]);   % coefrep is nterms-by-lp-by-npts
val = sum(coefrep.*nmonomrep,1);     % val is 1-by-lp-by-npts

% Reshape val from 1-by-lp-by-npts to lp-by-npts
if npts==1
    val = val(:);
elseif lp==1
    val = val(:)';
else
    val = squeeze(val);
end


function [ n_data, n, x, fx ] = p_polynomial_values ( n_data )

%*****************************************************************************80
%
%% P_POLYNOMIAL_VALUES returns values of the Legendre polynomials P(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 22;

  fx_vec = [ ...
      0.1000000000000000E+01, ...
      0.2500000000000000E+00, ...
     -0.4062500000000000E+00, ...
     -0.3359375000000000E+00, ...
      0.1577148437500000E+00, ...
      0.3397216796875000E+00, ...
      0.2427673339843750E-01, ...
     -0.2799186706542969E+00, ...
     -0.1524540185928345E+00, ...
      0.1768244206905365E+00, ...
      0.2212002165615559E+00, ...
      0.0000000000000000E+00, ...
     -0.1475000000000000E+00, ...
     -0.2800000000000000E+00, ...
     -0.3825000000000000E+00, ...
     -0.4400000000000000E+00, ...
     -0.4375000000000000E+00, ...
     -0.3600000000000000E+00, ...
     -0.1925000000000000E+00, ...
      0.8000000000000000E-01, ...
      0.4725000000000000E+00, ...
      0.1000000000000000E+01 ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3 ];

  x_vec = [ ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.25E+00, ...
     0.00E+00, ...
     0.10E+00, ...
     0.20E+00, ...
     0.30E+00, ...
     0.40E+00, ...
     0.50E+00, ...
     0.60E+00, ...
     0.70E+00, ...
     0.80E+00, ...
     0.90E+00, ...
     1.00E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function xform = find_xform(c, d)
%Finds the xform that maps from c to d
%in this case the xform is a simple translation and scaling
%xform is a [3 x 3] matrix which maps coordinates from c's frame to
%d's frame.  the transformation is applied to homogeneous
%coordinaes

%convert bounding box to cornners
xs(:,1) = c([1 2])';
xs(:,2) = c([3 2])';
xs(:,3) = c([1 4])';
xs(:,4) = c([3 4])';

ys(:,1) = d([1 2])';
ys(:,2) = d([3 2])';
ys(:,3) = d([1 4])';
ys(:,4) = d([3 4])';

xs(3,:) = 1;
ys(3,:) = 1;

A=ys*pinv(xs);
A(abs(A)<.000001) = 0;

xform = A;


function [in2] = km22in2(km2)
% Convert area from square kilometers to square inches.
% Chad A. Greene 2012
in2 = km2*1550003100.006;


function psi = conv(psi1,psi2,varargin)
% convolution of an SO3Kernel function with a function or a kernel on SO(3)
%
% We convolute an SO3Kernel $f$ with another SO3Kernel or an SO3Fun $g$
% by the convolution
%
% $$ (f *_L g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(q^{-1}\,R) \, dq $$
%
% which in this case is similar to the so caled right sided convolution,
% see SO3FunHarmonic/conv.
%
% The convolution of an SO3Kernel with an S2Kernel or an S2Fun $h$ is
% defined by
%
% $$ (f * h)(\xi) =  \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot h(q^{-1}\,\xi) \, dq $$.
%
%
% Syntax
%   psi = conv(psi1,psi2)
%   SO3F2 = conv(psi1,SO3F1)
%   sF2 = conv(psi1,sF1)
%   phi2 = conv(psi1,phi1)
%   psi = conv(psi1)
%
% Input
%  psi1, psi2 - @SO3Kernel
%  phi1       - @S2Kernel
%  SO3F1      - @SO3Fun
%  sF1        - @S2Fun
%
% Output
%  psi   - @SO3Kernel
%  SO3F2 - @SO3Fun
%  sF2   - @S2Fun
%  phi2  - @S2Kernel
%
% See also
% SO3FunHarmonic/conv SO3FunRBF/conv S2FunHarmonic/conv S2Kernel/conv

if nargin == 1, psi2 = psi1; end

% ------------------- convolution with a SO3Fun -------------------
% In case psi2 is a SO3Fun, convolute the SO3Fun with the kernel
% conv is commutative if kernel is included
if isa(psi2,'SO3Fun')
  psi = conv(psi2,psi1,varargin{:});
  return
end

% ------------------- convolution with a S2Fun -------------------
if isa(psi2,'S2Fun')
  sF = S2FunHarmonic(psi2);
  L = min(psi1.bandwidth,sF.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = psi1.A(l+1) * sF.fhat(l^2+1:(l+1)^2) ./ (2*l+1);
  end

  if isa(psi2,'S2FunHarmonicSym')
    warning(['There is no symmetry given for the SO3Kernel function. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the symmetry of the S2Fun.'])
  end
  psi = S2FunHarmonic(fhat);
  return
end

% ------------------- convolution with a S2Kernel -------------------
if isa(psi2,'S2Kernel')
  L = min(psi1.bandwidth,psi2.bandwidth);
  l = (0:L);
  psi = S2Kernel(psi1.A(1:L+1) .* psi2.A(1:L+1) ./ (2*l+1));
  return
end

% ------------------- convolution of SO3Kernels -------------------
if isnumeric(psi1)
  psi = conv(psi2,psi1,varargin{:});
  return
end

% extract Legendre coefficients of psi1
A1 = psi1.A(:);

% extract Legendre coefficients of psi2
if isnumeric(psi2)
  A2 = psi2(:);
else
  A2 = psi2.A(:);
  A2 = A2 ./ (2*(0:length(A2)-1)+1).';
end

% multiplication in harmonic domain
L = min(psi1.bandwidth,psi2.bandwidth);
psi = SO3Kernel(A1(1:L+1) .* A2(1:L+1));

end


function t = bartlett_sample ( m, df, sigma )

%*****************************************************************************80
%
%% BARTLETT_SAMPLE samples the Bartlett distribution.
%
%  Discussion:
%
%    If the matrix T is sampled from the Bartlett distribution, then
%    the matrix W = T' * T is a sample from the Wishart distribution.
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Input, real SIGMA(M,M), the covariance matrix, which should be
%    a symmetric positive definite matrix.
%
%    Output, real T(M,M), the sample matrix from the Bartlett distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BARTLETT_SAMPLE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'BARTLETT_SAMPLE - Fatal error!\n' );
  end
%
%  Get the upper triangular Cholesky factor of SIGMA.
%
  r = chol ( sigma );
%
%  Sample the unit Bartlett distribution.
%
  tu = bartlett_unit_sample ( m, df );
%
%  Construct the matrix.
%
  t = tu * r;

  return
end


function nearest = find_closest1 ( m, nr, r, ns, s )

%*****************************************************************************80
%
%% FIND_CLOSEST1 finds the nearest R point to each S point.
%
%  Discussion:
%
%    We are given R, a set of NR points in M dimensions.
%
%    We are given S, a set of NS points in M dimensions.
%
%    For each S(I) in S, we seek the index J of the point R(J)
%    which is nearest to S(I) over all points in R.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer NR, the number of cell generators.
%
%    Input, real R(M,NR), the cell generators.
%
%    Input, integer NS, the number of sample points.
%
%    Input, real S(M,NS), the points to be checked.
%
%    Output, integer NEAREST(NS), the index of the nearest cell generators.
%
  nearest = zeros ( ns, 1 );

  for js = 1 : ns

    distance = Inf;
    nearest(js) = -1;

    for jr = 1 : nr

      dist_sq = 0.0;
      for i = 1 : m
        dist_sq = dist_sq + ( r(i,jr) - s(i,js) )^2;
      end

      if ( dist_sq < distance )
        distance = dist_sq;
        nearest(js) = jr;
      end

    end

  end

  return
end


function R = fit_rotation(S)
  % FIT_ROTATION find rotation for a given covariance matrix
  %
  % R = fit_rotation(S)
  %
  % Inputs:
  %   S  n by n covariance matrix
  % Outputs:
  %   R  n by n rotation matrix closest to S
  %

  % svd
  [su,ss,sv]=svd(S);
  R = sv*su';
  % if reflection then flip last column
  if( det(R) < 0 )
    su(:,end) = -su(:,end);
    R = sv*su';
  end
  % should definitely be rotation now
  %assert( det(R) >= 0 );
end


function mbr = mbrot()
    i_list = 1:1024;
    mbr = nan(1024,1024,3);
    for i = i_list(:)'
        for j = i_list(:)'

            mbr(i,j,1) = red(i,j);
            mbr(i,j,2) = green(i,j);
            mbr(i,j,3) = blue(i,j);
        end
    end

    figure; image(mbr./max(mbr(:)));

    function out = red(i, j)
        a = 0;
        b = 0;
        d = 0;
        n = 0;
        while (a*a+(b*b)<4 && n<1024)
            b=2*a*b+(j-1)/5e4+.06;
            a=a*a-d+(i-1)/5e4+.34;
            d = b*b;
            n = n+1;
        end
        out = n/4;
    end

    function out = green(i, j)
        out = 4 * red(i, j);
    end

    function out = blue(i, j)
        out = 6 * red(i, j);
    end
end


function [disp_row, disp_col, scale_ind] = optimize_scores(scores_fs, iterations)

% Maximizes the continuous convolution response (classification scores).
% Find the size of the output.
[sz1, sz2, num_scales] = size(scores_fs);
output_sz = [sz1 sz2];

% Do the grid search step by finding the maximum in the sampled response
% for each scale.
sampled_scores = sample_fs(scores_fs);
[max_resp_row, max_row] = max(sampled_scores, [], 1);
[init_max_score, max_col] = max(max_resp_row, [], 2);
max_row_perm = permute(max_row, [2 3 1]);
col = max_col(:)';
row = max_row_perm(sub2ind(size(max_row_perm), col, 1:size(sampled_scores,3)));

% Shift and rescale the coordinate system to [-pi, pi]
trans_row = mod(row - 1 + floor((output_sz(1)-1)/2), output_sz(1)) - floor((output_sz(1)-1)/2);
trans_col = mod(col - 1 + floor((output_sz(2)-1)/2), output_sz(2)) - floor((output_sz(2)-1)/2);
init_pos_y = permute(2*pi * trans_row / output_sz(1), [1 3 2]);
init_pos_x = permute(2*pi * trans_col / output_sz(2), [1 3 2]);

% Set the current maximum to the sampled one
max_pos_y = init_pos_y;
max_pos_x = init_pos_x;

% construct grid
ky = -ceil((output_sz(1) - 1)/2) : floor((output_sz(1) - 1)/2);
kx = (-ceil((output_sz(2) - 1)/2) : floor((output_sz(2) - 1)/2))';

% pre-compute complex exponential
exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

ky2 = ky.*ky;
kx2 = kx.*kx;

iter = 1;
while iter <= iterations
    % Compute gradient
    ky_exp_ky = bsxfun(@times, ky, exp_iky);
    kx_exp_kx = bsxfun(@times, kx, exp_ikx);
    y_resp = mtimesx(exp_iky, scores_fs, 'speed');
    resp_x = mtimesx(scores_fs, exp_ikx, 'speed');
    grad_y = -imag(mtimesx(ky_exp_ky, resp_x, 'speed'));
    grad_x = -imag(mtimesx(y_resp, kx_exp_kx, 'speed'));

    % Compute Hessian
    ival = 1i * mtimesx(exp_iky, resp_x, 'speed');
    H_yy = real(-mtimesx(bsxfun(@times, ky2, exp_iky), resp_x, 'speed') + ival);
    H_xx = real(-mtimesx(y_resp, bsxfun(@times, kx2, exp_ikx), 'speed') + ival);
    H_xy = real(-mtimesx(ky_exp_ky, mtimesx(scores_fs, kx_exp_kx, 'speed'), 'speed'));
    det_H = H_yy .* H_xx - H_xy .* H_xy;

    % Compute new position using newtons method
    max_pos_y = max_pos_y - (H_xx .* grad_y - H_xy .* grad_x) ./ det_H;
    max_pos_x = max_pos_x - (H_yy .* grad_x - H_xy .* grad_y) ./ det_H;

    % Evaluate maximum
    exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
    exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

    iter = iter + 1;
end

% Evaluate the Fourier series at the estimated locations to find the
% corresponding scores.
max_score = real(mtimesx(mtimesx(exp_iky, scores_fs, 'speed'), exp_ikx, 'speed'));

% check for scales that have not increased in score
ind = max_score < init_max_score;
max_score(ind) = init_max_score(ind);
max_pos_y(ind) = init_pos_y(ind);
max_pos_x(ind) = init_pos_x(ind);

% Find the scale with the maximum response
[max_scale_response, scale_ind] = max(max_score(:));

% Scale the coordinate system to output_sz
disp_row = (mod(max_pos_y(1,1,scale_ind) + pi, 2*pi) - pi) / (2*pi) * output_sz(1);
disp_col = (mod(max_pos_x(1,1,scale_ind) + pi, 2*pi) - pi) / (2*pi) * output_sz(2);
end


% Frequency response 3rd Order Bessel HPF
%  and G array.
% File:  c:\M_files\short_updates\UsingGarray2.m
% 02/10/07
% See Word file UsingGarray2.doc for schemtic and equations.
clear;clc;
% unit suffixes
u=1e-6;K=1e3;m=1e-3;u=1e-6;n=1e-9;p=1e-12;
% component values
R1=52*K;R2=75*K;R3=287*K;
C1=1.55*n;C2=C1;C3=C1;
Ein=1; % Unity input for (normalized) transfer function
N=3; % Number of capacitors = order of circuit
%
% Form W, Q, S, and P arrays:
%
W=[R2 -R2 0;0 R1 R3-R1;0 0 R3];
Q=-[1 0 0;1 1 0;1 1 1];S=Ein*[1;1;1];P=diag([C1 C2 C3]);
%
% Get A, B, D, & E arrays:
%
C=inv(W*P);A=C*Q;B=C*S;
% D & E
D=[0 0 0];E=0;
%
F=[0 0 R3]; % R3 is the coefficient of both iC1 and iC2 in Vo2
% which are derivative terms (iC=dVc/dt, etc.)
G=F*P;
%
% * * * * * * * * * * * * Frequency response * * * * * * * * * * * *
%
I=eye(N); % identity matrix
%
% Log frequency sweep from BF to BF+ND
%
BF=2;ND=2;PD=50;NP=ND*PD+1;Fr=logspace(BF,BF+ND,NP);
for i=1:NP
   s=2*pi*Fr(i)*j; % j = sqrt(-1)
   stm=(s*I-A)\B;
   v2=abs((D+G*A)*stm+E+G*B);Vo(i)=20*log10(v2);
   asym(i)=60*log10(Fr(i)/1000); % +60 dB/decade slope
end
%
h=plot(log10(Fr),Vo,'r',log10(Fr),asym,'k--');
set(h,'LineWidth',2);
grid on;
axis([2 4 -60 20]);
ylabel('dBV');title('Bessel HPF');
xlabel('Log Freq(Hz)');
legend('Vo','Asymptote');
figure(1);
%


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Celestial Computing with MATLAB

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

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


classdef DCF < dagnn.ElementWise
%DCF  layer
%   Discriminant Correlation Filters(DCF)
%
%   QiangWang, 2016
% -------------------------------------------------------------------------------------------------------------------------
    properties
        win_size = [3,3];
        sigma = 1;
    end
    properties (Transient)
        yf = [];
        lambda = 1e-4;
    end
    methods
        function outputs = forward(obj, inputs, params)

            xf = fft2(inputs{1});% target region
            zf = fft2(inputs{2});% search region
            xf_conj = conj(xf);
            [h,w,c,~] = size(xf);
            hwc = h*w*c;

            useGPU = isa(xf, 'gpuArray');
            if isempty(obj.yf)
                obj.initYF(useGPU);
            end

            kxxf = sum(xf .* xf_conj, 3) ./ hwc;
            alphaf = bsxfun(@rdivide, obj.yf, (kxxf + obj.lambda));
            kzxf = sum(zf .* xf_conj, 3) ./ hwc;
            outputs{1} = real(ifft2(alphaf .* kzxf));
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)

            dldrf = fft2(derOutputs{1});
            xf = fft2(inputs{1});% target region
            zf = fft2(inputs{2});% search region
            xf_conj = conj(xf);

            [h,w,c,~] = size(xf);
            hwc = h*w*c;

            kxxf = sum(xf .* xf_conj, 3) ./ hwc +obj.lambda;

            alphaf = bsxfun(@rdivide,obj.yf, kxxf);
            dldz = real(ifft2(bsxfun(@times,dldrf.*conj(alphaf),xf)))/hwc;
            kzxf = sum(zf .* xf_conj, 3) ./ hwc;
            dldx = real(ifft2(bsxfun(@times,conj(dldrf).*alphaf,zf)-...
            2*bsxfun(@times,xf,real(dldrf.*conj(alphaf.*kzxf)./kxxf))))/hwc;

            derInputs{1} = dldx;
            derInputs{2} = dldz;
            derParams = {};
        end

        function initYF(obj, useGPU)
            yf_ = single(fft2(gaussian_shaped_labels(obj.sigma, obj.win_size)));
            lambda_ = gather(obj.lambda);
            if useGPU
                obj.yf = gpuArray(yf_);
                obj.lambda = gpuArray(lambda_);
            else
                obj.yf = yf_;
                obj.lambda = lambda_;
            end
        end

        function obj = reset(obj)
            obj.yf = [] ;
            obj.lambda = 1e-4;
        end

        function obj = DCF(varargin)
            obj.load(varargin);
            obj.win_size = obj.win_size;
            obj.sigma = obj.sigma ;
        end
    end
end

function labels = gaussian_shaped_labels(sigma, sz)%kcf
[rs, cs] = ndgrid((1:sz(1)) - floor(sz(1)/2), (1:sz(2)) - floor(sz(2)/2));
labels = exp(-0.5 / sigma^2 * (rs.^2 + cs.^2));
labels = circshift(labels, -floor(sz(1:2) / 2) + 1);
assert(labels(1,1) == 1)
end


function [X1, Y1] = Direction2CubeMap(D, r, c)
%
%        [X1, Y1] = Direction2CubeMap(D, r, c)
%
%
%        Input:
%           -D: 3D directions of the img format
%        Output:
%           -X1: X coordinates in the CubeMap format
%           -Y1: Y coordinates in the CubeMap format
%
%     Copyright (C) 2011  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/>.
%

[rD, cD, ~] = size(D);

X1 = zeros(rD, cD);
Y1 = zeros(rD, cD);
totD = rD * cD;

face_cm = round((r / 4 + c / 3) / 2);

%Calculating faces' directions
Mul = [1, 1, 1, -1, -1, -1];

T = [2, 1, 3, 2, 1, 3];
A = [1, 3, 1, 1, 3, 1];
B = [3, 2, 2, 3, 2, 2];

X1_1 = [1.5,2.5, 1.5, 1.5, 0.5, 1.5];
X1_2 = [0.5,0.5, 0.5,-0.5, 0.5,-0.5];
Y1_1 = [2.5,1.5, 3.5, 0.5, 1.5, 1.5];
Y1_2 = [0.5,0.5,-0.5, 0.5,-0.5,-0.5];

for i=1:6
    indx = find(    (Mul(i) * D(:,:,T(i)) > 0)&...
                    (Mul(i) * D(:,:,T(i)) >= abs(D(:,:,A(i))))&...
                    (Mul(i) * D(:,:,T(i)) >= abs(D(:,:,B(i)))));

    indx_shifted = indx + (T(i) - 1) * totD;

    X1(indx) = X1_1(i) + X1_2(i) * D(indx + (A(i) - 1) * totD) ./ D(indx_shifted);
    Y1(indx) = Y1_1(i) + Y1_2(i) * D(indx + (B(i) - 1) * totD) ./ D(indx_shifted);
end

X1 = RemoveSpecials(X1) * face_cm;
Y1 = flipud(RemoveSpecials(Y1) * face_cm);

end


function value = mean(SO3F, varargin)
% Calculates the mean value of a SO3Fun or calculates the mean
% along a specified dimension of a vector-valued SO3Fun
%
% Syntax
%   value = mean(SO3F)
%   SO3F = mean(SO3F, d)
%
% Input
%  SO3F - @SO3Fun
%  d - dimension to take the mean value over
%
% Output
%  SO3F  - @SO3Fun
%  value - double
%
% Description
%
% If SO3F is a 3x3 SO3Fun then
% |mean(SO3F)| returns a 3x3 matrix with the mean values of each function
% |mean(SO3F, 1)| returns a 1x3 SO3Fun which contains the pointwise mean values along the first dimension
%
% Example
%   %generate SO3Funs
%   SO3F1 = SO3Fun.dubna
%   SO3F2 = SO3FunHandle(@(rot) SO3F1.eval(rot))
%   A = ones(2,3);
%   SO3F3 = SO3F2.*A
%
%   %calculate mean values of SO3Funs
%   mean(SO3F2)
%   mean(SO3F3)
%   mean(SO3F3,2)
%   mean(SO3F3,1)
%

% mean along specific dimension
if nargin>1 && isnumeric(varargin{1})
  value = SO3FunHandle(@(rot) mean(SO3F.eval(rot),varargin{1}+1),SO3F.SRight,SO3F.SLeft);
  return
end

% mean value of a SO3Fun
res = get_option(varargin,'resolution',2.5*degree);
nodes = equispacedSO3Grid(SO3F.SRight,SO3F.SLeft,'resolution',res);
value = mean(SO3F.eval(nodes(:)));
value = reshape(value,size(SO3F));
if isalmostreal(value,'componentwise')
  value = real(value);
end

end


function points3d = mytriangualation(matchedPoints1,matchedPoints2,cam1, cam2)
points2d(:,:,1)=matchedPoints1;
points2d(:,:,2)=matchedPoints2;
cam(:,:,1)=cam1;
cam(:,:,2)=cam2;
nPoints = size(points2d, 1);
points3d = zeros(nPoints, 3, 'like', points2d);

for i = 1:nPoints
    pairs=squeeze(points2d(i, :, :))';
    A = zeros(4, 4);
    for j = 1:2
        P = cam(:,:,j)';
        A(2*j-1,:)=pairs(j, 1)*P(3,:)-P(1,:);
        A(2*j,:)=pairs(j, 2)*P(3,:)-P(2,:);
    end
    [~,~,V] = svd(A);
    X = V(:, end);
    X = X/X(end);
    points3d(i, :) = X(1:3)';
end