function [V,nr,nre]=lcon2vert(A,b,Aeq,beq,TOL,checkbounds)
%An extension of Michael Kleder's con2vert function, used for finding the
%vertices of a bounded polyhedron in R^n, given its representation as a set
%of linear constraints. This wrapper extends the capabilities of con2vert to
%also handle cases where the  polyhedron has zero volume in R^n, i.e., where the
%polyhedron is defined by both equality and inequality constraints.
%
%SYNTAX:
%
%  [V,nr,nre]=lcon2vert(A,b,Aeq,beq,TOL)
%
%The rows of the N x n matrix V are a series of N vertices of the polyhedron
%in R^n, defined by the linear constraints
%
%   A*x  <= b
%   Aeq*x = beq
%
%By default, Aeq=beq=[], implying no equality constraints. The output "nr"
%lists non-redundant inequality constraints, and "nre" lists non-redundant
%equality constraints.
%
%The optional TOL argument is a tolerance used for both rank-estimation and
%for testing feasibility of the equality constraints. Default=1e-10.
%The default can also be obtained by passing TOL=[];
%
%NOTE: It is important that the region specified by the inequality system A*x<=b
%have non-zero volume in R^n. For example, A=b=[1;-1] is not legal input data,
%because the only solution to A*x<=b is x=1, which has zero volume in R^1. The
%proper way to express a zero-volume region is with the addition of
%equality constraint data, as for example Aeq=1, beq=1, A=1,b=100.
%
%EXAMPLE:
%
%The 3D region defined by x+y+z=1, x>=0, y>=0, z>=0
%is described by the following constraint data.
%
%
%     A =
%
%         0.4082   -0.8165    0.4082
%         0.4082    0.4082   -0.8165
%        -0.8165    0.4082    0.4082
%
%
%     b =
%
%         0.4082
%         0.4082
%         0.4082
%
%
%     Aeq =
%
%         0.5774    0.5774    0.5774
%
%
%     beq =
%
%         0.5774
%
%
%  >> V=lcon2vert(A,b,Aeq,beq)
%
%         V =
%
%             1.0000    0.0000    0.0000
%             0.0000    0.0000    1.0000
%            -0.0000    1.0000    0.0000
%
%

  %%initial argument parsing

  nre=[];
  nr=[];
  if nargin<5 || isempty(TOL), TOL=1e-10; end
  if nargin<6, checkbounds=true; end

  switch nargin

      case 0

           error 'At least 1 input argument required'

      case 1

         b=[]; Aeq=[]; beq=[];

      case 2

          Aeq=[]; beq=[];

      case 3

          beq=[];
          error 'Since argument Aeq specified, beq must also be specified'

  end

  b=b(:); beq=beq(:);

  if xor(isempty(A), isempty(b))
     error 'Since argument A specified, b must also be specified'
  end

  if xor(isempty(Aeq), isempty(beq))
        error 'Since argument Aeq specified, beq must also be specified'
  end

  nn=max(size(A,2)*~isempty(A),size(Aeq,2)*~isempty(Aeq));

  if ~isempty(A) && ~isempty(Aeq) && ( size(A,2)~=nn || size(Aeq,2)~=nn)

      error 'A and Aeq must have the same number of columns if both non-empty'

  end

  inequalityConstrained=~isempty(A);
  equalityConstrained=~isempty(Aeq);

 [A,b]=rownormalize(A,b);
 [Aeq,beq]=rownormalize(Aeq,beq);

  if equalityConstrained && nargout>2

        [discard,nre]=lindep([Aeq,beq].',TOL);  %#ok<ASGLU>

        if ~isempty(nre) %reduce the equality constraints

            Aeq=Aeq(nre,:);
            beq=beq(nre);

        else
            equalityConstrained=false;
        end

   end

   %%Find 1 solution to equality constraints within tolerance

   if equalityConstrained

       Neq=null(Aeq);

       x0=pinv(Aeq)*beq;

       if norm(Aeq*x0-beq)>TOL*norm(beq)  %infeasible

          nre=[]; nr=[]; %All constraints redundant for empty polytopes
          V=[];
          return;

       elseif isempty(Neq)

           if inequalityConstrained && ~all(A*x0<=b)

              nre=[]; nr=[]; %All constraints redundant for empty polytopes
              V=[];
              return;

           else %inequality constraints all satisfied, including vacuously

               V=x0(:).';
               nre=(1:nn).'; %Equality constraints determine everything.
               nr=[];%All inequality constraints are therefore redundant.
               return

           end

       end

       %rkAeq= nn - size(Neq,2);

  end

    %%
  if inequalityConstrained && equalityConstrained

   AAA=A*Neq;
   bbb=b-A*x0;

  elseif inequalityConstrained

    AAA=A;
    bbb=b;

  elseif equalityConstrained && ~inequalityConstrained

       error('Non-bounding constraints detected. (Consider box constraints on variables.)')

  end

  nnn=size(AAA,2);

  if nnn==1 %Special case

     idxu=sign(AAA)==1;
     idxl=sign(AAA)==-1;
     idx0=sign(AAA)==0;

     Q=bbb./AAA;
     U=Q;
       U(~idxu)=inf;
     L=Q;
       L(~idxl)=-inf;

     [ub,uloc]=min(U);
     [lb,lloc]=max(L);

     if ~all(bbb(idx0)>=0) || ub<lb %infeasible

         V=[]; nr=[]; nre=[];
         return

     elseif ~isfinite(ub) || ~isfinite(lb)

         error('Non-bounding constraints detected. (Consider box constraints on variables.)')

     end

     Zt=[lb;ub];

     if nargout>1
        nr=unique([lloc,uloc]); nr=nr(:);
     end

  else

          if nargout>1
           [Zt,nr]=con2vert(AAA,bbb,TOL,checkbounds);
          else
            Zt=con2vert(AAA,bbb,TOL,checkbounds);
          end

  end

  if equalityConstrained && ~isempty(Zt)

      V=bsxfun(@plus,Zt*Neq.',x0(:).');

  else

      V=Zt;

  end

  if isempty(V)
     nr=[]; nre=[];
  end

 function [V,nr] = con2vert(A,b,TOL,checkbounds)
% CON2VERT - convert a convex set of constraint inequalities into the set
%            of vertices at the intersections of those inequalities;i.e.,
%            solve the "vertex enumeration" problem. Additionally,
%            identify redundant entries in the list of inequalities.
%
% V = con2vert(A,b)
% [V,nr] = con2vert(A,b)
%
% Converts the polytope (convex polygon, polyhedron, etc.) defined by the
% system of inequalities A*x <= b into a list of vertices V. Each ROW
% of V is a vertex. For n variables:
% A = m x n matrix, where m >= n (m constraints, n variables)
% b = m x 1 vector (m constraints)
% V = p x n matrix (p vertices, n variables)
% nr = list of the rows in A which are NOT redundant constraints
%
% NOTES: (1) This program employs a primal-dual polytope method.
%        (2) In dimensions higher than 2, redundant vertices can
%            appear using this method. This program detects redundancies
%            at up to 6 digits of precision, then returns the
%            unique vertices.
%        (3) Non-bounding constraints give erroneous results; therefore,
%            the program detects non-bounding constraints and returns
%            an error. You may wish to implement large "box" constraints
%            on your variables if you need to induce bounding. For example,
%            if x is a person's height in feet, the box constraint
%            -1 <= x <= 1000 would be a reasonable choice to induce
%            boundedness, since no possible solution for x would be
%            prohibited by the bounding box.
%        (4) This program requires that the feasible region have some
%            finite extent in all dimensions. For example, the feasible
%            region cannot be a line segment in 2-D space, or a plane
%            in 3-D space.
%        (5) At least two dimensions are required.
%        (6) See companion function VERT2CON.
%        (7) ver 1.0: initial version, June 2005
%        (8) ver 1.1: enhanced redundancy checks, July 2005
%        (9) Written by Michael Kleder
%
%Modified by Matt Jacobson - March 30, 2011
%

   %%%3/4/2012 Improved boundedness test - unfortunately slower than Michael Kleder's
   if checkbounds

    [aa,bb,aaeq,bbeq]=vert2lcon(A,TOL);

    if any(bb<=0) || ~isempty(bbeq)
        error('Non-bounding constraints detected. (Consider box constraints on variables.)')
    end

    clear aa bb aaeq bbeq

   end

   dim=size(A,2);

   %%%Matt J initialization
   if strictinpoly(b,TOL)

       c=zeros(dim,1);

   else

            slackfun=@(c)b-A*c;

            %Initializer0
            c = pinv(A)*b; %02/17/2012 -replaced with pinv()
            s=slackfun(c);

            if ~approxinpoly(s,TOL) %Initializer1

                c=Initializer1(TOL,A,b,c);
                s=slackfun(c);

            end

            if  ~approxinpoly(s,TOL)  %Attempt refinement

                %disp 'It is unusually difficult to find an interior point of your polytope. This may take some time... '
                %disp ' '

                c=Initializer2(TOL,A,b,c);
                %[c,fval]=Initializer1(TOL,A,b,c,10000);
                s=slackfun(c);

            end

            if ~approxinpoly(s,TOL)
                    %error('Unable to locate a point near the interior of the feasible region.')
                    V=[];
                    nr=[];
                    return
            end

           if ~strictinpoly(s,TOL) %Added 02/17/2012 to handle initializers too close to polytope surface

                %disp 'Recursing...'

                idx=(  abs(s)<=max(s)*TOL );

                Amod=A; bmod=b;
                 Amod(idx,:)=[];
                 bmod(idx)=[];

                Aeq=A(idx,:); %pick the nearest face to c
                beq=b(idx);

                faceVertices=lcon2vert(Amod,bmod,Aeq,beq,TOL,1);
                if isempty(faceVertices)
                   disp 'Something''s wrong. Couldn''t find face vertices. Possibly polyhedron is unbounded.'
                   keyboard
                end

                c=faceVertices(1,:).';  %Take any vertex - find local recession cone vector
                s=slackfun(c);

                idx=(  abs(s)<=max(s)*TOL );

                Asub=A(idx,:); bsub=b(idx,:);

                [aa,bb,aaeq,bbeq]=vert2lcon(Asub);
                aa=[aa;aaeq;-aaeq];
                bb=[bb;bbeq;-bbeq];

                clear aaeq bbeq

                [bmin,idx]=min(bb);

                 if bmin>=-TOL
                   disp 'Something''s wrong. We should have found a recession vector (bb<0).'
                   keyboard
                 end

                Aeq2=null(aa(idx,:)).';
                beq2=Aeq2*c;  %find intersection of polytope with line through facet centroid.

                linetips = lcon2vert(A,b,Aeq2,beq2,TOL,1);

                if size(linetips,1)<2
                   disp 'Failed to identify line segment through interior.'
                   disp 'Possibly {x: Aeq*x=beq} has weak intersection with interior({x: Ax<=b}).'
                   keyboard
                end

                lineCentroid=mean(linetips);%Relies on boundedness

                clear aa bb

                c=lineCentroid(:);
                s=slackfun(c);

            end

            b = s;
   end
    %%%end Matt J initialization

    D=bsxfun(@rdivide,A,b);

    k = convhulln(D);
    nr = unique(k(:));

    G  = zeros(size(k,1),dim);
    ee=ones(size(k,2),1);
    discard=false( 1, size(k,1) );

    for ix = 1:size(k,1) %02/17/2012 - modified

        F = D(k(ix,:),:);
        if lindep(F,TOL)<dim;
            discard(ix)=1;
            continue;
        end

        G(ix,:)=F\ee;

    end

    G(discard,:)=[];

    V = bsxfun(@plus, G, c.');

    [discard,I]=unique( round(V*1e6),'rows');
    V=V(I,:);

return

function [c,fval]=Initializer1(TOL, A,b,c,maxIter)

    thresh=-10*max(eps(b));

    if nargin>4
     [c,fval]=fminsearch(@(x) max([thresh;A*x-b]), c,optimset('MaxIter',maxIter));
    else
     [c,fval]=fminsearch(@(x) max([thresh;A*x-b]), c);
    end

return

function c=Initializer2(TOL,A,b,c)
 %norm(  (I-A*pinv(A))*(s-b) )  subj. to s>=0

    maxIter=100000;

    [mm,nn]=size(A);

     Ap=pinv(A);
     Aaug=speye(mm)-A*Ap;
     Aaugt=Aaug.';

    M=Aaugt*Aaug;
    C=sum(abs(M),2);
     C(C<=0)=min(C(C>0));

    slack=b-A*c;
    slack(slack<0)=0;

        %     relto=norm(b);
        %     relto =relto + (relto==0);
        %
        %      relres=norm(A*c-b)/relto;

    IterThresh=maxIter;
    s=slack;
    ii=0;
    %for ii=1:maxIter
    while ii<=2*maxIter %HARDCODE

       ii=ii+1;
       if ii>IterThresh,
           %warning 'This is taking a lot of iterations'
           IterThresh=IterThresh+maxIter;
       end

     s=s-Aaugt*(Aaug*(s-b))./C;
     s(s<0)=0;

       c=Ap*(b-s);
       %slack=b-A*c;
       %relres=norm(slack)/relto;
       %if all(0<slack,1)||relres<1e-6||ii==maxIter, break;  end

    end

return

function [r,idx,Xsub]=lindep(X,tol)
%Extract a linearly independent set of columns of a given matrix X
%
%    [r,idx,Xsub]=lindep(X)
%
%in:
%
%  X: The given input matrix
%  tol: A rank estimation tolerance. Default=1e-10
%
%out:
%
% r: rank estimate
% idx:  Indices (into X) of linearly independent columns
% Xsub: Extracted linearly independent columns of X

   if ~nnz(X) %X has no non-zeros and hence no independent columns

       Xsub=[]; idx=[];
       return
   end

   if nargin<2, tol=1e-10; end

     [Q, R, E] = qr(X,0);

     diagr = abs(diag(R));

     %Rank estimation
     r = find(diagr >= tol*diagr(1), 1, 'last'); %rank estimation

     if nargout>1
      idx=sort(E(1:r));
        idx=idx(:);
     end

     if nargout>2
      Xsub=X(:,idx);
     end

 function [A,b]=rownormalize(A,b)
 %Modifies A,b data pair so that norm of rows of A is either 0 or 1

  if isempty(A), return; end

  normsA=sqrt(sum(A.^2,2));
  idx=normsA>0;
  A(idx,:)=bsxfun(@rdivide,A(idx,:),normsA(idx));
  b(idx)=b(idx)./normsA(idx);

 function tf=approxinpoly(s,TOL)

   smax=max(s);

   if smax<=0
      tf=false; return
   end

   tf=all(s>=-smax*TOL);

  function tf=strictinpoly(s,TOL)

   smax=max(s);

   if smax<=0
      tf=false; return
   end

   tf=all(s>=smax*TOL);


% qqdiagram() - Empirical quantile-quantile diagram.
%
% Description:
%               The quantiles (percentiles) of the input distribution Y are plotted (Y-axis)
%               against the corresponding quantiles of the input distribution X.
%               If only X is given, the corresponding quantiles are plotted (Y-axis)
%               against the quantiles of a Gaussian distribution ('Normal plot').
%               Two black dots indicate the lower and upper quartiles.
%               If the data in X and Y belong the same distribution the plot will be linear.
%               In this case,the red and black reference lines (.-.-.-.-) will overlap.
%               This will be true also if the data in X and Y belong to two distributions with
%               the same shape, one distribution being rescaled and shifted with respect to the
%               other.
%               If only X is given, a line is plotted to indicate the mean of X, and a segment
%               is plotted to indicate the standard deviation of X. If the data in X are normally
%               distributed, the red and black reference lines (.-.-.-.-) will overlap.
%
% Usage:
%   >>  ah  =  qqdiagram( x, y, pk );
%
% Inputs:
%   x       - vector of observations
%
% Optional inputs:
%   y       - second vector of observation to compare the first to
%   pk      - the empirical quantiles will be estimated at the values in pk [0..1]
%
% Author: Luca Finelli, CNL / Salk Institute - SCCN, 20 August 2002
%
% Reference: Stahel W., Statistische Datenanalyse, Vieweg, Braunschweig/Wiesbaden, 1995
%
% See also:
%   quantile(), signalstat(), eeglab()

% Copyright (C) 2002 Luca Finelli, Salk/SCCN, La Jolla, CA
%
% Reference: Stahel, W. Statistische Datenanalyse, Vieweg, Braunschweig/Wiesbaden 1995
%
% 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 qqdiagram( x , y, pk )

if nargin < 1
	help qqdiagram;
	return;
end;

if (nargin == 3 & (any(pk > 1) | any(pk < 0)))
    error('qqdiagram(): elements in pk must be between 0 and 1');
end

if nargin==1
	y=x;
	nn=max(1000,10*length(y))+1;
	x=randn(1,nn);
end

if nargin < 3
	nx=sum(~isnan(x));
	ny=sum(~isnan(y));
   	k=min(nx,ny);
    pk=((1:k) - 0.5) ./ k;  % values to estimate the empirical quantiles at
else
    k=length(pk);
end

if nx==k
    xx=sort(x(~isnan(x)));
else
    xx=quantile(x(~isnan(x)),pk);
end

if ny==k
    yy=sort(y(~isnan(y)));
else
    yy=quantile(y(~isnan(y)),pk);
end

% QQ diagram
plot(xx,yy,'+')
hold on

% x-axis range
maxx=max(xx);
minx=min(xx);
rangex=maxx-minx;
xmin=minx-rangex/50;
xmax=maxx+rangex/50;

% Quartiles
xqrt1=quantile(x,0.25); xqrt3=quantile(x,0.75);
yqrt1=quantile(y,0.25); yqrt3=quantile(y,0.75);

plot([xqrt1 xqrt3],[yqrt1 yqrt3],'k-','LineWidth',2); % IQR range

% Drawing the line
sigma=(yqrt3-yqrt1)/(xqrt3-xqrt1);
cy=(yqrt1 + yqrt3)/2;

if nargin ==1
    maxy=max(y);
    miny=min(y);
    rangey=maxy-miny;
	ymin=miny-rangey/50;
	ymax=maxy+rangey/50;

	plot([(miny-cy)/sigma (maxy-cy)/sigma],[miny maxy],'r-.') % the line
    % For normally distributed data, the slope of the plot line
    % is equal to the ratio of the standard deviation of the distributions
	plot([0 (maxy-mean(y))/std(y)],[mean(y) maxy],'k-.') % the ideal line

	xlim=get(gca,'XLim');
	plot([1 1],[ymin  mean(y)+std(y)],'k--')
	plot([1 1],[mean(y)  mean(y)+std(y)],'k-','LineWidth',2)
        % textx = 1.0;
        % texty = mean(y)+3.0*rangey/50.0;
	% text(double(textx), double(texty),' St. Dev.','horizontalalignment','center')
    set(gca,'xtick',get(gca,'xtick'));  % show that vertical line is at 1 sd
	plot([0 0],[ymin  mean(y)],'k--')
	plot(xlim,[mean(y) mean(y)],'k--')
	% text(double(xlim(1)), double(mean(y)+rangey/50),'Mean X')
	plot([xqrt1  xqrt3],[yqrt1 yqrt3],'k.','MarkerSize',10)
	set(gca,'XLim',[xmin xmax],'YLim',[ymin ymax])
	xlabel('Standard Normal Quantiles')
	ylabel('X Quantiles')
else
    cx=(xqrt1 + xqrt3)/2;
    maxy=cy+sigma*(max(x)-cx);
	miny=cy-sigma*(cx-min(x));

	plot([min(x) max(x)],[miny maxy],'r-.'); % the line
    xlabel('X Quantiles');
    ylabel('Y Quantiles');
end


function S = prolateSurfaceArea(elli, varargin)
%PROLATESURFACEAREA  Approximated surface area of a prolate ellipsoid.
%
%   S = prolateSurfaceArea(R1,R2)
%
%   Example
%   prolateSurfaceArea
%
%   See also
%   geom3d, ellipsoidSurfaceArea, oblateSurfaceArea
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2015-07-03,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2015 INRA - Cepia Software Platform.

%% Parse input argument

if size(elli, 2) == 7
    R1 = elli(:, 4);
    R2 = elli(:, 5);

elseif size(elli, 2) == 1 && ~isempty(varargin)
    R1 = elli(:, 1);
    R2 = varargin{1};
end

assert(R1 > R2, 'first radius must be larger than second radius');

% surface theorique d'un ellipsoide prolate
% cf http://fr.wikipedia.org/wiki/Ellipso%C3%AFde_de_r%C3%A9volution
e = sqrt(R1.^2 - R2.^2) ./ R1;
S = 2 * pi * R2.^2 + 2 * pi * R1 .* R2 .* asin(e) ./ e;


function SDP = chordal_relax_point_cloud_registration(problem)
%% Apply a chordal sparse second-order relaxation to point cloud registration
%% Generate SDP relaxations with multiple smaller blocks
%% This relaxation is likely to be looser than the single block relaxation
%% Depending on multivariate polynomial package SPOT
%% Heng Yang, June 29, 2021

fprintf('\n===================================================================')
fprintf('\nApplying Chordal SDP relaxation to point cloud registration')
fprintf('\n===================================================================\n')
t0              = tic;

%% define POP variables
N               = problem.N;
cloudA          = problem.cloudA;
cloudB          = problem.cloudB;
noiseBoundSq    = problem.noiseBoundSq;
tBound          = problem.translationBound;
tBoundSq        = tBound^2;
barc2           = 1.0;

nrPrimalVars    = 9 + 3 + N;
p               = msspoly('p',nrPrimalVars);
r               = p(1:9);
R               = reshape(r,3,3);
col1            = r(1:3);
col2            = r(4:6);
col3            = r(7:9);
t               = p(10:12);
theta           = p(13:nrPrimalVars);
x               = [r;t];

%% compute the cost function
residuals = [];
for i = 1:N
    ai               = cloudA(:,i);
    bi               = cloudB(:,i);
    res              = bi'*bi + ai'*ai + (t'*t) - 2*(bi'*(R*ai)) - (2)*(bi'*t) + (2)*(t'*(R*ai));
    residuals        = [residuals; res / noiseBoundSq];
end
f_cost = [];
for i = 1:N
    f_cost           = [f_cost; (1+theta(i))/2 * residuals(i) + (1-theta(i))/2 * barc2];
end

%% Define the equality and inequality constraints
h_x = [1.0-col1'*col1;...
       1.0-col2'*col2;...
       1.0-col3'*col3;... % columns unit length
       col1'*col2;...
       col2'*col3;...
       col3'*col1;... % colums orthogonal
       cross(col1,col2) - col3;...
       cross(col2,col3) - col1;...
       cross(col3,col1) - col2]; % columns righthandedness

h_theta = [];
for i = 1:N
    h_theta =[h_theta; 1-theta(i)^2];
end

g_x = tBoundSq - t'*t; % Translation bounded

%% Formulate the chordal sparse second-order relaxation
%% the 0-th block [1;x] * [1;x]'
basis0          = [1;x];
n0              = length(basis0);
basis_x0        = 1;

pop0            = [mykron(basis_x0,h_x);...
                   mykron(basis0,basis0)];
[~,degmat,coef_all] = decomp(pop0);
coef_all            = coef_all';
dim_loc0        = length(basis_x0) * length(h_x);
n0delta         = triangle_number(n0);
nterms          = size(degmat,1);
m_mom0          = n0delta - nterms;

assert(m_mom0==0,'The zero-th blk should have 0 moment constraints.')

coef_mom    = coef_all(:,dim_loc0+1:end);
coef_mom    = coef_mom';
B           = {};
B_normalize = {};

for i = 1:nterms
    [row,~,~]   = find(coef_mom(:,i));
    SDP_coli    = floor((row-1)./n0) + 1;
    SDP_rowi    = mod(row-1,n0) + 1;
    nnz         = length(SDP_rowi);

    Bi          = sparse(SDP_rowi,SDP_coli,ones(nnz,1),n0,n0);
    B{end+1}    = Bi;
    B_normalize{end+1} = Bi/nnz;
end

coef_loc0       = coef_all(:,1:dim_loc0);
A0_local        = {};

for i = 1:dim_loc0
    [rowi,~,vi] = find(coef_loc0(:,i));
    Ai      = sparse(n0,n0);
    for j   = 1:length(rowi)
        Ai  = Ai + vi(j) * B_normalize{rowi(j)};
    end
    A0_local = [A0_local;{Ai}];
end
A0_0    = sparse([1],[1],[1],n0,n0);
% The first block satisfies A0(X0) = b0;
A0      = [{A0_0};A0_local];
b0      = sparse(1,1,1,length(A0),1);

%% the 1-N blocks [1;x;theta(i);theta(i)*x]' * [1;x;theta(i);theta(i)*x]
%% Since there is an inequality constraint too, it will generate 2*N blocks
Aall            = {};
A1all           = {};
ball            = [];
Call            = {};
A0append        = {};
for blkidx = 1:N
    basis       = [1;x;theta(blkidx);theta(blkidx)*x];
    n           = length(basis);
    basis_x     = monomials(theta(blkidx),1:2);
    basis_theta = monomials(x,0:2);
    basis_g     = [1;theta(blkidx)];

    out         = gen_chordal_subblk_pcr(...
                    basis,basis_x,h_x,basis_theta,h_theta(blkidx),g_x,basis_g,f_cost(blkidx));
    Acell       = out.A;
    A1          = out.A1;
    b           = out.b;
    C           = out.C;

    % add constraint that the top-left [1;x]*[1;x]' block is the same as
    % the 0-th block
    A0blk = {};
    for i = 1:n0
        for j = i:n0
            if i == j
                A0i  = sparse(i,j,-1,n0,n0);
                Ai   = sparse(i,j,1,n,n);
            else
                A0i  = sparse([i,j],[j,i],[-0.5,-0.5],n0,n0);
                Ai   = sparse([i,j],[j,i],[0.5,0.5],n,n);
            end
            A0blk    = [A0blk;{A0i}];
            Acell    = [Acell;{Ai}];
        end
    end

    ball             = [ball;b;sparse(n0delta,1)];
    A0append{end+1}  = A0blk;
    Aall{end+1}      = Acell;
    Call             = [Call;C];
    A1all{end+1}     = A1;
end

%% Convert to standard SDPT3 format
b           = [b0;ball];
blk         = cell(2*N+1,2);
blk{1,1}    = 's'; blk{1,2} = n0;
n1          = out.blk{2,2};
n1delta     = triangle_number(n1);
for i = 1:N
    blk{2*i,1} = 's';
    blk{2*i,2} = n;
    blk{2*i+1,1} = 's';
    blk{2*i+1,2} = n1;
end

A0t     = sparsesvec(blk(1,:),A0);
for i = 1:N
    A0t = [A0t,...
           sparse(n0delta,out.m),...
           sparsesvec(blk(1,:),A0append{i})];
end
ndelta  = triangle_number(n);
At    = {A0t};
for i = 1:N
    Ait = [sparse(ndelta,length(b0)),...
           sparse(ndelta,(i-1)*length(Aall{i})),...
           sparsesvec(blk(2*i,:),Aall{i}),...
           sparse(ndelta,(N-i)*length(Aall{i}))];

    A1it = [sparse(n1delta,length(b0)),...
            sparse(n1delta,(i-1)*length(Aall{i})),...
            sparse(n1delta,out.m_mom+out.m_loc),...
            sparsesvec(blk(2*i+1,:),A1all{i}),...
            sparse(n1delta,n0delta),...
            sparse(n1delta,(N-i)*length(Aall{i}))];
    At  = [At;{Ait};{A1it}];
end

SDP.blk = blk;
SDP.At  = At;
SDP.n   = n;
SDP.m   = length(b);
SDP.C   = [{sparse(n0,n0)};Call];
SDP.b   = b;

tf    = toc(t0);
fprintf('\nDone in %g seconds.\n',tf);
fprintf('===================================================================\n')

%% Convert to sedumi
fprintf('Convert to sedumi')
t0 = tic;

sK.s  = [n0];
for i = 1:N
    sK.s        = [sK.s,n,n1];
end

A0t     = sparsevec(blk(1,:),A0);
n0sq    = n0^2;
for i = 1:N
    A0t = [A0t,...
           sparse(n0sq,out.m),...
           sparsevec(blk(1,:),A0append{i})];
end

nsq     = n^2;
n1sq    = n1^2;
At      = {A0t};
for i = 1:N
    Ait = [sparse(nsq,length(b0)),...
           sparse(nsq,(i-1)*length(Aall{i})),...
           sparsevec(blk(2*i,:),Aall{i}),...
           sparse(nsq,(N-i)*length(Aall{i}))];

    A1it = [sparse(n1sq,length(b0)),...
            sparse(n1sq,(i-1)*length(Aall{i})),...
            sparse(n1sq,out.m_mom+out.m_loc),...
            sparsevec(blk(2*i+1,:),A1all{i}),...
            sparse(n1sq,n0delta),...
            sparse(n1sq,(N-i)*length(Aall{i}))];
    At  = [At;{Ait};{A1it}];
end

sdata.K     = sK;
sdata.At    = cat(1,At{:});
sdata.b     = b;

sc          = [];
for i = 1:length(SDP.C)
    sc      = [sc;sparsevec(blk(i,:),SDP.C(i))];
end
sdata.c     = sc;

SDP.sedumi   = sdata;

tf    = toc(t0);
fprintf('\nDone in %g seconds.\n',tf);
fprintf('===================================================================\n')

end


% ANGLE_ERROR
% VARGPLVM

function [m,m_vec] = angle_error(Y1,Y2)

% usage [m,e] = angle_error(Y1,Y2);
% where Y1 and Y2 are matrices of same size
%
% e is a 54 vector corr to rms error in each angle across all data points
% m is the mean of e
%
% ERRORS GIVEN ARE IN DEGREES

[d,n] = size(Y1);

mat_error = acos(cos((pi/180)*(Y1-Y2)));
mat_error = (180/pi)*mat_error;
% cos and acos to handle wrap around effects in angle space

sq_mat_error = mat_error.*mat_error;

m_vec = zeros(d,1);

%m_vec = mean(mat_error,2);
%L1 norm
m_vec = sqrt(mean(sq_mat_error,2));

m = mean(m_vec);


function [result coeff] = RS_PF_OLS_Wald(y,x1,x2)
% Calculates OLS Wald-Test
% H0: beta=0 in the model
% y = x1*beta+x2*gamma+eps with HAC consistent var-covar
% If want to test all coefficients, simply use (y,x1) and do not include x2

q = 0;
n = size(y,1);
p = size(x1,2);

if nargin>2;
    q = size(x2,2);
    x = [x1,x2];
    R = [eye(p),zeros(p,q)];
else
    x = x1;
    R = eye(p);
end;

coeff = ((inv(x'*x))*(x'*y));
nlag  = round(n^(1/4));

% Compute Newey-West adjusted heteroscedastic-serial consistent
% least-squares regression
nwresult   = bear.RS_PF_nwest(y,x,nlag);
varbetahat = nwresult.vcv;

result = (R*coeff)'/(R*varbetahat*R')*R*coeff;


function this = hs_optical_flow(varargin)
%
%HS_OPTICAL_FLOW   Optical flow computation with Horn & Schunck method
%       B.K.P. Horn and B.G. Schunck. Determining optical flow.
%       Artificial Intelligence, 16:185-203, Aug. 1981.
%       http://people.csail.mit.edu/bkph/papers/Optical_Flow_OPT.pdf
%
%   HS_OPTICAL_FLOW([IMGS]) constructs a HS optical flow object
%   with the optional image sequence IMGS ([n x m x 2] array).
%   HS_OPTICAL_FLOW(O) constructs HS optical flow object by copying O.
%
%   This is a member function of the class 'hs_optical_flow'.

%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2007-10-30 $
%   $Revision: $
%
% Copyright 2007-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

error(nargchk(0, 1, length(varargin)));

  switch (length(varargin))
    case 0

      this.images               = [];
      this.lambda               = 80;   %5e-2;
      this.lambda_q             = 80;   %not used, for consistency with other program
      this.gnc_iters            = 1;    %not used, for consistency with other program
      this.pyramid_levels       = 4; %

      this.pyramid_spacing      = 2; %         % default to the prolongation/restriction filter

      this.max_warping_iters    = 10;           % # of warping/linearization per pyramid level
      this.median_filter_size   = [];
      this.texture              = false;        % apply to the image pyramid;

      this.solver               = 'backslash';  % pcg, sor
%       this.solver               =  'sor';      % MAYANK's.
      this.sor_max_iters        = 1e1;          % 100 seems sufficient
      this.interpolation_method = 'cubic';      % 'bi-cubic', 'cubic', 'bi-linear'
      this.deriv_filter         = [1 -8 0 8 -1]/12;
      this.display              = false;
      this.limit_update         = true;
      this.sigmaD2              = 1;                 % data term
      this.sigmaS2              = 1;                 % spatial term

      this.sigmaP               = 1;                 % sigma of Gaussian for performing "texture" decomposition
      this.weightP              = 1;                 % images -weightPxGaussian*images

      this.mf_iter              = 1;

      method = 'quadratic';
      this.spatial_filters = {[1 -1], [1; -1]};
      for i = 1:length(this.spatial_filters);
          this.rho_spatial_u{i}   = robust_function(method, 1); % 0.1
          this.rho_spatial_v{i}   = robust_function(method, 1);
      end;
      this.rho_data        = robust_function(method, 1); % 6.3

      this.color_images     = [];

      this = class(this, 'hs_optical_flow');

    case 1
      if isa(varargin{1}, 'hs_optical_flow')
        this = other;

      else
          this = hs_optical_flow;
      end

    otherwise

      error('Incompatible arguments!');

  end


clc;
close all;
clearvars;
rng('default');

n = 8;
A1 = gallery('binomial',n);
A2 = A1;

r = 4;
Q_factors = zeros(n, r);
for i=1:r
    [v, beta] = spx.la.house.gen(A2(i:n, i));
    Q_factors(i+1:n, i) = v(2:n-i+1);
    A2(i:n, i:n) = spx.la.house.premul(A2(i:n, i:n), v, beta)
    v_full = [zeros(i-1, 1); v];
end
% We compute the factors W and Y for Q = Q_1 * Q_2 * .. * Q_r
% such that Q = I - WY'
[W, Y] = spx.la.house.wy(Q_factors);
% We pre-multiply with I - WY'
A3 = spx.la.house.wy_premul(A2, W, Y);
A1
A3
max(max(abs(A1 - A3)))


clear all

% Build mex function
mex -R2018a Wigner_d_fast.c

%% Compare different Wigner-d methods
N = 512
beta =rand*pi;

% past method
tic
a = Wigner_D(N,beta);
toc

% new faster method
tic
b = Wigner_d_fast(N,beta);
toc

% create cell array of all Wigner-ds with harmonic degree smaller then L
tic
c = Wigner_d_recursion(N,beta);
toc

% get new Wigner-d from previous two
tic
d = Wigner_d_recursion(c{end-1},c{end-2},beta);
toc

% get new Wigner-d from previous two (faster)
tic
e = Wigner_d_fast(c{end-1},c{end-2},beta);
toc

max(max(abs(a-b)))

max(max(abs(d-e)))


function [out] = baseflow_7(p1,p2,S,dt)
%baseflow_7

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Non-linear outflow from a reservoir
% Constraints:  f <= S/dt
%               S >= 0
% @(Inputs):    p1   - time coefficient [d-1]
%               p2   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               dt   - time step size [d]

out = min(S/dt,p1.*max(0,S).^p2);

end


% STE_WLP Short-time-energy-weighted linear prediction
%   [A,w] = ste_wlp(s,p,m,k)
%
% Description
%   This function fits linear prediction coefficients to the analysis
%   frame using the basic form of weighted linear prediction (WLP), i.e.,
%   by weighting each value of the squared prediction error by the short-time
%   energy (STE) of the previous samples.
%
% Inputs
%   s      : Speech signal frame [samples]
%   p      : Order of WLP analysis
%   m      : Length of the STE window (default to p)
%   k      : Delay of the STE window (default to 1)
%
% Outputs
%   A      : Linear prediction inverse filter coefficients
%   w      : the STE weighting function
%
% Notes
%   Even though coefficients are solved using a criterion similar to the
%   autocorrelation method of linear prediction, because of the weighting
%   the filter is not guaranteed to be stable. If a stable synthesis filter
%   is required, use STE_SWLP instead.
%
% Example
%   A = ste_wlp(s,p) gives the linear predictive inverse filter
%   coefficients optimized using STE-WLP
%
% References
%  [1] C. Ma, Y. Kamp and L. F. Willems, "Robust signal selection for
%  linear prediction analysis of voiced speech", Speech Communication, vol.
%  12, no. 1, pp. 6981, 1993.
%
% Copyright (c) 2013 Aalto University
%
% 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 Common Speech Processing Repository
% http://covarep.github.io/covarep/
%
% Octave Compatible
%
% Author
%  Jouni Pohjalainen jouni.pohjalainen@aalto.fi
%
% $Id <info set by the versioning system> $

function [A,w] = ste_wlp(s,p,m,k)

% handle the special case of all-zero frames
if all(s==0)
  s = eps*randn(size(s));
end

N = length(s);

% default value for the STE window length
if nargin<3
    m = p;
end

% default value for the STE window lag
if nargin<4
    k = 1;
end

% compute STE weighting function
w = stew(s,m,p,k)+eps;

% apply the square root of the weighting function to the delayed versions
% of the signal from lag 0 to lag p
wsr = sqrt(w(1:(N+p)));
Y = zeros(N+p,p+1);
for i1=0:p
    Y(:,i1+1) = [zeros(i1,1);s;zeros(p-i1,1)].*wsr;
end

% compute weighted autocorrelations
R = (Y'*Y)/N;

% solve the p predictor coefficients
A = R(2:end,2:end)\R(2:end,1);

% convert to inverse filter form
A = [1;-A]';

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

function w = stew(x,m,p,k)
% STE weighting for an order p predictor using window of length m delayed
% by k samples
w = conv([zeros(k,1);x(:)].^2,ones(m,1));
w = [w;zeros(p-m,1)];


function cc_levels_minmax_display ( )

%*****************************************************************************80
%
%% CC_LEVELS_MINMAX_DISPLAY displays grids generated by CC_LEVELS_MINMAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CC_LEVELS_MINMAX_DISPLAY:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Display the nested 2D Clenshaw-Curtis grids\n' );
  fprintf ( 1, '  generated by CC_LEVELS_MINMAX.\n' );

  dim_num = 2;

  while ( 1 )
%
%  Get user input.
%
    level_min = input ( 'Enter LEVEL_MIN or RETURN to exit;' );

    if ( isempty ( level_min ) )
      break
    end

    if ( level_min < 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  We require 0 <= LEVEL_MIN\n' );
      continue
    end

    level_max = input ( 'Enter LEVEL_MAX or RETURN to exit;' );

    if ( isempty ( level_max ) )
      break
    end

    if ( level_max < level_min )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  We require LEVEL_MIN <= LEVEL_MAX!\n' );
      continue
    end
%
%  Compute data.
%
    [ grid_num, point_num ] = cc_levels_minmax_size ( dim_num, ...
      level_min, level_max );

    [ grid_level, grid_order, grid_point ] = cc_levels_minmax ( dim_num, ...
      level_min, level_max,  grid_num, point_num );

    clf
%
%  We have to name the axes in order to control the grid.
%
    axes_handle = axes;
%
%  Plot the points.
%
    handle = scatter ( grid_point(1,:), grid_point(2,:), 'filled' );
%
%  Force the plotting region to be square, not rectangular.
%
    axis square
%
%  Request grid lines.
%
    grid on
%
%  Specify the location of the grid lines, and suppress labeling.
%
    set ( axes_handle, 'xtick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
    set ( axes_handle, 'xticklabel', [] );
    set ( axes_handle, 'ytick', [ -1, -.75, -.5, -.25, 0, .25, .50, .75, 1] );
    set ( axes_handle, 'yticklabel', [] );
%
%  Make the plotting region slightly bigger than the data.
%
    axis ( [ -1.1, 1.1, -1.1, 1.1 ] )
%
%  Title
%
    s = sprintf ( '%d <= LEVEL <= %d', level_min, level_max );
    title ( s );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CC_LEVELS_MINMAX_DISPLAY:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function count = compute_counts(data, sz)
% COMPUTE_COUNTS Count the number of times each combination of discrete assignments occurs
% count = compute_counts(data, sz)
%
% data(i,t) is the value of variable i in case t
% sz(i) : values for variable i are assumed to be in [1:sz(i)]
%
% Example: to compute a transition matrix for an HMM from a sequence of labeled states:
% transmat = mk_stochastic(compute_counts([seq(1:end-1); seq(2:end)], [nstates nstates]));

assert(length(sz) == size(data, 1));
P = prod(sz);
indices = subv2ind(sz, data'); % each row of data' is a case
%count = histc(indices, 1:P);
count = hist(indices, 1:P);
count = myreshape(count, sz);


function [x,t] = tfristft(tfr,t,h,trace);
%TFRISTFT Inverse Short time Fourier transform.
%	[X,T]=TFRSTFT(tfr,T,H,TRACE) computes the inverse short-time
%	Fourier transform of a discrete-time signal X. This function
%	may be used for time-frequency synthesis of signals.
%
%	X     : signal.
%	T     : time instant(s)          (default : 1:length(X)).
%	H     : frequency smoothing window, H being normalized so as to
%	        be  of unit energy.      (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR   : time-frequency decomposition (complex values). The
%	        frequency axis is graduated from -0.5 to 0.5.
%
%	Example :
%        t=200+(-128:127); sig=[fmconst(200,0.2);fmconst(200,0.4)];
%        h=hamming(57); tfr=tfrstft(sig,t,256,h,1);
%        sigsyn=tfristft(tfr,t,h,1);
%        plot(t,abs(sigsyn-sig(t)))
%
%	See also all the time-frequency representations listed in
%	the file CONTENTS (TFR*)

%	F. Auger, November 1996.
%
%  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin<3),
 error('At least 3 parameters required');
elseif (nargin==3),
 trace=0;
end;

[N,NbPoints]=size(tfr);
[trow,tcol] =size(t);
[hrow,hcol] =size(h); Lh=(hrow-1)/2;

if (trow~=1),
 error('T must only have one row');
elseif (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
elseif (tcol~=NbPoints)
 error('tfr should have as many columns as t has rows.');
end;

Deltat=t(2:tcol)-t(1:tcol-1);
Mini=min(Deltat); Maxi=max(Deltat);
if (Mini~=1) & (Maxi~=1),
 error('The tfr must be computed at each time sample.');
end;

h=h/norm(h);

if trace, disp('Inverse Short-time Fourier transform'); end;
tfr=ifft(tfr);

x=zeros(tcol,1);

for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 valuestj=max([1,icol-N/2,icol-Lh]):min([tcol,icol+N/2,icol+Lh]);
 for tj=valuestj,
  tau=icol-tj; indices= rem(N+tau,N)+1;
  % fprintf('%g %g %g\n',tj,tau,indices);
  x(icol,1)=x(icol,1)+tfr(indices,tj)*h(Lh+1+tau);
 end;
 x(icol,1)=x(icol,1)/sum(abs(h(Lh+1+icol-valuestj)).^2);
end;

if trace, fprintf('\n'); end;


function type = year_to_type_hebrew ( y )

%*****************************************************************************80
%
%% YEAR_TO_TYPE_HEBREW returns the type of a Hebrew year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 332.
%
%  Parameters:
%
%    Input, integer Y, the Hebrew year.
%    Nonpositive years are illegal input.
%
%    Output, integer TYPE, the year type.
%    1, Common, Deficient, 12 months, 353 days;
%    2, Common, Regular, 12 months, 354 days;
%    3, Common, Abundant, 12 months, 355 days;
%    4, Embolismic, Deficient, 13 months, 383 days;
%    5, Embolismic, Regular, 13 months, 384 days;
%    6, Embolismic, Abundant, 13 months, 385 days.
%
  if ( y <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YEAR_TO_TYPE_HEBREW - Fatal error!\n' );
    fprintf ( 1, '  Illegal input Y = 0.\n' );
    error ( 'YEAR_TO_TYPE_HEBREW - Fatal error!' );
  end

  jed = new_year_to_jed_hebrew ( y );

  jed2 = new_year_to_jed_hebrew ( y + 1 );

  year_length = round ( jed2 - jed );

       if ( year_length == 353 )
    type = 1;
  elseif ( year_length == 354 )
    type = 2;
  elseif ( year_length == 355 )
    type = 3;
  elseif ( year_length == 383 )
    type = 4;
  elseif ( year_length == 384 )
    type = 5;
  elseif ( year_length == 385 )
    type = 6;
  else
    type = 0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YEAR_TO_TYPE_HEBREW - Fatal error!\n' );
    fprintf ( 1, '  Computed an illegal type = %d\n', type );
    error ( 'YEAR_TO_TYPE_HEBREW - Fatal error!' );
  end

  return
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logistic regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell you
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

k = num_labels;
for i = 1:k

    % Set Initial theta
    initial_theta = zeros(n + 1, 1);

    % Set options for fminunc
    options = optimset('GradObj', 'on', 'MaxIter', 50);

    % Run fmincg to obtain the optimal theta
    % This function will return theta and the cost
    [initial_theta] = fmincg (@(t)(lrCostFunction(t, X, (y == i), lambda)),initial_theta, options);

    all_theta(i,:) = initial_theta';
end

% =========================================================================

end


% Generate weight matrix for the task of quadratic matching
function  [W, acceptedPairs] = generateWeightMatrix(M, B, config)

    N = size(M, 2);
    W = -10000*ones(N*N, N*N);
    acceptedPairs = 0;

    % Loop through all possible pairs
    for p=1:N
        for q=1:N
            for s=1:N
                for t=1:N
                    if (p~=s && q~=t)
                        i = p + (q-1)*N;
                        j = s + (t-1)*N;
                        dDiff = abs( distance(M,p,s) - distance(B,q,t));
                        psDist = distance(M, p,s);
                        qtDist = distance(B, q,t);

                        if dDiff <= config.dDiffThresh && psDist >= config.pairDistThresh && qtDist >= config.pairDistThresh
                            W (i,j) = exp(-dDiff);  % can also use 1./dDiff; %
                            acceptedPairs = acceptedPairs + 1;
                        end
                    end
                end
            end
        end
    end

end


function meanShiftImExplore( I, X, sigSpt, sigRng, show )
% Visualization to help choose sigmas for meanShiftIm.
%
% Displays the original image I, and prompts user to select a point on the
% image.  For given point, calculates the distance (both spatial and range)
% to every other point in the image.   It shows the results in a number of
% panes, which include 1) the original image I, 2) Srange - similarity
% based on range only, 3) Seuc - similarity based on Euclidean distance
% only, and 4) overall similarity.  Finally, in each image the green dot
% (possibly occluded) shows the original point, and the blue dot shows the
% new mean of the window after 1 step of meanShift.
%
% USAGE
%  meanShiftImExplore( I, X, sigSpt, sigRng, [show] )
%
% INPUTS
%  I       - MxN image for display
%  X       - MxNxP data array, P may be 1 (X may be same as I)
%  sigSpt  - integer specifying spatial standard deviation
%  sigRng  - value specifying the standard deviation of the range data
%  show    - [1] will display results in figure(show)
%
% OUTPUTS
%
% EXAMPLE
%  I=double(imread('cameraman.tif'))/255;
%  meanShiftImExplore( I, I, 5, .2, 1 );
%
% See also MEANSHIFTIM
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<5 ); show = 1; end
[mrows, ncols, p] = size(X);

%%% get input point
figure(show); clf; im(I);
[c,r] = ginput(1);
r=round(r); c=round(c);

%%% get D and S
[gridRs, gridCs] = ndgrid( 1:mrows, 1:ncols );
Deuc = ((gridRs-r).^2 + (gridCs-c).^2) / sigSpt^2;
x = X(r,c,:); x = x(:)';  Xflat = reshape(X,[],p);
Drange = pdist2( x, Xflat );
Drange = reshape( Drange, mrows, ncols ) / sigRng^2;
D = Drange + Deuc;

S = exp( -D );
Srange = exp( -Drange );
Seuc = exp( -Deuc );

%%% new c and r [stretched for display]
c2 = (gridCs .* S); c2 = sum( c2(:) ) / sum(S(:));
r2 = (gridRs .* S); r2 = sum( r2(:) ) / sum(S(:));
%c2 = c+(c2-c)*2; r2 = r+(r2-r)*2;

%%% show
figure(show); clf;
subplot(2,2,1); im(I);
hold('on'); plot( c, r, '.g' ); plot( c2, r2, '.b' ); hold('off');
subplot(2,2,2); im(Srange);
hold('on'); plot( c, r, '.g' ); plot( c2, r2, '.b' ); hold('off');
subplot(2,2,3); im(Seuc);
hold('on'); plot( c, r, '.g' ); plot( c2, r2, '.b' ); hold('off');
subplot(2,2,4); im(S);
hold('on'); plot( c, r, '.g' ); plot( c2, r2, '.b' ); hold('off');


%quad_stokes   set up Stokes problem in quadrilateral domain
%   IFISS scriptfile: DJS; 13 May 2006.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
clear
%% define geometry
pde=3; domain=8; enclosed=0;
quad_domain
load quad_grid
%
%% set up matrices
qmethod=default('Q1-Q1/Q1-P0/Q2-Q1/Q2-P1: 1/2/3/4? (default Q1-P0)',2);
qmethod=qmethod-1;
if qmethod==2,
   [x,y,xy,xyp,mp] = q2q1grid(x,y,xy,mv,bound);
   [A,B,Q,G,Bx,By,f,g] = stokes_q2q1(xy,xyp,mv,mp);
elseif qmethod==3,
   [x,y,xy,xyp] = q2p1grid(x,y,xy,mv,bound);
   [A,B,Q,G,Bx,By,f,g] = stokes_q2p1(xy,xyp,mv);
elseif qmethod==0
   [ev,ee,ebound,xyp] = q1q1grid(x,y,xy,mv,bound,mbound);
   [A,B,Q,C,G,Bx,By,f,g] = stokes_q1q1(xy,ev);
elseif qmethod==1
   [ev,ee,ebound,xyp] = q1p0grid(x,y,xy,mv,bound,mbound);
   [A,B,Q,C,G,Bx,By,f,g] = stokes_q1p0(xy,xyp,mv,ev);
end
gohome
cd datafiles
save square_stokes_nobc pde domain qmethod  A B Q f g xy xyp mbound bound x y
save square_stokes_nobc Bx By  -append
if qmethod==1
   save square_stokes_nobc C G ev ee ebound -append
elseif qmethod==0
   save square_stokes_nobc C G ev ee ebound mv enclosed -append
elseif qmethod==2
   save square_stokes_nobc G mv mp -append
else
   save square_stokes_nobc G mv  -append
end
fprintf('system matrices saved in square_stokes_nobc.mat ...\n')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Y, W, Thres, sigma] = TwoLayerAGH_Train(X, Anchor, r, s, sigma, options)
%
% Two-Layer Anchor Graph Hashing Training
% Written by Wei Liu (wliu@ee.columbia.edu)
% X = nXdim input data
% Anchor = mXdim anchor points (m<<n)
% r = number of hash bits
% s = number of nearest anchors
% sigma: Gaussian RBF kernel width parameter; if no input (sigma=0) this function will
%        self-estimate and return a value.
% Y = nXr binary codes (Y_ij in {1,0})
% W = mX(r/2) projection matrix in spectral space
% Thres = 2X(r/2) threshold matrix
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[n,dim] = size(X);
m = size(Anchor,1);

%% get Z
Z = zeros(n,m);
Dis = sqdist(X',Anchor');
% clear X;
% clear Anchor;

val = zeros(n,s);
pos = val;
for i = 1:s
    [val(:,i),pos(:,i)] = min(Dis,[],2);
    tep = (pos(:,i)-1)*n+[1:n]';
    Dis(tep) = 1e60;
end
clear Dis;
clear tep;

if sigma == 0
   sigma = mean(val(:,s).^0.5);
end

switch lower(options.CodingMethod)
    case {lower('Cosine')}
        if isfield(options,'bNormalized') && options.bNormalized
        else
            X = NormalizeFea(X);
        end
        Anchor = NormalizeFea(Anchor);
        S = full(X*Anchor');
        linidx = [1:size(pos,1)]';
        val = S(sub2ind(size(S),linidx(:,ones(1,size(pos,2))),pos));
    case {lower('Gaussian')}
        val = exp(-val/(1/1*sigma^2));
    otherwise
        error('method does not exist!');
end

val = repmat(sum(val,2).^-1,1,s).*val; %% normalize
tep = (pos-1)*n+repmat([1:n]',1,s);
Z([tep]) = [val];
Z = sparse(Z);
clear tep;
clear val;
clear pos;

%% compute eigensystem
lamda = sum(Z);
M = Z'*Z;
M = diag(lamda.^-0.5)*M*diag(lamda.^-0.5);
[W,V] = eig(full(M));
clear M;
eigenvalue = diag(V)';
clear V;
[eigenvalue,order] = sort(eigenvalue,'descend');
W = W(:,order);
clear order;
ind = find(eigenvalue>0 & eigenvalue<1-1e-3);
eigenvalue = eigenvalue(ind);
W = W(:,ind);
W = diag(lamda.^-0.5)*W(:,1:r/2)*diag(eigenvalue(1:r/2).^-0.5)*sqrt(n);
clear ind;
clear eigenvalue;
clear lambda;

%% get binary codes
Y = Z*W;
Y1 = Y;
Thres = zeros(2,r/2);
for k = 1:r/2
    pos = find(Y(:,k)>0);
    neg = setdiff([1:n],pos);
    n1 = length(pos);
    n2 = n-n1;
    get = Y(:,k);
    get(neg) = -1*get(neg);
    right = full(get'*Z);
    clear get;
    right = right./lamda;
    left = sum(Z(pos,:));
    s1 = sum(left.*right,2);
    left1 = left./lamda;
    s2 = n1-sum(left.*left1,2);
    s3 = sum(Y(pos,k),1);
    clear left
    clear left1;
    clear right;

    Thres(1,k) = (2*s3+n2*(s3-s1)/s2)/n;
    Thres(2,k) = (n1*(s3-s1)/s2-2*s3)/n;
    Y1(pos,k) = Y1(pos,k)-Thres(1,k);
    Y1(neg,k) = -Y1(neg,k)+Thres(2,k);
    clear pos;
    clear neg;
end
clear Z;
Y = [Y,Y1];
clear Y1;

Y = (Y>0); %% logical format


function [grad, grad_W, grad_b] = B_tconv(prev_layer, curr_layer, future_layers, skip_grad)
input = prev_layer{1}.a;
[D,T,N] = size(input);
% Each row of W is a DxP filter to be applied along the time axis. We have
% totally H such filters.
W = curr_layer.W;
b = curr_layer.b;
[H,DP] = size(W);
P = DP/D;
halfP = (P-1)/2;

if 0
    % Instead of using conv2, we splice input and use matrix transform
    % We first put the N samples into a big sample, separated by zeros
    if strcmpi(class(input), 'gpuArray')
        input2 = gpuArray.zeros(D,T+P,N);
    else
        input2 = zeros(D,T+P,N);
    end
    input2(:,halfP+1:halfP+T,:) = input;
    X = reshape(input2, D, (T+P)*N);
    context = -halfP : halfP;
    X2 = ExpandContext_v2(X, context);
else
    X2 = curr_layer.X2;
end

X3 = reshape(X2, size(X2,1), size(X2,2)/N,N);

if isfield(curr_layer, 'range') && strcmpi(curr_layer.range, 'valid') == 0
    X3 = X3(:,halfP+1:halfP+T,:);
else
    X3 = X3(:,halfP*2+1:T,:);
end
X4 = reshape(X3, size(X3,1), size(X3,2)*N);

future_grad = GetFutureGrad(future_layers, curr_layer);
if strcmpi(class(future_grad), 'gpuArray')
    grad = gpuArray.zeros(D,T,N);
else
    grad = zeros(D,T,N);
end

fg = reshape(future_grad, H, size(future_grad,2)*N);

% Y2 = F_affine_transform(X2, W, b);
grad_W = fg * X4';
grad_b = sum(fg,2);

if skip_grad==0
    gradX4 = W' * fg;
    gradX4 = reshape(gradX4, size(gradX4,1), size(gradX4,2)/N, N);

    % now we need to distribute gradX4 to the input
    if strcmpi(class(input), 'gpuArray')
        gradX2 = gpuArray.zeros(size(X2,1), size(X2,2)/N,N);
    else
        gradX2 = zeros(size(X2,1), size(X2,2)/N,N);
    end
    if isfield(curr_layer, 'range') && strcmpi(curr_layer.range, 'valid') == 0
        gradX2(:,halfP+1:halfP+T,:) = gradX4;
    else
        gradX2(:,halfP*2+1:T,:) = gradX4;
    end
    gradX2 = reshape(gradX2, size(X2,1), size(X2,2));
    fl{1}.grad = gradX2;
    fl{1}.name = 'dummy';
    gradX = B_splice(fl, P);
    grad = reshape(gradX, D, T+P, N);
    grad = grad(:, halfP+1:halfP+T,:);
end

end


function D = kn2sd(K)
% Transform a kernel matrix (or inner product matrix) to a squared distance matrix
% Input:
%   K: n x n kernel matrix
% Ouput:
%   D: n x n squared distance matrix
% Written by Mo Chen (sth4nth@gmail.com).
d = diag(K);
D = -2*K+bsxfun(@plus,d,d');


% [INPUT]
% data = A float t-by-2 matrix (-Inf,Inf) representing the model input.
% type = A string representing the type of metric to calculate:
%   - 'GG' for Gonzalo-Granger Component Metric;
%   - 'H' for Hasbrouck Information Metric.
% lag_max = An integer [2,t-2] representing the maximum lag order to be evaluated (optional, default=10).
% lag_sel = A string representing the lag order selection criteria (optional, default='AIC'):
%   - 'AIC' for Akaike's Information Criterion;
%   - 'BIC' for Bayesian Information Criterion;
%   - 'FPE' for Final Prediction Error;
%   - 'HQIC' for Hannan-Quinn Information Criterion.
%
% [OUTPUT]
% m1 = A float [0,1] representing the first value of the metric.
% m2 = A float [0,1] representing the second value of the metric.
% lag = An integer [1,lag_max] representing the selected lag order.

function [m1,m2,lag] = price_discovery(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('data',@(x)validateattributes(x,{'double'},{'real' 'finite' '2d' 'nonempty' 'size' [NaN 2]}));
        ip.addRequired('type',@(x)any(validatestring(x,{'GG' 'H'})));
        ip.addOptional('lag_max',10,@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 1 'scalar'}));
        ip.addOptional('lag_sel','AIC',@(x)any(validatestring(x,{'AIC' 'BIC' 'FPE' 'HQIC'})));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [data,lag_max] = validate_input(ipr.data,ipr.lag_max);
    type = ipr.type;
    lag_sel = ipr.lag_sel;

    nargoutchk(2,3);

    [m1,m2,lag] = price_discovery_internal(data,type,lag_max,lag_sel);

end

function [m1,m2,lag] = price_discovery_internal(data,type,lag_max,lag_sel)

    t = size(data,1);
    b = t - 2;

    if (lag_max == 1)
        lag = 1;
    else
        lag = select_lag_order(data,lag_max,lag_sel);
    end

    lag_seq = 1:(lag + 1);

    [~,~,e] = regress(data(:,2),[ones(size(data,1),1) data(:,1)]);
    ect = e((lag + 2):end);

    x1_diff = diff(data(:,1));
    x1 = cell2mat(arrayfun(@(i)x1_diff(i:(b - lag + i)),lag_seq,'UniformOutput',false));

    x2_diff = diff(data(:,2));
    x2 = cell2mat(arrayfun(@(i)x2_diff(i:(b - lag + i)),lag_seq,'UniformOutput',false));

    [b1,~,e1] = regress(x1(:,1),[ones(size(ect,1),1) ect x1(:,2:end-1) x2(:,2:end-1)]);
    [b2,~,e2] = regress(x2(:,1),[ones(size(ect,1),1) ect x1(:,2:end-1) x2(:,2:end-1)]);

    d1 = b1(2);
    d2 = b2(2);

    if (strcmp(type,'GG'))
        v = abs(d1) / (abs(d1) + abs(d2));
    else
        s1 = std(e1);
        s2 = std(e2);
        rho = corr(e1,e2);

        vp = ((-d2 * s1) + (d1 * rho * s2)) ^ 2;
        v = vp / (vp + ((d1 * s2 * sqrt(1 - rho^2)) ^ 2));
    end

    m1 = min(max(v,0),1);
    m2 = 1 - m1;

end

function lag = select_lag_order(data,lag_max,lag_sel)

    t = size(data,1);
    lag_sam = lag_max + 1;
    k = t - lag_sam + 1;

    data_lag = zeros(k,lag_sam * 2);

    for i = 1:2
        sv = ((lag_sam - 1) * 2) + i;

        data_i = data(:,i);
        offs = repmat(1:k,1,lag_sam) + repelem(lag_sam:-1:1,1,k) - 1;

        data_lag(:,i:2:sv) = reshape(data_i(offs),[k lag_sam]);
    end

    data_lag = data_lag(:,3:end);
    s = size(data_lag,1);

    data_y = data(lag_sam:end,:);

    ni = 2:2:(2 * lag_max);
    rhs = [ones(k,1) (lag_sam:k+lag_sam-1).'];
    crit = zeros(lag_max,1);

    for i = 1:lag_max
        data_x = [data_lag(:,1:ni(i)) rhs];

        r = zeros(k,2);

        parfor j = 1:2
            [~,~,e] = regress(data_y(:,j),data_x);
            r(:,j) = e;
        end

        cp = zeros(2);

        for cp_i = 1:2
            for cp_j = 1:2
                cp(cp_i,cp_j) = r(:,cp_i).' * r(:,cp_j);
            end
        end

        sigmad = det(cp / k);
        d = (i * 4) + 4;

        switch (lag_sel)
            case 'AIC'
                crit(i) = log(sigmad) + ((2 / s) * d);
            case 'BIC'
                crit(i) = log(sigmad) + ((log(s) / s) * d);
            case 'FPE'
                ns = size(data_x,2);
                crit(i) = ((s + ns) / (s - ns))^2 * sigmad;
            otherwise
                crit(i) = log(sigmad) + (2 * (log(log(s)) / s) * d);
        end
    end

    [~,lag] = min(crit);

end

function [data,lag_max] = validate_input(data,lag_max)

    t = size(data,1);
    b = t - 2;

    if (t < 5)
        error('The value of ''data'' is invalid. Expected input to be a matrix with at least 5 rows.');
    end

    if (lag_max > b)
        error(['The value of ''lag_max'' is invalid. Expected input to be less than or equal to ' num2str(b) '.']);
    end

end


function [x,esq,j] = kmeanlbg(d,k)
%KMEANLBG Vector quantisation using the Linde-Buzo-Gray algorithm [X,ESQ,J]=(D,K)
%
%Inputs:
% D contains data vectors (one per row)
% K is number of centres required
%
%Outputs:
% X is output row vectors (K rows)
% ESQ is mean square error
% J indicates which centre each data vector belongs to
%
%  Implements LBG K-means algorithm:
% Linde, Y., A. Buzo, and R. M. Gray,
% "An Algorithm for vector quantiser design,"
% IEEE Trans Communications, vol. 28, pp.84-95, Jan 1980.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: kmeanlbg.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nc=size(d,2);
[x,esq,j]=kmeans(d,1);
m=1;
while m<k
   n=min(m,k-m);
   m=m+n;
   e=1e-4*sqrt(esq)*rand(1,nc);
   [x,esq,j]=kmeans(d,m,[x(1:n,:)+e(ones(n,1),:); x(1:n,:)-e(ones(n,1),:); x(n+1:m-n,:)]);
end


function [center, rotMat] = imPrincipalAxes(img, varargin)
% Computes principal axes of a 2D/3D binary image.
%
%   [CENTER, ROTMAT] = imPrincipalAxes(IMG)
%
%   (Note: currently only implemented for binary images)
%
%   Example
%     % Compute principal axes of a discretized 3D ellipsoid
%     % (requires the MatGeom toolbox)
%     elli = [50.12 50.23 50.34  30 20 10   30 20 10];
%     img = discreteEllipsoid(1:100, 1:100, 1:100, elli);
%     [center, rotMat] = imPrincipalAxes(img);
%     center
%     center =
%        51.1209   51.2245   51.3454
%     rotation3dToEulerAngles(rotMat)
%     ans =
%        30.0107   19.9733   10.0252
%
%
%   See also
%     imEquivalentEllipse, imEquivalentEllipsoid, imMoment, principalAxes
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2019-08-09,    using Matlab 9.6.0.1072779 (R2019a)
% Copyright 2019 INRAE - Cepia Software Platform.

%% Default parameters

spacing = [1 1 1];
origin = [1 1 1];

%% Parse inputs

dim = size(img);

% check image data type
binary = islogical(img);
if ~binary
    error('Requires a binary image as input');
end

%% Computation of Principal axes

if length(dim) == 2
    % extract points of the current particle
    inds = find(img > 0);
    [y, x] = ind2sub(dim, inds);

    % convert to user coordinates
    x = x * spacing(1) + origin(1);
    y = y * spacing(2) + origin(2);

    % compute approximate location of ellipsoid center
    xc = mean(x);
    yc = mean(y);
    center = [xc yc];

    % recenter points (should be better for numerical accuracy)
    x = (x - xc);
    y = (y - yc);
    points = [x y];

    % compute the covariance matrix
    covPts = cov(points, 1) + diag(spacing(1:2) / 12);

elseif length(dim) == 3
    % extract points of the current particle
    inds = find(img > 0);
    [y, x, z] = ind2sub(dim, inds);

    % convert to user coordinates
    x = x * spacing(1) + origin(1);
    y = y * spacing(2) + origin(2);
    z = z * spacing(3) + origin(3);

    % compute approximate location of ellipsoid center
    xc = mean(x);
    yc = mean(y);
    zc = mean(z);
    center = [xc yc zc];

    % recenter points (should be better for numerical accuracy)
    x = (x - xc);
    y = (y - yc);
    z = (z - zc);
    points = [x y z];

    % compute the covariance matrix
    covPts = cov(points, 1) +  diag(spacing(1:3) / 12);

else
    error('MatImage:imPrincipalAxes', ...
        'Dimension of input image must be either 2 or 3');
end

% perform a principal component analysis with 3 variables,
% to extract inertia axes
[U, S] = svd(covPts);

% sort axes from greater to lower
[S, ind] = sort(diag(S), 'descend'); %#ok<ASGLU>

% format U to ensure first axis points to positive x direction
U = U(ind, :);
if U(1,1) < 0
    U = -U;
    % keep matrix determinant positive
    U(:,3) = -U(:,3);
end
rotMat = U;


function L = chol_up (A)
%CHOL_UP up-looking Cholesky factorization.
% Example:
%   L = chol_up (A)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

n = size (A) ;
L = zeros (n) ;
for k = 1:n
    L (k,1:k-1) = (L (1:k-1,1:k-1) \ A (1:k-1,k))' ;
    L (k,k) = sqrt (A (k,k) - L (k,1:k-1) * L (k,1:k-1)') ;
end


function Svals = Simulate_RegimeSwitching_Diffusion_Unbiased( N_sim, T, S_0, drift_vec, sigma_vec, Q, initial_state)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Simulates Terminal Value (S_T) of Regime Switching Diffusion Models
%        This scheme is unbiased, but only generates the terminal S values
% Returns: vector of size N_sim
% Author: Justin Lars Kirkby
%
% -----------------
% Params
% -----------------
% N_sim = # samples
% T = time to maturity, ie path is on [0,T]
% S_0 = initial underlying value (e.g. S_0=100)
% drift_vec = vector of drift coefficient by regime state, e.g. r_i - q_i, where r is interest rate in state i, and q is div yield
% sigma_vec = diffusion coefficients in each state

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

Svals = zeros(N_sim,1);
lambdas = -diag(Q);  % Transition rates
P = (Q + diag(lambdas))./lambdas;  % Remove diagonal, no self-transition
cdfs = cumsum(P, 2);

for n = 1:N_sim
   S = S_0;
   t_last = 0;
   state = initial_state;
   while t_last < T
       tau = exprnd(lambdas(state));
       t = min(T, t_last + tau);
       tau = t - t_last;

       Sigsqdt = sigma_vec(state)*sqrt(tau);
       drift = (drift_vec(state) - 0.5*sigma_vec(state)^2)*tau;

       S = S.*exp(drift + Sigsqdt*randn());  %log scheme
       state = next_state(cdfs(state, :));
       t_last = t;
   end
   Svals(n) = S;
end

end

function j = next_state(cdf)
u = rand();
j = find(u <= cdf, 1);
end


function result = compare(c1,c2, tolerance)

%tstoolbox/@core/compare
%   Syntax:
%     * compare(c1,c2, tolerance)
%
%   Input Arguments:
%     * c1,c2 core object of two signals
%     * tolerance tolerance of the signals's RMS value (default
%       tolerance=1e-6)
%
%   compare compare two signals whether they have equal values slight
%   differences due to rounding errors are ignored depending on the value
%   of tolerance when signals are found to be not equal, a zero is
%   returned.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

if nargin < 2
	help(mfilename)
	return
end

if nargin < 3
	tolerance = 1e-6;
end

if dlens(c1) ~= dlens(c2)
	result = 0;
else
	ref = data(rms(c1));
	diff = data(rms(c1-c2));
	if any((diff ./ ref) > tolerance)
		result = 0;
	else
		result = 1;
	end
end


classdef libsvm_CL

%  libsvm_CL is a Support Vector Machine (SVM) classifier object (CL)
%   that uses the LIBSVM package (i.e., this function is Neural Decoding Toolbox
%   wrapper for the LIBSVM package).  A support vector machine is a classifier
%   that learns a function f that minimize the hinge loss between the training
%   data and labels, while also applying a penalty to more complex f (the penalty
%   is based on the norm of f in a RKHS).  The SVM has a parameter C, that controls
%   the trade off between a smaller empirical loss (i.e., smaller prediction error
%   on the training set), a more complex function f.  Support vector machine can use
%   different kernels to create nonlinear decision boundaries (several are supported here).
%
%  SVMs are designed to work on binary classification problems.  In order to
%   support multi-class classification, we use two different methods.  The first method is
%   called 'all-pairs' and works by training separate classifiers for all pairs of labels
%   (i.e., if there are 100 different classes then nchoosek(100, 2) = 4950 different classifiers
%   are trained.  Testing the classifier in all-pairs involves having all classifiers
%   classify the test point, and then the class label is given to the class the was chosen most
%   often by the binary classifiers (in the case of a tie in the number of classes that won a contest
%   the class label is randomly chosen).  The decision values for all-pairs are the number
%   of contests won by each class (for each test point).
%
%  The second multi-class method is called 'one-vs-all' classification
%   and works by training one classifier for each class (thus if there are 100 classes
%   there will be 100 classifiers) using data from one class as the positive labels,
%   and data from all the other classes as the negative labels.  A test point is
%   then run through these different classifiers and the class that has the largest
%   SVM prediction value is returned as the predicted label
%   (i.e., SVMs create a function f(x) = y, and the class label is
%   usually given as sign(y), however here we are comparing the actual y values to determine
%   the label).  The decision values here are the f(x) = y values returned by the SVM. Our limited
%   tests have found all pairs is faster and gives slightly more accuracy results so it is the
%   default (although the decision values might be considered more crude).
%
%
%  Function options and default values:
%
%  The following values can be set to change this classifiers behavior (here we
%  are assuming that svm = libsvm_CL)
%
%  - svm.C: scalar  (default svm.C = 1). This is the 1/regularization constant that determines
%       the trade off between the fit to the training data and the amount of regularization/simplicity
%       of the learned function.  A larger value of C means more emphasis on a better fit to the training data.
%
%  - svm.kernel:  'linear',  'polynomial'/'poly', or 'gaussian'/'rbf' (default 'linear).  The type of kernel used
%       which controls the class of functions that classifier is built from.
%
%       -  If svm.kernel = 'polynomial', then the following options must be set:
%           - svm.poly_degree: scalar (there is no default value so this must be set).  The degree of the of the polynomial.
%           - svm.poly_offset: scalar (default svm.poly_offset = 0);  a constant offset if a polynomntal kernel is used
%
%       -  If svm.kernel = 'gaussian', then the following options must be set:
%           - svm.gaussian_gamma (there is no default value so this must be set).  This controls the fall off
%             of the radial basis function kernel (larger values mean a slower fall off).
%
%  - svm.additional_libsvm_options (default svm.additional_libsvm_options = '').  This allows one to add additional
%     control of the classifiers behavior using a string that is in LIBSVM format.  For more details see:
%     http://www.csie.ntu.edu.tw/~cjlin/libsvm/
%
%  - svm.multiclass_classificaion_scheme (default value is 'all_pairs').  This field can
%    be set to 'all_pairs' or 'one_vs_all' and determines if a one-vs-all or an all-pairs
%    multi-class classification scheme is used, as described above (for binary problems
%    both all-pairs and one-vs-all will return the same predicted labels, although the
%    decision values will differ).
%
% Like all CL objects, there are two main methods, which are:
%
%  1.  cl = train(cl, XTr, YTr)
%       This method takes the training data (XTr, YTr) to train the support vector machine.
%        XTr and XTe are in the form [num_features x num_examples]
%        YTr is in the form [num_examples x 1]
%
%  2.  [predicted_labels decision_values] = test(cl, XTe)
%        This method takes the test data and produces a [num_test_points x 1] vector of
%         predicted labels based on the  function learned from the training set.  It also
%         returns a matrix of dimension [num_test_points x num_classes] of decision values.
%
%
%  Notes:
%   1.  If there is a tie among the decision values, then one of tied the classes
%          is chosen randomly as the predicted label.
%

%==========================================================================

%     This code is part of the Neural Decoding Toolbox.
%     Copyright (C) 2011 by Ethan Meyers (emeyers@mit.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 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/>.

%==========================================================================

  properties
        C = 1;  % inverse of the regularization constant - higher values cause more emphasis to be place on the
                %  emperical loss (and cause the classifier use more complex functions, which could lead to overfitting).
        kernel = 'linear';  % the type of kernel used in the SVM.  Choices are 'linear', 'polynomial'/'poly', or 'gaussian'/'rbf'.
        poly_degree = [];  % if a polynomial classifier is used, the degree of the polynomial must be set
        poly_offset = 1;  % a constant offset if a polynomntal kernel is used (default is to use an offset of 1)
        gaussian_gamma = [];  % this controlls the falloff of the Gaussian kernel (larger values mean slower falloff = wider Gaussian functions)
        additional_libsvm_options = '';  % can set this to use additional libsvm options that are not explicitly supported by this wrapper
        multiclass_classificaion_scheme = 'all_pairs';   % options are 'one_vs_all' or 'all_pairs'.
  end

  properties (GetAccess = 'public', SetAccess = 'private')
        labels = [];  % all the unique labels for each class
        model = [];  % the model learned from the training data
  end

    methods

        % constructor
        function cl = libsvm_CL
        end

        function cl = train(cl, XTr, YTr)

            % added sanity check
            if size(YTr, 2) ~= size(XTr, 2)  &&  size(YTr, 1) ~= size(XTr, 2)
               error('Number of columns in YTr, and XTr must be the same (i.e., there must be one and exactly one label for each data point)')
            end

            cl.labels = unique(YTr);

            % create the libsvm arguments for the kernel
            cl.kernel = lower(cl.kernel);
            if strcmp(cl.kernel, 'poly'), cl.kernel = 'polynomial'; end
            if strcmp(cl.kernel, 'rbf'), cl.kernel = 'gaussian'; end

            switch cl.kernel
              case 'linear'
                kernelstring = '-t 0 ';
              case 'polynomial'
                kernelstring = ['-t 1 -g 1 -r ' cl.poly_offset ' -d ' cl.poly_degree ' '];
              case 'gaussian'
                kernelstring = ['-t 2 -g ' cl.gaussian_gamma ' '];
            end

            cstring = ['-c ' num2str(cl.C) ' '];  %  create the libsvm arguments for the (inverse of the) regularization constant
            basicstring = ['-s 0 '];   % use C-SVC  (which is the default anyway)
            probstring = '';  %['-b 1 '];

            paramstring = [basicstring cstring kernelstring probstring cl.additional_libsvm_options];

            if strcmp(cl.multiclass_classificaion_scheme, 'all_pairs')

                cl.model = svmtrain2(YTr, double(XTr'), paramstring);  % renamed the libsvm function svmtrain2 because the Matlab bioinformatics toolbox now has a funciton named svmtrain
                %cl.model = svmtrain(YTr, double(XTr'), paramstring);

            elseif strcmp(cl.multiclass_classificaion_scheme, 'one_vs_all')

                % do one vs. all training
               for iClass = 1:length(cl.labels)

                   reformatted_test_data = [XTr(:, (YTr == cl.labels(iClass))) XTr(:, (YTr ~= cl.labels(iClass)))];
                   curr_labels = zeros(size(YTr));
                   curr_labels(1:length(find(YTr == cl.labels(iClass)))) = 1;
                   the_weight = length(find(YTr ~= cl.labels(iClass)))./length(find(YTr == cl.labels(iClass)));
                   cl.model{iClass} = svmtrain2(curr_labels, double(reformatted_test_data'), paramstring);
                   %cl.model{iClass} = svmtrain(curr_labels, double(reformatted_test_data'), paramstring);

               end
            end

        end

        function [predicted_labels decision_values] = test(cl, XTe)

            junk_fake_xte_labels = ones(size(XTe,2),1);

            if strcmp(cl.multiclass_classificaion_scheme, 'all_pairs')

                [predic_label, junk_fake_accuracy, libsvm_format_all_pairs_decision_vals] = svmpredict(junk_fake_xte_labels, double(XTe'), cl.model);   % edited by Ethan to work with libsvm 2.86 (compared to 2.8)

                predicted_labels = predic_label(:,1);
                if (size(predic_label,2)>1)
                    libsvm_format_all_pairs_decision_vals = predic_label(:,2);
                end

                % Creating the decision values that are the number of 'all pairs' classifiers that are in favor of a given class
                %  (rather than the f(x) values that classifier returns)

                % For each test point, all of the 'all pairs' decision values are in each row of the libsvm_format_all_pairs_decision_vals matrix
                % the order goes:  [(1 vs 2), (1 vs 3), (1 vs 4), ... (1 vs num_classes), (2 vs 3), (2 vs 4)... (2 vs num_classes), ... etc... (num_classes -1 vs num_classes)]
                % Below I reparse this into a matrix of the form:  allpairs_decision_vals(iClass1, iClass2, iTestPoint) = libsvm_decision_value for iTestPoint destinguishing between iClass1 and iClass2
                % more negative values indicate preference for iClass1 and positive values indicate iClass2

                num_labels = length(cl.model.Label);
                end_inds = cumsum((cl.model.nr_class -1):-1:1);
                start_inds = end_inds + 1;
                start_inds = [1 start_inds(1:end-1)];

                start_inds(end + 1) = 2;
                end_inds(end + 1) = 1;

                allpairs_decision_vals(1, :, :) = libsvm_format_all_pairs_decision_vals(:, start_inds(1):end_inds(1))';
                for iLabel = 2:num_labels

                    for iPrevLabel = 1:(iLabel -1)
                        curr_decision_vals(iPrevLabel, :) = squeeze(allpairs_decision_vals(iPrevLabel, iLabel-1, :) .* -1);
                    end
                    allpairs_decision_vals(iLabel, :, :) = [curr_decision_vals; libsvm_format_all_pairs_decision_vals(:, start_inds(iLabel):end_inds(iLabel))'];

                end

                % the final decision values are the number of all-pairs wins that a given class has
                for iPoints = 1:size(allpairs_decision_vals, 3)
                    decision_values(iPoints, :) = (sum(sign((allpairs_decision_vals(:, :, iPoints))), 2)');
                end

                % LibSVM's final result is based on the class that has the most 'all pairs wins' (and for ties it takes the lowest class number)
                % since this can introduce a bias (particularly in the confusion matrix), I will randomly select between classes that are tied by
                % adding a small amount of noise to the number of all pairs wins (I am doing this instead of using the randmax function b/c I want
                % this noise in the decision_values when the rank results and other functions are calculated from the decision values)

                decision_values = decision_values + eps .* rand(size(decision_values));

                [vals ind] = max(decision_values');
                predicted_labels = ind';

                predicted_labels = cl.labels(predicted_labels);

            elseif strcmp(cl.multiclass_classificaion_scheme, 'one_vs_all')

               % do one vs. all testing
               for iClass = 1:length(cl.labels)

                   [predic_label, junk_fake_accuracy, libsvm_format_all_pairs_decision_vals] = svmpredict(junk_fake_xte_labels, double(XTe'), cl.model{iClass});
                   decision_values(:, iClass) = libsvm_format_all_pairs_decision_vals;

                   % % linear f(x)  % the decision values are the same as these when a linear kernel is used...
                   %w = cl.model{iClass}.SVs' * cl.model{iClass}.sv_coef;
                   %linear_pred_vals(:, iClass) = (XTe' * w) - cl.model{iClass}.rho;
                   %all_weights(:, iClass) = w;
                   %all_b(:, iClass) = -1 .* cl.model{iClass}.rho;
               end

               [val ind] = randmax(decision_values');
               predicted_labels = ind';

               predicted_labels = cl.labels(predicted_labels);

            end   % end one vs all

        end  % end test method

    end  % end public methods

end   % end classdef


function L = dist2poly ( p, edgexy, lim )

%*****************************************************************************80
%
% Find the minimum distance from the points in P to the polygon defined by
% the edges in EDGEXY. LIM is an optional argument that defines an upper
% bound on the distance for each point.

% Uses (something like?) a double sweep-line approach to reduce the number
% of edges that are required to be tested in order to determine the closest
% edge for each point. On average only size(EDGEXY)/4 comparisons need to
% be made for each point.
%
%  Author:
%
%    Darren Engwirda
%
if nargin<3
   lim = [];
end
np = size(p,1);
ne = size(edgexy,1);
if isempty(lim)
   lim = inf*ones(np,1);
end

% Choose the direction with the biggest range as the "y-coordinate" for the
% test. This should ensure that the sorting is done along the best
% direction for long and skinny problems wrt either the x or y axes.
dxy = max(p)-min(p);
if dxy(1)>dxy(2)
    % Flip co-ords if x range is bigger
    p       = p(:,[2,1]);
    edgexy  = edgexy(:,[2,1,4,3]);
end

% Ensure edgexy(:,[1,2]) contains the lower y value
swap           = edgexy(:,4)<edgexy(:,2);
edgexy(swap,:) = edgexy(swap,[3,4,1,2]);

% Sort edges
[i,i]          = sort(edgexy(:,2));                                        % Sort edges by lower y value
edgexy_lower   = edgexy(i,:);
[i,i]          = sort(edgexy(:,4));                                        % Sort edges by upper y value
edgexy_upper   = edgexy(i,:);

% Mean edge y value
ymean = 0.5*( sum(sum(edgexy(:,[2,4]))) )/ne;

% Alloc output
L = zeros(np,1);

% Loop through points
tol = 1000.0*eps*max(dxy);
for k = 1:np

   x = p(k,1);
   y = p(k,2);
   d = lim(k);

   if y<ymean

      % Loop through edges bottom up
      for j = 1:ne
         y2 = edgexy_lower(j,4);
         if y2>=(y-d)
            y1 = edgexy_lower(j,2);
            if y1<=(y+d)

               x1 = edgexy_lower(j,1);
               x2 = edgexy_lower(j,3);

               if x1<x2
                  xmin = x1;
                  xmax = x2;
               else
                  xmin = x2;
                  xmax = x1;
               end

               if xmin<=(x+d) && xmax>=(x-d)
                  % Calculate the distance along the normal projection from [x,y] to the jth edge
                  x2mx1 = x2-x1;
                  y2my1 = y2-y1;

                  r = ((x-x1)*x2mx1+(y-y1)*y2my1)/(x2mx1^2+y2my1^2);
                  if r>1.0                                                 % Limit to wall endpoints
                     r = 1.0;
                  elseif r<0.0
                     r = 0.0;
                  end

                  dj = (x1+r*x2mx1-x)^2+(y1+r*y2my1-y)^2;
                  if (dj<d^2) && (dj>tol)
                     d = sqrt(dj);
                  end

               end

            else
               break
            end
         end
      end

   else

      % Loop through edges top down
      for j = ne:-1:1
         y1 = edgexy_upper(j,2);
         if y1<=(y+d)
            y2 = edgexy_upper(j,4);
            if y2>=(y-d)

               x1 = edgexy_upper(j,1);
               x2 = edgexy_upper(j,3);

               if x1<x2
                  xmin = x1;
                  xmax = x2;
               else
                  xmin = x2;
                  xmax = x1;
               end

               if xmin<=(x+d) && xmax>=(x-d)
                  % Calculate the distance along the normal projection from [x,y] to the jth edge
                  x2mx1 = x2-x1;
                  y2my1 = y2-y1;

                  r = ((x-x1)*x2mx1+(y-y1)*y2my1)/(x2mx1^2+y2my1^2);
                  if r>1.0                                                 % Limit to wall endpoints
                     r = 1.0;
                  elseif r<0.0
                     r = 0.0;
                  end

                  dj = (x1+r*x2mx1-x)^2+(y1+r*y2my1-y)^2;
                  if (dj<d^2) && (dj>tol)
                     d = sqrt(dj);
                  end

               end

            else
               break
            end
         end
      end

   end

   L(k) = d;

end

end      % dist2poly()


function pass = test_periodicbvp
% TAD, 10 Jan 2014

tol = 1e-8;

%% Building blocks
dom = [-pi pi];
[Z, I, D, C] = linop.primitiveOperators(dom);
[z, E, s] = linop.primitiveFunctionals(dom);
x = chebfun('x', dom);
c = sin(x.^2);
C = operatorBlock.mult(c);
El = E(dom(1));
Er = E(dom(end));

%% Solve a linear system
L = [ D^2, -I, sin(x); C, D, chebfun(0,dom); functionalBlock.zero(dom), El, 4 ];
f = [sin(x); chebfun(0,dom); 1 ];
L = addbc(L,'periodic');

%%

type = {@chebcolloc2, @chebcolloc1, @ultraS, @chebcolloc2, @chebcolloc1, @ultraS};
prefs = cheboppref;
prefs.bvpTol = 1e-14;

w = [];
for k = 1:6
    prefs.discretization = type{k};
    w = linsolve(L,f,prefs);

    %%
    % check the ODEs
    w1 = w{1};  w2 = w{2};  w3 = w{3};
    f1 = f{1};  f2 = f{2}; f3 = f{3};
    residual1 = diff(w1,2) - w2 + sin(x).*w3 - f1;
    err(k,1) = norm( residual1 );
    residual2 = c.*w1 + diff(w2) + 0 - f2;
    err(k,2) = norm( residual2 );
    err(k,3) = abs( 0 + feval(w2, dom(1)) + 4*w3 - f3 );

    %%
    % check the BCs
    v = w{2};  u = w{1};
    Du = D*u;  Dv = D*v;
    err(k,4) = abs( u(pi) - u(-pi) );
    err(k,5) = abs( v(pi) - v(-pi) );
    err(k,6) = abs( Du(pi) - Du(-pi) );

    %%
    % check continuity
    err(k,7) = feval(u, pi/2, 'left') - feval(u, pi/2, 'right');
    err(k,8) = feval(v, pi/2, 'left') - feval(v, pi/2, 'right');
    err(k,9) = feval(Du, pi/2, 'left') - feval(Du, pi/2, 'right');
    err(k,10) = feval(u, -pi/2, 'left') - feval(u, -pi/2, 'right');
    err(k,11) = feval(v, -pi/2, 'left') - feval(v, -pi/2, 'right');
    err(k,12) = feval(Du, -pi/2, 'left') - feval(Du, -pi/2, 'right');

    if ( k == 2 )
        % introduce breakpoints
        f = [abs(cos(x)); 0*x; 1 ];
    end

end

%% Test for TRIGCOLLOC.

% Domain.
dom = [0 2*pi];

% Differentiation operators.
D = operatorBlock.diff(dom);
D2 = operatorBlock.diff(dom, 2);

% Multiplication operators.
a1 = chebfun(@(x) 1 + sin(2*x), dom);
A1 = operatorBlock.mult(a1);
a0 = chebfun(@(x) 1 + cos(x), dom);
A0 = operatorBlock.mult(a0);

% Linop.
L = linop(D2 + A1*D + A0);

% Rhs.
f = chebfun(@(x) cos(x), dom);

% Solve it with TRIGCOLLOC.
prefs.discretization = @trigcolloc;
u = linsolve(L, f, prefs);
u = u{1};

err(1, 13) = norm(diff(u,2) + a1.*diff(u) + a0.*u - f);
err(2, 13) = abs(u(2*pi) - u(0));
err(3, 13) = abs(feval(diff(u), 2*pi) - feval(diff(u), 0));

%%
pass = err < tol;

end


function [W B] = box_decomp(U)
%BOX_DECOMP

% TODO: doc
% centering is recommended

% 2^n vertex!

M = [min(U); max(U)];
L = M(2,:) - M(1,:);

[n1 n2] = size(U);

%%% TODO: this is a hack
% if n2==2
% 	A = [linspace(0,1,n1)' linspace(1,0,n1)'];
% 	if norm(U*(U'*A)-A) < 1e-10
% 		% linear
% 		W = A;
% 		B = inv(U'*A);
% 		return
% 	end
% end

m = 2^n2;
B = zeros(m, n2);

% TODO: make it less ugly + efficient
r = 0:2:2*n2-2;
z = ones(1,n2);
z(1) = 0;
for i = 1:m
	z = nexti(z);
	B(i,:) = M(z+r);
end

W = zeros(n1,m);
for j = 1:n1
	q = [(M(2,:)-U(j,:))./L; (U(j,:)-M(1,:))./L];
	z = ones(1,n2);
	z(1) = 0;
	for i = 1:m
		z = nexti(z);
		W(j,i) = prod(q(z+r));
	end
end
%W = [U ones(size(U,1),1)]/[B ones(m,1)];

% next index
function z = nexti(z)
i = 1;
z(i) = z(i) + 1;
while z(i) > 2
	z(i) = 1;
	i = i + 1;
	z(i) = z(i) + 1;
end


function [OLS_Bhat, OLS_betahat, OLS_sigmahat, OLS_forecast_estimates, biclag]=arbicloop(data_endo,data_endo_a,const,p,n, m, Fperiods, Fband)

% function [OLS_Bhat{ii}, OLS_betahat{ii}, OLS_sigmahat{ii}, OLS_forecast_estimates]=arbicloop(data_endo,data_endo_a,const,p,n)
% computes individual OLS estimations of autoregressive models and record their residual variances, as stated p16 of technical guide
% inputs:  - matrix 'data_endo': matrix of endogenous data used for model estimation
%          - integer 'const': 0-1 value to determine if a constant is included in the model
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'Fperiods': number of forecast periods
%          - scalar 'Fband': confidence level for forecasts
% outputs: - cell 'OLS_Bhat' :
%          - cell 'OLS_betahat' :
%          - cell 'OLS_sigmahat': residual variance of individual AR models estimated for each endogenous variable
%          - cell 'OLS_forecast_estimates :

bic=[];
nvar=1;
%loop over lags(p)
for ii=1:p
  [sigmahatlag]=bear.arloop(data_endo,const,ii,n);
   bic=[bic -2*log(sigmahatlag)+(log(length(data_endo))*nvar)];
end

%optimising VAR for AR - for each one.
[bicmin, biclag]=min(bic', [], 1);

% Estimating and forecasting for BIC optimised model
% loop over columns of data_endo
for ii=1:n
% Estimating BIC optimised VAR and record parameters and residual variance
[OLS_Bhat{ii}, OLS_betahat{ii}, OLS_sigmahat{ii},~,~,~,~,~,~,~,~,~,~,~,~]=bear.olsvar(data_endo(:,ii),[],const,biclag(ii));
% Forecasting for BIC optimised model
[OLS_forecast_estimates{ii}]=bear.olsforecast(data_endo_a(:,ii),[],Fperiods,OLS_betahat{ii},OLS_Bhat{ii},OLS_sigmahat{ii},1,m,biclag(ii),biclag(ii)+1,const,Fband);
end


function pass = test_scl( pref )
% Check correct vertical scaling.

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

% Scale invariant
f = chebfun2( @(x,y) cos(x.*y) );
g = chebfun2( @(x,y) eps*cos(x.*y) );
err = norm( eps*f - g);
pass(j) = ( err  <  tol ); j = j + 1;

% hscale invariant
f = chebfun2( @(x,y) cos(x.*y) );
g = chebfun2( @(x,y) cos(x/eps.*y/eps), eps*[-1 1 -1 1] );
err = abs( f(1,1) - g(eps,eps) );
pass(j) = ( err  < tol ); j = j + 1;
err = abs( f(pi/6,1) - g(eps*pi/6,eps) );
pass(j) = ( err  < tol ); j = j + 1;
end


function fm=oz(x)
%OZ FM=OZ(X) is the nonlinear residual for the
%   Ornstein-Zernike equations.
%
% Evaluate the nonlinearity as a sum of the substitution
% and compact parts.
%
fm=ozsub(x)+ozinteg(x);
%
% This is the substitution part.
%
function km=ozsub(x)
global L U rho
n=length(x); n2=n/2; h=x(1:n2); c=x(n2+1:n);
nl=U.*exp(h-c);
mup=h+1- nl;
mdown=c+1-nl;
km=[mup',mdown']';
%
% This is the compact part of the nonlinearity. The upper
% component is zero, the lower is rho*L*c*h.
%
% rho and L are scalars for this problem
%
function km=ozinteg(x)
global L U rho
L3=L*L*L*rho;
n=length(x); n2=n/2; h=x(1:n2); c=x(n2+1:n);
kd=L3*convk(c,h);
kd(1)=2*kd(2)-kd(3);
kd(n2)=2*kd(n2-1)-kd(n2-2);
kup=zeros(n2,1);
km=[kup',kd']';
%
% Discrete convolution with the Hankel transform.
%
function hc=convk(h,c)
Lx=1.0;
n=length(h);
dr=Lx/n;
th=hank2(h); tc=hank2(c); fhc=th.*tc; hc=ihank2(fhc);
%
% Hankel transform using the fast sine transform.
%
function hf=hank2(f)
nf=length(f); n=nf-1; m=nf-2;
h=1/n; beta=2*(m+1)*(h^3); ff=beta;
p=1:m; p=p';
hft=ff*lsint(p.*f(2:n))./p;
hf=[0,hft',0]';
%
% Inverse Hankel transform using the fast sine transform.
%
function ihf=ihank2(f)
nf=length(f); n=nf-1; m=nf-2;
h=1/n; beta=2*(m+1)*(h^3); ff=2/(n*beta);
p=1:m; p=p';
hft=ff*lsint(p.*f(2:n))./p;
p1=1:n; p1=p1'; p1=p1.*p1; p1(n)=p1(n)/2;
ihf=[0,hft',0]';
% LSINT
% Fast sine transform with MATLAB's FFT.
%
function lf=lsint(f)
n=length(f);
ft=-fft([0,f']',2*n+2);
lf=imag(ft(2:n+1));


function grid_weight = sparse_grid_weights_cfn ( dim_num, level_max, rule, ...
  point_num, grid_index )

%*****************************************************************************80
%
%% SPARSE_GRID_WEIGHTS_CFN computes sparse grid weights based on a CFN 1D rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer RULE, the index of the rule.
%    1, "CC", Clenshaw Curtis Closed Fully Nested rule.
%    2, "F1", Fejer 1 Open Fully Nested rule.
%    3, "F2", Fejer 2 Open Fully Nested rule.
%    4, "GP", Gauss Patterson Open Fully Nested rule.
%    5, "GL", Gauss Legendre Open Weakly Nested rule.
%    6, "GH", Gauss Hermite Open Weakly Nested rule.
%    7, "LG", Gauss Laguerre Open Non Nested rule.
%
%    Input, integer POINT_NUM, the total number of points in
%    the grids.
%
%    Input, integer GRID_INDEX(DIM_NUM,POINT_NUM), a list of
%    point indices, representing a subset of the product grid of level
%    LEVEL_MAX, representing (exactly once) each point that will show up in a
%    sparse grid of level LEVEL_MAX.
%
%    Output, real GRID_WEIGHT(POINT_NUM), the weights
%    associated with the sparse grid points.
%
  if ( level_max == 0 )
    grid_weight(1:point_num) = 2.0^dim_num;
    return
  end

  grid_weight(1:point_num) = 0.0;

  level_min = max ( 0, level_max + 1 - dim_num );

  for level = level_min : level_max
%
%  The middle loop generates the next partition LEVEL_1D(1:DIM_NUM)
%  that adds up to LEVEL.
%
    level_1d = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 );

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );
%
%  Transform each 1D level to a corresponding 1D order.
%
      order_1d = level_to_order_closed ( dim_num, level_1d );
%
%  The product of the 1D orders gives us the number of points in this grid.
%
      order_nd = prod ( order_1d(1:dim_num) );
%
%  Generate the indices of the points corresponding to the grid.
%
      grid_index2 = multigrid_index_cfn ( dim_num, order_1d, order_nd );
%
%  Compute the weights for this grid.
%
      grid_weight2 = product_weights ( dim_num, order_1d, order_nd, rule );
%
%  Adjust the grid indices to reflect LEVEL_MAX.
%
      grid_index2 = multigrid_scale_closed ( dim_num, order_nd, ...
        level_max, level_1d, grid_index2 );
%
%  Now determine the coefficient.
%
      coeff = r8_mop ( level_max - level ) ...
        * r8_choose ( dim_num - 1, level_max - level );

      for point2 = 1 : order_nd

        for point = 1 : point_num

          if ( all ( ...
            grid_index2(1:dim_num,point2) == grid_index(1:dim_num,point) ...
          ) )
            grid_weight(point) = grid_weight(point) ...
              + coeff * grid_weight2(point2);
            break
          end

        end

      end

      if ( ~more )
        break
      end

    end

  end

  return
end