function [u1, u2, u3, u4] = controller(current_state, desired_state, model_param, K)

J = model_param.I; % Moment of inertia wrt body frame

% position error
ep = current_state(1:3)-desired_state.pos;

% velocity error
ev = current_state(4:6)-desired_state.vel;

% desired force, F_des
Kp = K.Kp;
Kv = K.Kv;
KR = K.KR;
K_omega = K.K_omega;
m = model_param.mass;
g = model_param.grav;
Fd = -Kp.*ep -Kv.*ev + m*g*[0 0 1]' + m*desired_state.acc;

% desired input u1
yaw = current_state(9);
roll = current_state(7);
pitch = current_state(8);
R = ROTZ(yaw)*ROTX(roll)*ROTY(pitch); % Current rotation
% current z-axis in body frame
zb = R(:,3);
u1 = Fd'*zb;

current_acc = -g*[0 0 1]'+u1*zb;

% accel error
ea = current_acc-desired_state.acc;
% Desired force derivative
Fd_dot = -Kp*ev -Kv*ea + m*desired_state.jerk;
% desired rotation, Rd = [xbd ybd zbd]
zbd = Fd/norm(Fd);

% ybd = zbd X xcd/norm(zbd X xcd)
% xcd
yawd = desired_state.yaw;
yawd_dot = desired_state.yawdot;
yawd_2dot = desired_state.yawddot;

xcd = [cos(yawd) sin(yawd) 0]';
ybd = hat_optr(zbd)*xcd/norm(hat_optr(zbd)*xcd);
% xbd = ybd X zbd
xbd = hat_optr(ybd)*zbd;
Rd1 = [xbd ybd zbd];
Rd2 = [-xbd -ybd zbd];

% eR orientation error
R1 = (Rd1'*R - R'*Rd1);
R2 = (Rd2'*R - R'*Rd2);

eR1 = 1/2*vee_optr(R1);
eR2 = 1/2*vee_optr(R2);
% if norm(eR1) >= norm(eR2)
%     Rd = Rd2;
%     eR = eR2;
%     xbd = -xbd;
%     ybd = -ybd;
% else
    Rd = Rd1;
    eR = eR1;
% end

current_omega = current_state(10:12);

% Desired omega^ = Rd'*R
Fd_norm_dot = Fd'*Fd_dot/norm(Fd);
zbd_dot = (Fd_dot*norm(Fd) - Fd*Fd_norm_dot)/norm(Fd)^2;
xcd_dot = [-sin(yawd) cos(yawd) 0]'*desired_state.yawdot;

zbd_x_xcd_dot = hat_optr(zbd_dot)*xcd + hat_optr(zbd)*xcd_dot;
zbd_xcd_norm_dot = (hat_optr(zbd)*xcd)'*(zbd_x_xcd_dot)/norm(hat_optr(zbd)*xcd);

ybd_dot = (zbd_x_xcd_dot*norm(hat_optr(zbd)*xcd) - hat_optr(zbd)*xcd*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;
xbd_dot = hat_optr(ybd_dot)*zbd + hat_optr(ybd)*zbd_dot;

Rd_dot = [xbd_dot ybd_dot zbd_dot];
wd_hat = Rd'*Rd_dot;
wd = vee_optr(wd_hat);
% error
ew = current_omega - R'*Rd*wd;

% Desired angular accerelation^ = Rd_dot'*Rd_dot + Rd'*Rd_2dot;
R_dot = R*hat_optr(current_omega);
zb_dot = R_dot(:,3);
u1_dot = Fd_dot'*zb + Fd'*zb_dot;
current_jerk = (u1_dot*zb + u1*zb_dot)/m;
ej = current_jerk - desired_state.jerk;
Fd_2dot = -Kp*ea -Kv*ej + m*desired_state.snap;
zbd_2dot = (Fd_2dot*norm(Fd)-Fd_dot*Fd_norm_dot)/norm(Fd)^2 -...
           (((Fd_dot'*Fd_dot + Fd'*Fd_2dot)*Fd + (Fd'*Fd_dot)*Fd_dot)*norm(Fd)^3 -...
           (Fd'*Fd_dot)*Fd * 3*norm(Fd)*(Fd'*Fd_dot))/norm(Fd)^4;

% ybd_2dot
xcd_2dot = [-cos(yawd)*yawd_dot^2-sin(yawd)*yawd_2dot, -sin(yawd)*yawd_dot^2+cos(yawd)*yawd_2dot, 0]';

ybd_2dot_1 = ((hat_optr(zbd_2dot)*xcd + hat_optr(zbd)*xcd_2dot)*norm(hat_optr(zbd)*xcd)-...
             zbd_x_xcd_dot*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;

zbd_x_xcd_2dot = hat_optr(zbd_2dot)*xcd + 2*hat_optr(zbd_dot)*xcd_dot + hat_optr(zbd)*xcd_2dot;
zbd_xcd_norm_2dot = ((zbd_x_xcd_dot'*zbd_x_xcd_dot + (hat_optr(zbd)*xcd)'*zbd_x_xcd_2dot)*norm(hat_optr(zbd)*xcd)-...
                    ((hat_optr(zbd)*xcd)'*zbd_x_xcd_dot)*zbd_xcd_norm_dot)/norm(hat_optr(zbd)*xcd)^2;

ybd_2dot_2 = ((zbd_x_xcd_dot*zbd_xcd_norm_dot + hat_optr(zbd)*xcd*zbd_xcd_norm_2dot)*norm(hat_optr(zbd)*xcd)^2-...
              2*hat_optr(zbd)*xcd*norm(hat_optr(zbd)*xcd)*zbd_xcd_norm_dot^2)/norm(hat_optr(zbd)*xcd)^4;

ybd_2dot = ybd_2dot_1 - ybd_2dot_2;

xbd_2dot = hat_optr(ybd_2dot)*zbd + 2*hat_optr(ybd_dot)*zbd_dot + hat_optr(ybd)*zbd_2dot;

Rd_2dot = [xbd_2dot ybd_2dot zbd_2dot];
wd_dot_hat = Rd_dot'*Rd_dot + Rd'*Rd_2dot;
wd_dot = vee_optr(wd_dot_hat);

% Moment
M = -KR.*eR -K_omega.*ew + hat_optr(current_omega)*J*current_omega -J*(hat_optr(current_omega)*R'*Rd*wd - R'*Rd*wd_dot);

u2 = M(1);u3 = M(2); u4 = M(3);


%% Angular correlation between spherical harmonics profiles
%% Reference:
%%     Adam W. Anderson.
%%     Measurement of Fiber Orientation Distributions Using High Angular Resolution Diffusion Imaging.
%%     Magn. Reson. Med., 54(5):1194-1206, 2005.  (Eq.(71))
%% Example:
%%     [corr] = angular_correlation(coeff_u,coeff_v);
function [corr] = angular_correlation(coeff_u,coeff_v)
%%=============================================================
%% Project:   Spherical Harmonics
%% Module:    $RCSfile: angular_correlation.m,v $
%% Language:  MATLAB
%% Author:    $Author: bjian $
%% Date:      $Date: 2007/12/27 06:23:35 $
%% Version:   $Revision: 1.3 $
%%=============================================================

corr = (coeff_u'*coeff_v)/(norm(coeff_u)*norm(coeff_v));


function res = innerprod( x, y, dir, upto )
    %INNERPROD Inner product between two TT/MPS tensors.
    %   innerprod(X,Y) computes the inner product between the TT/MPS tensors X and Y.
    %   Assumes that the first rank of both tensors, X.rank(1) and Y.rank(1), is 1.
    %   The last rank may be different from 1, resulting in a matrix of size
    %   [X.rank(end), Y.rank(end)].
    %
    %   See also NORM

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    if ~exist( 'dir', 'var' )
        dir = 'LR';
    end
    if ~exist( 'upto', 'var' )
        if strcmpi( dir, 'LR')
            upto = x.order;
        else
            upto = 1;
        end
    end

    % Left-to-Right procedure
    if strcmpi( dir, 'LR')

        res = unfold( x.U{1}, 'left')' * unfold( y.U{1}, 'left');

        for i = 2:upto
            tmp = tensorprod_ttemps( x.U{i}, res', 1);
            res = unfold( tmp, 'left')' * unfold( y.U{i}, 'left');
        end

    % Right-to-Left procedure
    elseif strcmpi( dir, 'RL')
        d = x.order;
        res = conj(unfold( x.U{d}, 'right')) * unfold( y.U{d}, 'right').';

        for i = d-1:-1:upto
            tmp = tensorprod_ttemps( x.U{i}, res', 3);
            res = conj(unfold( tmp, 'right')) * unfold( y.U{i}, 'right').';
        end

    else
        error('Unknown direction specified. Choose either LR (default) or RL')
    end

end


function [forecastmatrix]=forecastsim(data_endo_a,data_exo_p,beta,n,p,k,horizon)

% function [forecastmatrix]=forecastsim(data_endo_a,data_exo_p,beta,n,p,k,horizon)
% computes the matrix of unconditional forecasts, using the chain rule of forecasts (see technical guide p38)
% inputs:  - matrix 'data_endo_a': matrix of pre-forecast endogenous data
%          - matrix 'data_exo_p': predicted values for the exogenous variables over the forecast periods
%          - vector 'beta': vectorised form of VAR coefficients (definined in 1.1.12)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'horizon': number of forecast periods
% outputs: - matrix 'forecast_matrix': matrix recording the forecast values

% uses the chain rule of forecasting, p38 of the technical guide

% compute the reduced matrix Y
Y=data_endo_a(end-p+1:end,:);

% reshape beta to obtain B
B=reshape(beta,k,n);

% repeat the process for periods T+1 to T+h
for jj=1:horizon

% step 1
% use the function lagx to obtain the matrix temp
temp=bear.lagx(Y,p-1);

% step 2
% define the reduced regressor matrix X
% if no exogenous variable is present at all in the model (neither constant nor other exogenous), define X only from the endogenous variables
if isempty(data_exo_p)==1
X=[temp(end,:)];
% if there are exogenous variables, concatenate them next to the endogenous
else
X=[temp(end,:) data_exo_p(jj,:)];
end

% step 3
% obtain the predicted value for T+jj
yp=X*B;

% step 4
% concatenate yp to the top of Y
Y=[Y;yp];

% repeat until values are obtained for T+h
end

% record the values in the matrix forecastmatrix
forecastmatrix=Y(p+1:end,:);


%  Figure 10.57      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_57.m is a script for disk read/write head control design
% the design is based on 1/s^2 with robustness
% with respect to a resonance of damping zeta = z at wm.
% the design calls for a single lead selected to maximize wc
% the phase margin is set by alpha = a
% the resonance is 'gain stabilized' with GM
% the time is scaled to milliseconds
clf;
wm= 5*pi
z=.05;
GM = 4;
a=0.1;
numGo=1;
denGo= [1 0 0];
sysGo=tf(numGo,denGo);
numG= [1/(50*pi) 1];
denG=[1/(25*pi^2) 1/(50*pi) 1  0 0];
sysG=tf(numG,denG);
b=GM/(2*z);
% PM >50; select alpha = 0.1
T= (1/wm)*sqrt(b/a^1.5);
wc = 1/(sqrt(a)*T)
% K = sqrt(a)*wc^2
K= sqrt(a)*wm^2/b;
numD =K*sqrt(a)*[T 1];
denD = [a*T 1];
sysD=tf(numD,denD);
sysOLo=sysD*sysGo;
sysOL=sysD*sysG;
figure(1)
clf
% bode(sysOLo);
% hold on
w=logspace(-1,2);
[mag,ph]=bode(sysOL,w);
subplot(211);
% convert to db
magdb=20*log10(mag(:,:));
magdb1=[magdb; zeros(size(magdb))];
semilogx(w,magdb1);
grid;
title('Fig. 10.57 Disk drive control with gain-stabilized resonance');
xlabel('\omega (rad/sec)');
ylabel('Magnitude (db)');
subplot(212);
semilogx(w,[ph(:,:); -180*ones(size(ph(:,:)))]);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
pause;
%hold off
[Gm,Pm,Wcg,Wcp] = margin(mag,ph,w)


function poisson_simulation_test02 ( )

%*****************************************************************************80
%
%% POISSON_SIMULATION_TEST02 simulates waiting for a given length of time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2012
%
%  Author:
%
%    John Burkardt
%
  lambda = 0.5;
  t = 1000.0;
  test_num = 20000;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POISSON_SIMULATION_TEST02:\n' );
  fprintf ( 1, '  POISSON_FIXED_EVENTS simulates a Poisson process\n' );
  fprintf ( 1, '  counting the number of events that occur during\n' );
  fprintf ( 1, '  a given time.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Simulate a Poisson process, for which, on average,\n' );
  fprintf ( 1, '  LAMBDA events occur per unit time.\n' );
  fprintf ( 1, '  Run for a total of %g time units.\n', t );
  fprintf ( 1, '  LAMBDA = %g\n', lambda );

  n = zeros ( test_num, 1 );
  for test = 1 : test_num
    n(test) = poisson_fixed_time ( lambda, t );
  end

  n_mean = mean ( n );
  n_var = var ( n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Mean number of events = %g\n', n_mean );
  fprintf ( 1, '  Variance = %g,  STD = %g\n', n_var, sqrt ( n_var ) );

  bins = 30;
  hist ( n, bins );
  xlabel ( '<--Number of events-->' )
  ylabel ( '<--Frequency-->' )
  grid on
  title ( ' Number of Poisson events observed over 1000 time units' );

  filename = 'poisson_events.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot file saved as "%s"\n', filename );

  return
end


function [col4row,row4col,gain]=assign2DHungarian(C,maximize)
%%ASSIGN2DHUNGARIAN Solve the two-dimensional assignment problem with a
%          rectangular cost matrix C using the O(n^4) complexity Hungarian
%          algorithm. The problem being solved can be formulated as
%          minimize (or maximize)
%          \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}C_{i,j}*x_{i,j}
%          subject to
%          \sum_{j=1}^{numCol}x_{i,j}<=1 for all i
%          \sum_{i=1}^{numRow}x_{i,j}=1 for all j
%          x_{i,j}=0 or 1.
%          Assuming that numCol<=numRow. If numCol>numRow, then the
%          inequality and inequality conditions are switched. The function
%          assign2D uses an O(n^3) shortest augmenting path modification of
%          the Hungarian algorithm and thus should be preferred over this
%          algorithm.
%
%INPUTS: C A numRowXnumCol cost matrix that does not contain any NaNs and
%          where the largest finite element minus the smallest element is a
%          finite quantity (does not overflow) when performing minimization
%          and where the smallest finite element minus the largest
%          element is finite when performing maximization. Forbidden
%          assignments can be given costs of +Inf for minimization and -Inf
%          for maximization.
% maximize If true, the minimization problem is transformed into a
%          maximization problem. The default if this parameter is omitted
%          or an empty matrix is passed is false.
%
%OUTPUTS: col4row A numRowX1 vector where the entry in each element is an
%                 assignment of the element in that row to a column. 0
%                 entries signify unassigned rows. If the problem is
%                 infeasible, this is an empty matrix.
%         row4col A numColX1 vector where the entry in each element is an
%                 assignment of the element in that column to a row. 0
%                 entries signify unassigned columns. If the problem is
%                 infeasible, this is an empty matrix.
%            gain The sum of the values of the assigned elements in C. If
%                 the problem is infeasible, this is -4
%
%This function implements the O(n^4) complexity Hungarian algorithm, which
%is the combination of algorithms 4.2 and 4.3 of Chapter 4.2 of [1]. The
%algorithm has been slightly modified to handle rectangular matrices and to
%transform the input matrix to have all positice elements before running
%the algorithm, so that the algorithm can work on general matrices.
%Additionally, the algorithm contains a floating-point comparison to zero,
%which has been replaced by a threshold to make the algorithm more robust
%when handling non-integer costs.
%
%See the comments to assign2D for examples. The function assign2D
%
%REFERENCES:
%[1] R. Burkard, M. Dell'Amico, and S. Martello, Assignment Problems.
%    Philadelphia: Society for Industrial and Applied Mathematics, 2009.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(maximize))
    maximize=false;
end

numU=size(C,1);
numV=size(C,2);

didTranspose=false;
if(numU>numV)
    C=C';

    temp=numU;
    numU=numV;
    numV=temp;

    didTranspose=true;
end

%The cost matrix must have all non-negative elements for the assignment
%algorithm to work. This forces all of the elements to be positive. The
%delta is added back in when computing the gain in the end.
if(maximize==true)
    CDelta=max(C(:));

    %If C is all negative, do not shift.
    if(CDelta<0)
        CDelta=0;
    end

    C=-C+CDelta;
else
    CDelta=min(C(:));

    %If C is all positive, do not shift.
    if(CDelta>0)
        CDelta=0;
    end

    C=C-CDelta;
end

UScanned=zeros(numU,1);%Allocate space
VLabeledUnscanned=zeros(numV,1);%Allocate space
pred=zeros(numV,1);%Allocate space

VLabeled=zeros(numV,1);%Allocate space.

unassignedU=1:numU;
numUnassignedU=numU;
%Quick indicator of which vertex is assigned.
assignedBool=false(numU,1);
numAssignedU=0;

%Allocate and initialize.
u=zeros(numU,1);
v=zeros(numV,1);
row4col=zeros(numV,1);
col4row=zeros(numU,1);

while(numAssignedU<numU)
    kIdx=1;
    k=unassignedU(kIdx);
    while(assignedBool(k)==false)
        %%%GET THE ALTERNATING PATH

        %Unlabeled vertices.
        VUnlabeled=1:numV;
        numUnlabeled=numV;
        numLabeledUnscanned=0;
        numScannedU=0;
        numLabeled=0;

        fail=false;
        sink=0;
        i=k;
        while(fail==false&&sink==0)
            numScannedU=numScannedU+1;
            UScanned(numScannedU)=i;
            jIdx=1;
            while(jIdx<=numUnlabeled)
                j=VUnlabeled(jIdx);
                cost=C(i,j)-u(i)-v(j);
                %Ad-hoc threshold for testing if zero.
                if(cost<=4*eps(C(i,j)))
                    pred(j)=i;
                    %Remove from the list of unlabeled vertices.
                    VUnlabeled(jIdx)=VUnlabeled(numUnlabeled);
                    numUnlabeled=numUnlabeled-1;

                    %Add to the list of labeled but unscanned vertices.
                    numLabeledUnscanned=numLabeledUnscanned+1;
                    VLabeledUnscanned(numLabeledUnscanned)=j;

                    %Also just add to the list of labeled vertices.
                    numLabeled=numLabeled+1;
                    VLabeled(numLabeled)=j;
                else
                    jIdx=jIdx+1;
                end
            end
            if(numLabeledUnscanned==0)
                fail=true;
            else
                %Scan the last labeled, unscanned vertex.
                j=VLabeledUnscanned(numLabeledUnscanned);
                numLabeledUnscanned=numLabeledUnscanned-1;

                if(row4col(j)==0)
                    sink=j;
                else
                    i=row4col(j);
                end
            end
        end

        %%UPDATE USING THE ALTERNATING PATH
        if(sink>0)
            %Increase the primal solution.
            assignedBool(k)=true;
            numAssignedU=numAssignedU+1;
            unassignedU(kIdx)=unassignedU(numUnassignedU);
            numUnassignedU=numUnassignedU-1;

            j=sink;
            while(1)
                i=pred(j);
                row4col(j)=i;
                h=col4row(i);
                col4row(i)=j;
                j=h;
                if(i==k)
                    break;
                end
            end
        else
            %Update the dual solution.
            deltaVal=Inf;
            for curU=1:numScannedU
                i=UScanned(curU);
                for curV=1:numUnlabeled
                    j=VUnlabeled(curV);
                    cost=C(i,j)-u(i)-v(j);

                    if(cost<deltaVal)
                        deltaVal=cost;
                    end
                end
            end

            %If the problem is infeasible.
            if(~isfinite(deltaVal))
                col4row=[];
                row4col=[];
                gain=-1;
                return;
            end

            for curU=1:numScannedU
                i=UScanned(curU);
                u(i)=u(i)+deltaVal;
            end

            for curV=1:numLabeled
                j=VLabeled(curV);
                v(j)=v(j)-deltaVal;
            end
        end
    end
end

if(nargout>2)
    gain=0;
    for curRow=1:numU
        gain=gain+C(curRow,col4row(curRow));
    end

    %Adjust the gain for the initial offset of the cost matrix.
    if(maximize)
        gain=-gain+CDelta*numU;
    else
        gain=gain+CDelta*numU;
    end
end

if(didTranspose)
    temp=col4row;
    col4row=row4col;
    row4col=temp;
end
end

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


function diff = hist_dist( hist1, hist2, method )
    switch method
        case 'x2'
            % diff = 0.5 * sum((hist1 - hist2).^2) ./ sum(hist1 + hist2 + eps);
            diff = 0.5 * sum( (hist1 - hist2).^2 ./ (hist1 + hist2 + eps) );
        case 'jsd'      % Jensen-Shannon Divergence
            diff = 0.5*(sum(hist1.*log((hist1+eps)./(hist2+eps))) + sum(hist2.*log((hist2+eps)./(hist1+eps))));
        otherwise
            error( 'unknown type for computing histogram distance' );
    end
end


function [outputCONTOUR,varargout] = TRACE_MooreNeighbourhood(data2D,varargin)
% TRACE_MooreNeighbourhood  2D boundary tracing using the Moore neighbourhood
%==========================================================================
% FILENAME:     TRACE_MooreNeighbourhood.m
% AUTHOR:       Adam H. Aitkenhead
% DATE:         12th April 2010
% PURPOSE:      2D boundary tracing using the Moore neighbourhood
%
% USAGE:        [listCONTOUR,listNORMALS] = TRACE_MooreNeighbourhood(data2D,pixelFIRST)
%%
% INPUT PARAMETERS:  data2D      - Mandatory.  An LxM array containing the
%                                  2D data.  All pixels within the region
%                                  to be traced must equal 1, and all
%                                  pixels outside the region must equal 0.
%                    pixelFIRST  - Optional.  A (2x1) array containing the
%                                  [x;y] coordinates of any one pixel
%                                  located on the edge of the region to be
%                                  traced.
%
% OUTPUT PARAMETERS: listCONTOUR - Mandatory.  An Nx2 array containing the
%                                  coordinates of the pixels located around
%                                  the edge of the region to be traced.
%                                  The pixels are listed in a clockwise
%                                  direction around the contour.
%                    listNORMALS - Optional.  An Nx2 array containing the
%                                  (approximate) normal vectors pointing
%                                  out of the region of interest.  The
%                                  vectors are listed in a clockwise
%                                  direction around the contour, and
%                                  correspond to each pixel listed in
%                                  listCONTOUR.
%
% EXAMPLE:      data2D = [0,0,0,0,0,0; 0,1,0,1,0,0; 0,1,1,1,1,0; 0,1,1,1,0,0; 0,1,1,1,1,0; 0,0,0,0,0,0];
%               [listCONTOUR,listNORMALS] = TRACE_MooreNeighbourhood(data2D);
%               imagesc(data2D)
%               colormap('gray')
%               hold on
%                 plot(listCONTOUR(:,2),listCONTOUR(:,1),'ro-','LineWidth',2)
%                 plot([listCONTOUR(:,2),listCONTOUR(:,2)+listNORMALS(:,2)]',[listCONTOUR(:,1),listCONTOUR(:,1)+listNORMALS(:,1)]','b','LineWidth',2)
%               hold off
%
% NOTES:      - The algorithm ends when the *second* pixel in the loop is
%               revisited, entered from the same direction as it was
%               entered on its first visit.  The second pixel is used
%               rather than the first pixel for two reasons:  1. The
%               first pixel may be encountered more than once while going
%               around the contour;  2. It is possible that the first pixel
%               will never be entered from the same direction as it was on
%               its first visit.
%             - The code does not identify any internal holes which exist
%               in the region to be traced.
%             - If more than one separate region exists in the image, the
%               code only traces one of these regions.  The user can define
%               which region by choosing an appropriate starting pixel
%               <pixelFIRST>.  Alternatively, the first region encountered
%               by the algorithm will be traced.
%
% REFERENCES: - For a guide to Contour Tracing, including a description of
%               Moore-Neighbour Tracing, see the tutorial by Abeer George
%               Ghuneim at:
%                http://www.imageprocessingplace.com/downloads_V3/
%                  root_downloads/tutorials/
%                  contour_tracing_Abeer_George_Ghuneim/index.html
%==========================================================================

%==========================================================================
% VERSION:  USER:  CHANGES:
% -------   -----  --------
% 090512    AHA    Original version
% 091021    AHA    Improved the calculation of the normals
% 100412    AHA    General housekeeping
%==========================================================================

%================================================
%  READ INPUT PARAMETERS
%================================================

% Configure the array which contains the region to be contour-traced:
if nargin<1
  error(' ERROR:  A 2D array is required as input.');
end

% Define pixelFIRST, the start coordinate (x,y) for the Moore-neighbour tracing algorithm.  This must be an edge pixel on the region to be contour-traced.
if nargin==2
  pixelFIRST = varargin{1};
else %If pixelFIRST hasn't been defined by the user, define it now:
  [tempR,tempC] = find(data2D==1);
  pixelFIRST = [tempR(1);tempC(1)];
end

%Make sure pixelFIRST is a column vector, rather than a row vector
if size(pixelFIRST,2)==2
  pixelFIRST = pixelFIRST';
end

%Makes sure data2D is a binary image
if numel(unique(data2D))~=2  ||  min(data2D(:))~=0  ||  max(data2D(:))~=1
  error(' Input image must be an array composed only of 0s and 1s.')
end

% Add a one-pixel border around the array before doing the analysis.
% This prevents the code from failing if the region to be traced extends to the edge of the array.
data2Doriginal = data2D;
data2D         = [zeros(1,size(data2Doriginal,2)+2);zeros(size(data2Doriginal,1),1),data2Doriginal,zeros(size(data2Doriginal,1),1);zeros(1,size(data2Doriginal,2)+2)];
pixelFIRST     = pixelFIRST + 1;

%================================================
%  INITIAL DEFINITIONS
%================================================

%Define the initial direction of entry (x,y) into the first pixel.  This
%direction must point from the outside to the inside.
tempI = find(data2D(pixelFIRST(1)-1:pixelFIRST(1)+1,pixelFIRST(2))==0);
if isempty(tempI)==0
  directionFIRST = [2-tempI(1);0];
else
  tempI = find(data2D(pixelFIRST(1),pixelFIRST(2)-1:pixelFIRST(2)+1)==0);
  if isempty(tempI)==0
    directionFIRST = [0;2-tempI(1)];
  else
    imagesc(data2D)
    save('temp.txt','data2D','-ascii','-tabs')
    error(' ERROR:  Unable to identify a vector pointing into the first pixel on the contour.');
  end
end

%Create an array for the output.  It will hold the (x,y) coordinates of
%every pixel around the border of the region to be contour-traced.  The
%directions are also recorded, and will be used as part of the test
%criterion to end the algorithm:
listCONTOUR   = pixelFIRST;
listDIRECTION = directionFIRST;

%Create a variable which will be used to tell the algorithm when to stop:
checkLOOPEND = 0;

%================================================
%  PERFORM THE MOORE NEIGHBOURHOOD TRACING
%================================================

ROTangle     = -pi/2;   %radians
ROTmatrix90  = [ cos(ROTangle) , -sin(ROTangle) ; sin(ROTangle) , cos(ROTangle) ];
ROTangle     = -pi;   %radians
ROTmatrix180 = [ cos(ROTangle) , -sin(ROTangle) ; sin(ROTangle) , cos(ROTangle) ];

tempPIX = pixelFIRST;
tempDIR = directionFIRST;

while checkLOOPEND==0

  tempDIR = round(ROTmatrix180*tempDIR);
  tempPIX = tempPIX + tempDIR;
  if data2D(tempPIX(1),tempPIX(2)) == 1
    listCONTOUR   = [listCONTOUR,tempPIX];
    listDIRECTION = [listDIRECTION,tempDIR];
  else
    tempDIR = round(ROTmatrix90*tempDIR);
    tempPIX = tempPIX + tempDIR;
    if data2D(tempPIX(1),tempPIX(2)) == 1
      listCONTOUR   = [listCONTOUR,tempPIX];
      listDIRECTION = [listDIRECTION,tempDIR];
    else
      tempDIR = round(ROTmatrix90*tempDIR);
      tempPIX = tempPIX + tempDIR;
      if data2D(tempPIX(1),tempPIX(2)) == 1
        listCONTOUR   = [listCONTOUR,tempPIX];
        listDIRECTION = [listDIRECTION,tempDIR];
      else
        tempPIX = tempPIX + tempDIR;
        if data2D(tempPIX(1),tempPIX(2)) == 1
          listCONTOUR   = [listCONTOUR,tempPIX];
          listDIRECTION = [listDIRECTION,tempDIR];
        else
          tempDIR = round(ROTmatrix90*tempDIR);
          tempPIX = tempPIX + tempDIR;
          if data2D(tempPIX(1),tempPIX(2)) == 1
            listCONTOUR   = [listCONTOUR,tempPIX];
            listDIRECTION = [listDIRECTION,tempDIR];
          else
            tempPIX = tempPIX + tempDIR;
            if data2D(tempPIX(1),tempPIX(2)) == 1
              listCONTOUR   = [listCONTOUR,tempPIX];
              listDIRECTION = [listDIRECTION,tempDIR];
            else
              tempDIR = round(ROTmatrix90*tempDIR);
              tempPIX = tempPIX + tempDIR;
              if data2D(tempPIX(1),tempPIX(2)) == 1
                listCONTOUR   = [listCONTOUR,tempPIX];
                listDIRECTION = [listDIRECTION,tempDIR];
              else
                tempPIX = tempPIX + tempDIR;
                if data2D(tempPIX(1),tempPIX(2)) == 1
                  listCONTOUR   = [listCONTOUR,tempPIX];
                  listDIRECTION = [listDIRECTION,tempDIR];
                else
                  tempDIR = round(ROTmatrix90*tempDIR);
                  tempPIX = tempPIX + tempDIR;
                  if data2D(tempPIX(1),tempPIX(2)) == 1
                    listCONTOUR   = [listCONTOUR,tempPIX];
                    listDIRECTION = [listDIRECTION,tempDIR];
                  else
                    tempDIR = round(ROTmatrix90*tempDIR);
                    tempPIX = tempPIX + tempDIR;
                    if data2D(tempPIX(1),tempPIX(2)) == 1
                      listCONTOUR   = [listCONTOUR,tempPIX];
                      listDIRECTION = [listDIRECTION,tempDIR];
                    end %if

                  end %if
                end %if
              end %if
            end %if
          end %if
        end %if
      end %if
    end %if
  end %if

  if size(listCONTOUR,2)>2  &&  min(isequal(tempPIX,listCONTOUR(:,2)))  &&  min(isequal(tempDIR,listDIRECTION(:,2)))
    %The stop criterion has been met, so tell the while-loop to exit:
    checkLOOPEND=1;
    %Remove the extra col from the results corresponding to the second
    %visit to the second pixel.
    listCONTOUR         = listCONTOUR(:,1:end-1);
    listDIRECTION       = listDIRECTION(:,1:end-1);
    listENTRYDIRECTIONS = -listDIRECTION;
    %Make sure the normal vector for the first pixel is the same on its
    %first visit as on its last vist:
    listENTRYDIRECTIONS(:,1) = listENTRYDIRECTIONS(:,end);
  end %if

end %while

% Adjust the results to remove the effect of the one-pixel border that was
% added around the array before the contour was traced:
listCONTOUR = listCONTOUR - 1;

%================================================
%  COMPUTE THE NORMALS
%  A combination of two methods is used to compute the normals:
%  -1-  The gradient of the two surrounding contour points is found, and
%       then rotated 90 degrees to get the gradient at the point of
%       interest.
%  -2-  The midpoint of the two surrounding points is found, and the vector
%       from this point to the point of interest is found.
%  Method 1 handles concave edges and diagonal edges better, but gets
%  confused in places where the region of interest ends in a one-pixel-wide
%  line, since at that point the two surrounding points are coincident.
%================================================

% Remove last point, which is a duplicate of the first point.
listCONTOURtemp = listCONTOUR(:,1:end-1);

% Method 1:  Gradient:
listGRADIENTS = (circshift(listCONTOURtemp,[0,-1])-circshift(listCONTOURtemp,[0,1]))/2;  % Calculate the gradient of the two surrounding contour-points.
ROTangle      = pi/2;   %radians                                                         % Rotate the gradient by +90 degree to produce the normals.
ROTmatrix     = [ cos(ROTangle) , -sin(ROTangle) ; sin(ROTangle) , cos(ROTangle) ];
listNORMALS_G = ROTmatrix * listGRADIENTS;

% Method 2:  Midpoint:
listMIDPOINT  = (circshift(listCONTOURtemp,[0,-1])+circshift(listCONTOURtemp,[0,1]))/2;  %Find the midpoint of the two surrounding contour-points.
listNORMALS_M = (listCONTOURtemp - listMIDPOINT)./2;                                     % Calculate the vector to the point of interest

% Combine results from the two methods:
method1sign = sign(round(10*listNORMALS_G));
method2sign = sign(round(10*listNORMALS_M));
method1sign(method1sign==0) = method2sign(method1sign==0);
listNORMALS_M = method1sign.*abs(listNORMALS_M);
listNORMALS        = listNORMALS_M;
listNORMALS(:,:,2) = listNORMALS_G;
listNORMALS = mean(listNORMALS,3);

% For cases where the ROI is larger than a single pixel, ensure the normals are units vectors
if size(listNORMALS,2)>1
  normalLENGTHS = sqrt(sum(listNORMALS.^2,1));
  listNORMALS   = listNORMALS./([1;1]*normalLENGTHS);
else
  listNORMALS = [-1;0];
end

% Replace last point, which is a duplicate of the first point.
listNORMALS   = [listNORMALS,listNORMALS(:,1)];

%================================================
%  DEFINE THE OUTPUT ARGUMENTS
%================================================

outputCONTOUR = listCONTOUR';
listNORMALS   = listNORMALS';

if nargout == 2
  varargout(1) = {listNORMALS};
end


function fem1d_spectral_numeric_test ( )

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_SPECTRAL_NUMERIC_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the FEM1D_SPECTRAL_NUMERIC library.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Run FEM1D_SPECTRAL_NUMERIC with an increasing number of basis functions.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N        L2-error        H1-error\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 2 : 11
    [ l2, h1 ] = fem1d_spectral_numeric ( n );
    fprintf ( 1, '  %2d  %14.6g  %14.6g\n', n, l2, h1 );
  end

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

  return
end


function square_symq_rule_test01 ( degree, n )

%*****************************************************************************80
%
%% SQUARE_SYMQ_RULE_TEST01 calls SQUARESYMQ for a quadrature rule of given order.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.
%
%    Input, integer N, the number of nodes.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_SYMQ_RULE_TEST01\n' );
  fprintf ( 1, '  Symmetric quadrature rule for a square.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );

  area = 4.0;
%
%  Retrieve and print a symmetric quadrature rule.
%
  [ x, w ] = square_symq ( degree, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes N = %d\n', n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     J  W       X       Y\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %4d  %14.6g  %14.6g  %14.6g\n', j, w(j), x(1,j), x(2,j) );
  end

  d = sum ( w(1:n) );

  fprintf ( 1, '   Sum  %14.6g\n', d );
  fprintf ( 1, '  Area  %14.6g\n', area );

  return
end


function post_3d ( p, t )

%*****************************************************************************80
%
%% POST_3D is a postprocessor for the tetrahedronal mesh.
%
%  Copyright:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    31 July 2009
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, P(N,3), the coordinates of nodes.
%
%    Input, integer T(NT,4), the indices of nodes that make up the tetrahedrons.
%
  [ node_num, junk ] = size ( p );
  [ tetra_num, junk ] = size ( t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  %d nodes were used.\n', node_num );
  fprintf ( 1, '  %d tetrahedrons were created.\n', tetra_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  QUALITY MEASURES:\n');
  fprintf ( 1, '  A value of 1.00 is best, and 0.00 is the worst.\n' );

  q = simp_qual_3d ( p, t, 1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Measure Min         Mean        Max         Var\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  #1  %10f  %10f  %10f  %10f\n', ...
    min ( q ), mean ( q ), max ( q ), var ( q ) );

  bad = find ( q == min ( q ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The tetrahedron of minimum quality is number %d\n', bad(1) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node        X             Y             Z\n' );
  fprintf ( 1, '\n' );

  for j = 1 : 4
    n = t(bad(1),j);
    fprintf ( 1, '  %4d  %12f  %12f  %12f\n',  n, p(n,1:3) );
  end

  return
end


% Copyright (C) 1999 Paul Kienzle <pkienzle@users.sf.net>
% Copyright (C) 2006 Peter V. Lanspeary, <peter.lanspeary@.adelaide.edu.au>
%
% 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/>.

% -*- texinfo -*-
% @deftypefn  {Function File} {[@var{a}, @var{v}, @var{ref}] =} levinson (@var{acf})
% @deftypefnx {Function File} {[@dots{}] =} levinson (@var{acf}, @var{p})
%
% Use the Durbin-Levinson algorithm to solve:
%    toeplitz(acf(1:p)) * x = -acf(2:p+1).
% The solution [1, x'] is the denominator of an all pole filter
% approximation to the signal x which generated the autocorrelation
% function acf.
%
% acf is the autocorrelation function for lags 0 to p.
% p defaults to length(acf)-1.
% Returns
%   a=[1, x'] the denominator filter coefficients.
%   v= variance of the white noise = square of the numerator constant
%   ref = reflection coefficients = coefficients of the lattice
%         implementation of the filter
% Use freqz(sqrt(v),a) to plot the power spectrum.
%
% REFERENCE
% [1] Steven M. Kay and Stanley Lawrence Marple Jr.:
%   "Spectrum analysis -- a modern perspective",
%   Proceedings of the IEEE, Vol 69, pp 1380-1419, Nov., 1981
% @end deftypefn

% Based on:
%    yulewalker.m
%    Copyright (C) 1995 Friedrich Leisch <Friedrich.Leisch@ci.tuwien.ac.at>
%    GPL license

% FIXME: Matlab doesn't return reflection coefficients and
%        errors in addition to the polynomial a.
% FIXME: What is the difference between aryule, levinson,
%        ac2poly, ac2ar, lpc, etc.?

function [a, v, ref] = oc_levinson (acf, p)

if ( nargin<1 )
    print_usage;
elseif( ~isvector(acf) || length(acf)<2 )
    error( 'levinson: arg 1 (acf) must be vector of length >1\n');
elseif ( nargin>1 && ( ~isscalar(p) || fix(p)~=p ) )
    error( 'levinson: arg 2 (p) must be integer >0\n');
else
    if ((nargin == 1)||(p>=length(acf)))
        p = length(acf) - 1;
    end
    if( size(acf,2)>1 )
        acf=acf(:);
    end      % force a column vector

    if nargout < 3 && p < 100
        % direct solution [O(p^3), but no loops so slightly faster for small p]
        %   Kay & Marple Eqn (2.39)
        R = toeplitz(acf(1:p), conj(acf(1:p)));
        a = R \ -acf(2:p+1);
        a = [ 1, a.' ];
        v = real( a*conj(acf(1:p+1)) );
    else
        % durbin-levinson [O(p^2), so significantly faster for large p]
        %   Kay & Marple Eqns (2.42-2.46)
        ref = zeros(p,1);
        g = -acf(2)/acf(1);
        a = g;
        v = real( ( 1 - g*conj(g)) * acf(1) );
        ref(1) = g;
        for t = 2 : p
            g = -(acf(t+1) + a * acf(t:-1:2)) / v;
            a = [ a+g*conj(a(t-1:-1:1)), g ];
            v = v * ( 1 - real(g*conj(g)) ) ;
            ref(t) = g;
        end
        a = [1, a];
    end
end

end


function x = pvand ( n, alpha, b )

%*****************************************************************************80
%
%% PVAND solves a Vandermonde system A * x = b.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 November 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ake Bjorck, Victor Pereyra,
%    Solution of Vandermonde Systems of Equations,
%    Mathematics of Computation,
%    Volume 24, Number 112, October 1970, pages 893-903.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA(N), the parameters that define the matrix.
%    The values should be distinct.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  x(1:n) = b(1:n);

  for k = 1 : n - 1
    for j = n : -1 : k + 1
      x(j) = x(j) - alpha(k) * x(j-1);
    end
  end

  for k = n - 1 : -1 : 1
    for j = k + 1 : n
      x(j) = x(j) / ( alpha(j) - alpha(j-k) );
    end
    for j = k : n - 1
      x(j) = x(j) - x(j+1);
    end
  end

  return
end


function [ r, seed ] = r4_uniform_01 ( seed )

%*****************************************************************************80
%
%% R4_UNIFORM_01 returns a unit pseudorandom R4.
%
%  Discussion:
%
%    This routine implements the recursion
%
%      seed = 16807 * seed mod ( 2**31 - 1 )
%      r = seed / ( 2**31 - 1 )
%
%    The integer arithmetic never requires more than 32 bits,
%    including a sign bit.
%
%    If the initial seed is 12345, then the first three computations are
%
%      Input     Output      R4_UNIFORM_01
%      SEED      SEED
%
%         12345   207482415  0.096616
%     207482415  1790989824  0.833995
%    1790989824  2035175616  0.947702
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley Interscience, page 95, 1998.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer SEED, the integer "seed" used to generate
%    the output random number.  SEED should not be 0.
%
%    Output, real R, a random value between 0 and 1.
%
%    Output, integer SEED, the updated seed.  This would
%    normally be used as the input seed on the next call.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R4_UNIFORM_01 - Fatal error!' );
  end

  seed = floor ( seed );

  seed = mod ( seed, 2147483647 );

  if ( seed < 0 )
    seed = seed + 2147483647;
  end

  k = floor ( seed / 127773 );

  seed = 16807 * ( seed - k * 127773 ) - k * 2836;

  if ( seed < 0 )
    seed = seed + 2147483647;
  end

  r = seed * 4.656612875E-10;

  return
end


function a = conex1_inverse ( alpha )

%*****************************************************************************80
%
%% CONEX1_INVERSE returns the inverse of the CONEX1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar defining A.
%    A common value is 100.0.
%
%    Output, real A(4,4), the matrix.
%
  a(1,1) =  1.0;
  a(1,2) =  1.0 - alpha;
  a(1,3) =        alpha;
  a(1,4) =  2.0;

  a(2,1) =  0.0;
  a(2,2) =  1.0 + alpha;
  a(2,3) =      - alpha;
  a(2,4) =  0.0;

  a(3,1) =  0.0;
  a(3,2) = -1.0;
  a(3,3) =  1.0;
  a(3,4) =  1.0 / alpha;

  a(4,1) = 0.0;
  a(4,2) = 0.0;
  a(4,3) = 0.0;
  a(4,4) = 1.0 / alpha;

  return
end


% demo for mixed-effects analysis (2-levels hierachical model)
% This script simulates and analyses data under two different hierarchical
% models, namely (i) H0: group-mean=0, and (ii) H1: group-mean~=0.
% The effect of H1 and H0 onto both the group-mean parameters estimates
% (given H1) and the log-Bayes factors (comparisons of H1 vs H0) is
% assessed using Monte-Carlo simulations.
% The trick for performing a mixed-effects analysis using the structure of
% the generative model of VBA is to use hidden states, where "initial
% conditions" (x_0) serve as the group mean, x_1 are subject-dependant
% effects, and the precision (alpha) of the transition density
% p(x_1|x_0,alpha) captures the between-subject variance. This trick
% however, can be used here because we deal with a static model (a
% within-subject GLM).

clear variables
close all

% Choose basic settings for simulations
ns = 32; % number of subjects
n = 256; % number of observations per subject
f_fname = @g_RFX;
g_fname = @g_RFX;
alpha = 1e1;
sigma = 1e0;
theta = [];
phi = [];
x0 = zeros(ns,1);

X2 = [ones(ns,1),zeros(ns,ns-1)]; % 2d-level design matrix
u = [];

% Build options structure for temporal integration of SDE
inF.ns = ns;
inF.X = X2;
options.inF = inF;

% Build priors for model inversion
priors.muX0 = zeros(ns,1);
priors.SigmaX0 = 0*eye(ns);
priors.a_alpha = 1e0;
priors.b_alpha = 1e0;
priors.a_sigma = 1e0;
priors.b_sigma = 1e0;

% Build options and dim stuctures for model inversion
options.priors = priors;
options.DisplayWin = 0;
dim.n_theta = 0;
dim.n_phi = 0;
dim.n = ns;

Nmcmc = 32;
p = cell(2,2,Nmcmc);
o = cell(2,2,Nmcmc);
F = zeros(2,2,Nmcmc);
X0 = zeros(2,Nmcmc);
for ii=1:Nmcmc
    % simulate data with and without 2nd-level effect
    X1 = randn(n,ns); % 1st-level design matrix
    options.inG.X = X1;
    for i=1:2
        x0(1) = 2-i;
        [y,x,x0,eta,e] = VBA_simulate (1,f_fname,g_fname,theta,phi,u,alpha,sigma,options,x0);
        % Invert model with and without 2nd-level effect
        for j=1:2
            options.priors.SigmaX0(1,1) = 2-j; % group mean effect
            [p{i,j,ii},o{i,j,ii}] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);
            F(i,j,ii) = o{i,j,ii}.F;
            if j==1
                % extract estimaetd group-mean
                X0(i,ii) = p{i,j,ii}.muX0(1);
            end
        end
    end
end

hf = figure('color',[1 1 1]);
pos = get(hf,'position');
set(hf,'position',pos.*[1 1 1.5 1]);
models = {'H1','H0'};

ha = subplot(1,2,1,'parent',hf);
mx0 = mean(X0,2);
vx0 = var(X0,[],2)./Nmcmc;
plotUncertainTimeSeries(mx0,vx0,[],ha);
set(ha,'xlim',[0,3],'xtick',[1,2],'xticklabels',models)
xlabel(ha,'type of simulated data')
ylabel(ha,['E[X0|y,H1]'])
title(ha,'estimated group effect (under H1)')
box(ha,'off')

ha = subplot(1,2,2,'parent',hf);
dF = F(:,1,:) - F(:,2,:);
mdF = mean(dF,3);
vdF = var(dF,[],3)./Nmcmc;
plotUncertainTimeSeries(mdF,vdF,[],ha);
set(ha,'xlim',[0,3],'xtick',[1,2],'xticklabels',models)
xlabel(ha,'type of simulated data')
ylabel(ha,['log p(y|',models{1},') - log p(y|',models{2},')'])
title(ha,'evidence for a group effect')
box(ha,'off')

VBA_getSubplots ();


function [o,count,SSQ,S4M] = sumskipnan(i,DIM)
% SUMSKIPNAN adds all non-NaN values.
%
% All NaN's are skipped; NaN's are considered as missing values.
% SUMSKIPNAN of NaN's only  gives O; and the number of valid elements is return.
% SUMSKIPNAN is also the elementary function for calculating
% various statistics (e.g. MEAN, STD, VAR, RMS, MEANSQ, SKEWNESS,
% KURTOSIS, MOMENT, STATISTIC etc.) from data with missing values.
% SUMSKIPNAN implements the DIMENSION-argument for data with missing values.
% Also the second output argument return the number of valid elements (not NaNs)
%
% Y = sumskipnan(x [,DIM])
% [Y,N,SSQ] = sumskipnan(x [,DIM])
%
% DIM	dimension
%	1 sum of columns
%	2 sum of rows
%	default or []: first DIMENSION with more than 1 element
%
% Y	resulting sum
% N	number of valid (not missing) elements
% SSQ	sum of squares
%
% The mean & standard error of the mean and
%	Y./N & sqrt((SSQ-Y.*Y./N)./(N.*max(N-1,0)));
% the mean square & the standard error of the mean square and
% 	SSQ./N & sqrt((S4M-SSQ.^2./N)./(N.*max(N-1,0)))
%
% features:
% - can deal with NaN's (missing values)
% - implements dimension argument.
% - compatible with Matlab and Octave
%
% see also: SUM, NANSUM, MEAN, STD, VAR, RMS, MEANSQ,
%      SSQ, MOMENT, SKEWNESS, KURTOSIS, SEM

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

%	$Revision: 1.23 $
%	$Id: sumskipnan.m,v 1.23 2003/10/31 18:15:38 schloegl Exp $
%    Copyright (C) 2000-2003 by Alois Schloegl <a.schloegl@ieee.org>

if nargin<2,
        DIM = [];
end;

% an efficient implementation in C of the following lines
% could significantly increase performance
% only one loop and only one check for isnan is needed.
% A MEX-Implementation is available in sumskipnan.cpp.
%
% Outline of the algorithm:
% for { k=1,o=0,count=0; k++; k<N}
% 	if ~isnan(i(k))
% 	{ 	o     += i(k);
%               count += 1;
%		tmp    = i(k)*i(k)
%		o2    += tmp;
%		o3    += tmp.*tmp;
%       };

if isempty(DIM),
        DIM=min(find(size(i)>1));
        if isempty(DIM), DIM = 1; end;
end;
if nargout>1,
        count = sum(~isnan(i),DIM);
end;

%if flag_implicit_skip_nan, %%% skip always NaN's
i(isnan(i)) = 0;
%end;
o = sum(i,DIM);
if nargout>2,
        i = real(i).^2 + imag(i).^2;
        SSQ = sum(i,DIM);
        if nargout>3,
                S4M = sum(i.^2,DIM);
        end;
end;


%% 2D Riemann Problem: Dimensional Splitting with Front Tracking
% In this example, we consider the 2D scalar conservation law
%
% $$ u_t + (u^2)_x/2 + (u^2)_y/2 = 0$$
%
% with Riemann initial data, that is, with an intial function that has a
% constant value in each of the four quadrants and discontinuities along
% the x- and y-axes.
%
% To solve the one-dimensional problems arising as part of the operator
% splitting, we will use front tracking followed by a projection step onto
% underlying regular grid.

%% Initial setup
N = 100;
h = 2/N;
x = -1:h:1;
y = 0.5*(x(1:end-1)+x(2:end));
T = 1.0;
delta = 0.1/sqrt(N);
[X,Y]=meshgrid(y,y);

%% Exact solutions
% We consider two different setups. In both setups u0=1/4 in the second and
% fourth quadrant. Setting u0=1/2 in the first quadrant and u0=-1/2 in the
% third quadrant gives a self-similar wave pattern where the four original
% constant states are separated by two pairs of rarefaction waves. For each
% pair, the two rarefaction waves meet in a sharp kink along the line y =
% x. Switching the values in the first and third quadrant gives a wave
% pattern that consists of the four constant states separated by six shocks
% forming two triple points at (3t/8,−t/8) and (−t/8, 3t/8).
subplot(2,2,1), pcolor(X,Y,rpsola(X,Y,0)), shading flat; colorbar, title('Problem A')
subplot(2,2,2), pcolor(X,Y,rpsolb(X,Y,0)), shading flat; colorbar, title('Problem B')
subplot(2,2,3), pcolor(X,Y,rpsola(X,Y,1)), shading flat; colorbar
subplot(2,2,4), pcolor(X,Y,rpsolb(X,Y,1)), shading flat; colorbar

%% Riemann problem B: shocks
% First, we study the error for the second Riemann problem as a function of
% the number of operator splitting steps
u0     = rpsolb(X,Y,0);
utrue  = rpsolb(X,Y,1);
rel    = sum(sum(abs(utrue)));
for i=1:4,
  Nstep = 4^(i-1);
  u     = DimSplit(u0,x,x,'burgerRsol','burgerRsol',delta,Nstep,T,'wbar');
  feil  = sum(sum(abs(utrue-u)))/rel;

  subplot(2,2,i);
  pcolor(y,y,u), axis equal image;  colorbar, shading flat
  title(sprintf('Grid: %dx%d  Steps: %d', N, N, Nstep));
  xlabel(sprintf('L1 error: %4.2f %%', 100*feil));
  p=get(gca,'position'); p([3 4])=p([3 4])+0.03;
  set(gca,'position',p,'XTickLabel',[]);
end;
%%
% From the figure, we see that the error is a convex function of the number
% of splitting steps. With few splitting steps, the error is dominated by
% the splitting error and the strong self-sharpening effects present near
% the shocks will counteract the smearing introduced by the projection
% operator that brings the front tracking solution back from a possibly
% irregular grid and back to the underlying Cartesian grid. As the number
% of time steps increases, the number of projections increases and the
% self-sharpening effects of diminish so that the smearing errors become
% dominant. Because the two error mechanisms work in different directions,
% a minimum error is observed for an intermediate number of splitting
% steps.

%% Convergence test
% We perform a grid-refinement study for the two Riemann problems, fixing
% the CFL number to 10 and report the error as a logarithmic plot
nu=10;
errA=ones(1,6); errB=errA;
for i=1:6,
  N     = 8*2^i;
  h     = 2/N;
  x     = -1:h:1;
  y     = 0.5*(x(1:end-1)+x(2:end));
  delta = 0.1/sqrt(N);
  [X,Y] = meshgrid(y,y);

  % Riemann problem A
  u       = DimSplit(rpsola(X,Y,0), x,x,'burgerRsol','burgerRsol', ...
                     delta, round(T/(nu*h)), T,'wbar');
  utrue   = rpsola(X,Y,1);
  rel     = sum(sum(abs(utrue)));
  errA(i) = sum(sum(abs(utrue-u)))/rel;

  % Riemann problem B
  u       = DimSplit(rpsolb(X,Y,0), x,x,'burgerRsol','burgerRsol', ...
                     delta, round(T/(nu*h)), T,'wbar');
  utrue   = rpsolb(X,Y,1);
  rel     = sum(sum(abs(utrue)));
  errB(i) = sum(sum(abs(utrue-u)))/rel;
end
clf, semilogy(4:9,errA,'-o',4:9,errB,'x-')
xlabel('log2 of grid size'); ylabel('Relative L1 error');
legend('Problem A','Problem B');
%%
% As pointed out above, the strong self-sharpening effects present near the
% shocks in Riemann problem B will counteract the smearing introduced by
% the projection operator leading to a convergence rate of approximately
% one. For the smooth solution resulting from Riemann problem A, the
% self-sharpening effects are much weaker, thereby giving a lower
% convergence rate.


function cdf_discrete_test ( )

%*****************************************************************************80
%
%% CDF_DISCRETE_TEST tests CDF_DISCRETE_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CDF_DISCRETE_TEST\n' );
  fprintf ( 1, '  CDF_DISCRETE_VALUE evaluates the CDF associated with a\n' );
  fprintf ( 1, '  discrete histogram.\n' );
%
%  Set up the discrete histogram from sample data.
%
  s_num = 5;
  s_min = 0.0;
  s_max = 10.0;
  s = [ 2.0, 2.0, 4.0, 5.0, 9.0 ];
  fprintf ( 1, '\n' );
  fprintf ( 1, '  S_MIN = %g\n', s_min );
  fprintf ( 1, '  S_MAX = %g\n', s_max );
  r8vec_print ( s_num, s, '  Sample data:' );

  [ x_num, x, y ] = setup_discrete ( s_num, s, s_min, s_max );

  r8vec2_print ( x_num, x, y, '  Discrete histogram data:' );
%
%  Evaluate the discrete CDF.
%
  v_num = 21;
  v = linspace ( s_min, s_max, v_num );
  cdf = cdf_discrete_value ( x_num, x, y, v_num, v );
  r8vec2_print ( v_num, v, cdf, '  Discrete CDF table:' );
%
%  Plot the discrete CDF.
%
  plot ( v, cdf, 'bo-', 'Linewidth', 3 )
  grid on
  xlabel ( '<--- X --->' )
  ylabel ( '<--- CDF(X) --->' )
  title ( 'Discrete CDF function' )

  filename = 'cdf_discrete_test.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Discrete CDF plotted as "%s".\n', filename );

  return
end


function [dv, deltaV1, deltaV2, deltaV1R, deltaV2R, xfrOrbit, deltaV1NTW, deltaV2NTW] = twoBurnOrbitChangeObjFunc(x, iniOrbit, finOrbit, gmuXfr)
%twoBurnOrbitChangeObjFunc Summary of this function goes here
%   Detailed explanation goes here
    burn1TA = AngleZero2Pi(x(1));
    burn2TA = AngleZero2Pi(x(2));
    xfrArcTOF = x(3);

%     if(iniOrbit(2)<1E-5 && finOrbit(2)<1E-5)
%         if(finOrbit(2)<1E-5)
%             finOrbit(2) = finOrbit(2) + 1E-5;
%         else
%             iniOrbit(2) = iniOrbit(2) + 1E-5;
%         end
%     end

%     if(iniOrbit(3)<1E-3 && finOrbit(3)<1E-3)
%         finOrbit(3) = finOrbit(3) + 1E-3;
%     end

    if(length(iniOrbit) >= 8)
        gmuXfr1 = iniOrbit(8);
    else
        gmuXfr1 = gmuXfr;
    end

    if(length(finOrbit) >= 8)
        gmuXfr2 = finOrbit(8);
    else
        gmuXfr2 = gmuXfr;
    end

    [rVect1,vVect1Pre]=getStatefromKepler(iniOrbit(1), iniOrbit(2), iniOrbit(3), iniOrbit(4), iniOrbit(5), burn1TA, gmuXfr1);
    [rVect2,vVect2Post]=getStatefromKepler(finOrbit(1), finOrbit(2), finOrbit(3), finOrbit(4), finOrbit(5), burn2TA, gmuXfr2);

    xfrArcTOFDays = xfrArcTOF/(86400);
    [vVect1PostSW,vVect2PreSW]=orbit.lambert(rVect1', rVect2', xfrArcTOFDays, 0, gmuXfr);
    [vVect1PostLW,vVect2PreLW]=orbit.lambert(rVect1', rVect2', -xfrArcTOFDays, 0, gmuXfr);

    deltaV1SW = vVect1PostSW' - vVect1Pre;
    deltaV1LW = vVect1PostLW' - vVect1Pre;
    deltaV2SW = vVect2Post - vVect2PreSW';
    deltaV2LW = vVect2Post - vVect2PreLW';

    dvSW = norm(deltaV1SW) + norm(deltaV2SW);
    dvLW = norm(deltaV1LW) + norm(deltaV2LW);
    if(dvSW < dvLW)
        dv = dvSW;
        deltaV1 = deltaV1SW;
        deltaV2 = deltaV2SW;
        [sma, ecc, inc, longAscNode, ArgPeri, TA1] = getKeplerFromState(rVect1,vVect1PostSW,gmuXfr);
        [~, ~, ~, ~, ~, TA2] = getKeplerFromState(rVect2,vVect2PreSW,gmuXfr);
        xfrOrbit = [sma, ecc, inc, longAscNode, ArgPeri, TA1, TA2];
        if(ecc<1.0)
            xfrOrbit(6) = AngleZero2Pi(TA1);
            xfrOrbit(7) = AngleZero2Pi(TA2);
        end

        deltaV1NTW = getNTWdvVect(deltaV1, rVect1, vVect1Pre);
        deltaV2NTW = getNTWdvVect(deltaV2, rVect2, vVect2PreSW);
    else
        dv = dvLW;
        deltaV1 = deltaV1LW;
        deltaV2 = deltaV2LW;
        [sma, ecc, inc, longAscNode, ArgPeri, TA1] = getKeplerFromState(rVect1,vVect1PostLW,gmuXfr);
        [~, ~, ~, ~, ~, TA2] = getKeplerFromState(rVect2,vVect2PreLW,gmuXfr);
        xfrOrbit = [sma, ecc, inc, longAscNode, ArgPeri, TA1, TA2];
        if(ecc<1.0)
            xfrOrbit(6) = AngleZero2Pi(TA1);
            xfrOrbit(7) = AngleZero2Pi(TA2);
        end

        deltaV1NTW = getNTWdvVect(deltaV1, rVect1, vVect1Pre);
        deltaV2NTW = getNTWdvVect(deltaV2, rVect2, vVect2PreLW);
    end
    deltaV1R = rVect1;
    deltaV2R = rVect2;
end


function [t1, t2, alpha, TransMat, Displ] = estimate_rigid_from_displ(T, mask)
    % t1 - translation in first dimension
    % t2 - translation in second dimension
    % alpha - angle (in radians). Rotation is performed around (0,0) point.
    %           keep in mind, that upper left pixel has index (1,1).
    % TransMat - transformation matrix for homogeneous coordinates
    % Displ - displacement field for estimated rigid transformation
    %
    % T - displacement field to be approximated
    %
    % Example usage
    % alpha = 0.2;
    % t1=5; t2=-9;
    % TT = displ_from_matrix_2d([cos(alpha), -sin(alpha), t1; sin(alpha), cos(alpha), t2; 0, 0, 1], [256,256], [], true);
    % [et1, et2, ealpha, eTransMat, eDispl] = estimate_rigid_from_displ(TT);
    % (1 - norm(fl(TT-Displ))/norm(fl(TT))) * 100 % -- percent explained by a rigid rotation
    if isempty(mask)
        mask = ones(size(T, 1), size(T, 2));
    end
    mask = logical(mask);

    sz = [size(T, 1), size(T, 2)];
    [n1, n2] = ndgrid(1:sz(1), 1:sz(2));
    T(:,:,1) = T(:,:,1) + n1;
    T(:,:,2) = T(:,:,2) + n2;
    ND = cat(3, n1, n2);

    T = reshape(T, [size(T,1)*size(T, 2), 2]);
    ND = reshape(ND, [size(ND,1)*size(ND, 2), 2]);
    T = T(mask, :)';
    ND = ND(mask, :)';
    T = [T; ones(1, size(T, 2))];
    ND = [ND; ones(1, size(ND, 2))];

    opts = [];
    opts.display = 'off';
    objf = @(x) fgval(x(1), x(2), x(3), T, ND);
%     fl = @(x) x(:);
%     x0 = [mean(fl(T(:,:,1))); mean(fl(T(:,:,2))); 0];
    x0 = [0;0;0];
    xmin = minFunc(objf, x0, opts);
    t1 = xmin(1);
    t2 = xmin(2);
    alpha = xmin(3) * pi / 180;
    TransMat = [cos(alpha), -sin(alpha), t1; ...
                sin(alpha), cos(alpha), t2; ...
                0, 0, 1];
    Displ = displ_from_matrix_2d(TransMat, sz, [], true);
end

function f = fval(t1, t2, alpha, T, ND)
    alpha = alpha * pi / 180; % just to keep t1,t2 and alpha in the same scale

%     pts = reshape(T, [size(T,1)*size(T,2), 2]);
%     pts = [pts, ones(size(pts,1), 1)]';
%     pts0 = reshape(ND, [size(T,1)*size(T,2), 2]);
%     pts0 = [pts0, ones(size(pts0,1), 1)]';

    ca = cos(alpha);
    sa = sin(alpha);
    R = [ca, -sa, t1; sa, ca, t2; 0, 0, 1];
%     ptsm = R * pts0;
%     f = sum(sqrt( sum( mask.*(pts - ptsm).^2, 1) ));
    ptsm = R * ND;
    f = sum(sqrt( sum((T - ptsm).^2, 1) ));
end

function [f, g] = fgval(t1, t2, alpha, T, ND)
    dx = 0.001;
    dx = 1e-5;

    f = fval(t1,t2,alpha,T, ND);
    fp1 = fval(t1 + dx, t2, alpha, T, ND);
    fp2 = fval(t1, t2 + dx, alpha, T, ND);
    fp3 = fval(t1, t2, alpha + dx, T, ND);
    g = [(fp1 - f) / dx; (fp2 - f) / dx; (fp3 - f) / dx];
end


function cdf = student_noncentral_cdf ( x, idf, d )

%*****************************************************************************80
%
%% STUDENT_NONCENTRAL_CDF evaluates the noncentral Student T CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Algorithm AS 5,
%    Applied Statistics,
%    Volume 17, 1968, page 193.
%
%  Parameters:
%
%    Input, real X, the argument of the CDF.
%
%    Input, integer IDF, the number of degrees of freedom.
%
%    Input, real D, the noncentrality parameter.
%
%    Output, real CDF, the value of the CDF.
%
  a_max = 100;
  emin = 12.5;

  f = idf;

  if ( idf == 1 )

    a = x / sqrt ( f );
    b = f / ( f + x * x );
    drb = d * sqrt ( b );

    cdf2 = normal_01_cdf ( drb );
    cdf = 1.0 - cdf2 + 2.0 * tfn ( drb, a );

  elseif ( idf <= a_max )

    a = x / sqrt ( f );
    b = f / ( f + x * x );
    drb = d * sqrt ( b );
    sum2 = 0.0;

    fmkm2 = 0.0;
    if ( abs ( drb ) < emin )
      cdf2 = normal_01_cdf ( a * drb );
      fmkm2 = a * sqrt ( b ) * exp ( -0.5 * drb * drb ) * cdf2 / sqrt ( 2.0 * pi );
    end

    fmkm1 = b * d * a * fmkm2;
    if ( abs ( d ) < emin )
      fmkm1 = fmkm1 + 0.5 * b * a * exp ( - 0.5 * d * d ) / pi;
    end

    if ( mod ( idf, 2 ) == 0 )
      sum2 = fmkm2;
    else
      sum2 = fmkm1;
    end

    ak = 1.0;

    for k = 2 : 2 : idf - 2

      fk = k;

      fmkm2 = b * ( d * a * ak * fmkm1 + fmkm2 ) * ( fk - 1.0 ) / fk;

      ak = 1.0 / ( ak * ( fk - 1.0 ) );
      fmkm1 = b * ( d * a * ak * fmkm2 + fmkm1 ) * fk / ( fk + 1.0 );

      if ( mod ( idf, 2 ) == 0 )
        sum2 = sum2 + fmkm2;
      else
        sum2 = sum2 + fmkm1;
      end

      ak = 1.0 / ( ak * fk );

    end

    if ( mod ( idf, 2 ) == 0 )
      cdf2 = normal_01_cdf ( d );
      cdf = 1.0 - cdf2 + sum2 * sqrt ( 2.0 * pi );
    else
      cdf2 = normal_01_cdf ( drb );
      cdf = 1.0 - cdf2 + 2.0 * ( sum2 + tfn ( drb, a ) );
    end
%
%  Normal approximation.
%
  else

    a = sqrt ( 0.5 * f ) * exp ( gammaln ( 0.5 * ( f - 1.0 ) ) ...
      - gammaln ( 0.5 * f ) ) * d;

    temp = ( x - a ) / sqrt ( f * ( 1.0 + d * d ) / ( f - 2.0 ) - a * a );

    cdf2 = normal_01_cdf ( temp );
    cdf = cdf2;

  end

  return
end


function result = triangle_sub ( func, xval, yval, nsub, norder, xtab, ytab, ...
  weight )

%*****************************************************************************80
%
%% TRIANGLE_SUB carries out quadrature over subdivisions of a triangular region.
%
%  Integration region:
%
%    Points (X,Y) such that:
%
%      (X,Y) =
%          ALPHA                * ( XVAL(1), YVAL(1) )
%        + BETA                 * ( XVAL(2), YVAL(2) )
%        + ( 1 - ALPHA - BETA ) * ( XVAL(3), YVAL(3) )
%      0 <= ALPHA <= 1 - BETA
%      0 <= BETA <= 1 - ALPHA
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied function of
%    two variables which is to be integrated, of the form:
%      function value = func ( x, y )
%
%    Input, real XVAL(3), YVAL(3), the coordinates of the triangle vertices.
%
%    Input, integer NSUB, the number of subdivisions of each side of the
%    input triangle to be made.  NSUB = 1 means no subdivisions are made.
%    NSUB = 3 means that each side of the triangle is subdivided into
%    three portions, and that the original triangle is subdivided into
%    NSUB**2 triangles.  NSUB must be at least 1.
%
%    Input, integer NORDER, the order of the rule.
%
%    Input, real XTAB(NORDER), YTAB(NORDER), the abscissas.
%
%    Input, real WEIGHT(NORDER), the weights of the rule.
%
%    Output, real RESULT, the approximate integral of the function.
%

%
%  Initialize RESULT, the approximate integral.
%
  result = 0.0E+00;
%
%  NSUB must be positive.
%
  if ( nsub <= 0 )
    return;
  end
%
%  Initialize QUAD, the quadrature sum.
%
  quad = 0.0E+00;
%
%  The sub-triangles can be grouped into NSUB strips.
%
  for i = 1 : nsub

    temp1 = 0.0;
    temp2 = i / nsub;

    x2 = xval(2) + temp1 * ( xval(3) - xval(2) ) ...
                 + temp2 * ( xval(1) - xval(2) );

    y2 = yval(2) + temp1 * ( yval(3) - yval(2) ) ...
                 + temp2 * ( yval(1) - yval(2) );

    temp1 = 0.0;
    temp2 = ( i - 1 ) / nsub;

    x3 = xval(2) + temp1 * ( xval(3) - xval(2) ) ...
                 + temp2 * ( xval(1) - xval(2) );

    y3 = yval(2) + temp1 * ( yval(3) - yval(2) ) ...
                 + temp2 * ( yval(1) - yval(2) );
%
%  There are 2*I-1 triangles in strip number I.
%  The next triangle in the strip shares two nodes with the previous one.
%  Compute its corners, (X1,Y1), (X2,Y2), (X3,Y3).
%
    for j = 1 : 2*i-1

      x1 = x2;
      y1 = y2;
      x2 = x3;
      y2 = y3;
      temp1 = floor ( ( j + 1 ) / 2 ) / nsub;
      temp2 = floor ( i - 1 - ( j / 2 ) ) / nsub;

      x3 = xval(2) + temp1 * ( xval(3) - xval(2) ) ...
                   + temp2 * ( xval(1) - xval(2) );

      y3 = yval(2) + temp1 * ( yval(3) - yval(2) ) ...
                   + temp2 * ( yval(1) - yval(2) );
%
%  Now integrate over the triangle, mapping the points ( XTAB(K), YTAB(K) )
%  into the triangle.
%
      for k = 1 : norder

        x = x2 + xtab(k) * ( x3 - x2 ) + ytab(k) * ( x1 - x2 );
        y = y2 + xtab(k) * ( y3 - y2 ) + ytab(k) * ( y1 - y2 );
        quad = quad + weight(k) * feval ( func, x, y );

       end

    end

  end

  volume = triangle_volume ( xval, yval ) / ( nsub * nsub );
  result = quad * volume;

  return
end


function mb = trimeshMeanBreadth(vertices, faces)
%TRIMESHMEANBREADTH Mean breadth of a triangular mesh.
%
%   MB = trimeshMeanBreadth(VERTICES, FACES)
%   Computes the mean breadth (proporitonal to the integral of mean
%   curvature) of a triangular mesh.
%
%   Example
%     [V, F] = createCube;
%     F2 = triangulateFaces(F);
%     MB = trimeshMeanBreadth(V, F2)
%     MB =
%         1.5000
%
%   See also
%   meshes3d, trimeshSurfaceArea, trimeshEdgeFaces, polyhedronMeanBreadth
%
%   References
%   Stoyan D., Kendall W.S., Mecke J. (1995) "Stochastic Geometry and its
%       Applications", John Wiley and Sons, p. 26
%   Ohser, J., Muescklich, F. (2000) "Statistical Analysis of
%       Microstructures in Materials Sciences", John Wiley and Sons, p.352

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2015-08-19, using Matlab 8.5.0.197613 (R2015a)
% Copyright 2015-2022 INRA - Cepia Software Platform

%% Check input validity

if size(faces, 2) ~= 3
    error('meshes3d:trimeshMeanBreadth:NonTriangularMesh', ...
        'Requires a triangular mesh as input');
end

%% Compute edge and edgeFaces arrays
% Uses the same code as in trimeshEdgeFaces

% compute vertex indices of each edge (in increasing index order)
edges = sort([faces(:,[1 2]) ; faces(:,[2 3]) ; faces(:,[3 1])], 2);

% create an array to keep indices of faces "creating" each edge
nFaces = size(faces, 1);
edgeFaceInds = repmat( (1:nFaces)', 3, 1);

% sort edges, keeping indices
[edges, ia, ib] = unique(edges, 'rows'); %#ok<ASGLU>
nEdges = size(edges, 1);

% allocate memory for result
edgeFaces = zeros(nEdges, 2);

% iterate over edges, to identify incident faces
for iEdge = 1:nEdges
    inds = find(ib == iEdge);
    edgeFaces(iEdge, 1:length(inds)) = edgeFaceInds(inds);
end

%% Compute dihedral angle for each edge

% compute normal of each face
normals = meshFaceNormals(vertices, faces);

% allocate memory for resulting angles
alpha = zeros(nEdges, 1);

% iterate over edges
for iEdge = 1:nEdges
    % indices of adjacent faces
    indFace1 = edgeFaces(iEdge, 1);
    indFace2 = edgeFaces(iEdge, 2);

    % normal vector of adjacent faces
    normal1 = normals(indFace1, :);
    normal2 = normals(indFace2, :);

    % compute dihedral angle of two vectors
    alpha(iEdge) = vectorAngle3d(normal1, normal2);
end

%% Compute mean breadth
% integrate the dihedral angles weighted by the length of each edge

% compute length of each edge
lengths = meshEdgeLength(vertices, edges);

% compute product of length by angles
mb = sum(alpha .* lengths) / (4*pi);


% Hopf_Bif - Animation of Hopf bifurcation of a limit cycle from the origin.
% NOTE: Hopf_System.m must be in the same directory as Hopf_Bif.m.
% Copyright Springer 2013.
clear
Max=120;global b;
for j = 1:Max
    F(j) = getframe;
    b=j/40;   % mu goes from 0 to 4.
    options = odeset('RelTol',1e-4,'AbsTol',1e-4);
    x0=1;y0=1;
    [t,x]=ode45(@Hopf_System,[0 100],[x0 y0],options);
    plot(x(:,1),x(:,2),'b');
    axis([0 5 0 5])
    fsize=15;
    set(gca,'xtick',0:1:5,'FontSize',fsize)
    set(gca,'ytick',0:1:5,'FontSize',fsize)
    xlabel('x(t)','FontSize',fsize)
    ylabel('y(t)','FontSize',fsize)
    title('Hopf Bifurcation','FontSize',15);
    F(j) = getframe;
end
movie(F,5)
% End of Program.


function H = cross( F, G )
%CROSS   Vector cross product.
%   CROSS(F, G) returns the cross product of the CHEBFUN2V objects F and G. If F
%   and G both have two components, then it returns the CHEBFUN2 representing
%       CROSS(F,G) = F(1) * G(2) - F(2) * G(1)
%   where F = (F(1); F(2)) and G = (G(1); G(2)). If F and G have three
%   components then it returns the CHEBFUN2V representing the 3D cross
%   product.

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

% Empty check:
if ( isempty( F ) || isempty( G ) )
    H = chebfun2v;
    return
end

% Get number of components:
Fc = F.components;
Gc = G.components;

% Do curl:
if ( F.nComponents == 2 && G.nComponents == 2 )  % 2D curl
    H = Fc{1} .* Gc{2} - Fc{2} .* Gc{1};
elseif ( F.nComponents == 3 && G.nComponents == 3 ) % 3D curl
    H = [ Fc{2} .* Gc{3} - Fc{3} .* Gc{2} ; ...
          Fc{3} .* Gc{1} - Fc{1} .* Gc{3} ; ...
          Fc{1} .* Gc{2} - Fc{2} .* Gc{1} ];
else
    error('CHEBFUN:CHEBFUN2V:cross:components', ...
        'CHEBFUN2V objects must be both 2- or 3-vectors.');
end

end


function vr = rotateVector(v, angle)
%ROTATEVECTOR Rotate a vector by a given angle.
%
%   VR = rotateVector(V, THETA)
%   Rotate the vector V by an angle THETA, given in radians.
%
%   Example
%   rotateVector([1 0], pi/2)
%   ans =
%       0   1
%
%   See also
%   vectors2d, transformVector, createRotation
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-04-14,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% precomputes angles
cot = cos(angle);
sit = sin(angle);

% compute rotated coordinates
vr = [cot * v(:,1) - sit * v(:,2) , sit * v(:,1) + cot * v(:,2)];


function [sinx, cosx] = SinCosNorm(sinx, cosx)
%SINCOSNORM  Normalize sinx and cosx
%
%   [SINX, COSX] = SINCOSNORM(SINX, COSX) normalize SINX and COSX so that
%   SINX^2 + COSX^2 = 1.  SINX and COSX can be any shape.

  r = hypot(sinx, cosx);
  sinx = sinx ./ r;
  cosx = cosx ./ r;
end


function u = nvecs(X,n,r,opts)
%NVECS Compute the leading mode-n vectors for a ktensor.
%
%   U = NVECS(X,n,r) computes the r leading eigenvalues of Xn*Xn'
%   (where Xn is the mode-n matricization of X), which provides
%   information about the mode-n fibers. In two-dimensions, the r
%   leading mode-1 vectors are the same as the r left singular vectors
%   and the r leading mode-2 vectors are the same as the r right
%   singular vectors.
%
%   U = NVECS(X,n,r,OPTS) specifies options:
%   OPTS.eigsopts: options passed to the EIGS routine [struct('disp',0)]
%   OPTS.flipsign: make each column's largest element positive [true]
%
%   See also KTENSOR, TENMAT, EIGS.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if ~exist('opts','var')
    opts = struct;
end

if isfield(opts,'eigsopts')
    eigsopts = opts.eigsopts;
else
    eigsopts.disp = 0;
end

% Compute Xn * Xn' excluding the nth factor
M = X.lambda * X.lambda';
for i = 1:ndims(X)
    if i == n, continue, end;
    M = M .* (X.u{i}' * X.u{i});
end

% Compute Xn * Xn'
Y = X.u{n} * M * X.u{n}';

[u,d] = eigs(Y, r, 'LM', eigsopts);

if isfield(opts,'flipsign')
    flipsign = opts.flipsign;
else
    flipsign = true;
end

if flipsign
    % Make the largest magnitude element be positive
    [val,loc] = max(abs(u));
    for i = 1:r
        if u(loc(i),i) < 0
            u(:,i) = u(:,i) * -1;
        end
    end
end


function demoNapoleon(varargin)
%DEMONAPOLEON  Small demo of a theorem on triangles
%
%   output = demoNapoleon(input)
%
%   Example
%   demoNapoleon
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2013-08-19,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

%% base triangle

% choose 3 points on the plane
A = [10 8];
B = [3 2];
C = [12 3];

% create a polygon for the base triangle
ABC = [A ; B ; C];

% Draw the base triangle
figure;
drawPolygon(ABC, 'color', 'k', 'lineWidth', 2);
axis equal;
axis([-5 20 -8 12]);
hold on;

%% Equliateral triangles

% create an equilateral triangle from each side, pointing outwards
tAB = createTriangle(B, A);
tBC = createTriangle(C, B);
tAC = createTriangle(A, C);

% draw each equilateral triangle
drawPolygon(tAB, 'color', 'b');
drawPolygon(tBC, 'color', 'b');
drawPolygon(tAC, 'color', 'b');

%% Compute centroid of each triangle

% compute centroids
c1 = polygonCentroid(tAB);
c2 = polygonCentroid(tBC);
c3 = polygonCentroid(tAC);

% draw the triangle formed by the centroids
triC = [c1; c2; c3];
drawPolygon(triC, 'lineWidth', 2, 'color', [0 .8 0]);
drawPoint(triC, 'marker', 'o', 'markerSize', 8, 'markerfacecolor', [0 .8 0]);

function tri = createTriangle(p1, p2)

d12 = distancePoints(p1, p2);
c1 = [p1 d12];
c2 = [p2 d12];
inters = intersectCircles(c1, c2);

indPos = isCounterClockwise(p1, p2, inters) > 0;
p3 = inters(indPos, :);

tri = [p1; p2; p3];


function value = p12_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P12_F evaluates the integrand for problem 12.
%
%  Discussion:
%
%    The highly oscillatory nature of the integrand makes this
%    a difficult and perhaps even dubious test.
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    product ( 1 <= i <= dim_num ) ( i * cos ( i * x(i) ) )
%
%  Exact Integral:
%
%    product ( 1 <= I <= DIM_NUM ) sin ( i )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Harald Niederreiter,
%    Implementation and Tests of Low-Discrepancy Sequences,
%    ACM Transactions on Modeling and Computer Simulation,
%    Volume 2, Number 3, July 1992, pages 195-213.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 1.0;

  for point = 1 : point_num
    for dim = 1 : dim_num
      value(point) = value(point) * dim * cos ( dim * x(dim,point) );
    end
  end

  p12_i4 ( 'I', '#', point_num );

  return
end


function [x1,err] = perform_debiasing(A,x,y, options)

% perform_debiasing - remove bias by orthogonal projection
%
%   x1 = perform_debiasing(A,x,y, options);
%
%   Compute x1 with same support I=find(abs(x)>Thresh) as x that minimize
%       min | y - A(:,I)*x(I) |
%   Thresh is set in options.Thresh
%
%   Usefull to remove some bias after L1 minimization.
%
%   If A is an implicit callback function or if options.use_pinv=0, then it
%   used an itertive gradient descent.
%
%   Copyright (c) 2008 Gabriel Peyre

options.null = 0;
use_pinv = getoptions(options, 'use_pinv', 1);
Thresh = getoptions(options, 'Thresh', 1e-5);
verb = getoptions(options, 'verb', 1);

if isnumeric(A) && use_pinv
    if size(x,2)>1
        % multiple signals
        x1 = x*0;
        for i=1:size(x,2)
            [x1(:,i),err] = perform_debiasing(A,x(:,i),y(:,i), options);
        end
        return;
    end
    % use explicit pseudo inverse
    I = find(abs(x)>Thresh);
    x1 = x*0;
    x1(I) = A(:,I)\y;
    err = [];
    return;
end

niter = getoptions(options, 'niter_debiasing', 100);
tau = getoptions(options, 'tau', 1);
x1 = getoptions(options, 'xguess', x);

err = [];
x1 = perform_support_projection(x1,x,Thresh);
for i=1:niter
    if verb
        progressbar(i,niter);
    end
    % gradient descent
    % x <- x + 1/tau * A'*( y-A*x )
    r   = y - applyop_single( A, x1,+1,options  );
    err(i) = norm(r, 'fro');
    if not(iscell(x))
        x1  = x1 + 1/tau * applyop_single( A, r,-1,options  );
    else
        xx = applyop_single( A, r,-1,options  );
        for k=1:length(x)
            x1{k}  = x1{k} + 1/tau * xx{k};
        end
    end
    % project on support
    x1 = perform_support_projection(x1,x,Thresh);
end

%%
function x = perform_support_projection(x,x0,Thresh)

if iscell(x)
    for i=1:length(x)
        x{i} = perform_support_projection(x{i},x0{i},Thresh);
    end
    return;
end
% impose same support
x(abs(x0)<Thresh) = 0;

%%
function y = applyop_single(A,x,dir,options, pA)

if isempty(A)
    y = x;
    return;
end
if isnumeric(A)
    if dir==1
        y = A*x;
    elseif dir==-1
        y = A'*x;
    else
        if exist('pA') && not(isempty(pA))
            y = pA*x;
        else
            y = pinv(A)*x;
        end
    end
else
    y = feval( A,  x, dir, options );
end


%
%  Convert a column vector to the vector in the domain D
%   (the inverse of the function RepresentationVector)
%
%   Syntax:  v = RepresentationInverse(z, B)
%
%   Input:  z -- (vector, sparse) column vector in sparse format
%                    representing the vector v
%           B -- (matrix/string/cell) Basis info of the vector space:
%                (i) If B is an mxn matrix, the vector space consists of
%                    mxn matrices spanned by nonzero entries of B
%                (ii) If B is a character string, the vector space consists
%                    of polynomials spanned by the support of B
%                (iii) If B is a 1xm cell, the vector space is a
%                   a Cartesian product of m vector spaces, each component
%                   is either a matrix space or a polynomial space
%                 Specs of B:
%                 * length(B) = number of spaces in the Cartesian product
%                 * each cell entry B{j} contains one of the following
%                   (i) B{j} is an mxn matrix: the j-th component of u
%                      is an mxn matrix spanned by nonzero entries of B
%                   (ii) B{j} is a character string: the j-th component of
%                      u is a polynomial spanned by the support of B
%                   (iii)B{j} is a 1x2 cell: the j-th component of u is
%                      a polynomial with
%                             B{j}{1} = cell of variable names such as
%                                          {'x','y','z'}
%                             B{j}{2} = tuple degree such as [3 2 5]
%                                 for degrees in each variable
%                         implying the complete monomial basis of the
%                         variables under the tuple degree for u{j}
%                   (iv) a 1x3 cell with variable names, tuple degree,
%                         term indices for a polynomial component u{j}
%  Output:  v -- (matrix/string/cell) a (general) vector that is either
%                (i) an mxn matrix, or
%                (ii) a character string representing a polynomial, or
%                (iii) a cell array, each v{i} is either (i) or (ii) above
%
% Example:
%
% >> full(z).'   % ans =
%     0     2     3     1     0     4     5     0     0    -7     0     6
%
% >> u = RepresentationInverse(z, {ones(3,2),'1+x+x*y+x*y^2+x^3+x^5'});
% >> u{:}  % ans =
%      0     1
%      2     0
%      3     4
% ans =
% 5 - 7*x^5 + 6*x*y^2
%


function [inform, x] = SteepDescent(fun, x, sdparams)
%  Implements steepest descent using simple Wolfe conditions via StepSize.m.
%
%  Implementation Parameters:
%    * STOP when
%      - Objective function gradient has norm less than gtol.
%      - maxit steps have been taken.
%    * ftol = 1.0e-20
%    * gtol = 1.0e-4
%    * xtol = 1.0e-20
%    * maxit = 1000
%    * p_k = -grad(f(x_k))
%    * alfa_k = max(10*xtol, f'(x_{k-1})*p_{k-1}*alfa_{k-1}/(f'(x_k)*p_k))
%
%  Input:
%    fun      - a pointer to a function
%    x        - the following structure:
%               * x.p - the starting point values
%               * x.f - the function value of x.p
%               * x.g - the gradient value of x.p
%    sdparams - the following structure, as an example:
%         sdparams = struct('maxit',1000,'toler',1.0e-4);
%
%  Output:
%    inform - structure containing two fields:
%      * inform.status - 1 if gradient tolerance was
%                        achieved;
%                        0 if not.
%      * inform.iter   - the number of steps taken
%    x      - the solution structure, with point, function,
%             and gradient evaluations at the solution point.

%  Number of function and gradient evaluations.
global numf numg
numf = 0;
numg = 0;

%  Populate local caching of nparams parameters, and params parameters.
toler = sdparams.toler;  % Set gradient tolerance.
maxit = sdparams.maxit;  % Set maximum number of allowed iterations.
xtol = 1.0e-20;  % Set point tolerance.
ftol = 1.0e-20;  % Set function tolerance.
gtol = toler;  % Set gradient tolerance.

%  Initialize parameter structure for StepSize function call.
params = struct('ftol', ftol, 'gtol', gtol, 'xtol', xtol);

% Below was tailored to geodesic functions.
% If calling geodesic function...
% if isfield(sdparams, 'geoparams') && isstruct(sdparams.geoparams)
%     params.geoparams = sdparams.geoparams;
%     x.f = feval(fun, x.p, 1, params.geoparams);
%     x.g = feval(fun, x.p, 2, params.geoparams);
% end
% params.geoparams = -1;

%  Compute function and gradient at starting point.
x.f = feval(fun, x.p, 1);
x.g = feval(fun, x.p, 2);

alfa = 1;  % Initial alpha value.
iter = 0;  % Number of iterations.
while iter < maxit && norm(x.g) >= toler
    pg = x.g;  % Store previous gradient value.
    d = -x.g;  % Steepest descent direction.

    %  Get step size satisfying simple Wolfe conditions.
    [alfa, x] = StepSize(fun, x, d, alfa, params);
    alfa = max(10*xtol, pg'*d*alfa / (x.g'*(-x.g)));  % Calculate new alpha.
    iter = iter + 1;  % Increment step counter.
end
inform.iter = iter;  % Number of iterations = iter at this point.

%  If norm of gradient is less than tolerance, the method succeeded.
if norm(x.g) < toler
    inform.status = 1;  % Update status to success indicator, 1.
else
    inform.status = 0;  % Update status to failure indicator, 0.
end
return;  % Return inform and final point x
end


function Dist = sc_pdist2( X, Y, metric )
% Calculates the pairwise distance between sets of vectors.
%
% Let X be an D-by-M matrix representing m points in D-dimensional space
% and Y be an D-by-N matrix representing another set of points in the same
% space. This function computes the M-by-N distance matrix Dist where Dist(i,j)
% is the distance between X(:,j) and Y(:,j).  This function has been
% optimized where possible, with most of the distance computations
% requiring few or no loops.
%
% The metric can be one of the following:
%
% 'euclidean' / 'sqeuclidean':
%   Euclidean / SQUARED Euclidean distance.  Note that 'sqeuclidean'
%   is significantly faster.
%
% 'chisq' 'cs'
%   The chi-squared distance between two vectors is defined as:
%    d(x,y) = sum( (xi-yi)^2 / (xi+yi) ) / 2;
%   The chi-squared distance is useful when comparing histograms.
%
%  'hell' 'hik'
%   The hellinger's distance is dedined as:
%    d(x,y) = sum (x^.5 - y^.5) .^ 2
%
% 'cos'
%   Distance is defined as the cosine of the angle between two vectors.
%
% 'emd'
%   Earth Mover's Distance (EMD) between positive vectors (histograms).
%   Note for 1D, with all histograms having equal weight, there is a simple
%   closed form for the calculation of the EMD.  The EMD between histograms
%   x and y is given by the sum(abs(cdf(x)-cdf(y))), where cdf is the
%   cumulative distribution function (computed simply by cumsum).
%
% 'L1','LINF'
%   The L1 distance between two vectors is defined as:  sum(abs(x-y));
%   The L-Inf distance is defined as: max(abs(x-y));
%
% USAGE
%  Dist = sc_pdist2( X, Y, [metric] )
%
% INPUTS
%  X        - [D x M] matrix of M D-dimensional vectors
%  Y        - [D x N] matrix of N D-dimensional vectors
%  metric   - ['sqeuclidean'], 'chisq', 'cosine', 'emd', 'euclidean', 'L0'
%              'L1', 'Linf', 'hik'
%
% OUTPUTS
%  Dist        - [M x N] distance matrix
% % %
% (C) Shicai Yang, 2012
% Institute of Systems Engineering, Southeast University, Nanjing

if( nargin<3 || isempty(metric) );
    metric=0;
end;

switch metric
    case {0,'sqeuclidean','sqe','seu'}
        Dist = distEucSq(X,Y);
    case {'euclidean','euc','l2','L2'}
        Dist = sqrt(distEucSq(X,Y));
    case {'L0','hamming','ham','l0'}
        Dist = distL0(X,Y);
    case {'L1','cityblock','cit','l1'}
        Dist = distL1(X,Y);
    case {'Linf','linf','chebychev','che'}
        Dist = distLinf(X,Y);
    case {'cosine','cos'}
        Dist = distCosine(X,Y);
    case {'emd','EMD'}
        Dist = distEmd(X,Y);
    case {'chisq','cs','chi2'}
        Dist = distChiSq(X,Y);
    case {'hellingers','hell','hik'}
        Dist = distHell(X,Y);
    otherwise
        error(['pdist2 - unknown metric: ' metric]);
end
% Dist = max(0,Dist);
end

function D = distL0( X, Y )
    D=pdist2(X',Y','hamming');
end

function D = distL1( X, Y )
%     m = size(X,2);  n = size(Y,2);
%     mOnes = ones(1,m); D = zeros(m,n);
%     for i=1:n
%         yi = Y(:,i);  yi = yi(:,mOnes);
%         D(:,i) = sum( abs( X-yi));
%     end
    D=pdist2(X',Y','cityblock');
end

function D = distLinf( X, Y )
%     m = size(X,2);  n = size(Y,2);
%     mOnes = ones(1,m); D = zeros(m,n);
%     for i=1:n
%         yi = Y(:,i);  yi = yi(:,mOnes);
%         D(:,i) = max( abs( X-yi));
%     end
    D=pdist2(X',Y','chebychev');
end

function D = distCosine( X, Y )
    p=size(X,1);
    XX = sqrt(sum(X.*X))+eps; X = X ./ XX(ones(1,p),:);
    YY = sqrt(sum(Y.*Y))+eps; Y = Y ./ YY(ones(1,p),:);
    D = 1 - X'*Y;
end

function D = distEmd( X, Y )
    Xcdf = cumsum(X,1);
    Ycdf = cumsum(Y,1);
%     m = size(X,2);  n = size(Y,2);
%     mOnes = ones(1,m); D = zeros(m,n);
%     for i=1:n
%       ycdf = Ycdf(:,i);
%       ycdfRep = ycdf(:,mOnes);
%       D(:,i) = sum(abs(Xcdf - ycdfRep));
%     end
    D = zeros(size(X,2),size(Y,2),'double');
    for i=1:size(Y,2)
      D(:,i) = sum(abs(bsxfun(@minus,Xcdf,Ycdf(:,i))));
    end
end

function D = distChiSq( X, Y )
% note: supposedly it's possible to implement this without a loop!
%     m = size(X,2);  n = size(Y,2);
%     mOnes = ones(1,m); D = zeros(m,n);
%     for i=1:n
%       yi = Y(:,i);  yiRep = yi(:,mOnes);
%       s = yiRep + X;    d = yiRep - X;
%       D(:,i) = sum( d.^2 ./ (s+eps));
%     end
%     D = D/2;
    D = zeros(size(X,2),size(Y,2),'double');
    for i=1:size(Y,2)
      yi = Y(:,i);
      s=bsxfun(@plus,X,yi);
      d=bsxfun(@times,X,yi);
      D(:,i) = sum(s-4*d./(s+eps),1);
    end
    D = D/2;
end

function D = distEucSq( X, Y )
    XX = sum(X.*X);
    YY = sum(Y.*Y);
    D = bsxfun(@plus,XX',YY)-2*X'*Y;
end

function D = distHell( X, Y )
    m = size(X,2);  n = size(Y,2);
    D = zeros(m,n,'double');
    X=X.^.5;Y=Y.^.5;
    for i=1:n
      yi = Y(:,i);
      s=bsxfun(@minus,X,yi);
      D(:,i) = sum(s.^2,1);
    end
end


function ival = c8mat_is_unitary ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_IS_UNITARY checks whether a complex matrix is unitary.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the row and column dimensions
%    of the matrix.  M and N must be positive.
%
%    Input, complex A(M,N), the matrix.
%
%    Output, integer IVAL:
%    -1, the matrix is not unitary; M /= N.
%    -2, the matrix is not unitary; row I * column I /= 1;
%    -3, the matrix is not unitary; row I * column J /= 0;
%    1, the matrix is unitary.
%
  tol = 0.0001;

  if ( m ~= n )
    ival = -1;
    return
  end

  for i = 1 : n

    test = a(i,1:n) * conj ( a(1:n,i) ) - 1.0;

    if ( tol < abs ( test ) )
      ival = -2;
      return
    end

    for j = i + 1 : n

      test = a(i,1:n ) * conj ( a(1:n,j) );

      if ( tol < abs ( test ) )
        ival = -3;
        return
      end

    end

  end

  ival = 1;

  return
end


function legcoeffs = legvals2legcoeffs(legvals)
%LEGVALS2LEGCOEFFS  Convert Legendre values to Legendre coefficients.
% 	LEGCOEFFS = LEGVALS2LEGCOEFFS(LEGVALS) converts the column vector
%   LEGVALS representing values on a Legendre grid (i.e, F(LEGPTS)) to a
%   vector LEGCOEFFS representing the coefficients of the series
%       F(X) = C_LEG(1)*P0(X) + ... + C_LEG(N)*P{N-1}(X).
%
% See also CHEBFUN.IDLT, LEGPTS.

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

% This command is a wrapper for chebfun/idlt.
legcoeffs = chebfun.idlt(legvals);

end


function stats = chip_histogram_features( varargin )
% ------------
% Description:
% ------------
%  This function is to obtain state of the art histogram based features
% such as:
%   Mean
%   Variance
%   Skewness
%   Kurtosis
%   Energy
%   Entropy
% ---------
% History:
% ---------
% Creation: beta         Date: 09/11/2007
%----------
% Example:
%----------
% Stats = chip_histogram_features( I,'NumLevels',9,'G',[] )
%
% -----------
% Author:
% -----------
%    (C)Xunkai Wei <xunkai.wei@gmail.com>
%    Beijing Aeronautical Technology Research Center
%    Beijing %9203-12,10076
%

% Parameter checking
[I, NL, GL] = ParseInputs(varargin{:});
% Scale I so that it contains integers between 1 and NL.
if GL(2) == GL(1)
    SI = ones(size(I));
else
    slope = (NL-1) / (GL(2) - GL(1));
    intercept = 1 - (slope*(GL(1)));
    SI = round(imlincomb(slope,I,intercept,'double'));
end
% Clip values if user had a value that is outside of the range, e.g., double
% image = [0 .5 2;0 1 1]; 2 is outside of [0,1]. The order of the following
% lines matters in the event that NL = 0.
SI(SI > NL) = NL;
SI(SI < 1) = 1;
%--------------------------------------------------------------------------
% 1. Calculate histogram for all scaled gray level from 1 to NL
%--------------------------------------------------------------------------
% Get image size
s = size(SI);
% Generate gray level vector
Gray_vector = 1:NL;
% intialize parameters
Histogram = zeros(1,NL);
% Using inline function numel, make it easy
for i =1:NL
    Histogram(i) = numel(find(SI==i));
end
%--------------------------------------------------------------------------
% 2. Now calculate its histogram statistics
%--------------------------------------------------------------------------
% Calculate obtains the approximate probability density of occurrence of the intensity
% levels
Prob                = Histogram./(s(1)*s(2));
% 2.1 Mean
Mean                = sum(Prob.*Gray_vector);
% 2.2 Variance
Variance            = sum(Prob.*(Gray_vector-Mean).^2);
% 2.3 Skewness
Skewness            = calculateSkewness(Gray_vector,Prob,Mean,Variance);
% 2.4 Kurtosis
Kurtosis            = calculateKurtosis(Gray_vector,Prob,Mean,Variance);
% 2.5 Energy
Energy              = sum(Prob.*Prob);
% 2.6 Entropy
Entropy             = -sum(Prob.*log(Prob));
%-------------------------------------------------------------------------
% 3. Insert all features and return
%--------------------------------------------------------------------------
stats =[Mean Variance Skewness Kurtosis  Energy  Entropy];
% End of funtion
%--------------------------------------------------------------------------
% Utility functions
%--------------------------------------------------------------------------
function Skewness = calculateSkewness(Gray_vector,Prob,Mean,Variance)
% Calculate Skewness
term1    = Prob.*(Gray_vector-Mean).^3;
term2    = sqrt(Variance);
Skewness = term2^(-3)*sum(term1);

function Kurtosis = calculateKurtosis(Gray_vector,Prob,Mean,Variance)
% Calculate Kurtosis
term1    = Prob.*(Gray_vector-Mean).^4;
term2    = sqrt(Variance);
Kurtosis = term2^(-4)*sum(term1);

function [I, nl, gl] = ParseInputs(varargin)
% parsing parameter checking
% Inputs must be max seven item
iptchecknargin(1,5,nargin,mfilename);
%
% Check I
I = varargin{1};
iptcheckinput(I,{'logical','numeric'},{'2d','real','nonsparse'}, ...
    mfilename,'I',1);
% ------------------------
% Assign Defaults
% -------------------------
%
if islogical(I)
    nl = 2;
else
    nl = 8;
end
gl = getrangefromclass(I);

% Parse Input Arguments
if nargin ~= 1

    paramStrings = {'NumLevels','GrayLimits'};

    for k = 2:2:nargin

        param = lower(varargin{k});
        inputStr = iptcheckstrs(param, paramStrings, mfilename, 'PARAM', k);
        idx = k + 1;  %Advance index to the VALUE portion of the input.
        if idx > nargin
            eid = sprintf('Images:%s:missingParameterValue', mfilename);
            msg = sprintf('Parameter ''%s'' must be followed by a value.', inputStr);
            error(eid,'%s', msg);
        end

        switch (inputStr)
            case 'NumLevels'
                nl = varargin{idx};
                iptcheckinput(nl,{'logical','numeric'},...
                    {'real','integer','nonnegative','nonempty','nonsparse'},...
                    mfilename, 'NL', idx);
                if numel(nl) > 1
                    eid = sprintf('Images:%s:invalidNumLevels',mfilename);
                    msg = 'NL cannot contain more than one element.';
                    error(eid,'%s',msg);
                elseif islogical(I) && nl ~= 2
                    eid = sprintf('Images:%s:invalidNumLevelsForBinary',mfilename);
                    msg = 'NL must be two for a binary image.';
                    error(eid,'%s',msg);
                end
                nl = double(nl);

            case 'GrayLimits'

                gl = varargin{idx};
                iptcheckinput(gl,{'logical','numeric'},{'vector','real'},...
                    mfilename, 'GL', idx);
                if isempty(gl)
                    gl = [min(I(:)) max(I(:))];
                elseif numel(gl) ~= 2
                    eid = sprintf('Images:%s:invalidGrayLimitsSize',mfilename);
                    msg = 'GL must be a two-element vector.';
                    error(eid,'%s',msg);
                end
                gl = double(gl);
        end
    end
end


function [K, grad] = covSEard(hyp, x, z, i)

% Squared Exponential covariance function with Automatic Relevance Detemination
% (ARD) distance measure. The covariance function is parameterized as:
%
% k(x^p,x^q) = sf2 * exp(-(x^p - x^q)'*inv(P)*(x^p - x^q)/2)
%
% where the P matrix is diagonal with ARD parameters ell_1^2,...,ell_D^2, where
% D is the dimension of the input space and sf2 is the signal variance. The
% hyperparameters are:
%
% hyp = [ log(ell_1)
%         log(ell_2)
%          .
%         log(ell_D)
%         log(sqrt(sf2)) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<2, K = '(D+1)'; return; end              % report number of parameters
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = numel(z)==0; dg = strcmp(z,'diag') && numel(z)>0;        % determine mode

[n,D] = size(x);
ell = exp(hyp(1:D));                               % characteristic length scale
sf2 = exp(2*hyp(D+1));                                         % signal variance

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

K = sf2*exp(-K/2);                                                  % covariance

if nargout >= 2
	grad = bsxfun(@times, diag(1./(ell.^2))*bsxfun(@minus, x', z'), K');
end

if nargin>3                                                        % derivatives
  if i<=D                                              % length scale parameters
    if dg
      K = K*0;
    else
      if xeqz
        K = K.*sq_dist(x(:,i)'/ell(i));
      else
        K = K.*sq_dist(x(:,i)'/ell(i),z(:,i)'/ell(i));
      end
    end
  elseif i==D+1                                            % magnitude parameter
    K = 2*K;
  else
    error('Unknown hyperparameter')
  end
end