function numgrad = computeNumericalGradient(J, theta)
% numgrad = computeNumericalGradient(J, theta)
% theta: a vector of parameters
% J: a function that outputs a real-number. Calling y = J(theta) will return the
% function value at theta.

% Initialize numgrad with zeros
numgrad = zeros(size(theta));

%% ---------- YOUR CODE HERE --------------------------------------
% Instructions:
% Implement numerical gradient checking, and return the result in numgrad.
% (See Section 2.3 of the lecture notes.)
% You should write code so that numgrad(i) is (the numerical approximation to) the
% partial derivative of J with respect to the i-th input argument, evaluated at theta.
% I.e., numgrad(i) should be the (approximately) the partial derivative of J with
% respect to theta(i).
%
% Hint: You will probably want to compute the elements of numgrad one at a time.

epsilon = 1e-4;

for i =1:length(numgrad)
    oldT = theta(i);
    theta(i)=oldT+epsilon;
    pos = J(theta);
    theta(i)=oldT-epsilon;
    neg = J(theta);
    numgrad(i) = (pos-neg)/(2*epsilon);
    theta(i)=oldT;
    if mod(i,100)==0
       fprintf('Done with %d\n',i);
    end;
end;

%% ---------------------------------------------------------------
end


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)

m = size(X, 1); % Number of training examples

% You need to return the following variables correctly
p = zeros(m, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%

for i=1:m,
	if sigmoid(X(i,:)*theta) >= 0.5,
		p(i) = 1;
	else
		p(i) = 0;
	end;
end;

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

end


function ds = dsegment ( p, pv )

%*****************************************************************************80
%
%% DSEGMENT computes the distance of points to line segments.
%
%  Discussion:
%
%    This is a pure MATLAB version of the DSEGMENT routine.
%    Normally, the authors of DISTMESH recommend using a faster procedure
%    in which a MATLAB MEX interface is used to call a compiled C code.
%    But if that is not possible, you can fall back on this slower
%    version.
%
%    Note that the line segments are defined by successive pairs of
%    vertices.  In the very common case that the vertices represent
%    a polygon, it is necessary that the list be extended by repeating
%    the first vertex at the end; otherwise the definition of the
%    polygon, and the distance computation, will be incomplete.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Parameters:
%
%    Input, real P(NP,2), a set of points.
%
%    Input, real PV(NVS,2), a set of vertices.  Sequential
%    pairs define NVS-1 line segments.
%
%    Output, real DS(NP,NVS-1); DS(I,J) is the (unsigned) distance
%    of point P(I,:) from line segment PV(J,:)--PV(J+1,:).
%
  nvs = size ( pv, 1 );
  np = size ( p, 1 );

  ds = zeros ( np,nvs-1);

  for iv = 1 : nvs-1
    for ip = 1 : np

      v(1:2) = [ pv(iv+1,1) - pv(iv,1), pv(iv+1,2) - pv(iv,2) ];

      w(1:2) = [ p(ip,1) - pv(iv,1), p(ip,2) - pv(iv,2) ];

      c1 = v * w';
      c2 = v * v';

      if ( c1 <= 0.0 )
        ds(ip,iv) = sqrt ( sum ( ( p(ip,1:2) - pv(iv,1:2) ).^2 ) );
      elseif ( c2 <= c1 )
        ds(ip,iv) = sqrt ( sum ( ( p(ip,1:2) - pv(iv+1,1:2) ).^2 ) );
      else
        ds(ip,iv) = sqrt ( sum ( ( p(ip,1:2) ...
          - pv(iv,1:2) - c1 / c2 * v(1:2) ).^2 ) );
      end

    end
  end

  return
end


%wrap - Set radian angles in range [0,2pi] or [-pi,pi].
%
%  USAGE
%
%    y = wrap(x,range)
%
%    x              angles in radians
%    range          optional:  1 for [-pi,pi] (default)
%                              2 for [0,2pi]
%
%  SEE ALSO
%
%    See also isradians.
%

% Copyright (C) 2010-2011 by Michaël Zugaro
%
% 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.

function y = wrap(x,range)

% Check number of parameters
if nargin < 1,
  error('Incorrect number of parameters (type ''help wrap'' for details).');
end

if nargin < 2,
	range = 1;
end

if ~isa(x,'double'), y = []; return; end

% Determine angle in [0,2*pi]
y = mod(x,2*pi);

% Change range if necessary
if range == 1,
	change = y > pi;
	y(change) = y(change)-2*pi;
end


%%%
%> @brief  computes the homogeneous transformation A of the pose vector v
%> @param  v pose vector
%> @return A homogeneous transformation
%> @author Giorgio Grisetti
%%%
function A = v2t(v)
% V2T vector to homogeneous transformation
c = cos(v(3));
s = sin(v(3));
A = [c, -s, v(1);
     s,  c, v(2);
     0   0  1];
end


function [A,B,D,E,I]=mdds(X)
% gets A, B, D, E & I arrays for magnet driver
N=2;U=3;M=1;Y=1;
A1=zeros(U+N);B2=zeros(U+N,N+M);
%
% X = [R1 R2 R3 C1 C2]
%
R1=X(1);R2=X(2);R3=X(3);C1=X(4);C2=X(5);
%
% V1 V2 V3 iC1 iC2  column labels for A1
% 1  2  3  4	5
%
A1(1,1)=-1/R1;A1(1,4)=-1;A1(1,5)=-1; % From 1st equation
A1(2,1)=1/R2;A1(2,2)=-1/R2;A1(2,4)=-1; % 2nd equation
A1(3,3)=1;A1(3,4)=-R3;A1(3,5)=-R3; % V3 - iC1*R3 - iC2*R3 = 0
A1(4,2)=1;A1(4,3)=-1; % V2 - V3 = E1
A1(5,1)=1;A1(5,3)=-1; % V1 - V3 = E2
%
E1=1;E2=1;Ein=1;
% E1  E2   Ein  Column labels for B2
%  1   2    3
B2(4,1)=E1;
B2(5,2)=E2;
B2(1,3)=-Ein/R1;
%
P=diag([C1 C2]);
%
% Template statements (same for every circuit):
V=A1\B2;H=V(U+1:U+N,1:N+M);I=eye(N);
AB=P\H;A=AB(1:N,1:N);B=AB(1:N,N+1:N+M);
D=V(Y:Y,1:N);E=V(Y:Y,N+1:N+M);


function [q]=zn2fpd(input)
% [q]=zn2fpd(input)
% Ziegler-Nichols controller for 2nd order processes
% This function computes parameters of the controller (q0, q1, q2, q3, q4).
% Controller is based on forward rectangular method of discretization
% replacing derivation by a four-point difference.
% Transfer function of the controller is as follows:
%
%            q0 + q1*z^-1 + q2*z^-2 + q3*z^-3 + q4*z^-4
% G(z^-1) = --------------------------------------------
%                           1 - z^-1
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2
% Gs(z^-1) = -----------------------
%             1 + a1*z^-1 + a2*z^-2
%
% Input: input ... input parameters
%                  input(1) ... a1
%                  input(2) ... b1
%                  input(3) ... a2
%                  input(4) ... b2
%                  input(5) ... sample time T0
% Output: qp ... controller parameters
%                qp(1) ... q0
%                qp(2) ... q1
%                qp(3) ... q2
%                qp(4) ... p1
%                qp(5) ... p2

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
T0 = input(5);

% compute ultimate gain and frequency
[Kpu, Tu] =  ultim([b1 b2],[a1 a2],T0);

Kp = 0.6*Kpu;
Ti = Tu/2;
Td = Tu/8;

q0 = Kp*(1 + T0/Ti + Td/(6*T0));
q1 = Kp*(-1 + Td/(3*T0));
q2 = Kp*(-Td/T0);
q3 = Kp*(Td/(3*T0));
q4 = Kp*(Td/(6*T0));

q=[q0;q1;q2;q3;q4];


function [EC,ec,degij] = edge_nei_overlap_bd(CIJ)
%EDGE_NEI_OVERLAP_BD     overlap amongst neighbors of two adjacent nodes
%
%   [EC,ec,degij] = edge_nei_bd(CIJ);
%
%   This function determines the neighbors of two nodes that are linked by
%   an edge, and then computes their overlap.  Connection matrix must be
%   binary and directed.  Entries of 'EC' that are 'inf' indicate that no
%   edge is present.  Entries of 'EC' that are 0 denote "local bridges",
%   i.e. edges that link completely non-overlapping neighborhoods.  Low
%   values of EC indicate edges that are "weak ties".
%
%   If CIJ is weighted, the weights are ignored. Neighbors of a node can be
%   linked by incoming, outgoing, or reciprocal connections.
%
%   Inputs:     CIJ,      directed (binary/weighted) connection matrix
%
%   Outputs:    EC,     edge neighborhood overlap matrix
%               ec,     edge neighborhood overlap per edge, in vector format
%               degij,  degrees of node pairs connected by each edge
%
%   Reference:
%
%       Easley and Kleinberg (2010) Networks, Crowds, and Markets.
%           Cambridge University Press, Chapter 3
%
%   Olaf Sporns, Indiana University, 2012

[ik,jk,ck] = find(CIJ);
lel = length(ck);
N = size(CIJ,1);

[~,~,deg] = degrees_dir(CIJ);

ec = zeros(1,lel);
degij = zeros(2,lel);
for e=1:lel
    neiik = setdiff(union(find(CIJ(ik(e),:)),find(CIJ(:,ik(e))')),[ik(e) jk(e)]);
    neijk = setdiff(union(find(CIJ(jk(e),:)),find(CIJ(:,jk(e))')),[ik(e) jk(e)]);
    ec(e) = length(intersect(neiik,neijk))/length(union(neiik,neijk));
    degij(:,e) = [deg(ik(e)) deg(jk(e))];
end;

ff = find(CIJ);
EC = 1./zeros(N);
EC(ff) = ec;                            %#ok<FNDSB>


function MD = mahalanobis(x,m,P)

% MAHALANOBIS  Mahalanobis distance
%   MD = MAHALANOBIS(X,M,P) computes the Mahalanobis distance from a point
%   X to a Gaussian of mean M and covariance P:
%
%   MD = sqrt((X-M)'*inv(P)*(X-M));
%
%   See also NEES.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

MD = sqrt((x-m)'/P*(x-m));

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [x, funVal, ValueL]=LeastR(A, y, z, opts)
%
%%
% Function LeastR
%      Least Squares Loss with the L1-norm Regularization
%
%% Problem
%
%  min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * ||x||_1
%
%  By default, rsL2=0.
%  When rsL2 is nonzero, this corresponds to the well-know elastic net.
%
%% 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 mx1)
%  z -        L_1 norm regularization parameter (z >=0)
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         Solution
%  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 February 18, 2010.
%
%% Related papers
%
% [1]  Jun Liu and Jieping Ye, Efficient Euclidean Projections
%      in Linear Time, ICML 2009.
%
% [2]  Jun Liu and Jieping Ye, Sparse Learning with Efficient Euclidean
%      Projections onto the L1 Ball, Technical Report ASU, 2008.
%
%% Related functions
%
%  sll_opts, initFactor, pathSolutionLeast
%  LeastC, nnLeastR, nnLeastC,
%
%%

%% Verify and initialize the parameters
%%

% Verify the number of input parameters
if (nargin <3)
    error('\n Inputs: A, y and z should be specified!\n');
elseif (nargin==3)
    opts=[];
end

% Get the size of the matrix A
[m,n]=size(A);% m -- sample size,n -- feature dimension%******************************

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

% Verify the value of z
if (z<0)
    error('\n z should be nonnegative!\n');
end

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

%% 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

%% Starting point initialization

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

% process the regularization parameter

% L2 norm regularization
if isfield(opts,'rsL2')
    rsL2=opts.rsL2;
    if (rsL2<0)
        error('\n opts.rsL2 should be nonnegative!');
    end
else
    rsL2=0;
end

% L1 norm regularization
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

    lambda_max=max(abs(ATy));
    lambda=z*lambda_max;

    rsL2=rsL2*lambda_max; % the input rsL2 is a ratio of lambda_max
end

% initialize a starting point
if opts.init==2
    %x=zeros(n,k);%*************************************
    x=zeros(n,1);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if (length(x)~=n)
            error('\n Check the input .x0');
        end
    else
        x=ATy;  % if .x0 is not specified, we use ratio*ATy,
        % where ratio is a positive value
    end
end

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

if (opts.init==0) % If .init=0, we set x=ratio*x by "initFactor"
    % Please refer to the function initFactor for detail

    x_norm=sum(abs(x)); x_2norm=x'*x;%*****************************corresponding to L1norm,Fnorm
    if x_norm>=1e-6
        ratio=initFactor(x_norm, Ax, y, lambda,'LeastR', rsL2, x_2norm);
        x=ratio*x;    Ax=ratio*Ax;
    end
end

%% The main program

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

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

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

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

    % alphap and alpha are used for computing the weight in forming search point
    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;

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

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

        % compute AT As
        if (opts.nFlag==0)
            ATAs=A'*As;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        g=ATAs-ATy + rsL2 * s;%************************** +rsL2*s*inv(sigma)

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

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

            % L1-norm regularized projection
            x=sign(v).*max(abs(v)-lambda / L,0);

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

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

            Av=Ax -As;
            r_sum=v'*v; l_sum=Av'*Av;

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

            % the condition is ||Av||_2^2 <= (L - rsL2) * ||v||_2^2
            if(l_sum <= r_sum * (L-rsL2))
                break;
            else
                L=max(2*L, l_sum/r_sum + rsL2);
                % fprintf('\n L=%5.6f',L);
            end
        end

        ValueL(iterStep)=L;

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

        xxp=x-xp;   Axy=Ax-y;
        funVal(iterStep)=Axy'* Axy/2 + rsL2/2 * x'*x + sum(abs(x)) * lambda;

        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=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% Reformulated problem + Nemirovski's scheme

% .mFlag=1, and .lFlag=0
%  refomulate the problem as the constrained convex optimization
%  problem, and then apply Armijo Goldstein line search scheme

% Problem:
%    min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * t' * 1
%    s.t.   |x| <= t

if(opts.mFlag==1 && opts.lFlag==0)

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

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);
    t=abs(x); tp=t;
    % t is the upper bound of absolute value of x

    % alphap and alpha are used for computing the weight in forming search point
    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; s_t= t + beta * (t -tp);

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

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

        % compute AT As
        if (opts.nFlag==0)
            ATAs=A'*As;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        g=ATAs-ATy + rsL2 * s;

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        tp=t;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the l1-norm regularized projection

            u=s-g/L;
            v= s_t - lambda / L;

            % projection
            [x, t]=ep1R(u, v, n);

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

            v_t=t-s_t;

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

            Av=Ax -As;
            r_sum=v'*v + v_t'*v_t; l_sum=Av'*Av + v'*v * rsL2;

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

            % the condition is ||Av||_2^2 + rsL2 * ||v||_2^2
            %                       <= L * (||v||_2^2 + ||v_t|| _2^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        ValueL(iterStep)=L;

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

        xxp=x-xp;   Axy=Ax-y;
        funVal(iterStep)=Axy'* Axy/2 + rsL2/2 * x'*x + sum(t) * lambda;

        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=sqrt(xxp'*xxp + norm(t-tp)^2);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp + tp'*tp);    norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end

end

%% adaptive line search

% .mFlag=1, and .lFlag=1
%  refomulate the problem as the constrained convex optimization
%  problem, and then apply adaptive line search scheme

% Problem:
%    min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * t' * 1
%    s.t.   |x| <= t

if(opts.mFlag==1 && opts.lFlag==1)

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

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    gamma=1;
    % we shall set the value of gamma = L,
    % where L is appropriate for the starting point

    xp=x; Axp=Ax;
    % store x and Ax
    xxp=zeros(n,1);
    % the difference of x and xp
    t=abs(x); tp=t;
    % t is the upper bound of absolute value of x

    % compute AT Ax
    if (opts.nFlag==0)
        ATAx=A'*Ax;
    elseif (opts.nFlag==1)
        ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
    else
        invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
    end

    % We begin the adaptive line search in the following
    %
    % Note that, in the line search, L and beta are changing

    for iterStep=1:opts.maxIter

        ATAxp=ATAx;
        % store ATAx to ATAxp

        if (iterStep~=1)
            % compute AT Ax
            if (opts.nFlag==0)
                ATAx=A'*Ax;
            elseif (opts.nFlag==1)
                ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
            else
                invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
            end
        end

        %--------- Line Search for L begins
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp;   s_t= t + beta * (t -tp);
                As=Ax + beta* (Ax-Axp);
                ATAs=ATAx + beta * (ATAx- ATAxp);
                % compute the search point s, A * s, and A' * A * s
            else
                alpha= (-1+ sqrt(5)) / 2;
                beta=0; s=x; s_t=t; As=Ax; ATAs=ATAx;
            end

            g=ATAs-ATy + rsL2 * s;
            % compute the gradient g

            % let s walk in a step in the antigradient of s
            u=s-g/L;
            v= s_t - lambda / L;

            % projection
            [xnew, tnew]=ep1R(u,v,n);

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

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

            Av=Axnew -As;
            r_sum=v'*v + v_t'*v_t; l_sum=Av'*Av + v'*v * rsL2;

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

            % the condition is ||Av||_2^2 + rsL2 * ||v||_2^2
            %                       <= L * (||v||_2^2 + ||v_t|| _2^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end
        %--------- Line Search for L ends

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;    x=xnew; xxp=x-xp;
        Axp=Ax;  Ax=Axnew;
        % update x and Ax with xnew and Axnew
        tp=t; t=tnew;
        % update tp and t

        Axy=Ax-y;
        funVal(iterStep)=Axy' * Axy/2 + rsL2/2 * x'*x + lambda * sum(t);
        % compute function value

        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=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp + tp'*tp);    norm_xxp=sqrt(xxp'*xxp+ norm(t-tp)^2);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%%
if(opts.mFlag==0 && opts.lFlag==1)
    error('\n The function does not support opts.mFlag=0 & opts.lFlag=1!');
end


function K = covCos(hyp, x, z, i)

% Stationary covariance function for a sinusoid with period p in 1d:
%
% k(x,z) = sf^2*cos(2*pi*(x-z)/p)
%
% where the hyperparameters are:
%
% hyp = [ log(p)
%         log(sf) ]
%
% Note that covPeriodicNoDC converges to covCos as ell goes to infinity.
%
% Copyright (c) by James Robert Lloyd, 2013-08-05.
%
% See also COVFUNCTIONS.M, COVPERIODICNODC.M.

if nargin<2, K = '2'; return; end                  % report number of parameters
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);
if D>1, error('Covariance is defined for 1d data only.'), end
p   = exp(hyp(1));
sf2 = exp(2*hyp(2));

% precompute distances
if dg                                                               % vector kxx
  K = zeros(size(x,1),1);
else
  if xeqz                                                 % symmetric matrix Kxx
    K = repmat(x,1,n) - repmat(x',n,1);
  else                                                   % cross covariances Kxz
    K = repmat(x,1,size(z,1)) - repmat(z',n,1);
  end
end

K = 2*pi*K/p;
if nargin<4                                                        % covariances
    K = sf2*cos(K);
else                                                               % derivatives
  if i==1
    K = sf2*sin(K).*K;
  elseif i==2
    K = 2*sf2*cos(K);
  else
    error('Unknown hyperparameter')
  end
end


function R = ProjectToSO3(mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes mat: A matrix near SO(3) to project to SO(3).
% Returns R representing the closest rotation matrix that is in SO(3).
% This function uses singular-value decomposition (see
% http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
% and is only appropriate for matrices close to SO(3).
% Example Inputs:
%
% clear; clc;
% mat = [ 0.675, 0.150,  0.720;
%         0.370, 0.771, -0.511;
%        -0.630, 0.619,  0.472];
% R = ProjectToSO3(mat)
%
% Output:
% R =
%    0.6790    0.1489    0.7189
%    0.3732    0.7732   -0.5127
%   -0.6322    0.6164    0.4694

[U, S, V] = svd(mat);
R = U * V';
if det(R) < 0
    % In this case the result may be far from mat.
    R = [R(:, 1: 2); -R(:, 3)];
end
end


% Compute the frequency range of the mel filterbank
% Inputs:
%   start_freq:     the lowerest linear frequency included in the calculation
%   linear_samp:    sampling frequency
%   N_mel:          number of mel banks used
% Output:
%   bin_lower_edge:    the lower edge linear frequency of each mel bank
%   bin_upper_edge:    the upper edge linear frequency of each mel bank

function [bin_lower_edge, bin_upper_edge] = mel_bank_range(start_freq, linear_samp, N_mel)

% get the mel version of start frequency and linear sampling rate
start_mel = linear2mel(start_freq);
samp_mel = linear2mel(linear_samp/2);

% slow implementation
if 0
    for i=1:N_mel
        % find the lower edge linear frequency of the bank
        tmp_mel = (i-1)*(samp_mel-start_mel) / (N_mel+1);
        bin_lower_edge(i) = mel2linear(tmp_mel+start_mel);
        % find the upper edge linear frequency of the bank
        tmp_mel = (i+1)*(samp_mel-start_mel) / (N_mel+1);
        bin_upper_edge(i) =  mel2linear(tmp_mel+start_mel);
    end
else
    % fast implementation
    i=1:N_mel;
    % find the lower edge linear frequency of the bank
    tmp_mel = (i-1)*(samp_mel-start_mel) / (N_mel+1);
    bin_lower_edge = mel2linear(tmp_mel+start_mel);
    % find the upper edge linear frequency of the bank
    tmp_mel = (i+1)*(samp_mel-start_mel) / (N_mel+1);
    bin_upper_edge =  mel2linear(tmp_mel+start_mel);
end


function r = dirrand(m,n)
% DIRRAND - Uniform Dirichlet random vectors
%
%    R = DIRRAND(M) returns vector of length M chosen from
%    Dirichlet(1,...,1) distribution.
%    R = DIRRAND(M,N) returns N such vectors in MxN matrix.
%
%   References: Gelman, Carlin, Stern, Dunson, Vehtari & Rubin (2013),
%               Bayesian Data Analysis, third edition, p. 583.
%
% Copyright (c) 1998-2004 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

if nargin < 2
  n=1;
end

% Generate random numbers from Gamma(1,1) distribution...
r=rand(m,n);
r=-reallog(r);
% ...and scale sum to unity
rs=sum(r);
for i1=1:m
  r(i1,:)=r(i1,:)./rs;
end


%% doubleContraction
% Below is a demonstration of the features of the |doubleContraction| function

%%
clear; close all; clc;

%% Syntax
% |C=doubleContraction(A,B,subContract);|

%% Description
% This function performs a double contraction of two tensors |A| and |B| to
% produce the output tensor |C|. The tensors are either both 3x3 second
% order tensors or |A| is a 4th order 3x3x3x3 tensor. The optional 3rd
% input |subContract| defines the indices to contract over for 4th order
% tensors e.g. |subContract=[1 2]| would contract over the first 2 indices.
%

%% Examples
%

%% Double contraction of a 2nd-order and a 2nd-order tensor

%%
% Creating an example tensor. Here a tensor |A| with all zeros except for
% the 3,3 component is created. Next a tensor |B| which, through double
% contraction, can "pick out" this scalar value is created. Finally the
% double contraction is performed illustrating this process.

%Create example tensor A
A=zeros(3,3); A(3,3)=pi; %Zeros with Azz=A33=pi
R=euler2DCM([0.25*pi 0.25*pi 0]); %Rotation matrix
A=R*A*R' %Rotated version of A

% Create 'structure' tensor example B
b=[0 0 1]*R.'; %Rotated Z vector
B=b.'*b %Rotated "structure" tensor for vector b

%%
% Double contraction using |doubleContraction|
s=doubleContraction(A,B)

%% Double contraction of a 4th-order and a 2nd-order tensor

%%
% Example for Hooke's law of linear elasticity

%Creating the 4th order linear elastic stiffness tensor
I=eye(3,3); %The 2nd order identity tensor
II1=dyadicProduct(I,I,1); %4th order base tensor 1
II3=dyadicProduct(I,I,3); %4th order base tensor 3

%Parameters for Hooke's law
mu=1; %The shear modulus
lambda=5; %The lambda lame parameter

%Compose stiffness tensor
C=lambda.*II1+2.*mu.*II3;

%Create strain tensor
E=rand(3,3); E=E*E'

%%
% Double contraction using |doubleContraction|
subContract=[3 4];
S=doubleContraction(C,E,subContract)

%% Using symbolic variables

%%
% Example for Hooke's law of linear elasticity

%Creating the 4th order linear elastic stiffness tensor
I=eye(3,3); %The 2nd order identity tensor
II1=dyadicProduct(I,I,1); %4th order base tensor 1
II3=dyadicProduct(I,I,3); %4th order base tensor 3

%Parameters for Hooke's law
try
    syms mu lambda
catch
    mu=1; %The shear modulus
    lambda=5; %The lambda lame parameter
end
%Compose stiffness tensor
C=lambda.*II1+2.*mu.*II3;

%Display Voigt mapped version
c=voigtMap(C)

%Create strain tensor
syms E1_1 E2_2 E3_3 E1_2 E1_3 E2_3;
E=[E1_1 E1_2 E1_3;...
   E1_2 E2_2 E2_3;...
   E1_3 E2_3 E3_3]

%%
% Double contraction using |doubleContraction|

subContract=[3 4];
S=doubleContraction(C,E,subContract)

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


% Fast community finding algorithm by M. Newman
% Source: "Fast algorithm for detecting community
%                   structure in networks", Mark Newman
%
% INPUTs: adjacency matrix, nxn
% OUTPUTs: sequential group (cluster) formation,
%          modularity metric for each cluster breakdown
%
% Other functions used: numEdges.m
% Note: To save the plot generated in this routine:
%       uncomment "print newmanCommFast_example.pdf"
%
% GB: last updated, Oct 12 2012

function [groups_hist,Q]=newmanCommFast(adj)

groups={};
groups_hist={};  % a list of groups at each step
Q=[];
n=size(adj,1);   % number of vertices
groups_hist{1}={};

for i=1:n
  groups{i}=i;   % each node starts out as one community
  groups_hist{1}{i}=i;
end

Q(1) = Qfn(groups,adj);

for s=1:n-1 % all possible iterations

  dQ=zeros(length(groups));

  for i=1:length(groups)
    for j=i+1:length(groups)
      group_i=groups{i};
      group_j=groups{j};

      dQ(i,j)=0; % default

      % check if connected
      connected=0;
      if sum(sum(adj([group_i group_j],[group_i group_j])))>sum(sum(adj(group_i,group_i)))+sum(sum(adj(group_j,group_j)))
	connected=1;
      end

      if connected && not(isempty(group_i)) && not(isempty(group_j))
	% join groups i and j?
	dQ(i,j)=deltaQ(groups,adj,i,j);
      end

    end
  end

  % pick max nonzero dQ
  dQ(find(dQ==0))=-Inf;
  [minv,minij]=max(dQ(:));

  [mini,minj]=ind2sub([size(dQ,1),size(dQ,1)],minij);   % i and j corresponding to min dQ
  groups{mini}=sort([groups{mini} groups{minj}]);
  groups{minj}=groups{length(groups)}; % swap minj with last group
  groups=groups(1:length(groups)-1);

  groups_hist{length(groups_hist)+1}=groups; % save current snapshot
  Q(length(Q)+1) = Qfn(groups,adj);

end % end of all iterations

num_modules=[];
for g=1:length(groups_hist)
  num_modules=[num_modules length(groups_hist{g})];
end

set(gcf,'Color',[1 1 1],'Colormap',jet);
plot(num_modules,Q,'ko-')
xlabel('number of modules / communities')
ylabel('modularity metric, Q')

% print newmanCommFast_example.pdf

function dQ=deltaQ(groups,adj,i,j)

% computing the delta Q between two community configurations
% dQ = 2(e_ij - ai*aj) or (Q_groups_(i,j)_merged - Q_original)
% inputs: current community assignments: groups, adjacency matrix, i,j to be joined
% outputs: delta Q value (real number)

% $$$ Q1=Qfn(groups,adj);
% $$$ groups{i}=[groups{i} groups{j}];
% $$$ groups{j}=groups{length(groups)};
% $$$ groups=groups(1:length(groups)-1);
% $$$ Q2=Qfn(groups,adj);
% $$$ dQ = Q2-Q1;

% alternative dQ = 2(e_ij - ai*aj) from paper;
nedges=numEdges(adj); % compute the total number of edges
e_ii = numEdges( adj(groups{i},groups{i}) ) / nedges;
e_jj = numEdges( adj(groups{j},groups{j}) ) / nedges;
e_ij = numEdges( adj([groups{i} groups{j}],[groups{i} groups{j}]) ) / nedges - e_ii - e_jj;

a_i=sum(sum(adj(groups{i},:)))/nedges-e_ii;
a_j=sum(sum(adj(groups{j},:)))/nedges-e_jj;

dQ = 2*(e_ij-a_i*a_j);

function Q=Qfn(modules,adj) % ....... same as modularityMetric.m ........

% Computing the modularity for the final module break-down
% Defined as: Q=sum_over_modules_i (eii-ai^2) (eq 5) in Newman and Girvan.
% eij = fraction of edges that connect community i to community j
% ai=sum_j (eij)

nedges=numEdges(adj); % compute the total number of edges

Q = 0;
for m=1:length(modules)

  e_mm=numEdges(adj(modules{m},modules{m}))/nedges;
  a_m=sum(sum(adj(modules{m},:)))/nedges - e_mm;  % counting e_mm only once

  Q = Q + (e_mm - a_m^2);
end


function [ num_int, pi ] = plane_imp_triangle_int_3d ( a, b, c, d, t )

%*****************************************************************************80
%
%% PLANE_IMP_TRIANGLE_INT_3D: intersection ( implicit plane, triangle ) in 3D.
%
%  Discussion:
%
%    The implicit form of a plane in 3D is:
%
%      A * X + B * Y + C * Z + D = 0
%
%    There may be 0, 1, 2 or 3 points of intersection returned.
%
%    If two intersection points are returned, then the entire line
%    between them comprises points of intersection.
%
%    If three intersection points are returned, then all points of
%    the triangle intersect the plane.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(3,3), the vertices of the triangle.
%
%    Input, real A, B, C, D, the implicit plane parameters.
%
%    Output, integer NUM_INT, the number of intersection points returned.
%
%    Output, real PI(3,3), the intersection points.
%
  dim_num = 3;
  num_int = 0;
  pi = [];
%
%  Compute the signed distances between the vertices and the plane.
%
  dist1 = a * t(1,1) + b * t(2,1) + c * t(3,1) + d;
  dist2 = a * t(1,2) + b * t(2,2) + c * t(3,2) + d;
  dist3 = a * t(1,3) + b * t(2,3) + c * t(3,3) + d;
%
%  Consider any zero distances.
%
  if ( dist1 == 0.0 )
    num_int = num_int + 1;
    pi(1:dim_num,num_int) = t(1:dim_num,1);
  end

  if ( dist2 == 0.0 )
    num_int = num_int + 1;
    pi(1:dim_num,num_int) = t(1:dim_num,2);
  end

  if ( dist3 == 0.0 )
    num_int = num_int + 1;
    pi(1:dim_num,num_int) = t(1:dim_num,3);
  end
%
%  If 2 or 3 of the nodes intersect, we're already done.
%
  if ( 2 <= num_int )
    return
  end
%
%  If one node intersects, then we're done unless the other two
%  are of opposite signs.
%
  if ( num_int == 1 )

    if ( dist1 == 0.0 )

      [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), t(1:dim_num,3), ...
        dist2, dist3, num_int, pi );

    elseif ( dist2 == 0.0 )

      [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), t(1:dim_num,3), ...
        dist1, dist3, num_int, pi );

    elseif ( dist3 == 0.0 )

      [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), t(1:dim_num,2), ...
        dist1, dist2, num_int, pi );

    end

    return

  end
%
%  All nodal distances are nonzero, and there is at least one
%  positive and one negative.
%
  if ( dist1 * dist2 < 0.0 & dist1 * dist3 < 0.0 )

    [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), t(1:dim_num,2), ...
      dist1, dist2, num_int, pi );

    [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), t(1:dim_num,3), ...
      dist1, dist3, num_int, pi );

  elseif ( dist2 * dist1 < 0.0 & dist2 * dist3 < 0.0 )

    [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), t(1:dim_num,1), ...
      dist2, dist1, num_int, pi );

    [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), t(1:dim_num,3), ...
      dist2, dist3, num_int, pi );

  elseif ( dist3 * dist1 < 0.0 & dist3 * dist2 < 0.0 )

    [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,3), t(1:dim_num,1), ...
      dist3, dist1, num_int, pi );

    [ num_int, pi ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,3), t(1:dim_num,2), ...
      dist3, dist2, num_int, pi );

  end

  return
end


function x=logistic(n,level,a,x0)
%Syntax: x=logistic(n,level,a,x0)
%________________________________
%
% Simulation of the Quadratic map.
%    x'=ax(1-x)
%
% x is the simulated time series.
% n is the number of the simulated points.
% level is the noise standard deviation divided by the standard deviation of the
%   noise-free time series. We assume Gaussian noise with zero mean.
% a is the parameter.
% x0 is the initial value for x.
%
%
% Reference:
%
% May R M (1976): Simple mathematical modelswith very complicated dynamics.
% Nature 261: 459-467
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 17 Nov 2001

if nargin<1 | isempty(n)==1
    n=500;
else
    % n must be scalar
    if sum(size(n))>2
        error('n must be scalar.');
    end
    % n must be positive
    if n<0
        error('n must be positive.');
    end
    % n must be an integer
    if round(n)-n~=0
        error('n must be an integer.');
    end
end

if nargin<2 | isempty(level)==1
    level=0;
else
    % level must be a scalar
    if sum(size(level))>2
        error('level must be scalar.');
    end
    % level must be positive
    if level<0
        error('level must be positive.');
    end
end

if nargin<3 | isempty(a)==1
    a=4;
else
    % a must be scalar
    if sum(size(a))>2
        error('a must be scalar.');
    end
end

if nargin<4 | isempty(x0)==1
    x0=0.1;
else
    % x0 must be scalar
    if sum(size(x0))>2
        error('x0 must be scalar.');
    end
end

% Initialize
x(1,1)=a*x0*(1-x0);

% Simulate
for i=2:n
    x(i,1)=a*x(i-1,1)*(1-x(i-1,1));
end

% Add normal white noise
x=x+randn(n,1)*level*std(x);


function [gradf, err] = tapas_riddersgradient(f, x, varargin)
% Calculates the gradient of the function f at point x according to Ridders' method:
%
% Ridders, CJF. (1982). Accurate computation of F'(x) and F'(x) F''(x). Advances in Engineering
%     Software, 4(2), 75-6.
%
% INPUT:
%    f             Function handle of a real function of n real variables which are passed as
%                  *one* vector with n elements
%    x             Point at which to differentiate f
%
% OUTPUT:
%    gradf         Gradient of f at x (row vector)
%    err           Error estimates (row vector)
%
% OPTIONS:
%    Optionally, the third argument of the function can be a structure containing further
%    settings for Ridder's method.
%
%    varargin{1}.init_h      Initial finite difference (default: 1)
%    varargin{1}.div         Divisor used to reduce h on each step (default: 1.2)
%    varargin{1}.min_steps   Minimum number of steps in h (default: 3)
%    varargin{1}.max_steps   Maximum number of steps in h (default: 100)
%    varargin{1}.tf          Terminate if last step worse than preceding by a factor of tf
%                            (default: 2)
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is released under the terms of the GNU General Public Licence (GPL), version 3. You can
% redistribute it and/or modify it under the terms of the GPL (either version 3 or, at your option,
% any later version). For further details, see the file COPYING or <http://www.gnu.org/licenses/>.

    n = length(x);

    % Defaults
    options.init_h     = 1;
    options.div        = 1.2;
    options.min_steps  = 3;
    options.max_steps  = 100;
    options.tf         = 2;
    gradf              = NaN(1,n);
    err                = NaN(1,n);

    % Overrides
    if nargin > 2
        options_ovr = varargin{1};

        if isfield(options_ovr,'init_h')
            options.init_h = options_ovr.init_h;
        end

        if isfield(options_ovr,'div')
            options.div = options_ovr.div;
        end

        if isfield(options_ovr,'min_steps')
            options.min_steps = options_ovr.min_steps;
        end

        if isfield(options_ovr,'max_steps')
            options.max_steps = options_ovr.max_steps;
        end

        if isfield(options_ovr,'tf')
            options.tf = options_ovr.tf;
        end
    end

    % Check if f and x match
    try
        f(x);
    catch err
        error('tapas:hgf:ridders:CannotEvalFun', 'Function cannot be evaluated at differentiation point.');
    end

    % Loop through argument variables
    for i = 1:n

        % Construct filehandle to be passed to riddersdiff
        fxih = @(xi) fxi(f,x,i,xi);

        % Calculate derivative
        [gradf(i), err(i)] = tapas_riddersdiff(fxih,x(i),options);
    end
end

function fxi = fxi(f,x,i,xi)
    xx    = x;
    xx(i) = xi;
    fxi   = f(xx);
end


function outsig=rampup(L,wintype)
%RAMPUP  Rising ramp function
%   Usage: outsig=rampup(L);
%
%   `rampup(L)` will return a rising ramp function of length *L*. The
%   ramp is a sinusoide starting from zero and ending at one. The ramp
%   is centered such that the first element is always 0 and the last
%   element is not quite 1, such that the ramp fits with following ones.
%
%   `rampup(L,wintype)` will use another window for ramping. This may be any
%   of the window types from |firwin|. Please see the help on |firwin| for
%   more information. The default is to use a piece of the Hann window.
%
%   See also: rampdown, rampsignal, firwin

complainif_argnonotinrange(nargin,1,2,mfilename);

if nargin==1
  wintype='hann';
end;

win=firwin(wintype,2*L,'inf');
outsig=win(L+1:2*L);


function [ r c ] = corner_ST( img,ptNum )
%[ r c ] = corner_ST( img )
%	Find corner points in image using Shi-Tomasi method
%   IMG should be gray.R and C are the coordinates of corners(col vec)

img = im2double(img);
h1 = fspecial('gauss',[6 6],1.5);
hd = imfilter(h1,[-1 1],'rep');
hd = hd(1:5,1:5); % use the HOG gradient
% hd = [-1 0 1];
imgdx = imfilter(img,hd,'rep');
imgdy = imfilter(img,hd','rep');

ker = fspecial('gauss',[5 5],1.5); % weight of the patch
imgdx2 = imfilter(imgdx.^2,ker,'rep');
imgdxdy = imfilter(imgdx.*imgdy,ker,'rep');
imgdy2 = imfilter(imgdy.^2,ker,'rep');

A = zeros(2,2,numel(img));
A(1,1,:) = imgdx2(:);
A(1,2,:) = imgdxdy(:);
A(2,1,:) = imgdxdy(:);
A(2,2,:) = imgdy2(:);
ev = real(eig2(A));

evTh = .1;
minEv = min(ev,[],2);
minEv = reshape(minEv,size(img));
R = (minEv > evTh*max(max(minEv)));
% fis(R)

Rmax = ordfilt2(minEv,9,ones(3));
Rcorner = (minEv == Rmax) & R; % non-maximal suppression
% fis(Rcorner)
I = find(Rcorner);

if exist('ptNum','var') && nnz(Rcorner) > ptNum
	selEvs = minEv(I); % find the ptNum corners with the largest ev
	[~,I1] = sort(selEvs,'descend');
	I = I(I1(1:ptNum));
end
[r c] = ind2sub(size(img),I); % points' positions

end

function ev = eig2(a) % eigenvalue of a 2*2 matrix
b = -(a(1,1,:)+a(2,2,:));
detA = a(1,1,:).*a(2,2,:)-a(1,2,:).*a(2,1,:);
delta = b.^2-4*detA;
ev = [-b+sqrt(delta),-b-sqrt(delta)]/2;
ev = reshape(ev,2,[])'; % N-by-2
end


function [logbf,t] = spm_bms_ttest (y,prior)
% Log Bayes Factor against null for one sample t-test
% FORMAT [logbf,t] = spm_bms_ttest (y,prior)
%
% y         [N x 1] data vector
% prior     'jzs' (default), 'unit'
%
% logbf     Log Bayes Factor
% t         t-statistic
%
% Default prior is 'jzs'. The different priors are described in
% [1] Rouder et al. (2009) Psych Bull Rev, 16(2),225-237.
%
% These tests consider the quantity d = mean / standard deviation.
% The unit information prior, 'unit', uses a zero mean unit variance
% Gaussian prior over d (the prior variance of d, sig_d^2, is unity).
%
% The (Jeffreys-Zellner-Snow) JZS prior, 'jzs', uses a Cauchy over d
% and an inverse chi^2 over sig_d^2.
%_______________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_bms_ttest.m 6038 2014-06-04 15:22:42Z will $

try
    pr=prior;
catch
    pr='jzs';
end

N=length(y);
m=mean(y);
s=std(y);
sem=s/sqrt(N);
t=m/sem;
v=N-1;

switch pr,
    case 'unit',
        rs=1;
    case 'jzs',
        logbf = jzs_logbf(t,v,N);
        return
    otherwise
        disp('Unknown prior in bayes_ttest');
        return
end

% For Unit Information prior, 'unit':
%
% The minimum value of logbf obtains when t=0
% This minimum value is logbf=-0.5*log(1+N*r)
% For r=1 logbf=-3 can be gotten with N=405 samples.

% See note 5 at end of [1]
t2=t^2;
nr=1+N*rs^2;
log_num=-0.5*N*log(1+t2/v);
log_denom1=-0.5*log(nr);
log_denom2=-0.5*N*log(1+t2/(nr*v));

logbf=log_num-log_denom1-log_denom2;
logbf=-logbf;

%-------------------------------------------------------
function [logbf] = jzs_logbf(t,v,N)
% JZS Log Bayes factor
%
% See equation (1) in
% Rouder et al. (2009) Psych Bull Rev, 16(2),225-237.

t2=t^2;
num=(1+t2/v)^(-0.5*(v+1));

gmax=1000;
denom=quad(@(g)jzs_integrand(g,t2,N,v),0,gmax);

logbf=log(num/denom);
logbf=-logbf;

%--------------------------------------------------------
function [j] = jzs_integrand(g,t2,N,v)

ng=1+N*g;
t1=ng.^(-0.5);
t2=(1+t2./(ng*v)).^(0.5*(v+1));
t3=(2*pi)^(-0.5)*(g.^(-1.5)).*exp(-1./(2*g));
j=t1.*t2.*t3;


function [R,S]=rga(G)
% RGA   Relative Gain Array
%
% R=rga(G) returns the Relative Gain Array, R based on the gain matrix, G.
%
% For square matrix, G, R is the normal RGA, defined as follows:
%   R = inv(G') .* G
% If G is nonsquare, R is the so called General RGA:
%   R = pinv(G') .* G
%
% The RGA is a very useful tool for control structure design. For a square
% system (the number of inputs equals to the number of output), the RGA can
% be used to find input-output pairs (one input to control one output).
% There are many publications on how to pair input and output using the
% RGA. The basic rule is:
% 1. Avoid input and output pairs which have negative relative gains.
% 2. Avoid input and output pairs which have large relative gains.
% 3. Select input and output pairs which have the relative gain close to 1.
%
% Interestingly, if the RGA is repeatedly calculated based on R, i.e.
%
% R = rga(R);
%
% Eventualy, the RGA will converge to a selection (0,1)-matrix. With the
% second output,
%
% [R,S] = rga(G) produces a selection matrix, S based on the repeated RGA.
%
% where S is (0,1) matrix with the same dimension as G. Each row and each
% column of S have only one element of 1, but all others are 0. The element
% of 1 indicates which input (column) should be used to pair which output
% (row).
% Note: The selection matrix, S, is only a recommendation. It may not
% satisfy the rules stated above and may not be appropriate for some
% systems.
%
% For nonsquare systems, the general RGA can also be used to select inputs
% if the number of inputs is larger than the number of outputs, or to
% choose outputs, if the system has more outputs than inputs. The second
% output in this case gives the input (output) effectiveness, E.
%
% [R,E] = rga(G)
%
% Where E is a column vector corresponding to the input (output). Inputs
% (outputs) with largest input (output) effectiveness are recommented to be
% selected for the control system.
%
% By Yi Cao at Cranfield University on 7th March 2008
%
% References:
%
% 1. Bristol, E.H., On a new measure of interactions for multivariable process
% control. IEEE Trans. Automatic Control, AC-11:133-134, 1966.
% 2. Cao, Y and Rossiter, D, An input pre-screening technique for control
% structure selection, Computers and Chemical Engineering, 21(6), pp.
% 563-569, 1997.

% Examples:
%
% Example 1: A random 5 x 5 matrix
%{
G = randn(5);
[R,S] = rga(G)
%}
% Example 2: A random 5 x 10 matrix for input selection
%{
G = randn(5,10);
[R,IE] = rga(G)
%}
% Example 3: A random 10 x 5 matrix for output selection
%{
G = randn(10,5);
[R,OE] = rga(G)
%}
%

% Check input and output
error(nargchk(1,1,nargin));
error(nargoutchk(0,2,nargout));

% The RGA
R=pinv(G.').*G;
if nargout<2
    return
end

[m,n]=size(G);
% The square case
if n==m
    S=R;
    while norm(round(S)-S)>1e-9
        S(S<0)=0;
        S = inv(S.').*S;
    end
    return
end

% The nonsquare case
S = sqrt(sum(R))';
if m>n
    S = sqrt(sum(R,2));
end


%% LSHAPESTOKES Poiseuille flow in a L-shaped domain
%
% Stokes equations on the square [-1,1]^2.
%
% Reference: page 237 in Finite Elements and Fast Iterative Solvers with
% Applications in Incompressible Fluid Dynamics. by Howard C. Elman, David
% J. Silvester, and Andrew J. Wathen.
%
% See also squareStokes, StokesP2P1
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear all;
%% Parameters
maxIt = 4; N = zeros(maxIt,1);
erru = zeros(maxIt,1); errp = zeros(maxIt,1);

%% Generate an initial mesh
[node,elem] = squaremesh([-1 1 -1 1], 0.25);
bdFlag = setboundary(node,elem,'Dirichlet');

%% PDE and options
pde = Stokesdata1;

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [u,p,edge,A] = StokesP2P1(node,elem,pde,bdFlag);
    N(k) = length(u)+length(p);
    if N(k) < 2e3 % show solution for small size
        figure(1);  showresult(node,elem,p);
    end
    % compute error
    uI = pde.exactu([node; (node(edge(:,1),:)+node(edge(:,2),:))/2]);
    erru(k) = sqrt((u-uI(:))'*A*(u-uI(:)));
    errp(k) = getL2error(node,elem,pde.exactp,p);
    % refine mesh
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
end

%% Plot convergence rates
figure(2); clf;
showrate2(N,erru,2,'-*','||Du_I-Du_h||',N,errp,2,'k-+','|| p - p_h||');


function p=legendrep(m,x)
%   function which construct Legendre polynomial Pm(x)
%   where M is the degree of polynomial and X is the variable.
%   Result - P is the char string that should be evaluated EVAL(P)
%   Example:
%   P=legendrep(2,'cos(theta)') will produce
%   P='(3*cos(theta).^2 -1)/2' which then can be evaluated as
%   theta=0.3; P=legendrep(2,'cos(theta)'); Lp=eval(P); produce
%   Lp = 0.8690
%   For Matlab R14 the following example can be used:
%   x=-5:.1:5; p=legendrep(5,'x .*cos(x)'); xp = eval(p);
%   figure; plot(x, xp, 'r.-'); grid

%% References:
%   Gradshteyn, Ryzhik "Table of Integrals Series and Products", 6th ed., p.973
%
%__________________________________________________
% 	Sergei Koptenko, Resonant Medical Inc., Toronto
%	sergei.koptenko@resonantmedical.com
%______________March/30/2004_______________________
%%
switch m
case 0
    p='1';     return
case 1
    p=x;     return
case 2
    p=['(3*' x '.^2 -1)/2'];     return
case 3
    p=['(5*' x '.^3 - 3 *' x ')/2'];     return
case 4
    p=['(35 *' x '.^4 - 30 * ' x '.^2 + 3)/8'];     return
case 5
    p=['( 63 * ' x '.^5 - 70 * ' x '.^3 + 15 *' x ' )/8'];     return
case 6
    p=['(231 *' x '.^6 - 315 * ' x '.^4 + 105 * ' x '.^2 -5)/16'];     return
 case 7
   p=['(429 * ' x '.^7 - 693 * ' x '.^5 +315 * ' x '.^3 -35 *' x ' )/16'];     return
case 8
    p=['(6435 *' x '.^8 -12012 *' x '.^6 + 6930 * ' x '.^4 -1260 * ' x '.^2 +35)/128'];     return
case 9
    p=['(12155 * ' x '.^9 -25740 * ' x '.^7 +18018 * ' x '.^5 -4620 * ' x '.^3 +315 *' x ' )/128'];
    return
case 10
       p=['(46189 *' x '.^10 -109395 *' x '.^8 +90090 *' x '.^6 -30030 * ' x '.^4 +3465 * ' x '.^2 -63)/256'];
    return
case 11
   p=['(88179 * ' x '.^11 -230945 * ' x '.^9 +218790 * ' x '.^7 -90090 * ' x '.^5 +15015 * ' x '.^3 -693 *' x ' )/256'];
    return
case 12
    p=['(676039 *' x '.^12 -1939938 *' x '.^10 +2078505 *' x '.^8 -1021020 *' x '.^6 +225225 * ' x '.^4 -18018 * ' x '.^2 +231)/1024'];
    return

otherwise
    iii=m-10;    %shift counter
   pp=strvcat(legendrep(11,x),legendrep(12,x)); % get last two members
    for ii=3:1:iii,   % Begin construct from 13th member
        p_ii=[num2str(1/(ii)) ' * (' num2str(2*ii-1) ' * ' x '.*(' ...
        deblank(pp(ii-1,:)) ')-' num2str(ii-1) '.*(' deblank(pp(ii-2,:)) '))'];
        pp=strvcat(pp, p_ii);
    end
p=deblank(pp(iii,:)); % remove traiing blanks
   return
end

return


function [A,b] = parallax(n)
%PARALLAX Stellar parallax problem with 28 fixed, real observations.
%
% [A,b] = parallax(n)
%
% Stellar parallax problem with 28 fixed, real observations.
%
% The underlying problem is a Fredholm integral equation of the
% first kind with kernel
%    K(s,t) = (1/sigma*sqrt(2*pi))*exp(-0.5*((s-t)/sigma)^2) ,
% and it is discretized by means of a Galerkin method with n
% orthonormal basis functions.  The right-hand side consists of
% a measured distribution function of stellar parallaxes, and its
% length is fixed, m = 26.  The exact solution, which represents
% the true distribution of stellar parallaxes, in not known.

% Reference: W. M. Smart, "Stellar Dynamics", Cambridge University Press,
% 1938; p. 30.

% Discretized by Galerkin method with orthonormal box functions;
% 2-D integration is done by means of the computational molecule:
%       1   4   1
%       4  16   4
%       1   4   1

% Per Christian Hansen, IMM, 09/16/92.

% Initialization.
a = 0; b = 0.1; m = 26; sigma = 0.014234;
hs = 0.130/m; hx = (b-a)/n; hsh = hs/2; hxh = hx/2;
ss = (-0.03 + [0:m-1]'*hs)*ones(1,n);
xx = ones(m,1)*(a + [0:n-1]*hx);

% Set up the matrix.
A =     16*exp(-0.5*((ss+hsh - xx-hxh)/sigma).^2);
A = A + 4*(exp(-0.5*((ss+hsh - xx    )/sigma).^2) + ...
           exp(-0.5*((ss+hsh - xx-hx )/sigma).^2) + ...
           exp(-0.5*((ss     - xx-hxh)/sigma).^2) + ...
           exp(-0.5*((ss+hs  - xx-hxh)/sigma).^2));
A = A +   (exp(-0.5*((ss     - xx    )/sigma).^2) + ...
           exp(-0.5*((ss+hs  - xx    )/sigma).^2) + ...
           exp(-0.5*((ss     - xx-hx )/sigma).^2) + ...
           exp(-0.5*((ss+hs  - xx-hx )/sigma).^2));
A = sqrt(hs*hx)/(36*sigma*sqrt(2*pi))*A;

% Set up the normalized right-hand side.
b = [3;7;7;17;27;39;46;51;56;50;43;45;43;32;33;29;...
     21;12;17;13;15;12;6;6;5;5]/(sqrt(hs)*640);


function [Y, spectrum] = sllle_wg(G, d)
%SLLLE_WG Solves the Locally Linear Embedding from weight graph
%
% $ Syntax $
%   - Y = sllle_wg(G, d)
%   - [Y, spectrum] = sllle_wg(G, d)
%
% $ Arguments $
%   - G:        The weights graph
%   - d:        The dimension of the embedding
%   - Y:        The sample coordinates in the embedded space
%   - spectrum: The spectrum of the embeded space dimensions
%
% $ Description $
%   - Y = sllle_wg(G, d) solves the locally linear embedding from
%     a given weight graph. G should be a graph of n nodes, where
%     the edge value from i-th node to j-th node, means the weights
%     on the i-th sample in constructing the j-th sample, (or the
%     construction of i-th sample to the j-th sample).
%     Y is solved by taking the eigenvectors of (I - W)(I - W)^T,
%     corresponding to the (d+1) smallest eigenvalues, and discarding
%     the smallest one.  In our output, the dimension is sorted in the
%     ascending order of eigenvalues.
%
%   - [Y, spectrum] = sllle_wg(G, d) also returns the spectrum of the
%     corresponding dimensions, a column vector of the corresponding
%     eigenvalues.
%
% $ Remarks $
%   - The dimension d should be strictly less than n.
%
% $ History $
%   - Created by Dahua Lin, on Sep 11st, 2006
%

%% parse and verify input

if nargin < 2
    raise_lackinput('sllle_wg', 2);
end

gi = slgraphinfo(G, {'square'});
if ~gi.valued
    error('sltoolbox:invalidarg', 'The graph G should be a valued graph');
end
if isnumeric(G)
    W = G;
else
    W = sladjmat(G, 'sparse', true);
end

n = gi.n;
if d >= n
    error('sltoolbox:invalidarg', 'd should be strictly less than n');
end

%% compute

if issparse(W)
    M = speye(n) - W;
else
    M = eye(n) - W;
end
clear W;
M = M * M';

[spectrum, Y] = slsymeig(M, d+1, 'ascend');
clear M;
spectrum = spectrum(2:d+1);
Y = Y(:, 2:d+1);
Y = Y';


function cnt = countcover(sz,blocksize,stepsize)
%COUNTCOVER Covering of signal samples by blocks
%  CNT = COUNTCOVER(SZ,BLOCKSIZE,STEPSIZE) assumes a p-dimensional signal
%  of size SZ=[N1 N2 ... Np] covered by (possibly overlapping) blocks of
%  size BLOCKSIZE=[M1 M2 ... Mp]. The blocks start at position (1,1,..,1)
%  and are shifted between them by steps of size STEPSIZE=[S1 S2 ... Sp].
%  COUNTCOVER returns a matrix the same size as the signal, containing in
%  each entry the number of blocks covering that sample.
%
%  See also IM2COLSTEP, COL2IMSTEP, IM2COL.

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

cnt = ones(sz);
for k = 1:length(sz)

  % this code is modified from function NDGRID, so it computes one
  % output argument of NDGRID at a time (to conserve memory)
  ids = (1:sz(k))';
  s = sz; s(k) = [];
  ids = reshape(ids(:,ones(1,prod(s))),[length(ids) s]);
  ids = permute(ids,[2:k 1 k+1:length(sz)]);

  cnt = cnt .* max( min(floor((ids-1)/stepsize(k)),floor((sz(k)-blocksize(k))/stepsize(k))) - ...
                    max(ceil((ids-blocksize(k))/stepsize(k)),0) + 1 , 0 );
end


function [x1,x2,nS]=bracket(S,x0,d,problem,stepsize)
%   Bracket the minimum (or maximum) of the objective function
%   in the search direction.
%
%   [x1,x2,nS]=bracket(S,x0,d,problem,stepsize)
%
%   S: objective function
%   x0: initial point
%   d: search direction vector
%   problem: (-1): minimum (default), (1): maximum
%   stepsize: initial stepsize (default = 0.01*norm(d))
%   [x1,x2]: unsorted lower and upper limits
%   nS: number of objective function evaluations

%   Copyright (c) 2001 by LASIM-DEQUI-UFRGS
%   $Revision: 1.0 $  $Date: 2001/07/04 21:45:10 $
%   Argimiro R. Secchi (arge@enq.ufrgs.br)

 if nargin < 3,
   error('bracket requires three input arguments');
 end
 if nargin < 4,
   problem=-1;
 end
 if nargin < 5,
   stepsize=0.01*norm(d);
 end

 d=d(:);
 x0=x0(:);
 j=0; nS=1;
 y0=feval(S,x0)*problem;

 while j < 2,
  x=x0+stepsize*d;
  y=feval(S,x)*problem;
  nS=nS+1;

  if y0 >= y,
    stepsize=-stepsize;
    j=j+1;
  else
    while y0 < y,
      stepsize=2*stepsize;
      y0=y;
      x=x+stepsize*d;
      y=feval(S,x)*problem;
      nS=nS+1;
    end
    j=1;
    break;
  end
 end

 x2=x;
 x1=x0+stepsize*(j-1)*d;


clear; clc;
% P2.4
x = [1,-2,4,6,-5,8,10];
n = [-4:2];

% P2.4a
[x11,n11] = sigshift(x,n,-2);
[x12,n12] = sigshift(x,n,4);
[x1,n1] = sigadd(3*x11,n11,x12,n12);
[x1,n1] = sigadd(x1,n1,-2*x,n);
[xe,xo,m] = evenodd(x1,n1);
figure(1);
subplot(2,2,1); stem(n1,x1,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4b
[x21,n21] = sigshift(x,n,-5);
[x22,n22] = sigshift(x,n,-4);
[x2,n2] = sigadd(5*x21,n21,4*x22,n22);
[x2,n2] = sigadd(x2,n2,3*x,n);
[xe,xo,m] = evenodd(x2,n2);
figure(2);
subplot(2,2,1); stem(n2,x2,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4c
[x31,n31] = sigshift(x,n,-4);
[x32,n32] = sigshift(x,n,1);
[x33,n33] = sigmult(x31,n31,x32,n32);
[x31,n31] = sigfold(x,n);    [x31,n31] = sigshift(x31,n31,2);
[x32,n32] = sigmult(x31,n31,x,n);
[x3,n3] = sigadd(x33,n33,x32,n32);
[xe,xo,m] = evenodd(x3,n3);
figure(3);
subplot(2,2,1); stem(n3,x3,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4d
[x41,n41] = sigshift(x,n,-2);
x42 = cos(0.1*pi*n);
[x42,n42] = sigmult(x42,n,x41,n41);
x43 = 2*exp(0.5*n);
[x41,n41] = sigmult(x43,n,x,n);
[x4,n4] = sigadd(x42,n42,x41,n41);
[xe,xo,m] = evenodd(x4,n4);
figure(4);
subplot(2,2,1); stem(n4,x4,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4e
x5 = zeros(size(n));
for k = 1:5,
    [x51,n51] = sigshift(x,n,k);
    x52 = n.*x51;
    x5 = sigadd(x5,n51,x52,n51);
end
[xe,xo,m] = evenodd(x5,n51);
figure(5);
subplot(2,2,1); stem(n51,x5,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');


function x = inv_digamma(y,niter)
% INV_DIGAMMA    Inverse of the digamma function.
%
% inv_digamma(y) returns x such that digamma(x) = y.

% a different algorithm is provided by Paul Fackler:
% http://www.american.edu/academic.depts/cas/econ/gaussres/pdf/loggamma.src

% Newton iteration to solve digamma(x)-y = 0
x = exp(y)+1/2;
i = find(y <= -2.22);
x(i) = -1./(y(i) - digamma(1));

% never need more than 5 iterations
if nargin < 2
  niter = 5;
end
for iter = 1:niter
  x = x - (digamma(x)-y)./trigamma(x);
end
return

% test
y = -3:0.01:0.1;
x = digamma(inv_digamma(y));
max(abs(x-y))
max(abs(x-y)./inv_digamma(y))


function [LIFT] = Lift2D()

% function [LIFT] = Lift2D()
% Purpose  : Compute surface to volume lift term for DG formulation

Globals2D;
Emat = zeros(Np, Nfaces*Nfp);

% face 1
faceR = r(Fmask(:,1));
V1D = Vandermonde1D(N, faceR);
massEdge1 = inv(V1D*V1D');
Emat(Fmask(:,1),1:Nfp) = massEdge1;

% face 2
faceR = r(Fmask(:,2));
V1D = Vandermonde1D(N, faceR);
massEdge2 = inv(V1D*V1D');
Emat(Fmask(:,2),Nfp+1:2*Nfp) = massEdge2;

% face 3
faceS = s(Fmask(:,3));
V1D = Vandermonde1D(N, faceS);
massEdge3 = inv(V1D*V1D');
Emat(Fmask(:,3),2*Nfp+1:3*Nfp) = massEdge3;

% inv(mass matrix)*\I_n (L_i,L_j)_{edge_n}
LIFT = V*(V'*Emat);
return


function [ num_int, pint ] = halfspace_imp_triangle_int_3d ( a, b, c, d, t )

%*****************************************************************************80
%
%% HALFSPACE_IMP_TRIANGLE_INT_3D: intersection ( implicit halfspace, triangle ) in 3D.
%
%  Discussion:
%
%    The implicit form of a half-space in 3D may be described as the set
%    of points (X,Y,Z) on or "above" an implicit plane:
%
%      0 <= A * X + B * Y + C * Z + D
%
%    The triangle is specified by listing its three vertices.
%
%    The intersection may be described by the number of vertices of the
%    triangle that are included in the halfspace, and by the location of
%    points between vertices that separate a side of the triangle into
%    an included part and an unincluded part.
%
%    0 vertices, 0 separators    (no intersection)
%    1 vertex, 0 separators      (point intersection)
%    2 vertices, 0 separators    (line intersection)
%    3 vertices, 0 separators    (triangle intersection)
%
%    1 vertex, 2 separators,     (intersection is a triangle)
%    2 vertices, 2 separators,   (intersection is a quadrilateral).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, D, the parameters that define the
%    implicit plane, which in turn define the implicit halfspace.
%
%    Input, real T(3,3), the vertices of the triangle.
%
%    Output, integer NUM_INT, the number of intersection points returned,
%    which will always be between 0 and 4.
%
%    Output, real PINT(3,4), the coordinates of the NUM_INT
%    intersection points.  The points will lie in sequence on the triangle.
%    Some points will be vertices, and some may be separators.
%

%
%  Compute the signed distances between the vertices and the plane.
%
  dist1 = a * t(1,1) + b * t(2,1) + c * t(3,1) + d;
  dist2 = a * t(1,2) + b * t(2,2) + c * t(3,2) + d;
  dist3 = a * t(1,3) + b * t(2,2) + c * t(3,3) + d;
%
%  Now we can find the intersections.
%
  [ num_int, pint ] = halfspace_triangle_int_3d ( t, dist1, dist2, dist3 );

  return
end


% ======================================================== %
% Files of the Matlab programs included in the book:       %
% Xin-She Yang, Nature-Inspired Metaheuristic Algorithms,  %
% Second Edition, Luniver Press, (2010).   www.luniver.com %
% ======================================================== %

% -------------------------------------------------------- %
% Firefly Algorithm for constrained optimization using     %
% for the design of a spring (benchmark)                   %
% by Xin-She Yang (Cambridge University) Copyright @2009   %
% -------------------------------------------------------- %

function fa_ndim
% parameters [n N_iteration alpha betamin gamma]
para=[20 500 0.5 0.2 1];

help fa_ndim.m

% Simple bounds/limits for d-dimensional problems
d=15;
Lb=zeros(1,d);
Ub=2*ones(1,d);

% Initial random guess
u0=Lb+(Ub-Lb).*rand(1,d);

[u,fval,NumEval]=ffa_mincon(@cost,u0,Lb,Ub,para);

% Display results
bestsolution=u
bestojb=fval
total_number_of_function_evaluations=NumEval

%%% Put your own cost/objective function here --------%%%
%% Cost or Objective function
 function z=cost(x)
% Exact solutions should be (1,1,...,1)
z=sum((x-1).^2);

%%% End of the part to be modified -------------------%%%

%%% --------------------------------------------------%%%
%%% Do not modify the following codes unless you want %%%
%%% to improve its performance etc                    %%%
% -------------------------------------------------------
% ===Start of the Firefly Algorithm Implementation ======
%         Lb = lower bounds/limits
%         Ub = upper bounds/limits
%   para == optional (to control the Firefly algorithm)
% Outputs: nbest   = the best solution found so far
%          fbest   = the best objective value
%      NumEval = number of evaluations: n*MaxGeneration
% Optional:
% The alpha can be reduced (as to reduce the randomness)
% ---------------------------------------------------------

% Start FA
function [nbest,fbest,NumEval]...
           =ffa_mincon(fhandle,u0, Lb, Ub, para)
% Check input parameters (otherwise set as default values)
if nargin<5, para=[20 500 0.25 0.20 1]; end
if nargin<4, Ub=[]; end
if nargin<3, Lb=[]; end
if nargin<2,
disp('Usuage: FA_mincon(@cost,u0,Lb,Ub,para)');
end

% n=number of fireflies
% MaxGeneration=number of pseudo time steps
% ------------------------------------------------
% alpha=0.25;      % Randomness 0--1 (highly random)
% betamn=0.20;     % minimum value of beta
% gamma=1;         % Absorption coefficient
% ------------------------------------------------
n=para(1);  MaxGeneration=para(2);
alpha=para(3); betamin=para(4); gamma=para(5);

% Total number of function evaluations
NumEval=n*MaxGeneration;

% Check if the upper bound & lower bound are the same size
if length(Lb) ~=length(Ub),
    disp('Simple bounds/limits are improper!');
    return
end

% Calcualte dimension
d=length(u0);

% Initial values of an array
zn=ones(n,1)*10^100;
% ------------------------------------------------
% generating the initial locations of n fireflies
[ns,Lightn]=init_ffa(n,d,Lb,Ub,u0);

% Iterations or pseudo time marching
for k=1:MaxGeneration,     %%%%% start iterations

% This line of reducing alpha is optional
 alpha=alpha_new(alpha,MaxGeneration);

% Evaluate new solutions (for all n fireflies)
for i=1:n,
   zn(i)=fhandle(ns(i,:));
   Lightn(i)=zn(i);
end

% Ranking fireflies by their light intensity/objectives
[Lightn,Index]=sort(zn);
ns_tmp=ns;
for i=1:n,
 ns(i,:)=ns_tmp(Index(i),:);
end

%% Find the current best
nso=ns; Lighto=Lightn;
nbest=ns(1,:); Lightbest=Lightn(1);

% For output only
fbest=Lightbest;

% Move all fireflies to the better locations
[ns]=ffa_move(n,d,ns,Lightn,nso,Lighto,nbest,...
      Lightbest,alpha,betamin,gamma,Lb,Ub);

end   %%%%% end of iterations

% -------------------------------------------------------
% ----- All the subfunctions are listed here ------------
% The initial locations of n fireflies
function [ns,Lightn]=init_ffa(n,d,Lb,Ub,u0)
  % if there are bounds/limits,
if length(Lb)>0,
   for i=1:n,
   ns(i,:)=Lb+(Ub-Lb).*rand(1,d);
   end
else
   % generate solutions around the random guess
   for i=1:n,
   ns(i,:)=u0+randn(1,d);
   end
end

% initial value before function evaluations
Lightn=ones(n,1)*10^100;

% Move all fireflies toward brighter ones
function [ns]=ffa_move(n,d,ns,Lightn,nso,Lighto,...
             nbest,Lightbest,alpha,betamin,gamma,Lb,Ub)
% Scaling of the system
scale=abs(Ub-Lb);

% Updating fireflies
for i=1:n,
% The attractiveness parameter beta=exp(-gamma*r)
   for j=1:n,
      r=sqrt(sum((ns(i,:)-ns(j,:)).^2));
      % Update moves
if Lightn(i)>Lighto(j), % Brighter and more attractive
   beta0=1; beta=(beta0-betamin)*exp(-gamma*r.^2)+betamin;
   tmpf=alpha.*(rand(1,d)-0.5).*scale;
   ns(i,:)=ns(i,:).*(1-beta)+nso(j,:).*beta+tmpf;
      end
   end % end for j

end % end for i

% Check if the updated solutions/locations are within limits
[ns]=findlimits(n,ns,Lb,Ub);

% This function is optional, as it is not in the original FA
% The idea to reduce randomness is to increase the convergence,
% however, if you reduce randomness too quickly, then premature
% convergence can occur. So use with care.
function alpha=alpha_new(alpha,NGen)
% alpha_n=alpha_0(1-delta)^NGen=10^(-4);
% alpha_0=0.9
delta=1-(10^(-4)/0.9)^(1/NGen);
alpha=(1-delta)*alpha;

% Make sure the fireflies are within the bounds/limits
function [ns]=findlimits(n,ns,Lb,Ub)
for i=1:n,
     % Apply the lower bound
  ns_tmp=ns(i,:);
  I=ns_tmp<Lb;
  ns_tmp(I)=Lb(I);

  % Apply the upper bounds
  J=ns_tmp>Ub;
  ns_tmp(J)=Ub(J);
  % Update this new move
  ns(i,:)=ns_tmp;
end

%% ==== End of Firefly Algorithm implementation ======


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

clear all;
close all;

numGs=1;
denGs=[1 0 0];  % s^2

numDs=.81*[1 .2];
denDs=[1 2];

numC=conv(numGs,numDs);
denC=conv(denGs,denDs);

[numCL,denCL]=feedback(numC,denC,1,1);
damp(denCL)

tf=30;
t=0:.2:tf;
y=step(numCL,denCL,t);

axis([0 30 0 1.5])
plot(t,y),grid
hold on

% discrete designs

T=1;
[numGz,denGz]=c2dm(numGs,denGs,T,'zoh');

numDz1=[.389 -.319];
denDz1=[1 -.135];

numDz2=.374*[1 -.85];
denDz2=[1 0];

numz1=conv(numGz,numDz1);
denz1=conv(denGz,denDz1);

numz2=conv(numGz,numDz2);
denz2=conv(denGz,denDz2);

[numCLz1,denCLz1]=feedback(numz1,denz1,1,1);
[numCLz2,denCLz2]=feedback(numz2,denz2,1,1);

ddamp(denCLz1,T)
ddamp(denCLz2,T)

N=tf/T+1;
td=0:1:tf;
yd1=dstep(numCLz1,denCLz1,N);
yd2=dstep(numCLz2,denCLz2,N);
plot(td,yd1,'-',td,yd1,'*')
plot(td,yd2,'-',td,yd2,'o')
text(10,.55,'----------- continuous design')
text(10,.75,'*----*----* discrete equivalent design')
text(10,.65,'o----o----o discrete design')
title('Fig. 8.19  Step responses of the various design methods')
xlabel('Time (sec)')
ylabel('Plant output')
hold off


function a = laguerre ( n )

%*****************************************************************************80
%
%% LAGUERRE returns the LAGUERRE matrix.
%
%  Example:
%
%    N = 8
%
%      1      .     .      .    .    .    .    .
%      1     -1     .      .    .    .    .    .
%      2     -4     1      .    .    .    .    .   /    2
%      6    -18     9     -1    .    .    .    .   /    6
%     24    -96    72    -16    1    .    .    .   /   24
%    120   -600   600   -200   25   -1    .    .   /  120
%    720  -4320  5400  -2400  450  -36    1    .   /  720
%   5040 -35280 52920 -29400 7350 -882   49   -1   / 5040
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is lower triangular.
%
%    The columns of A alternate in sign.
%
%    A(I,1) = 1
%    A(I,I) = (-1)^(I-1) / (I-1)!.
%
%    LAMBDA(I) = (-1)^(I-1) / (I-1)!.
%
%    det ( A ) = product ( 1 <= I <= N ) (-1)^(I-1) / (I-1)!
%
%    The I-th row contains the coefficients of the Laguerre
%    polynomial of degree I-1.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  if ( n <= 0 )
    a = [];
    return
  end

  a(1,1) = 1.0;

  if ( n == 1 )
    return
  end

  a(2,1) = 1.0;
  a(2,2) = -1.0;

  if ( n == 2 )
    return
  end

  for i = 3 : n
    for j = 1 : n
      if ( j == 1 )
        a(i,j) = (  ( 2 * i - 3 ) * a(i-1,j) ...
                 +  (   - i + 2 ) * a(i-2,j) ) ...
                 /  (     i - 1 );
      else
        a(i,j) = (  ( 2 * i - 3 ) * a(i-1,j) ...
                 -  (         1 ) * a(i-1,j-1) ...
                 +  (   - i + 2 ) * a(i-2,j) ) ...
                 /  (     i - 1 );
      end
    end
  end

  return
end


% function thetaOpt = StochasticGradientDescent (gradFunc, theta0, maxiter)
% runs gradient descent until convergence, returning the optimal parameters thetaOpt.
%
% Inputs:
% gradFunc      function handle to a function [cost, grad] = gradFunc(theta, i)
%               that computes the LR cost / objective function and the gradient
%               of the negative log likelihood of the ith data instance, given
%               parameters theta.
% theta0        initial value of theta to start gradient descent from.
% maxIter       number of iterations to run SGD for.
%
% Output:
% thetaOpt      optimal value of theta.
%
%
% Note - function handles may be new to some of you. Briefly, function handles
% are a way of passing a function to another function as an argument. The
% syntax for calling a function handle is exactly the same as for calling any
% other function.
%
% For more information, refer to the official documentation:
% Matlab - http://www.mathworks.com/help/techdoc/matlab_prog/f2-38133.html
% Octave - http://www.gnu.org/software/octave/doc/interpreter/Function-Handles.html
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function thetaOpt = StochasticGradientDescent (gradFunc, theta0, maxIter)

    % The grader will accept all answers that are near enough
    % to the optimal value, so don't worry about being off by one
    % iteration etc.

    thetaOpt = zeros(size(theta0));

    %%%%%%%%%%%%%%
    %%% Student code
    for i = 1:maxIter
	    [cost, grad] = gradFunc(theta0,i);
	    theta0 = theta0 - (0.1)*(grad)/(1+sqrt(i));
    end
    thetaOpt = theta0;
    %%%%%%%%%%%

end


function z = mutInfo(x, y)
% Compute mutual information I(x,y) of two discrete variables x and y.
% Input:
%   x, y: two integer vector of the same length
% Output:
%   z: mutual information z=I(x,y)
% Written by Mo Chen (sth4nth@gmail.com).
assert(numel(x) == numel(y));
n = numel(x);
x = reshape(x,1,n);
y = reshape(y,1,n);

l = min(min(x),min(y));
x = x-l+1;
y = y-l+1;
k = max(max(x),max(y));

idx = 1:n;
Mx = sparse(idx,x,1,n,k,n);
My = sparse(idx,y,1,n,k,n);
Pxy = nonzeros(Mx'*My/n); %joint distribution of x and y
Hxy = -dot(Pxy,log2(Pxy));

Px = nonzeros(mean(Mx,1));
Py = nonzeros(mean(My,1));

% entropy of Py and Px
Hx = -dot(Px,log2(Px));
Hy = -dot(Py,log2(Py));
% mutual information
z = Hx+Hy-Hxy;
z = max(0,z);


clear; clc;

%% DFT filter bank
N = 8;          % number of subbands
K = 16;         % overlapping factor
L = 2*K*N;      % length of analysis filters

hopt = fir1(L-1,1/N);       % prototype filter
[H,F] = make_bank_DFT(hopt,N);
H = sqrt(N)*H;                    % analysis section
F = sqrt(N)*F;                    % synthesis section

%% signals in the DFT filter bank
xn = sin(2*pi*0.01*(0:1000));
ITER = length(xn);
yn = zeros(ITER,1);
x = zeros(length(H),1);
b = zeros(length(F),1);

%% analysis and synthesis filter banks implementation
for n = 1:ITER
    x = [xn(n); x(1:end-1)];
    % block transformation is performed once for every N input samples
    if mod(n,N) == 0
        x_D = H.'*x;
        % =====================
        % insert subband processing here
        % =====================
        b = F*x_D + b;
    end
    yn(n) = real(b(1));
    b = [b(2:end); 0];
end

%% plot input and output signal
figure;
plot(0:length(xn)-1, xn); hold on;
plot(0:length(yn)-1, yn, 'r:');
xlabel('Time index, n'); ylabel('Amplitude');

%{
 %% plot frequency response
DFTpoint = 4096;
[Hz,w] = FreqResp(H, DFTpoint);
Hz = Hz.';
figure;
subplot(3,1,[1 2]); hold on;
for k = 1:N
    if mod(k,2) == 0
        plot(w/pi,20*log10(abs(Hz(k,:))));
    else
        plot(w/pi,20*log10(abs(Hz(k,:))),':');
    end
end
title('frequency response');
%}

github-repos/MATLAB/CharlesThaCat-acoustic-interference-cancellation/acoustic-interference-cancellation-edb394499ea6f9c96445a3e9613bd64a854c289e/Subband processing/Book/After reading Chapter 2 -- Design Analysis & Synthesis Filter bank/DFTFilterBank.m


%%                   runCoordinateDescent.m

% This test file implements the Coordinate Descent solver. The code builds
% a synthetic formulation of the Phase Retrieval problem, b = |Ax| and
% computes an estimate to x. The code finally plots a convergence curve
% and also makes a scatter plot of the true vs recovered solution.

% PAPER TITLE:
%              Coordinate Descent Algorithms for Phase Retrieval

% ARXIV LINK:
%              https://arxiv.org/abs/1706.03474

% 1) Each test script starts out by defining the length of the unknown
% signal, n and the number of measurements, m. These mesurements can be
% made complex by setting the isComplex flag to be true.
%
% 2) We then build the test problem by invoking the function
% 'buildTestProblem' which generates random gaussian measurements according
% to the user's choices in step(1). The function returns the measurement
% matrix 'A', the true signal 'xt' and the measurements 'b0'.
%
% 3) We set the options for the PR solver. For example, the maximum
% number of iterations, the tolerance value, the algorithm and initializer
% of choice. These options are controlled by setting the corresponding
% entries in the 'opts' struct.  Please see the user guide for a complete
% list of options.
%
% 4) We solve the phase retrieval problem by running the following line
% of code:
%   >>  [x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts)
% This solves the problem using the algorithm and initialization scheme
% specified by the user in the struct 'opts'.
%
% 5) Determine the optimal phase rotation so that the recovered solution
% matches the true solution as well as possible.
%
% 6) Report the relative reconstruction error. Plot residuals (a measure
% of error) against the number of iterations and plot the real part of the
% recovered signal against the real part of the original signal.

% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017
%% -----------------------------START----------------------------------

clc
clear
close all

n = 100;          % Dimension of unknown vector
m = 8 * n;        % Number of measurements
isComplex = true; % If the signal and measurements are complex

%%  Build a random test problem
fprintf('Building test problem...\n');
[A, xt, b0] = buildTestProblem(m, n, isComplex);

% Options
opts = struct;
opts.initMethod = 'Truncatedspectral';
% opts.initMethod = 'optimalspectral';
opts.algorithm = 'CoordinateDescent';
opts.isComplex = isComplex;
opts.maxIters = 500000;
opts.tol = 1e-6;
opts.verbose = 2;

%% Try to recover x
fprintf('Running algorithm...\n');
[x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts);

%% Determine the optimal phase rotation so that the recovered solution
%  matches the true solution as well as possible.
alpha = (x'*xt)/(x'*x);
x = alpha * x;

%% Determine the relative reconstruction error.  If the true signal was
%  recovered, the error should be very small - on the order of the numerical
%  accuracy of the solver.
reconError = norm(xt-x)/norm(xt);
fprintf('relative recon error = %d\n', reconError);

% Plot a graph of error(definition depends on if opts.xt is provided) versus
% the number of iterations.
plotErrorConvergence(outs, opts)

% Plot a graph of the recovered signal x against the true signal xt.
plotRecoveredVSOriginal(x,xt);


function K = covMaterniso(d, hyp, x, z, i)

% Matern covariance function with nu = d/2 and isotropic distance measure. For
% d=1 the function is also known as the exponential covariance function or the
% Ornstein-Uhlenbeck covariance in 1d. The covariance function is:
%
%   k(x^p,x^q) = sf^2 * f( sqrt(d)*r ) * exp(-sqrt(d)*r)
%
% with f(t)=1 for d=1, f(t)=1+t for d=3 and f(t)=1+t+t²/3 for d=5.
% Here r is the distance sqrt((x^p-x^q)'*inv(P)*(x^p-x^q)), P is ell times
% the unit matrix and sf2 is the signal variance. The hyperparameters are:
%
% hyp = [ log(ell)
%         log(sf) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<3, K = '2'; return; end                  % report number of parameters
if nargin<4, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

ell = exp(hyp(1));
sf2 = exp(2*hyp(2));
if all(d~=[1,3,5]), error('only 1, 3 and 5 allowed for d'), end         % degree

switch d
  case 1, f = @(t) 1;               df = @(t) 1;            % df(t) = f(t)-f'(t)
  case 3, f = @(t) 1 + t;           df = @(t) t;
  case 5, f = @(t) 1 + t.*(1+t/3);  df = @(t) t.*(1+t)/3;
end
          m = @(t,f) f(t).*exp(-t); dm = @(t,f) df(t).*exp(-t).*t;

% precompute distances
if dg                                                               % vector kxx
  K = zeros(size(x,1),1);
else
  if xeqz                                                 % symmetric matrix Kxx
    K = sq_dist(sqrt(d)/ell*x');
  else                                                   % cross covariances Kxz
    K = sq_dist(sqrt(d)/ell*x',sqrt(d)/ell*z');
  end
end

if nargin<5                                                        % covariances
  K = sf2*m(sqrt(K),f);
else                                                               % derivatives
  if i==1
    K = sf2*dm(sqrt(K),f);
  elseif i==2
    K = 2*sf2*m(sqrt(K),f);
  else
    error('Unknown hyperparameter')
  end
end


function p = mvnormpdfln(x, m, S, V)
% MVNORMPDFLN    log of multivariate normal density.
%   See MVNORMPDF for argument description.

log2pi = 1.83787706640935;
[d, n] = size(x);
if nargin == 1
  dx = x;
elseif isempty(m)
  dx = x;
else
  % m specified
  sz = size(m);
  if sz(1) ~= d
    error('rows(m) ~= rows(x)')
  end
  nm = sz(2);
  if nm == 1
    dx = x - repmat(m,1,n);
  elseif n == 1
    dx = repmat(x,1,nm) - m;
  elseif nm == n
    dx = x - m;
  else
    error('incompatible number of columns in x and m')
  end
end
if nargin < 3
  % unit variance
  p = -0.5*(d*log2pi + col_sum(dx.*dx));
  return
end
have_inv = 0;
if nargin == 3
  % standard deviation given
  if d == 1
    dx = dx./S;
    p = (-log(S) -0.5*log2pi) - 0.5*(dx.*dx);
    return;
  end
  if S(2,1) ~= 0
    error('S is not upper triangular')
  end
  if any(size(S) ~= [d d])
    error('S is not the right size')
  end
else
  if ischar(V)
    if strcmp(V,'inv')
      % inverse stddev given
      iS = S;
      have_inv = 1;
    else
      error('unknown directive')
    end
  elseif ischar(S)
    if strcmp(S,'inv')
      % inverse variance given
      if d == 1
	iS = sqrt(V);
      else
	iS = chol(V);
      end
      have_inv = 1;
    else
      error('unknown directive')
    end
  else
    % variance given
    if d == 1
      S = sqrt(V);
    else
      S = chol(V);
    end
  end
end
if have_inv
  if d == 1
    dx = iS .* dx;
    logdetiS = log(iS);
  else
    dx = iS*dx;
    logdetiS = sum(log(diag(iS)));
  end
else
  if d == 1
    dx = dx./S;
    logdetiS = -log(S);
  else
    dx = solve_tril(S',dx);
    %dx = S'\dx;
    logdetiS = -sum(log(diag(S)));
  end
end
p = (logdetiS -0.5*d*log2pi) -0.5*col_sum(dx.*dx);


% Local Regression and Likelihood, Figure 6.1.
%
% Derivative (local slope) estimation, for the Old Faithful Geyser Data.
% The 'deriv' argument specifies derivative estimation,
%   'deriv',1        First-order derivative.
%   'deriv',[1 1]    Second-order derivative.
%   'deriv',2        For bivariate fits, partial deriv. wrt second variable.
%   'deriv',[1 2]    Mixed second-order derivative.
%
% Density estimation is done on the log-scale. That is, the estimate
% is of g(x) = log(f(x)), where f(x) is the density.
%
% The relation between derivatives is therefore
%     f'(x) = f(x)g'(x) = g'(x)exp(g(x)).
% To estimate f'(x), we must estimate g(x) and g'(x) (fit1 and fit2 below),
% evaluate on a grid of points (p1 and p2), and apply the back-transformation.
%
% Disclaimer: I don't consider derivative estimation from noisy data
% to be a well-defined problem. Use at your own risk.
%
% Author: Catherine Loader
%
% NEED: m argument passed to lfmarg().

load geyser;
fit1 = locfit(geyser,'alpha',[0.1 0.6],'ll',1,'ur',6);
fit2 = locfit(geyser,'alpha',[0.1 0.6],'ll',1,'ur',6,'deriv',1);
z = lfmarg(fit1);
p1 = predict(fit1,z);
p2 = predict(fit2,z);
figure('Name','fig6_1: slope estimation: Old faithful data' );
plot(z{1},p2.*exp(p1));
xlabel('Eruption Duration (Minutes)');
ylabel('Density Derivative');