function [ X ] = hyperFcls( M, U )
%HYPERFCLS Performs fully constrained least squares on pixels of M.
% hyperFcls performs fully constrained least squares of each pixel in M
% using the endmember signatures of U.  Fully constrained least squares
% is least squares with the abundance sum-to-one constraint (ASC) and the
% abundance nonnegative constraint (ANC).
%
% Usage
%   [ X ] = hyperFcls( M, U )
% Inputs
%   M - HSI data matrix (p x N)
%   U - Matrix of endmembers (p x q)
% Outputs
%   X - Abundance maps (q x N)
%
% References
%   "Fully Constrained Least-Squares Based Linear Unmixing." Daniel Heinz,
% Chein-I Chang, and Mark L.G. Althouse. IEEE. 1999.

if (ndims(U) ~= 2)
    error('M must be a p x q matrix.');
end

[p1, N] = size(M);
[p2, q] = size(U);
if (p1 ~= p2)
    error('M and U must have the same number of spectral bands.');
end

p = p1;
X = zeros(q, N);
Mbckp = U;
for n1 = 1:N
    count = q;
    done = 0;
    ref = 1:q;
    r = M(:, n1);
    U = Mbckp;
    while not(done)
        als_hat = inv(U.'*U)*U.'*r;
        s = inv(U.'*U)*ones(count, 1);

        % IEEE Magazine method (http://www.planetary.brown.edu/pdfs/3096.pdf)
        % Contains correction to sign.  Error in original paper.
        afcls_hat = als_hat - inv(U.'*U)*ones(count, 1)*inv(ones(1, count)*inv(U.'*U)*ones(count, 1))*(ones(1, count)*als_hat-1);

        % See if all components are positive.  If so, then stop.
        if (sum(afcls_hat>0) == count)
            alpha = zeros(q, 1);
            alpha(ref) = afcls_hat;
            break;
        end
        % Multiply negative elements by their counterpart in the s vector.
        % Find largest abs(a_ij, s_ij) and remove entry from alpha.
        idx = find(afcls_hat<0);
        afcls_hat(idx) = afcls_hat(idx) ./ s(idx);
        [val, maxIdx] = max(abs(afcls_hat(idx)));
        maxIdx = idx(maxIdx);
        alpha(maxIdx) = 0;
        keep = setdiff(1:size(U, 2), maxIdx);
        U = U(:, keep);
        count = count - 1;
        ref = ref(keep);
    end
    X(:, n1) = alpha;
end

return;


function problem1 ( )

%*****************************************************************************80
%
%% PROBLEM1 is the main program for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROBLEM1\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  A test problem for FD1D_HEAT_STEADY.\n' );

  n = 11;

  a = 0.0;
  b = 1.0;

  ua = 0.0;
  ub = 1.0;

  [ x, u ] = fd1d_heat_steady ( n, a, b, ua, ub, @k1, @f1 );

  plot ( x, u )
  title ( 'Problem 1: Uniform K(X)' );
  xlabel ( 'A <= X <= B' );
  ylabel ( 'Temperature' );

  filename = 'problem1_nodes.txt';
  r8mat_write ( filename, 1, n, x );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X data written to "%s".\n', filename );
  filename = 'problem1_values.txt';
  r8mat_write ( filename, 1, n, u' );
  fprintf ( 1, '  U data written to "%s".\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROBLEM1\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );

  timestamp ( );

  return
end
function value = k1 ( x )

%*****************************************************************************80
%
%% K1 evaluates the heat transfer coefficient K(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the position.
%
%    Output, real VALUE, the value of K(X).
%
  value = 1.0;

  return
end
function value = f1 ( x )

%*****************************************************************************80
%
%% F1 evaluates the right hand side function F(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the position.
%
%    Output, real VALUE, the value of F(X).
%
  value = 0.0;

  return
end


function c = power(a,b)
% POWER implements a.^b, where either a or bor both is an adiff object.

switch [class(a),class(b)]

case 'adiffdouble'
   c = adiff( a.x.^b, rowmult(b.*a.x.^(b-1), a.dx), a.root);

case 'doubleadiff'
   c = adiff( a.^b.x, rowmult(a.^b.x.*log(a), b.dx), b.root);

case 'adiffadiff'
   c = exp(log(a).*b);
   % Commented out code does it the hard way:
   % cx  = a.x.^b.x;
   % cdx = rowmult(cx.*log(a.x), b.dx) + rowmult(a.x.^(b.x-1), a.dx);
   % c   = class(struct('x',cx,'dx',cdx),'adiff');

otherwise
   error(['Can''t do ',class(a),'.^',class(b)]);

end


function [x, c, funVal, ValueL]=tree_mcLogisticR(A, y, z, opts)
%
%%
% Function mcLogisticR:
%      Logistic Loss for Multi-class (task) Learning
%             with the tree structured group Lasso Regularization
%
%% Problem
%
%  min  - sum_{il} weight_{il} log( p_{il} ) + z * sum_i sum_j w_j ||x^i_{G_j}||
%
%  p_{il}= 1 / (1+ exp(-y_i (x_i' * a_i + c_l) ) ) denotes the probability
%  weight_{il} is the weight for the i-th sample in the l-th classifier
%                is a m x k matrix
%  c_l is the intercept for the l-th classfier, and is a 1xk vector
%  x_i denotes the i-th column of x
%  x^j denotes the j-th row of x
%  a_i' denotes the i-th row of A
%
%  y_i (either 1 or -1) is the response
%
%  In this implementation, we assume weight_{il}=1/(mk)
%
%  x^i denotes the i-th row of x
%
%  G_j's are nodes with tree structure
%
%  The tree structured group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%
%  opts.ind(1,:) contains the starting index
%  opts.ind(2,:) contains the ending index
%  opts.ind(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x^j is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind(1,1)=-1; opts.ind(2,1)=-1; and opts.ind(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x^j( opts.ind(1,j):opts.ind(2,j) ) denotes x^j_{G_j}. In this case,
%  the entries in opts.ind(1:2,:) are within 1 and k (the number of tasks).
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x^j( opts.G ( opts.ind(1,j):opts.ind(2,j) ) ) denotes x^j_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind(1:2,:) are within 1 and length(opts.G).
%
% The following example shows how G and ind works:
%
% G={ {1, 2}, {4, 5}, {3, 6}, {7, 8},
%     {1, 2, 3, 6}, {4, 5, 7, 8},
%     {1, 2, 3, 4, 5, 6, 7, 8} }.
%
% ind={ [1, 2, 100]', [3, 4, 100]', [5, 6, 100]', [7, 8, 100]',
%       [9, 12, 100]', [13, 16, 100]', [17, 24, 100]' },
%
% where each node has a weight of 100.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size m x k)
%  z -        Regularization parameter (z >=0)
%  opts-      optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         The obtained weight of size n x k
%  c-         The obtained intercept if size 1 x k
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on October 3, 2010.
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%% Related functions:
%
%  sll_opts
%
%%

%% Verify and initialize the parameters
%%
if (nargin <4)
    error('\n Inputs: A, y, z, and opts (.ind) should be specified!\n');
end

[m,n]=size(A);

k=size(y,2);

if (size(y,1) ~=m)
    error('\n Check the length of y!\n');
end

if (z<0)
    error('\n z should be nonnegative!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

% restart the program for better efficiency
%  this is a newly added function
if (~isfield(opts,'rStartNum'))
    opts.rStartNum=opts.maxIter;
else
    if (opts.rStartNum<=0)
        opts.rStartNum=opts.maxIter;
    end
end

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group and others

% Initialize ind
if (~isfield(opts,'ind'))
    error('\n In tree_mcLeastR, the field .ind should be specified');
else
    ind=opts.ind;

    if (size(ind,1)~=3)
        error('\n Check opts.ind');
    end
end

GFlag=0;
% if GFlag=1, we shall apply general_altra
if (isfield(opts,'G'))
    GFlag=1;

    G=opts.G;
    if (max(G) >k || max(G) <1)
        error('\n The input G is incorrect. It should be within %d and %d',1,k);
    end
end

%% Starting point initialization

% % % compute AT y
% % if (opts.nFlag==0)
% %     ATy =A'* y;
% % elseif (opts.nFlag==1)
% %     ATy= A'* y -  mu' * sum(y, 1);    ATy=ATy./repmat(nu, 1, k);
% % else
% %     invNu=y./repmat(nu, 1, k);        ATy=A'*invNu-mu' * sum(invNu, 1);
% % end

p_flag=(y==1);                  % the indices of the postive samples
m1=sum(p_flag,1);               % the total number of the positive samples
m2=m-m1;                        % the total number of the negative samples
% m1 and m2 are 1 x k vectors

% process the regularization parameter
if (opts.rFlag==0)
    lambda=z;
else % z here is the scaling factor lying in [0,1]
%     if (z<0 || z>1)
%         error('\n opts.rFlag=1, and z should be in [0,1]');
%     end

    % we compute ATb for computing lambda_max, when the input z is a ratio

    computedFlag=0;
    if (isfield(opts,'lambdaMax'))
        if (opts.lambdaMax~=-1)
            lambda=z*opts.lambdaMax;
            computedFlag=1;
        end
    end

    if (~computedFlag)

        b=zeros(m, k);                  % b is of size m x k
        b= b + p_flag.*repmat(m2,m,1);
        b= b -(~p_flag).*repmat(m1,m,1);
        b=b/m;

        % compute AT b
        if (opts.nFlag==0)
            ATb =A'* b;
        elseif (opts.nFlag==1)
            ATb= A'* b -  mu' * sum(b, 1);    ATb=ATb./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        ATb=A'*invNu-mu' * sum(invNu, 1);
        end

        if (GFlag==0)
            lambda_max=findLambdaMax_mt(ATb, n, k, ind, size(ind,2));
        else
            lambda_max=general_findLambdaMax_mt(ATb, n, k, G, ind, size(ind,2));
        end

        lambda_max= lambda_max / (m*k);

        % As .rFlag=1, we set lambda as a ratio of lambda_max
        lambda=z*lambda_max;
    end
end

% The following is for computing lambdaMax
% we use opts.lambdaMax=-1 to show that we need the computation.
%
% One can use this for setting up opts.lambdaMax
if (isfield(opts,'lambdaMax'))

    if (opts.lambdaMax==-1)

        b=zeros(m, k);                  % b is of size m x k
        b= b + p_flag.*repmat(m2,m,1);
        b= b -(~p_flag).*repmat(m1,m,1);
        b=b/m;

        % compute AT b
        if (opts.nFlag==0)
            ATb =A'* b;
        elseif (opts.nFlag==1)
            ATb= A'* b -  mu' * sum(b, 1);    ATb=ATb./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        ATb=A'*invNu-mu' * sum(invNu, 1);
        end

        % compute lambda_max
        if (GFlag==0)
            lambda_max=findLambdaMax_mt(ATb, n, k, ind, size(ind,2));
        else
            lambda_max=general_findLambdaMax_mt(ATb, n, k, G, ind, size(ind,2));
        end

        x=lambda_max;
        funVal=lambda_max;
        ValueL=lambda_max;

        return;
    end
end

% initialize a starting point
if opts.init==2
    x=zeros(n,k); c=log(m1./m2);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if ( size(x,1)~=n || size(x,2)~=k )
            error('\n Check the input .x0');
        end
    else
        x=zeros(n,k);
    end

    if isfield(opts,'c0')
        c=opts.c0;

        if ( length(c)~=k )
            error('\n Check the input .c0');
        end
    else
        c=log(m1./m2);
    end
end

% compute Ax= A * x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
end

%% The main program

if (opts.mFlag==0 && opts.lFlag==0)
    % The Armijo Goldstein line search schemes + accelearted gradient descent

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/(m*k); % the intial guess of the Lipschitz continuous gradient

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);
    cp=c;         ccp=zeros(1,k);

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;  sc=c + beta* ccp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % aa= - diag(y) * (A * s + sc)
        aa=- y.*(As+ repmat(sc,m,1));

        % fun_s is the logistic loss at the search point
        bb=max(aa,0);
        fun_s= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

        % compute prob=[p_1;p_2;...;p_m]
        prob=1./( 1+ exp(aa) );

        % b= - diag(y) * (1 - prob)
        b= -y.*(1-prob) / (m * k);

        gc=sum(b); % the gradient of c

        % compute g= AT b, the gradient of x
        if (opts.nFlag==0)
            g=A'*b;
        elseif (opts.nFlag==1)
            g= A'* b -  mu' * sum(b, 1);  g=g./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        g=A'*invNu-mu' * sum(invNu, 1);
        end

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        cp=c;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L; c= sc- gc/L;

            % tree overlapping group Lasso projection
            ind_work(1:2,:)=ind(1:2,:);
            ind_work(3,:)=ind(3,:) * (lambda / L);

            if (GFlag==0)
                x=altra_mt(v, n, k, ind_work, size(ind_work,2));
            else
                x=general_altra_mt(v, n, k, G, ind_work, size(ind_work,2));
            end

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute Ax= A * x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Ax+ repmat(c,m,1));

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

            r_sum=(norm(v,'fro')^2 + norm(c-sc,2)^2) / 2;
            l_sum=fun_x - fun_s - sum(sum(v.* g)) - (c-sc)* gc';

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + <v, g> + <c ,gc>
            %                           + L/2 * (<v,v> + <c-sc,c-sc> )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;

        xxp=x-xp;  ccp=c-cp;

        funVal(iterStep)=fun_x;

        for i=1:n
            xRow=x(i,:);

            if (GFlag==0)
                tree_norm=treeNorm(xRow, k, ind, size(ind,2));
            else
                tree_norm=general_treeNorm(xRow, k, G, ind, size(ind,2));
            end

            funVal(iterStep)=funVal(iterStep)+ lambda*tree_norm;
        end

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
else
    error('\n The function does not support opts.mFlag neq 0 & opts.lFlag neq 0!');
end


%  Figure 10.60      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% script for disk read/write head control design
% the design is based on 1/s^2 with robustness
% with respect to a resonance of damping zeta = z at wm.
% the design calls for a single lead selected to maximize wc
% the phase margin is set by alpha = a
% the compensator has a second order roll-off filter
% the resonance is 'gain stabilized' with GM
% the time is scaled to milliseconds
wm= 5*pi
z=.05;
GM = 4;
a=0.1;
% input the nominal plant with a gain of 1.
numGo=1;
denGo= [1 0 0];
sysGo=tf(numGo,denGo);
% input the plant with the resonance
numG= [1/(50*pi) 1];
denG=[1/(25*pi^2) 1/(50*pi) 1  0 0];
sysG=tf(numG,denG);
% Define the design constant
A = ((a*2*z)/(GM))^.333;
% PM >50; we have selected alpha = a = 0.1
Tinv= A*wm*sqrt(a);
T=1/Tinv;
wc =  A*wm
% K =  wc^2;
K=  wc^2;
% Input the lead compensation
numD =K*sqrt(a)*[T 1];
denD = [a*T 1];
sysD=tf(numD,denD);
% Now design the roll-off filter
w1=wm*sqrt(A/sqrt(a));
% design the roll-off damping
z1=.3 ;
numF=1;
denF=[1/w1^2  2*z1/w1 1];
sysF=tf(numF,denF);
% now compute the entire compensation
sysDc=sysD*sysF;
sysOLo=sysD*sysGo;
sysOL=sysDc*sysG;
% bode(sysOLo);
% hold on
clf;
sysH=tf(1,1);
sysCL=feedback(sysOL,sysH);
t=0:0.1:6;
[y,t]=step(sysCL,t);
plot(t,y);
xlabel('Time (msec)');
ylabel('Amplitude');
grid on
title('Fig. 10.60 Step response of disk control;lead plus roll-off ')


classdef Quadrature_Triangle < Quadrature

    methods
        function computeQuadrature(obj,order)
            computeQuadrature@Quadrature(obj,order);
            switch order
                case 'CONSTANT'
                    obj.ngaus = 1;
                    obj.weigp = 1/2;
                    obj.posgp = [1/3;1/3];

                case 'LINEAR'
                    obj.ngaus = 1;
                    obj.weigp = [1/2];
                    obj.posgp = [1/3;1/3];

                case 'QUADRATIC'
                    %obj.ngaus = 3;
                    %obj.weigp = [1/6;1/6;1/6];
                    %obj.posgp = [0,0.5;0.5,0;0.5,0.5]';
                    obj.ngaus = 3;
                    obj.weigp = [1/6;1/6;1/6];
                    obj.posgp = [2/3,1/6;1/6,1/6;1/6,2/3]';
                case 'CUBIC'
                    obj.ngaus = 4;
                    obj.weigp = [-27/96;25/96;25/96;25/96];
                    obj.posgp = [1/3,1/3;1/5,1/5;3/5,1/5;1/5,3/5]';

                case 'QUADRATIC2'
                    obj.ngaus = 2;
                    obj.weigp = [1/6;1/6;1/6];
                    obj.posgp = [1/6,1/6;2/3,1/6;1/6,2/3]';

                case 'QUADRATICMASS'
                    obj.ngaus = 3;
                    obj.weigp = [1/6;1/6;1/6];
                    obj.posgp = [2/3,1/6,1/6;1/6,2/3,1/6];
                otherwise
                    disp('Quadrature not implemented for triangle elements')
            end
        end
    end
end


function y = atanh_pos(x,rnd)
% rigorous atanh for nonnegative double vector x<1 rounded corresponding to rnd
% for internal use in rigorous atanh, acoth

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  major revision
%

  y = x;

  % atanh(x) = log( 1 + 2*x/(1-x) ) / 2

  index = ( x<0.33 );                    % 0 <= x < .33
  if any(index)
    X = x(index);
    setround(-rnd)
    E = 1 - X;
    setround(rnd)
    E = 2*X./E;
    y(index) = log_1( E , rnd ) / 2;     % 0 <= E < 1
  end

  index = ~index;
  if any(index)                          % .33 <= x <= 1
    X = x(index);
    setround(-rnd)
    E = 1 - X;
    setround(rnd)
    E = 1 + 2*X./E;
    y(index) = log_rnd( E , rnd ) / 2;
  end


function p_ = EntropyProg(p,A,b,Aeq,beq)
% This function computes the entropy-pooling change of measure, see
% "A. Meucci - Fully Flexible Views: Theory and Practice -
% The Risk Magazine, October 2008, p 97-102"
% available at www.symmys.com > Research > Working Papers

% Code by A. Meucci, November 2008
% Last version available at www.symmys.com > Teaching > MATLAB

K_=size(A,1);
K=size(Aeq,1);
A_=A';
b_=b';
Aeq_=Aeq';
beq_=beq';
x0=zeros(K_+K,1);
InqMat=-eye(K_+K); InqMat(K_+1:end,:)=[];
InqVec=zeros(K_,1);

options = optimset('GradObj','on','Hessian','on');
if ~K_
    v=fminunc(@nestedfunU,x0,options);
    p_=exp(log(p)-1-Aeq_*v);
else
    lv=fmincon(@nestedfunC,x0,InqMat,InqVec,[],[],[],[],[],options);
    l=lv(1:K_);
    v=lv(K_+1:end);
    p_=exp(log(p)-1-A_*l-Aeq_*v);
end

    function [mL g H] = nestedfunU(v)

        x=exp( log(p)-1-Aeq_*v );
        x=max(x,10^(-32));
        L=x'*(log(x)-log(p)+Aeq_*v)-beq_*v;
        mL=-L;

        g = [beq-Aeq*x];
        H = [Aeq*((x*ones(1,K)).*Aeq_)];  % Hessian computed by Chen Qing, Lin Daimin, Meng Yanyan, Wang Weijun
    end

    function [mL g H] = nestedfunC(lv)

        l=lv(1:K_);
        v=lv(K_+1:end);
        x=exp( log(p)-1-A_*l-Aeq_*v );
        x=max(x,10^(-32));
        L=x'*(log(x)-log(p))+l'*(A*x-b)+v'*(Aeq*x-beq);
        mL=-L;

        g = [b-A*x; beq-Aeq*x];
        H = [A*((x*ones(1,K_)).*A_)  A*((x*ones(1,K)).*Aeq_) % Hessian computed by Chen Qing, Lin Daimin, Meng Yanyan, Wang Weijun
            Aeq*((x*ones(1,K_)).*A_)   Aeq*((x*ones(1,K)).*Aeq_)];
    end
end


function [ p, q, pdf ] = normp ( z )

%*****************************************************************************80
%
%% NORMP computes the cumulative density of the standard normal distribution.
%
%  Discussion:
%
%    This is algorithm 5666 from Hart, et al.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Alan Miller.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
%    Charles Mesztenyi, John Rice, Henry Thacher,
%    Christoph Witzgall,
%    Computer Approximations,
%    Wiley, 1968,
%    LC: QA297.C64.
%
%  Parameters:
%
%    Input, real Z, divides the real ( kind = 8 ) line into two
%    semi-infinite intervals, over each of which the standard normal
%    distribution is to be integrated.
%
%    Output, real P, Q, the integrals of the standard normal
%    distribution over the intervals ( - Infinity, Z] and
%    [Z, + Infinity ), respectively.
%
%    Output, real PDF, the value of the standard normal distribution
%    at Z.
%
  cutoff = 7.071;
  p0 = 220.2068679123761;
  p1 = 221.2135961699311;
  p2 = 112.0792914978709;
  p3 = 33.91286607838300;
  p4 = 6.373962203531650;
  p5 = 0.7003830644436881;
  p6 = 0.03526249659989109;
  q0 = 440.4137358247522;
  q1 = 793.8265125199484;
  q2 = 637.3336333788311;
  q3 = 296.5642487796737;
  q4 = 86.78073220294608;
  q5 = 16.06417757920695;
  q6 = 1.755667163182642;
  q7 = 0.08838834764831844;
  root2pi = 2.506628274631001;

  zabs = abs ( z );
%
%  37 < |Z|.
%
  if ( 37.0  < zabs )

    pdf = 0.0;
    p = 0.0;
%
%  |Z| <= 37.
%
  else

    expntl = exp ( - 0.5  * zabs * zabs );
    pdf = expntl / root2pi;
%
%  |Z| < CUTOFF = 10 / sqrt(2).
%
    if ( zabs < cutoff )

      p = expntl * (((((( ...
          p6   * zabs ...
        + p5 ) * zabs ...
        + p4 ) * zabs ...
        + p3 ) * zabs ...
        + p2 ) * zabs ...
        + p1 ) * zabs ...
        + p0 ) / ((((((( ...
          q7   * zabs ...
        + q6 ) * zabs ...
        + q5 ) * zabs ...
        + q4 ) * zabs ...
        + q3 ) * zabs ...
        + q2 ) * zabs ...
        + q1 ) * zabs ...
        + q0 );
%
%  CUTOFF <= |Z|.
%
    else

      p = pdf / ( ...
        zabs + 1.0  / ( ...
        zabs + 2.0  / ( ...
        zabs + 3.0  / ( ...
        zabs + 4.0  / ( ...
        zabs + 0.65  )))));

    end

  end

  if ( z < 0.0  )
    q = 1.0  - p;
  else
    q = p;
    p = 1.0  - q;
  end

  return
end


%% Image Alignment using ECC algorithm
%
% This sample demonstrates the use of the ECC image alignment algorithm. When
% one image is given, the template image is artificially formed by a random
% warp given the motion type. Otherwise supply both images. If input warp
% matrix is not specified, the identity transformation is used to initialize
% the algorithm.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/image_alignment.cpp>
%

%% Options
INPUT_IMAGE = fullfile(mexopencv.root(), 'test', 'fruits.jpg');
LOAD_TEMPLATE = false;
LOAD_MAT = false;
MOTION_TYPE = 'Affine';

%%
% type of motion
MOTION_TYPE = validatestring(MOTION_TYPE, ...
    {'Translation', 'Euclidean', 'Affine', 'Homography'});

%%
% termination criteria (ECC iterations and convergnce epsilon)
CRIT = struct('type','Count+EPS', 'maxCount',50, 'epsilon',1e-4);
if CRIT.maxCount > 200
    warning('Too many iterations');
end

%% Input image
% load input image
imgInput = cv.imread(INPUT_IMAGE, 'Grayscale',true);

%% Template image
% initialize or load template image
M0 = [];
if LOAD_TEMPLATE
    % load an existing template image
    fmts = imformats();
    filtspec = strjoin(strcat('*.', [fmts.ext]), ';');
    [fn,fp] = uigetfile(filtspec, 'Select an image');
    if fp==0, error('No file selected'); end
    imgTemplate = cv.imread(fullfile(fp,fn), 'Grayscale',true);
else
    % create a template image by applying a random warp to input image
    imgInput = cv.resize(imgInput, [216 216]);
    opts = {'DSize',[200 200], 'Interpolation','Linear', 'WarpInverse',true};
    switch MOTION_TYPE
        case 'Translation'
            M0 = [1 0 rand*10+10;
                  0 1 rand*10+10];
            imgTemplate = cv.warpAffine(imgInput, M0, opts{:});
        case 'Euclidean'
            theta = pi/30 + pi*randi([-2 2])/180;
            M0 = [cos(theta) -sin(theta) rand*10+10;
                  sin(theta)  cos(theta) rand*10+10];
            imgTemplate = cv.warpAffine(imgInput, M0, opts{:});
        case 'Affine'
            M0 = [1-(rand*0.1-0.05)    rand*0.06-0.03 rand*10+10;
                     rand*0.06-0.03 1-(rand*0.1-0.05) rand*10+10];
            imgTemplate = cv.warpAffine(imgInput, M0, opts{:});
        case 'Homography'
            M0 = [1-(rand*0.1-0.05)    rand*0.06-0.03 rand*10+10;
                     rand*0.06-0.03 1-(rand*0.1-0.05) rand*10+10;
                     rand*2e-4+2e-4    rand*2e-4+2e-4 1];
            imgTemplate = cv.warpPerspective(imgInput, M0, opts{:});
    end
    display(M0)  % ground truth
end
sz = size(imgTemplate);

%% Input warp matrix
% initialize or load warp matrix
M = [];
if LOAD_MAT
    % load from a MAT-file
    uiopen('load');
    assert(~isempty(M), 'Failed to load warp matrix M');
    if strcmp(MOTION_TYPE, 'Homography')
        assert(isequal(size(M), [3 3]));
    else
        assert(isequal(size(M), [2 3]));
    end
else
    % identity matrix
    if strcmp(MOTION_TYPE, 'Homography')
        M = eye(3,3);
    else
        M = eye(2,3);
    end
    warning(['Performance Warning: Identity warp ideally assumes images ' ...
        'of similar size. If the deformation is strong, the identity ' ...
        'warp may not be a good initialization, and estimation may fail.']);
end
display(M)

%% Estimate transformation
fprintf('Estimating "%s" transformation...\n', MOTION_TYPE);
tic
M = cv.findTransformECC(imgTemplate, imgInput, ...
    'InputWarp',M, 'MotionType',MOTION_TYPE, 'Criteria',CRIT);
toc
display(M)

%%
% compare against ground truth
if ~isempty(M0)
    err = norm(M - M0)
end

%% Apply estimated transformation
% warped image
opts = {'DSize',[sz(2) sz(1)], 'Interpolation','Linear', 'WarpInverse',true};
if strcmp(MOTION_TYPE, 'Homography')
    imgWarped = cv.warpPerspective(imgInput, M, opts{:});
else
    imgWarped = cv.warpAffine(imgInput, M, opts{:});
end

%%
% compare against template image
if ~mexopencv.isOctave() && mexopencv.require('vision')
    imgError = imfuse(imgTemplate, imgWarped, 'diff');
else
    imgError = abs(double(imgTemplate) - double(imgWarped));
    imgError = imgError / max(imgError(:));
end

%%
% compute region boundary in input image corresponding to template image
pts = [1 1; sz(2) 1; sz(2) sz(1); 1 sz(1)]; % corners to warp (TL/TR/BR/BL)
pts(:,3) = 1;  % homogeneous coordinates
if strcmp(MOTION_TYPE, 'Homography')
    pts = pts * M.';
else
    pts = pts * [M; 0 0 1].';
end
pts = bsxfun(@rdivide, pts(:,1:2), pts(:,3));

%%
% show results
subplot(221), imshow(imgInput), title('image')
line(pts([1:end 1],1), pts([1:end 1],2), 'Color','m', 'LineWidth',2)
axis tight
subplot(222), imshow(imgTemplate), title('template')
subplot(223), imshow(imgWarped), title('warped image')
subplot(224), imshow(imgError), title('error')


function EXP_A = get_RS_matrix_exponential( Q, dt, xi, drifts, vols, psi_J)
N = length(xi);
m_0 = length(drifts);

Qt = dt*Q';
EXP_A = ones(m_0,m_0,N);

drifts = dt*drifts;
vols = (0.5*dt)*vols.^2;

if nargin < 8
    for j = 1:N
        EXP_A(:,:,j) = expm(Qt + diag(drifts*xi(j) - vols*xi(j)^2));  %This incorporates the dt
    end
else
    for j = 1:N
        EXP_A(:,:,j) = expm(Qt + diag(drifts*xi(j) - vols*xi(j)^2 + dt*psi_J(xi(j))));  %This incorporates the dt
    end
end

end


function varargout = chol(varargin)
%CHOL   Cholesky factorization of a DISKFUN.
%
%   R = CHOL( F ), if F is a nonnegative definite DISKFUN then this returns
%   an upper triangular quasimatrix so that R'*R is a decomposition of F.
%   If F is not nonnegative definite then an error is thrown.
%
%   L = CHOL(F, 'lower'), if F is a nonnegative definite DISKFUN then this
%   produces a lower triangular quasimatrix so that L*L' is a decomposition
%   of F. If F is not nonnegative definite then an error is thrown.
%
%   [R, p] = CHOL( F ), with two outputs never throwns an error message. If
%   F is nonnegative definite then p is 0 and R is the same as above. If F
%   is symmetric but negative definite or semidefinite then p is a positive
%   integer such that R has p columns and R'*R is a rank p nonnegative
%   definite DISKFUN that approximates F. This is particular useful when F
%   is nonnegative definite, but rounding error have perturbed it to be
%   semidefinite.
%
%   [L, p] = CHOL(F, 'lower') same as above but the first argument is lower
%   triangular.
%
%   For more information about the factorization:
%   A. Townsend and L. N. Trefethen, Continuous analogues of matrix
%   factorizations, Proc. Royal Soc. A., 2015.
%
% See also DISKFUN/LU, and DISKFUN/QR.

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

% Requires evaluations of f in polar coords once passed to separableApprox,
% so we make a chebfun2:
f = cart2pol(varargin{1}, 'cdr');

[varargout{1:nargout}] = chol@separableApprox(f, varargin{2:end});

end


function filterSteerable( theta )
% Steerable 2D Gaussian derivative filter (for visualization).
%
% This function is a demonstration of steerable filters.  The directional
% derivative of G in an arbitrary direction theta can be found by taking a
% linear combination of the directional derivatives dxG and dyG.
%
% USAGE
%  filterSteerable( theta )
%
% INPUTS
%  theta   - orientation in radians
%
% OUTPUTS
%
% EXAMPLE
%  filterSteerable( pi/4 );
%
% See also filterGauss
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

% Get G
[x,y]=meshgrid(-1:.1:1, -1:.1:1 );
r = sqrt( x.^2 + y.^2 );
G = exp( -r .* r *2 );

% get first derivatives of G.  note: d/dx(G)=-2x*G
phi = atan2( y, x );
dxG = r .* cos(phi) .* G;
dyG = r .* sin(phi) .* G;

% get directional derivative by taking linear comb in theta
Gtheta = cos(theta)*dxG + sin(theta)*dyG;

% dislpay (scale for visualization purposes)
GS = cat(3,G,dxG*2,dyG*2,Gtheta*2);
figure(1); montage2( GS );


function x = r8to_sl ( n, a, b, job )

%*****************************************************************************80
%
%% R8TO_SL solves a R8TO system.
%
%  Discussion:
%
%    The R8TO storage format is used for a Toeplitz matrix, which is constant
%    along diagonals.  Thus, in an N by N Toeplitz matrix, there are at most
%    2*N-1 distinct entries.  The format stores the N elements of the first
%    row, followed by the N-1 elements of the first column (skipping the
%    entry in the first row).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(2*N-1), the R8TO matrix.
%
%    Input, real B(N) the right hand side vector.
%
%    Input, integer JOB,
%    0 to solve A*X=B,
%    nonzero to solve A'*X=B.
%
%    Output, real X(N), the solution vector.
%
  if ( n < 1 )
    x = [];
    return
  end
%
%  Solve the system with the principal minor of order 1.
%
  r1 = a(1);
  x(1) = b(1) / r1;

  if ( n == 1 )
    return
  end
%
%  Recurrent process for solving the system with the Toeplitz matrix.
%
  for nsub = 2 : n
%
%  Compute multiples of the first and last columns of the inverse of
%  the principal minor of order NSUB.
%
    if ( job == 0 )
      r5 = a(n+nsub-1);
      r6 = a(nsub);
    else
      r5 = a(nsub);
      r6 = a(n+nsub-1);
    end

    if ( 2 < nsub )

      c1(nsub-1) = r2;

      for i = 1 : nsub-2
        if ( job == 0 )
          r5 = r5 + a(n+i) * c1(nsub-i);
          r6 = r6 + a(i+1) * c2(i);
        else
          r5 = r5 + a(i+1) * c1(nsub-i);
          r6 = r6 + a(n+i) * c2(i);
        end
      end

    end

    r2 = -r5 / r1;
    r3 = -r6 / r1;
    r1 = r1 + r5 * r3;

    if ( 2 < nsub )

      r6 = c2(1);
      c2(nsub-1) = 0.0E+00;

      for i = 2 : nsub-1
        r5 = c2(i);
        c2(i) = c1(i) * r3 + r6;
        c1(i) = c1(i) + r6 * r2;
        r6 = r5;
      end

    end

    c2(1) = r3;
%
%  Compute the solution of the system with the principal minor of order NSUB.
%
    if ( job == 0 )
      r5 = a(n+1:n+nsub-1) * x(nsub-1:-1:1)';
    else
      r5 = a(2:nsub) * x(nsub-1:-1:1)';
    end

    r6 = ( b(nsub) - r5 ) / r1;

    x(1:nsub-1) = x(1:nsub-1) + c2(1:nsub-1) * r6;
    x(nsub) = r6;

  end

  return
end


function [ ur, vr, pr ] = resid_stokes1 ( n, x, y )

%*****************************************************************************80
%
%% RESID_STOKES1 returns residuals of the exact Stokes solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Junping Wang, Yanqiu Wang, Xiu Ye,
%    A robust numerical method for Stokes equations based on divergence-free
%    H(div) finite element methods,
%    SIAM Journal on Scientific Computing,
%    Volume 31, Number 4, 2009, pages 2784-2802.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), Y(N), the coordinates of the points.
%
%    Output, real UR(N), VR(N), PR(N), the residuals in the U,
%    V and P equations.
%
  ur = zeros ( n, 1 );
  vr = zeros ( n, 1 );
  pr = zeros ( n, 1 );
%
%  Get the right hand sides.
%
  [ f, g, h ] = rhs_stokes1 ( n, x, y );
%
%  Form the functions and derivatives.
%
  for i = 1 : n

    u = - 2.0 ...
          * x(i) ^ 2 * ( x(i) - 1.0 ) ^ 2  ...
          * y(i) * ( y(i) - 1.0 ) * ( 2.0 * y(i) - 1.0 );

    ux = - 2.0  ...
          * ( 4.0 * x(i) ^ 3 - 6.0 * x(i) ^ 2  ...
          + 2.0 * x(i) ) ...
          * y(i) * ( y(i) - 1.0 ) * ( 2.0 * y(i) - 1.0 );

    uxx = - 2.0  ...
          * ( 12.0 * x(i) ^ 2 - 12.0 * x(i) + 2.0 ) ...
          * ( 2.0 * y(i) ^ 3 - 3.0 * y(i) ^ 2 + y(i) );

    uy = - 2.0  ...
          * x(i) ^ 2 * ( x(i) - 1.0 ) ^ 2  ...
          * ( 6.0 * y(i) ^ 2 - 3.0 * y(i) + 1.0 );

    uyy = - 2.0  ...
          * ( x(i) ^ 4 - 2.0 * x(i) ^ 3 + x(i) ^ 2 ) ...
          * ( 12.0 * y(i) - 6.0 );

    v =   2.0  ...
          * x(i) * ( x(i) - 1.0 ) * ( 2.0 * x(i) - 1.0 ) ...
          * y(i) ^ 2 * ( y(i) - 1.0 ) ^ 2;

    vx =   2.0  ...
          * ( 6.0 * x(i) ^ 2 - 6.0 * x(i) + 1.0 ) ...
          * y(i) ^ 2 * ( y(i) - 1.0 ) ^ 2;

    vxx =   2.0  ...
          * ( 12.0 * x(i) - 6.0 ) ...
          * y(i) ^ 2 * ( y(i) - 1.0 ) ^ 2;

    vy =   2.0  ...
          * x(i) * ( x(i) - 1.0 ) * ( 2.0 * x(i) - 1.0 ) ...
          * ( 4.0 * y(i) ^ 3 - 6.0 * y(i) ^ 2  ...
          + 2.0 * y(i) );

    vyy =   2.0  ...
          * x(i) * ( x(i) - 1.0 ) * ( 2.0 * x(i) - 1.0 ) ...
          * ( 12.0 * y(i) ^ 2 - 12.0 * y(i) + 2.0 );

    p = 0.0;
    px = 0.0;
    py = 0.0;

    ur(i) = px - ( uxx + uyy ) - f(i);
    vr(i) = py - ( vxx + vyy ) - g(i);
    pr(i) = ux + vy - h(i);

  end

  return
end


% SOSDEMO5 --- Upper bound for the structured singular value mu
% Section 3.5 of SOSTOOLS User's Manual
%

clear; echo on;
syms x1 x2 x3 x4 x5 x6 x7 x8;
vartable = [x1; x2; x3; x4; x5; x6; x7; x8];

% The matrix under consideration
alpha = 3 + sqrt(3);
beta = sqrt(3) - 1;
a = sqrt(2/alpha);
b = 1/sqrt(alpha);
c = b;
d = -sqrt(beta/alpha);
f = (1 + i)*sqrt(1/(alpha*beta));
U = [a 0; b b; c i*c; d f];
V = [0 a; b -b; c -i*c; -i*f -d];
M = U*V';

% Constructing A(x)'s
gam = 0.8724;

Z = monomials(vartable,1);
for i = 1:4
    H = M(i,:)'*M(i,:) - (gam^2)*sparse(i,i,1,4,4,1);
    H = [real(H) -imag(H); imag(H) real(H)];
    A{i} = (Z.')*H*Z;
end;

% =============================================
% Initialize the sum of squares program
prog = sosprogram(vartable);

% =============================================
% Define SOSP variables

% -- Q(x)'s -- : sums of squares
% Monomial vector: [x1; ... x8]
for i = 1:4
    [prog,Q{i}] = sossosvar(prog,Z);
end;

% -- r's -- : constant sum of squares
Z = monomials(vartable,0);
r = cell(4,4);
for i = 1:4
    for j = (i+1):4
        [prog,r{i,j}] = sossosvar(prog,Z,'wscoeff');
    end;
end;

% =============================================
% Next, define SOSP constraints

% Constraint : -sum(Qi(x)*Ai(x)) - sum(rij*Ai(x)*Aj(x)) + I(x) >= 0
expr = 0;
% Adding term
for i = 1:4
    expr = expr - A{i}*Q{i};
end;
for i = 1:4
    for j = (i+1):4
        expr = expr - A{i}*A{j}*r{i,j};
    end;
end;
% Constant term: I(x) = -(x1^4 + ... + x8^4)
I = -sum(vartable.^4);
expr = expr + I;

prog = sosineq(prog,expr);

% =============================================
% And call solver
prog = sossolve(prog);

% =============================================
% Program is feasible, thus 0.8724 is an upper bound for mu.

echo off


function w_nulldiff = sphNullformer_diff(order, src_dirs)
%SPHNULLFORMER_DIFF Beamweights for a beamformer with nulls at specified directions,
%and an omnidirectional constraint
%
%   For a set of K directions, computes the beamforming weights that places
%   nulls at them while trying to keep the overall response as
%   omnidirectional as possible. Useful when trying to extract
%   reverberant/diffuse sound while suppressing strong directional sources.
%   From a statistical formulation, it is similar to the beamformer
%   proposed in
%
%       Thiergart, O. and Habets, E.A., 2014.
%       Extracting reverberant sound using a linearly constrained minimum variance spatial filter.
%       IEEE Signal Processing Letters, 21(5), pp.630-634.
%
%   but in the SHD instead of the microphone signal domain.
%
%   Inputs:
%       order:  order of SH signals
%       src_dirs:  Kx2 [azi elev] directions to place the nulls
%
%   Outputs:
%       w_nulldiff:  (order+1)^2xK matrix of beamweights, one set per
%           column for each beamforming direction
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHNULLFORMER_DIFF.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    nSH = (order+1)^2;
    nSrc = size(src_dirs,1);
    src_dirs2 = [src_dirs(:,1) pi/2-src_dirs(:,2)]; % convert from azi-elev to azi-incl

    % steering vectors in the SHD
    Y_nulldiff = getSH(order, src_dirs2, 'real');

    % compute weight vector
    c = zeros(nSrc+1,1);
    c(1) = 1; % constraint vector
    g = zeros(nSH,1);
    g(1) = 1;
    A = [g Y_nulldiff'];
    w_nulldiff = pinv(A')*c;
end


function H = compute_infos(I)
level = 256;
dat = double(I);
p = zeros(1, level);
for i = 1 : size(dat, 1)
    for j = 1 : size(dat, 2)
        temp = dat(i,j);
        p(1, temp+1) = p(1, temp+1) + 1;
    end
end
p = p/(size(dat, 1)*size(dat, 2));
H = 0;
for i = 1 : level
    if p(i) ~= 0
        H = H + p(i)*log2(p(i));
    end
end
H = -H;


function homofil(im,d,r,c,n)
%%%%%%%%%%Butterworth high pass filter %%%%%%%%%%%%%%
A=zeros(r,c);
for i=1:r
    for j=1:c
        A(i,j)=(((i-r/2).^2+(j-c/2).^2)).^(.5);
        H(i,j)=1/(1+((d/A(i,j))^(2*n)));
    end
end
%%%%%%%%%%%%%Using it for my application as homomorphic filtering is
%%%%%%%%%%%%%application specific, taking the value of alphaL and alphaH
%%%%%%%%%%%%%values accordingly.
alphaL=.0999;
aplhaH=1.01;
H=((aplhaH-alphaL).*H)+alphaL;
H=1-H;
%%%%%log of image
im_l=log2(1+im);
%%%%%DFT of logged image
im_f=fft2(im_l);
%%%%%Filter Applying DFT image
im_nf=H.*im_f;
%%%%Inverse DFT of filtered image
im_n=abs(ifft2(im_nf));
%%%%%Inverse log
im_e=exp(im_n);
% subplot(1,2,2);
subplot(122)
imshow((im_e),[])

% figure
% imshow(H)


function combo_test18 ( )

%*****************************************************************************80
%
%% COMBO_TEST18 tests NPART_ENUM, _RSF_LEX_RANK, _RSF_LEX_SUCCESSOR, _RSF_LEX_UNRANK.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  npart = 3;
  n = 12;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST18\n' );
  fprintf ( 1, '  Partitions of N with NPART parts\n' );
  fprintf ( 1, '  in reverse standard form:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  NPART_ENUM enumerates,\n' );
  fprintf ( 1, '  NPART_RSF_LEX_RANK ranks,\n' );
  fprintf ( 1, '  NPART_RSF_LEX_SUCCESSOR lists;\n' );
  fprintf ( 1, '  NPART_RSF_LEX_UNRANK unranks.\n' );
%
%  Enumerate.
%
  npartitions = npart_enum ( n, npart );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  For N = %d\n', n );
  fprintf ( 1, '  and NPART = %d\n', npart );
  fprintf ( 1, '  the number of partitions is %d\n', npartitions );
  fprintf ( 1, ' \n' );
%
%  List.
%
  t = [];
  rank = -1;

  while ( 1 )

    rank_old = rank;

    [ t, rank ] = npart_rsf_lex_successor ( n, npart, t, rank );

    if ( rank <= rank_old )
      break
    end

    fprintf ( 1, '  %4d  ', rank );
    for i = 1 : npart
      fprintf ( 1, '%5d', t(i) );
    end
    fprintf ( 1, '\n' );

  end
%
%  Unrank.
%
  rank = floor ( npartitions / 3 );

  t = npart_rsf_lex_unrank ( rank, n, npart );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  The element of rank %d:\n', rank );
  fprintf ( 1, ' \n' );
  i4vec_print ( npart, t, '  The element:' );
%
%  Rank.
%
  rank = npart_rsf_lex_rank ( n, npart, t );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  The rank of the element is computed as %d:\n', rank );

  return
end


function stroud_test46 ( )

%*****************************************************************************80
%
%% TEST46 tests TORUS_5S2, TORUS_6S2, TORUS_14S.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  r1 = 0.5;
  r2 = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST46\n' );
  fprintf ( 1, '  For the interior of a torus,\n' );
  fprintf ( 1, '  TORUS_5S2,\n' );
  fprintf ( 1, '  TORUS_6S2, and\n' );
  fprintf ( 1, '  TORUS_5S2 approximate integrals.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Inner radius = %f\n', r1 );
  fprintf ( 1, '  Outer radius = %f\n', r2 );
  fprintf ( 1, '  Volume = %f\n', torus_volume_3d ( r1, r2 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Rule:        #5S2          #6S2          #14S\n' );
  fprintf ( 1, '    F(X)\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = torus_5s2 ( 'function_3d', r1, r2 );
    result2 = torus_6s2 ( 'function_3d', r1, r2 );
    result3 = torus_14s ( 'function_3d', r1, r2 );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %s  %12f  %12f  %12f\n', fname, result1, result2, result3 );

  end

  return
end


function xList=RKAdaptiveAtTimes(xInit,theTimes,f,initStepSize,order,solutionChoice,RelTol,AbsTol,maxSteps)
%%RKADAPTIVEATTIMES Perform multiple steps of Runge-Kutta propagation
%                   using an adaptive step size. Runge-Kutta methods are
%                   derivative-free techniques for solving ordinary
%                   differential equations. That is, integrating
%                   dx/dt=f(x,t) given initial conditions
%                   (xStart,tSpan(1)). More information on available
%                   algorithms for the steps is given in the comments to
%                   the function RungeKStep.
%
%INPUTS: xInit The initial value of the state (scalar or vector) over which
%              integration is being performed.
%     theTimes The times at which state estimates are desired. theTimes(1)
%              is the time of xInit.
%            f f(xVal,curT) returns the derivative of xVal taken at time
%              curT.
% initStepSize An optional initial step size (in t) to use for the
%              integration. If omitted or an empty matrix is passed, an
%              ad-hoc method is used to find an initial step size.
%        order The order of the Runge-Kutta method. If this parameter is
%              omitted, then the default order of 4 is used. Order can
%              range from 1 to 7.
%  solutionChoice When multiple formulae are implemented, this selects
%              which one to use. Otherwise, this parameter is not used.
%       RelTol The maximum relative error tolerance allowed, a positive
%              scalar. If omitted or an empty matrix is passed, the default
%              value of 1e-3 is used.
%       AbsTol The absolute error tolerance allowed, a positive scalar, of
%              the same for all components of x, or a positive NX1 vector.
%              If omitted or an empty matrix is passed, the default value
%              of 1e-6 is used.
%     maxSteps The maximum allowable number of steps to perform the
%              integration. If omitted, the default of 1024 is used.
%
%OUTPUTS: xList The state at the times given in times xList(:,1) is the
%               same as xInit.
%
%A detailed description of the adaptive step size algorithm can be found in
%the comments of RKAdaptiveOverRange.
%
%May 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<9||isempty(maxSteps))
    maxSteps=1024;
end

if(nargin<8||isempty(AbsTol))
    AbsTol=1e-6;
end

if(nargin<7||isempty(RelTol))
    RelTol=1e-3;
end
if(nargin<6||isempty(solutionChoice))
    solutionChoice=0;
end

if(nargin<5||isempty(order))
    order=5;
end

if(nargin<4||isempty(initStepSize))
    initStepSize=[];
end

xDim=length(xInit);
numTimes=length(theTimes);

xList=zeros(xDim,numTimes);
xList(:,1)=xInit;
for curTime=2:numTimes
    tSpan=theTimes(curTime-1:curTime);
    [xVals,~,~,~,~,initStepSize]=RKAdaptiveOverRange(xList(:,curTime-1),tSpan,f,initStepSize,0,order,solutionChoice,RelTol,AbsTol,maxSteps);
    xList(:,curTime)=xVals(:,end);
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function af = naca4gen(iaf)
%
% "naca4gen" Generates the NACA 4 digit airfoil coordinates with desired no.
% of panels (line elements) on it.
%      Author : Divahar Jayaraman (j.divahar@yahoo.com)
%
% INPUTS-------------------------------------------------------------------
%       iaf.designation = NACA 4 digit iaf.designation (eg. '2412') - STRING !
%                 iaf.n = no of panels (line elements) PER SIDE (upper/lower)
% iaf.HalfCosineSpacing = 1 for "half cosine x-spacing"
%                       = 0 to give "uniform x-spacing"
%          iaf.wantFile = 1 for creating airfoil data file (eg. 'naca2412.dat')
%                       = 0 to suppress writing into a file
%       iaf.datFilePath = Path where the data  file has to be created
%                         (eg. 'af_data_folder/naca4digitAF/')
%                         use only forward slash '/' (Just for OS portability)
%
% OUTPUTS------------------------------------------------------------------
% Data:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
%       af.x = x cordinate (nx1 array)
%       af.z = z cordinate (nx1 array)
%      af.xU = x cordinate of upper surface (nx1 array)
%      af.zU = z cordinate of upper surface (nx1 array)
%      af.xL = x cordinate of lower surface (nx1 array)
%      af.zL = z cordinate of lower surface (nx1 array)
%      af.xC = x cordinate of camber line (nx1 array)
%      af.zC = z cordinate of camber line (nx1 array)
%    af.name = Name of the airfoil
%  af.header = Airfoil name ; No of panels ; Type of spacing
%              (eg. 'NACA4412 : [50 panels,Uniform x-spacing]')
%
%
% File:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
% First line : Header eg. 'NACA4412 : [50 panels,Half cosine x-spacing]'
% Subsequent lines : (2*iaf.n+1) rows of x and z values
%
% Typical Inputs:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
% iaf.designation='2312';
% iaf.n=56;
% iaf.HalfCosineSpacing=1;
% iaf.wantFile=1;
% iaf.datFilePath='./'; % Current folder
% iaf.is_finiteTE=0;

% % [[Calculating key parameters-----------------------------------------]]
t=str2num(iaf.designation(3:4))/100;
m=str2num(iaf.designation(1))/100;
p=str2num(iaf.designation(2))/10;

a0= 0.2969;
a1=-0.1260;
a2=-0.3516;
a3= 0.2843;

if iaf.is_finiteTE ==1
    a4=-0.1015; % For finite thick TE
else
    a4=-0.1036;  % For zero thick TE
end

% % [[Giving x-spacing---------------------------------------------------]]
if iaf.HalfCosineSpacing==1
    beta=linspace(0,pi,iaf.n+1)';
    x=(0.5*(1-cos(beta))); % Half cosine based spacing
    iaf.header=['NACA' iaf.designation ' : [' num2str(2*iaf.n) 'panels,Half cosine x-spacing]'];
else
    x=linspace(0,1,iaf.n+1)';
    iaf.header=['NACA' iaf.designation ' : [' num2str(2*iaf.n) 'panels,Uniform x-spacing]'];
end

yt=(t/0.2)*(a0*sqrt(x)+a1*x+a2*x.^2+a3*x.^3+a4*x.^4);

xc1=x(find(x<=p));
xc2=x(find(x>p));
xc=[xc1 ; xc2];

if p==0
    xu=x;
    yu=yt;

    xl=x;
    yl=-yt;

    zc=zeros(size(xc));
else
    yc1=(m/p^2)*(2*p*xc1-xc1.^2);
    yc2=(m/(1-p)^2)*((1-2*p)+2*p*xc2-xc2.^2);
    zc=[yc1 ; yc2];

    dyc1_dx=(m/p^2)*(2*p-2*xc1);
    dyc2_dx=(m/(1-p)^2)*(2*p-2*xc2);
    dyc_dx=[dyc1_dx ; dyc2_dx];
    theta=atan(dyc_dx);

    xu=x-yt.*sin(theta);
    yu=zc+yt.*cos(theta);

    xl=x+yt.*sin(theta);
    yl=zc-yt.*cos(theta);
end
af.name=['NACA ' iaf.designation];

af.x=[flipud(xu) ; xl(2:end)];
af.z=[flipud(yu) ; yl(2:end)];

indx1=1:min( find(af.x==min(af.x)) );  % Upper surface indices
indx2=min( find(af.x==min(af.x)) ):length(af.x); % Lower surface indices
af.xU=af.x(indx1); % Upper Surface x
af.zU=af.z(indx1); % Upper Surface z
af.xL=af.x(indx2); % Lower Surface x
af.zL=af.z(indx2); % Lower Surface z

af.xC=xc;
af.zC=zc;

lecirFactor=0.8;
af.rLE=0.5*(a0*t/0.2)^2;

le_offs=0.5/100;
dyc_dx_le=(m/p^2)*( 2*p-2*le_offs );
theta_le=atan(dyc_dx_le);
af.xLEcenter=af.rLE*cos(theta_le);
af.yLEcenter=af.rLE*sin(theta_le);

% % [[Writing iaf data into file------------------------------------------]]
if iaf.wantFile==1
    F1=iaf.header;
    F2=num2str([af.x af.z]);
    F=strvcat(F1,F2);
    fileName=[iaf.datFilePath 'naca' iaf.designation '.dat'];
    dlmwrite(fileName,F,'delimiter','')
end


%% Part of discussion on ADC quadratic form calculations
% Probably old between Bob and Brian years ago.
%
% Could be fixed up or deprecated.
%

% Here is a standard ellipsoid.
[X,Y,Z] = ellipsoid(0,0,0,3,1,1);
figure(1), subplot(1,2,1), surf(X,Y,Z)
axis equal

sz = size(X);

% Here is how an ellipse looks like a peanut
%
% Assign a length equal to the square of their length on the ellipsoid.
% This corresponds to what I believe are the ADC (variance) values.  Notice
% the peanut shape.

% The length of each ellipsoid vector
tmp = [X(:),Y(:),Z(:)];
l = sqrt(diag(tmp*tmp'));
% Multiply each vector by its own length
tmp = diag(l)*tmp;

X2 = reshape(tmp(:,1),sz);
Y2 = reshape(tmp(:,2),sz);
Z2 = reshape(tmp(:,3),sz);
figure(1), subplot(1,2,2), surf(X2,Y2,Z2)


function [S, V, D, Sigma2] = MySVDtau(A, tau)
[m, n] = size(A);
if 2*m < n
    AAT = A*A';
    [S, Sigma2, D] = svd(AAT);
    Sigma2 = diag(Sigma2);
    V = sqrt(Sigma2);
    tol = max(size(A)) * eps(max(V));
    R = sum(V > max(tol, tau));

    %tol = min(size(A)) * eps(max(V));
    % R = sum(V > 1/tau)

%     tol = min(size(A)) * eps(max(V));
%     R = sum(V > tau)

    V = V(1:R);
    S = S(:,1:R);
    D = A'*S*diag(1./V);
    V = diag(V);
    return;
end
if m > 2*n
    [S, V, D, Sigma2] = MySVDtau(A', tau);
    mid = D;
    D = S;
    S = mid;
    return;
end
[S,V,D] = svd(A);
Sigma2 = diag(V).^2;
R = sum(diag(V) > tau);
S = S(:, 1:R);
V = V(1:R, 1:R);
D = D(:, 1:R);


function [ Xt,Yt,Zt ] = multiplyEuMat( EuMat, X,Y,Z )
%MULTIPLYEUMAT takes the X, Y, Z coordinates of an object and returns the
%coordinates Xt, Yt, Zt of same object rotated using a rotation matrix

Xt=X;
Yt=Y;
Zt=Z;

resvec=[1;1;1];
for i=1:numel(X)
   temp=[X(i);Y(i);Z(i)];
   resvec=EuMat*temp;
   Xt(i)=resvec(1);
   Yt(i)=resvec(2);
   Zt(i)=resvec(3);
end


function L = levin(ntscIm)
% LEVIN Compute an image Laplacian according to the *code* accompanying
% "Colorization Using Optimization" [Levin et al. 2004]. Note that this is not
% the same as what is described in the *text* of the paper. This matrix is not
% symmetric and should be treated as a system matrix rather than a quadratic
% coefficient matrix of an energy.
%
% L = levin(ntscIm)
%
% Inputs:
%   ntscIm  h by w  Luminance image
% Outputs:
%   L  w*h by w*h spares Laplacian
%
% Note: Anat Levin's code, Alec's interpretation

n=size(ntscIm,1); m=size(ntscIm,2);
imgSize=n*m;

%nI(:,:,1)=ntscIm(:,:,1);

indsM=reshape([1:imgSize],n,m);

wd=1;

len=0;
consts_len=0;
col_inds=zeros(imgSize*(2*wd+1)^2,1);
row_inds=zeros(imgSize*(2*wd+1)^2,1);
vals=zeros(imgSize*(2*wd+1)^2,1);
gvals=zeros(1,(2*wd+1)^2);

for j=1:m
   for i=1:n
      consts_len=consts_len+1;

      %if (~colorIm(i,j))
        tlen=0;
        for ii=max(1,i-wd):min(i+wd,n)
           for jj=max(1,j-wd):min(j+wd,m)

              if (ii~=i)|(jj~=j)
                 len=len+1; tlen=tlen+1;
                 row_inds(len)= consts_len;
                 col_inds(len)=indsM(ii,jj);
                 gvals(tlen)=ntscIm(ii,jj,1);
              end
           end
        end
        t_val=ntscIm(i,j,1);
        gvals(tlen+1)=t_val;
        c_var=mean((gvals(1:tlen+1)-mean(gvals(1:tlen+1))).^2);
        csig=c_var*0.6;
        mgv=min((gvals(1:tlen)-t_val).^2);
        if (csig<(-mgv/log(0.01)))
	   csig=-mgv/log(0.01);
	end
	if (csig<0.000002)
	   csig=0.000002;
        end

        gvals(1:tlen)=exp(-(gvals(1:tlen)-t_val).^2/csig);
        gvals(1:tlen)=gvals(1:tlen)/sum(gvals(1:tlen));
        vals(len-tlen+1:len)=-gvals(1:tlen);
      %end

      len=len+1;
      row_inds(len)= consts_len;
      col_inds(len)=indsM(i,j);
      vals(len)=1;

   end
end

vals=vals(1:len);
col_inds=col_inds(1:len);
row_inds=row_inds(1:len);

L=sparse(row_inds,col_inds,vals,consts_len,imgSize);


function [pnt, tri] = mesh_cube()

% MESH_CUBE creates a triangulated cube
%
% Use as
%   [pos, tri] = mesh_cube()
%
% See also MESH_TETRAHEDRON, MESH_OCTAHEDRON, MESH_ICOSAHEDRON, MESH_SPHERE, MESH_CONE

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

pnt = [
  -1 -1 -1
  -1  1 -1
   1  1 -1
   1 -1 -1
  -1 -1  1
  -1  1  1
   1  1  1
   1 -1  1
  ];

tri = [
  1 2 4
  2 3 4
  1 5 6
  1 6 2
  2 6 7
  2 7 3
  3 7 8
  3 8 4
  4 8 5
  4 5 1
  8 6 5
  8 7 6
  ];


function prob = survProbGenHazFun(t,b,hazardFun)
% survProbGeneralHazardFunction: Computes survival probability using the
% hazard function 'hazardFun'

h = @(t)hazardFun(t,b);

H = zeros(size(t));
H(1) = quad(h,0,t(1));
for i=2:length(H)
   H(i) = H(i-1) + quad(h,t(i-1),t(i));
end

prob = exp(-H);


function value = r8_log ( x )

%*****************************************************************************80
%
%% R8_LOG evaluates the logarithm of an R8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the logarithm of X.
%
  persistent alncen
  persistent alncs
  persistent center
  persistent nterms

  aln2 = 0.06814718055994530941723212145818;

  if ( isempty ( nterms ) )

    alncs = [ ...
      +0.13347199877973881561689386047187E+01, ...
      +0.69375628328411286281372438354225E-03, ...
      +0.42934039020450834506559210803662E-06, ...
      +0.28933847795432594580466440387587E-09, ...
      +0.20512517530340580901741813447726E-12, ...
      +0.15039717055497386574615153319999E-15, ...
      +0.11294540695636464284521613333333E-18, ...
      +0.86355788671171868881946666666666E-22, ...
      +0.66952990534350370613333333333333E-25, ...
      +0.52491557448151466666666666666666E-28, ...
      +0.41530540680362666666666666666666E-31 ]';

    center = [ 1.0, 1.25, 1.50, 1.75 ];

    alncen = [ ...
      +0.0, ...
      +0.22314355131420975576629509030983, ...
      +0.40546510810816438197801311546434, ...
      +0.55961578793542268627088850052682, ...
      +0.69314718055994530941723212145817 ]';

    nterms = r8_inits ( alncs, 11, 28.9 * r8_mach ( 3 ) );

  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_LOG - Fatal error!\n' );
    fprintf ( 1, '  X <= 0.0\n' );
    error ( 'R8_LOG - Fatal error!' )
  end

  [ y, n ] = r8_upak ( x );

  xn = n - 1;
  y = 2.0 * y;
  ntrval = r8_aint ( 4.0 * y - 2.5 );

  if ( ntrval == 5 )
    t = ( ( y - 1.0 ) - 1.0 ) / ( y + 2.0 );
  elseif ( ntrval < 5 )
    t = ( y - center(ntrval) ) / ( y + center(ntrval) );
  end

  t2 = t * t;
  value = 0.625 * xn + ( aln2 * xn + alncen(ntrval) ...
    + 2.0 * t + t * t2 ...
    * r8_csevl ( 578.0 * t2 - 1.0, alncs, nterms ) );

  return
end


function [X,rho,eta,F] = pnu(A,L,W,b,k,nu,sm)
%PNU "Preconditioned" version of Brakhage's nu-method.
%
% [X,rho,eta,F] = pnu(A,L,W,b,k,nu,sm)
%
% Performs k steps of a `preconditioned' version of Brakhage's
% nu-method for the problem
%    min || (A*L_p) x - b || ,
% where L_p is the A-weighted generalized inverse of L.  Notice
% that the matrix W holding a basis for the null space of L must
% also be specified.
%
% The routine returns all k solutions, stored as columns of
% the matrix X.  The solution seminorm and residual norm are returned
% in eta and rho, respectively.
%
% If nu is not specified, nu = .5 is the default value, which gives
% the Chebychev method of Nemirovskii and Polyak.
%
% If the generalized singular values sm of (A,L) are also provided,
% then pnu computes the filter factors associated with each step and
% stores them columnwise in the matrix F.

% Reference: H. Brakhage, "On ill-posed problems and the method of
% conjugate gradients"; in H. W. Engl & G. W. Groetsch, "Inverse and
% Ill-Posed Problems", Academic Press, 1987.

% Martin Hanke, Institut fuer Praktische Mathematik, Universitaet
% Karlsruhe and Per Christian Hansen, IMM, 06/25/92.

% Set parameters.
l_steps = 3;      % Number of Lanczos steps for est. of || A*L_p ||.
fudge   = 0.99;   % Scale A and b by fudge/|| A*L_p ||.
fudge_thr = 1e-4; % Used to prevent filter factors from exploding.

% Initialization.
if (k < 1), error('Number of steps k must be positive'), end
if (nargin==5), nu = .5; end
[m,n] = size(A); p = size(L,1); X = zeros(n,k);
if (nargout > 1)
  rho = zeros(k,1); eta = rho;
end;
if (nargin==7)
  F = zeros(n,k); Fd = zeros(n,1); s = (sm(:,1)./sm(:,2)).^2;
end
V = zeros(p,l_steps); B = zeros(l_steps+1,l_steps);
v = zeros(p,1);

% Prepare for computations with L_p.
[NAA,x_0] = pinit(W,A,b); x1 = x_0;

% Compute a rough estimate of || A*L_p || by means of a few steps of
% Lanczos bidiagonalization, and scale A and b such that || A*L_p || is
% slightly less than one.
b_0 = b - A*x_0; beta = norm(b_0); u = b_0/beta;
for i=1:l_steps
  r = ltsolve(L,A'*u,W,NAA) - beta*v;
  alpha = norm(r); v = r/alpha;
  B(i,i) = alpha; V(:,i) = v;
  p = A*lsolve(L,v,W,NAA) - alpha*u;
  beta = norm(p); u = p/beta;
  B(i+1,i) = beta;
end
scale = fudge/norm(B); A = scale*A;
if (nargin==7), s = scale^2*s; end

% Prepare for iteration.
x  = x_0;
z  = -scale*b_0;
r  = A'*z;
d1 = ltsolve(L,r);
d  = lsolve(L,d1,W,NAA);
if (nargout>2), x1 = L*x_0; end

% Iterate.
for j=0:k-1

  % Updates.
  alpha = 4*(j+nu)*(j+nu+0.5)/(j+2*nu)/(j+2*nu+0.5);
  beta  = -(j+nu)*(j+1)*(j+0.5)/(j+2*nu)/(j+2*nu+0.5)/(j+nu+1);
  Ad  = A*d; AAd = A'*Ad;
  x   = x - alpha*d;
  r   = r - alpha*AAd;
  rr1 = ltsolve(L,r);
  rr  = lsolve(L,rr1,W,NAA);
  d   = rr - beta*d;
  X(:,j+1) = x;
  if (nargout>1 )
    z = z - alpha*Ad; rho(j+1) = norm(z)/scale;
  end
  if (nargout>2)
    x1 = x1 - alpha*d1; d1 = rr1 - beta*d1;
    eta(j+1) = norm(x1);
  end

  % Filter factors.
  if (nargin==7)
    if (j==0)
      F(:,1) = alpha*s;
      Fd = s - s.*F(:,1) + beta*s;
    else
      F(:,j+1) = F(:,j) + alpha*Fd;
      Fd = s - s.*F(:,j+1) + beta*Fd;
    end
    if (j > 1)
      f = find(abs(F(:,j)-1) < fudge_thr & abs(F(:,j-1)-1) < fudge_thr);
      if ~isempty(f), F(f,j+1) = ones(length(f),1); end
    end
  end

end


function [data, clustPoints, idx, centers, slopes, lengths] = ...
    generateData( ...
        slope, ...
        slopeStd, ...
        numClusts, ...
        xClustAvgSep, ...
        yClustAvgSep, ...
        lengthAvg, ...
        lengthStd, ...
        lateralStd, ...
        totalPoints ...
    )
% GENERATEDATA Generates 2D data for clustering; data is created along
%              straight lines, which can be more or less parallel depending
%              on slopeStd argument.
%
% [data clustPoints idx centers slopes lengths] =
%    GENERATEDATA(slope, slopeStd, numClusts, xClustAvgSep, yClustAvgSep, ...
%                 lengthAvg, lengthStd, lateralStd, totalPoints)
%
% Inputs:
%        slope - Base direction of the lines on which clusters are based.
%     slopeStd - Standard deviation of the slope; used to obtain a random
%                slope variation from the normal distribution, which is
%                added to the base slope in order to obtain the final slope
%                of each cluster.
%    numClusts - Number of clusters (and therefore of lines) to generate.
% xClustAvgSep - Average separation of line centers along the X axis.
% yClustAvgSep - Average separation of line centers along the Y axis.
%    lengthAvg - The base length of lines on which clusters are based.
%    lengthStd - Standard deviation of line length; used to obtain a random
%                length variation from the normal distribution, which is
%                added to the base length in order to obtain the final
%                length of each line.
%   lateralStd - "Cluster fatness", i.e., the standard deviation of the
%                distance from each point to the respective line, in both x
%                and y directions; this distance is obtained from the
%                normal distribution.
%  totalPoints - Total points in generated data (will be
%                randomly divided among clusters).
%
% Outputs:
%         data - Matrix (totalPoints x 2) with the generated data
%  clustPoints - Vector (numClusts x 1) containing number of points in each
%                cluster
%          idx - Vector (totalPoints x 1) containing the cluster indices of
%                each point
%      centers - Matrix (numClusts x 2) containing centers from where
%                clusters were generated
%       slopes - Vector (numClusts x 1) containing the effective slopes
%                used to generate clusters
%      lengths - Vector (numClusts x 1) containing the effective lengths
%                used to generate clusters
%
% ----------------------------------------------------------
% Usage example:
%
%   [data cp idx] = GENERATEDATA(1, 0.5, 5, 15, 15, 5, 1, 2, 200);
%
% This creates 5 clusters with a total of 200 points, with a base slope
% of 1 (std=0.5), separated in average by 15 units in both x and y
% directions, with average length of 5 units (std=1) and a "fatness" or
% spread of 2 units.
%
% To take a quick look at the clusters just do:
%
%   scatter(data(:,1), data(:,2), 8, idx);

%  N. Fachada
%  Instituto Superior Técnico, Lisboa, Portugal

% Make sure totalPoints >= numClusts
if totalPoints < numClusts
    error('Number of points must be equal or larger than the number of clusters.');
end;

% Determine number of points in each cluster
clustPoints = abs(randn(numClusts, 1));
clustPoints = clustPoints / sum(clustPoints);
clustPoints = round(clustPoints * totalPoints);

% Make sure totalPoints is respected
while sum(clustPoints) < totalPoints
    % If one point is missing add it to the smaller cluster
    [C,I] = min(clustPoints);
    clustPoints(I(1)) = C + 1;
end;
while sum(clustPoints) > totalPoints
    % If there is one extra point, remove it from larger cluster
    [C,I] = max(clustPoints);
    clustPoints(I(1)) = C - 1;
end;

% Make sure there are no empty clusters
emptyClusts = find(clustPoints == 0);
if ~isempty(emptyClusts)
    % If there are empty clusters...
    numEmptyClusts = size(emptyClusts, 1);
    for i=1:numEmptyClusts
        % ...get a point from the largest cluster and assign it to the
        % empty cluster
        [C,I] = max(clustPoints);
        clustPoints(I(1)) = C - 1;
        clustPoints(emptyClusts(i)) = 1;
    end;
end;

% Initialize data matrix
data = zeros(sum(clustPoints), 2);

% Initialize idx (vector containing the cluster indices of each point)
idx = zeros(totalPoints, 1);

% Initialize lengths vector
lengths = zeros(numClusts, 1);

% Determine cluster centers
xCenters = xClustAvgSep * numClusts * (rand(numClusts, 1) - 0.5);
yCenters = yClustAvgSep * numClusts * (rand(numClusts, 1) - 0.5);
centers = [xCenters yCenters];

% Determine cluster slopes
slopes = slope + slopeStd * randn(numClusts, 1);

% Create clusters
for i=1:numClusts
    % Determine length of line where this cluster will be based
    lengths(i) = abs(lengthAvg + lengthStd*randn);
    % Determine how many points have been assigned to previous clusters
    sumClustPoints = 0;
    if i > 1
        sumClustPoints = sum(clustPoints(1:(i - 1)));
    end;
    % Create points for this cluster
    for j=1:clustPoints(i)
        % Determine where in the line the next point will be projected
        position = lengths(i) * rand - lengths(i) / 2;
        % Determine x coordinate of point projection
        delta_x = cos(atan(slopes(i))) * position;
        % Determine y coordinate of point projection
        delta_y = delta_x * slopes(i);
        % Get point distance from line in x coordinate
        delta_x = delta_x + lateralStd * randn;
        % Get point distance from line in y coordinate
        delta_y = delta_y + lateralStd * randn;
        % Determine the actual point
        data(sumClustPoints + j, :) = [(xCenters(i) + delta_x) (yCenters(i) + delta_y)];
    end;
    % Update idx
    idx(sumClustPoints + 1 : sumClustPoints + clustPoints(i)) = i;
end;


classdef CEC2020_F3 < PROBLEM
% <single> <real>
% Shifted and rotated Lunacek bi-Rastrigin function

%------------------------------- Reference --------------------------------
% C .T. Yue, K. V. Price, P. N. Suganthan, J. J. Liang, M. Z. Ali, B. Y.
% Qu, N. H. Awad, and P. P Biswas, Problem definitions and evaluation
% criteria for the CEC 2020 special session and competition on single
% objective bound constrained numerical optimization, Zhengzhou University,
% China and Nanyang Technological University, Singapore, 2019.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;      % Optimal decision vector
        Mat;	% Rotation matrix
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2020.mat'),'Data');
            obj.O = Data{3}.o;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 10
                obj.D   = 5;
                obj.Mat = Data{3}.M_5;
            elseif obj.D < 15
                obj.D   = 10;
                obj.Mat = Data{3}.M_10;
            elseif obj.D < 20
                obj.D   = 15;
                obj.Mat = Data{3}.M_15;
            else
                obj.D   = 20;
                obj.Mat = Data{3}.M_20;
            end
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            s   = 1 - 1/(2*sqrt(obj.D+20)-8.2);
            mu0 = 2.5;
            mu1 = -sqrt((mu0^2-1)/s);
            Y   = (PopDec-repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1))/10;
            tmp = 2*repmat(sign(obj.O(1:size(PopDec,2))),size(PopDec,1),1).*Y + mu0;
            Z   = (tmp-mu0)*obj.Mat';
            PopObj = 700 + min(sum((tmp-mu0).^2,2),obj.D+s*sum((tmp-mu1).^2,2)) + 10*(obj.D-sum(cos(2*pi*Z),2));
        end
    end
end


function Rinv = est_calcInvCovMat(AR,C,invCov,mode,MAXITER)
%
% For any M-variate VAR[p] process fit to data X = [X1, X2, ... Xn],
% this function returns the [(Mp)^2 x (Mp)^2] covariance (or inverse covariance)
% matrix of the process. This is a block-toeplitz, hermitian matrix with
% format as described in [2].
%
% Briefly, let M=nvars, and Rhat be comprised of M x M submatrices
%
%           Rhat(u,v) = [R11(u,v) ... R1M
%                           ...        ...
%                        RM1(u,v) ... RMM(u,v)]
%
% where Rij(u,v) = cov(Xi(t-u),Xj(t-v)) for i,j=1:M and u,v = 1:p. Then
% Rhat is the (cross-)covariance matrix of the VAR[p] process up to lag p
% with inverse covariance matrix Rinv = inverse(Rhat).
%
% Inputs:
%
%   AR:     [nvars x nvar*morder] matrix of vector autoregressive
%           coefficients
%   C:      nvars x nvars noise covariance matrix
%   invCov: {def: 1}
%           true: return inverse of processes covariance matrix
%           fase: return process covariance matrix
%   mode:   {def: 3} Determines the method used to compute the covariance matrix
%           1: uses the standard approach from [2]
%           2: same as 1, but using sparse matrices.
%           3: uses the iterative doubling algorithm of Anderson and Moore
%              as proposed in [1]. For large matrices, generally much faster
%              and less memory intensive.
%   MAXITER:{def: 100}
%           Number of iterations for doubling algorithm (mode=3)
%
% Outputs:
%
%   Rinv:   [(nvars*p)^2 x (nvars*p)^2] (inverse) covariance matrix of the
%           VAR[p] process.
%
%
% References:
%
% [1] Barone, (1987) J. of Time series Analysis vol. 8 no. 2
% [2] Lutkepohl, H. (2007) New Introduction to Time Series Analysis.
% Springer. Sec 2.1.4.
%
% See Also: est_calcInvCovMatFourier(), est_calcInvCovMatPDC
%
% Author: Tim Mullen 2010, SCCN/INC, UCSD
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

if nargin<4
    mode = 3;
end
if nargin<3
    invCov = 1;    % if 0, don't invert covariance matrix
end
if nargin<4
    MAXITER = 100;
end

% make sure the noise covariance matrix is valid
C = covfixer(C);

switch mode

    case 1

        % Standard covariance estimation. See [2] sec. 2.1.4

        M = size(C,1);          % nchans
        p = size(AR,2)/M;       % model order
        Mp=M*p;

        A = zeros(Mp);
        A(1:M,1:Mp) = AR;
        A(M+1:end,1:end-M) = eye(Mp-M);
        Eu = zeros(Mp);
        Eu(1:M,1:M) = C;

        Rinv = (eye(Mp^2)-kron(A,A))\Eu(:);

%         Rinv = reshape(Rinv,Mp,Mp)\eye(Mp);

        Rinv = inverse(reshape(Rinv,Mp,Mp));

    case 2

        % same as 1, but sparse approach

        M = size(C,1);          % nchans
        p = size(AR,2)/M;       % model order
        Mp=M*p;

        A = spalloc(Mp,Mp,M*Mp+M);
        A(1:M,1:Mp) = AR;
        A(M+1:end,1:end-M) = eye(Mp-M);

        Eu = spalloc(Mp,Mp,M^2);
        Eu(1:M,1:M) = single(C);

        Rinv = (speye(Mp^2)-kron(A,A))\Eu(:);

        % Rinv = symmlq(speye(Mp^2)-kron(A,A),Eu(:),1e-6,100);

        Rinv = full(reshape(Rinv,Mp,Mp)\speye(Mp));

    case 3

        % Anderson-Moore/Barone Doubling Algorithm [1]

        M = size(C,1);          % nchans
        p = size(AR,2)/M;       % model order
        Mp=M*p;
        A = zeros(Mp);
        A(1:M,1:Mp) = AR;
        A(M+1:end,1:end-M) = eye(Mp-M);

        K = [eye(M); zeros(M*(p-1),M)];
        N = {K*C*K'};
        N{2} = zeros(size(N{1}));

        for t=1:MAXITER
            N{2} = A*N{1}*A'+N{1};
            A = A^2;
            if norm(N{2}(:)-N{1}(:))<1e-15
                break
            end
            N{1}=N{2};
        end

        if t==MAXITER
            warning('SIFT:est_calcInvCovMat', 'Anderson-Moore did not converge. Results may be innacurate.\n');
        end

        % Rinv = L*N{2}*L';

        % ensure that the covariance matrix is a valid covariance matrix
        N{2} = covfixer(N{2});

        if invCov
            Rinv = inverse(N{2});
        else
            Rinv = N{2};
        end

end


function I = sum3(f)
%SUM3   Triple definite integral of a CHEBFUN3T over [a, b] x [c, d] x [e g].
%   This is a direct generalization of Clenshaw-Curtis quadrature as
%   implemented in Page 77 of Battles' PhD thesis.

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

if ( isempty(f) )
    I = [];
    return;
end

coeffs = f.coeffs;
% Rescaling factors:
rescaleFactor = 0.5*diff(f.domain(1:2));                 % (b - a)/2
rescaleFactor = rescaleFactor * 0.5*diff(f.domain(3:4)); % (d - c)/2
rescaleFactor = rescaleFactor * 0.5*diff(f.domain(5:6)); % (g - e)/2

if ( nargin == 1 )
    [nx, ny, nz] = size(coeffs);
    I1 = zeros(nx,ny);
    for i = 1:nx
        for j = 1:ny
            I1(i,j) = squeeze(coeffs(i,j,1:2:nz)).'*(2./(1-(0:2:nz-1)'.^2));
        end
    end
    I2 = zeros(nx,1);
    for i = 1:nx
        I2(i) = I1(i,1:2:ny)*(2./(1-(0:2:ny-1)'.^2));
    end
    I = I2(1:2:nx).'*(2./(1-(0:2:nx-1)'.^2));
end

% Rescale the output:
I = I*rescaleFactor;

end


function [ output ] = fista( par )
% fista for stably principal component pursuit problem
%{
par.M = M;
par.iter = 1000;
%lambda = 1/sqrt(max(size(M))); % default lambda
par.lambda_1 = 0.1;
par.lambda_2 = 0.001;
out = fista(par);
subplot(1,3,1),imagesc(M);
subplot(1,3,2),imagesc(out.L);
subplot(1,3,3),imagesc(out.S);
show_results(M,out.L,out.S,[],size(M,2),m,n);
%}
%%  Cun Mu and John Wright, Mar '14

M = par.M; % data matrix
[m,n] = size(M); d = min(m,n);
Omega = ones(m,n); % if not specified, full observation
obj_target = -inf;

if isfield(par,'Omega') Omega = par.Omega;  end
if isfield(par,'iter') iter_no = par.iter; end
if isfield(par,'obj') obj_target = par.obj; end

lambda_1 = par.lambda_1;
lambda_2 = par.lambda_2;
%display = par.display;
lips = 2;

L = zeros(m,n); S = zeros(m,n);
sv = round(d/10);
iter = 1; hist = 0;

L_hat = zeros(m,n); S_hat = zeros(m,n); t = 1;
L_pre = zeros(m,n); S_pre = zeros(m,n);

while true

  temp_L = L_hat - Omega.*(L_hat+S_hat-M)/lips;
  temp_S = S_hat - Omega.*(L_hat+S_hat-M)/lips;

  S = max(temp_S - lambda_2/lips, 0);
  S = S+min(temp_S + lambda_2/lips, 0);

  if choosvd(n, sv) == 1
    [U s V] = lansvd(temp_L, sv, 'L');
    %[U s V] = svd(temp_L, 'econ');
  else
    [U s V] = svd(temp_L, 'econ');
  end
  diagS = diag(s);
  svp = size(find(diagS > lambda_1/lips),1);
  if svp < sv
    flag = 1;
    sv = min(svp + 1, d);
  else
    flag = 0;
    sv = min(svp + round(0.05*d), d);
  end

  newSingularValues = diagS(1:svp) - lambda_1/lips;
  newNuclearNorm = sum(newSingularValues);

  L = U(:, 1:svp) * diag(diagS(1:svp) - lambda_1/lips) * V(:, 1:svp)';

  obj_value = 0.5*norm(Omega.*(L+S-M),'fro')^2 + lambda_1*newNuclearNorm ...
    +lambda_2*norm(vec(S),1);

  fprintf('this is the %d th iteration; sv = %d; obj. value = %d \n',...
    iter, sv, obj_value);

  hist(iter) = obj_value;

  if flag
    if obj_value < obj_target
      break;
    end
  end

  t_pre = t;
  t = (1+4*t^2)^0.5/2 + 0.5;
  alpha = (t_pre-1)/t;

  L_hat = L + alpha*(L-L_pre);
  S_hat = S + alpha*(S-S_pre);

  iter = iter +1;
  L_pre = L;  S_pre = S;

  if(iter > iter_no)
    break;
  end
end

output.L = L;
output.S = S;
output.hist = hist;
end


function [Dt, totalDist] = InterfiberZhangDistance(curve1, curve2, Tt)
%Distance between two 3D paths, defined as two 3xN sets of 3d coordinates.
%
% [Dt, totalDist] = InterfiberZhangDistance(curve1, curve2, [Tt=2])
%
% The terms Dt and totalDist need better definition here.
%
% The metric is described in Zhang et al. (2003) IEEE Transactions on
% Vizualization and Computer Graphics 9(4).  They write:
%
% �In order to emphasize important differences between a pair of
% trajectories, we average the distance between the curves only over the
% region where they are at least Tt apart; smaller differences are assumed
% to be insignificant�.
%
% Although arguable, it is natural to set Tt to data voxel size /not sure,
% before or after resampling/ (as Zhang 2003 did). Here the default value
% is set to 1mm despite the fact that out data are commonly 2x2x2.
%
% Calculated on a point by point basis.
% Note: by definition, if a point on a shorter fiber is closer to the other
% fiber than Tt, this point does NOT contribute to the overall
% curve-to-curve distance measure.
%
%Dt is above-the-threshold (that is, minus Tt) average point-to-curve
%distance  across all points on a shortest fiber to another fiber.
%
%  CLARIFY.
% The output variable totalDist is Dt+Tt, the average point-to-curve
% distance between the two fibers. These output arguments are different by
% a constant,  however included since the first one is the "definition" of
% the distance by Zhang et al., and the second one is easier to interpret.
% Also totalDist is reported as at least at Tt (even for two fibers which
% are closer than Tt).
%
%  Example:
%    curve1 = 10*rand(3,10);
%    curve2 = curve1 + 1;
%    Tt = 0;
%    [Dt, totalDist] = InterfiberZhangDistance(curve1, curve2,Tt)
%    [Dt, totalDist] = InterfiberZhangDistance(curve1, curve2,2)
%
%ER 11/2008
%ER 04/2009 added "totalDist" output and introduced a default value for Tt
%to be 0 (all points contribute to the distance measure).
%
% (c) Stanford VISTA Team

%Pairs of points in closer than Tt are not included when summing the
%distance between fibers.
if (~exist('Tt', 'var')), Tt=0; end

% Check curve sizes.
if size(curve1, 1) ~= 3 || size(curve2, 1) ~= 3
   error('Fiber coordinates must be represented as 3xN');
end

% Use  "nearpoints" to find the nearest point in curve 1 to curve 2.
% [indices, bestSqDist] = nearpoints(src, dest)
%-  src is a 3xM array of points
%-  dest is a 3xN array of points
%-  indices is a 1xM vector, whose elements identifies the closest point
%   in dest for each entry in src.
%   "bestSqDist" is the distance.
%

% For efficiency, choose which is the first term
if size(curve1, 2) < size(curve2, 2),
    [indices, bestSqDist] = nearpoints(curve1, curve2);
else
    [indices, bestSqDist] = nearpoints(curve2, curve1);
end

% Have a look at the data in the two curves.
% mrvNewGraphWin;
%  plot3(curve1(1,:),curve1(2,:),curve1(3,:),'bo'); hold on
%  plot3(curve2(1,:),curve2(2,:),curve2(3,:),'r.');
%

% I don't understand.  If there are no entries bigger than Tt, we set Dt to
% zero, I guess.
nPoints = length(find(bestSqDist > Tt));
if nPoints <= 0
    % Expansive but useless. Precisely,
    % Tt=sum(sqrt(bestSqDist))/length(bestSqDist);
    % We keep it as a constant Tt.
    Dt = 0;
else
    % Otherwise, we set Dt to this average.
    Dt = sum(sqrt(bestSqDist(bestSqDist > Tt)) - Tt) / nPoints;
end

% Then we add Tt back in.
totalDist = Dt + Tt;

return;

%%The slow way (gives equivalent result)
if size(curve1, 2)<size(curve2, 2)
    scurve=curve1;  %shorter
    lcurve=curve2; %longer curve
else
    scurve=curve2;
    lcurve=curve1;
end

countpoints=0;  sumdist=0;

for pnt=1:size(scurve,2)
    distVector = sqrt((lcurve(1, :)-scurve(1, pnt)).^2 + (lcurve(2, :)-scurve(2, pnt)).^2+(lcurve(3, :)-scurve(3, pnt)).^2);
    p2curveDist=min(distVector); %Shortest point to curve distance

    if p2curveDist>Tt
        countpoints=countpoints+1;
        sumdist= sumdist+ (p2curveDist-Tt);
    end

end

if countpoints>0
    Dt=sumdist/countpoints;
else
    Dt=0;
end


%% How do I know whether AR1 noise is an appropriate null hypothesis to test against?
% It is usually an appropriate null hypothesis if the theoretical AR1
% spectrum is  degreesa good model degrees for the power decay in the observed spectrum.
% I recommend to simply visually compare the two power spectra:

X=rednoise(200,.8);
[P,freq]=pburg(zscore(X),7,[],1);
aa=ar1(X);
Ptheoretical=(1-aa.^2)./(abs(1-aa.*exp(-2*pi*i*freq))).^2;
semilogy(freq,P/sum(P),freq,Ptheoretical/sum(Ptheoretical),'r');
legend('observed',sprintf('Theoretical AR1=%.2f',aa),'location','best')


% moving camera bundle adjustment problem

ncams = 10;  % number of cameras
npts = 110;  % number of landmark points
visprob = 0.65;  % probability of camera seeing a landmark
pixnoise = 0.5; % standard deviation of Gaussian noise added to camera projections
Tshift = SE3(-0.2, 0, 0);  % horizontal shift of camera at each view

% create a camera
cam = CentralCamera('default', 'noise', pixnoise)

% setup a bundle adjustment problem
ba = BundleAdjust(cam);

% create the camera nodes, all at the origin, and keep their handles ch(i)
for i=1:ncams
    if i == 1
        ch(i) = ba.add_camera( SE3, 'fixed' );
    else
        ch(i) = ba.add_camera( SE3 );
    end
end

% create a working volume containing npts random points
% x -3 -> 1
% y -2 -> 2
% z  4 -> 8
randinit
P = bsxfun(@plus, 2 * 2*(rand(3, npts) - 0.5), [-1, 0 , 6]');

% create the landmark nodes and keep their handles lh(j)
for j=1:numcols(P)
    lh(j) = ba.add_landmark( P(:,j) );
end

% slide the camera in the x-direciton
T = SE3;
for i=1:ncams
    % project all landmarks for this camera position
    [p, visible] = cam.project(P, 'pose', T);

    % find the subset of points that are visible
    for j=find(visible)'
        % add to the problem if visible
        if rand < visprob % with a probability
            ba.add_projection(ch(i), lh(j), p(:,j));
        end
    end
    T = T .* Tshift; % shift the camera
end

% display the problem summary
ba

figure(1)
ba.plot()

pause

% get the state vector
X = ba.getstate();

% get the initial error
ba.errors(X)

% display the Hessian
figure(2)
ba.spyH(X)
title('Hessian matrix sparsity')

% solve the problem
baf = ba.optimize(X);

figure(3)
baf.plot()


function  [ConSPix ConEdge]= find_connect_superpixel(labels, K, height ,width )
%%
% obtain the neighbour relationship of the super-pixels
% Input:
%         labels:    the super-pixel label obtained from SLIC
%         K:         the number of super-pixels
%         height:    the height of the image
%         width:     the width of the image
% Output:
%         ConPix:   the one layer neighbour relationship
%%%%=====================================================
ConSPix=zeros(K,K);
%the one outerboundary super
for i=1:height-1
    for j=1:width-1
        if labels(i,j)~=labels(i,j+1)
            ConSPix(labels(i,j) ,labels(i,j+1) )=1;
        end
        if labels(i,j)~=labels(i+1,j)
            ConSPix(labels(i,j) ,labels(i+1,j) )=1;
        end
    end
    if labels(i,j+1)~=labels(i+1,j+1)
        ConSPix(labels(i,j+1) ,labels(i+1,j+1 ) )=1;
    end
end
for j=1:width-1
    if labels(height,j)~=labels(height,j+1)
        ConSPix( labels(height,j),labels(height,j+1) )=1;
    end
end
for i=1:height-1
    for j=1:width-1
        if labels(i,j)~=labels(i+1,j+1)
            ConSPix( labels(i,j),labels(i+1,j+1) )=1;
        end
    end
end
for i=1:height-1
    for j=2:width
        if labels(i,j)~=labels(i+1,j-1)
            ConSPix( labels(i,j),labels(i+1,j-1) )=1;
        end
    end
end
 ConSPix = ConSPix + ConSPix';
 ConSPix(ConSPix>0)=1;
 [edges_x edges_y] = find(triu(ConSPix)==1);
 ConEdge = [edges_x edges_y];
 %[x y] = meshgrid(1:K,1:K);
 %ConEdge = [x(:) y(:)];