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

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

  grid_weight(1:point_num) = 0.0;

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

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

    while ( 1 )

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

      for point2 = 1 : order_nd

        for point = 1 : point_num

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

        end

      end

      if ( ~more )
        break
      end

    end

  end

  return
end


%  Figure 3.28      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%script to generate Fig. 3.28
%  Example 3.25

clf;
u=-1;  % magnitude of impulsive elevator input (Deg)
num=30*[1 -6];
den=[1 4 13 0];
t=0:.02:5;
y=impulse(u*num,den,t);
zero=[0 0];
tzero=[0 5];
axis([0 5 -2 16])
plot(t,y,'-',tzero,zero,'-'),grid
title('Fig. 3.28  Impulse response of aircraft altitude')
xlabel('Time (sec)')
ylabel('Altitude (ft)')


function [A_hat, E_hat, iter] = exact_alm_rpca(D, lambda, tol, maxIter)

% Oct 2009
% This matlab code implements the augmented Lagrange multiplier method for
% Robust PCA.
%
% D - m x n matrix of observations/data (required input)
%
% lambda - weight on sparse error term in the cost function
%
% tol - tolerance for stopping criterion.
%     - DEFAULT 1e-7 if omitted or -1.
%
% maxIter - maximum number of iterations
%         - DEFAULT 1000, if omitted or -1.
%
% Initialize A,E,Y,u
% while ~converged
%   minimize
%     L(A,E,Y,u) = |A|_* + lambda * |E|_1 + <Y,D-A-E> + mu/2 * |D-A-E|_F^2;
%   Y = Y + \mu * (D - A - E);
%   \mu = \rho * \mu;
% end
%
% Minming Chen, October 2009. Questions? v-minmch@microsoft.com ;
% Arvind Ganesh (abalasu2@illinois.edu)
%
% Copyright: Perception and Decision Laboratory, University of Illinois, Urbana-Champaign
%            Microsoft Research Asia, Beijing

[m,n] = size(D);

if(nargin < 2) lambda = 1 / sqrt(m); end
if(nargin < 3) tol = 1e-7; elseif(tol == -1) tol = 1e-7; end
if(nargin < 4) maxIter = 1000; elseif(maxIter == -1) maxIter = 1000; end

% initialize
Y = sign(D);
norm_two = norm(Y, 2); %norm_two = lansvd(Y, 1, 'L');
norm_inf = norm( Y(:), inf) / lambda;
dual_norm = max(norm_two, norm_inf);
Y = Y / dual_norm;

A_hat = zeros( m, n);
E_hat = zeros( m, n);
dnorm = norm(D, 'fro');
tolProj = 1e-6 * dnorm;
total_svd = 0;
mu = .5/norm_two; % this one can be tuned
rho = 6;          % this one can be tuned

iter = 0;
converged = false;
stopCriterion = 1;
sv = 5;
svp = sv;
while ~converged
    iter = iter + 1;
    % solve the primal problem by alternative projection
    primal_converged = false;
    primal_iter = 0;
    sv = sv + round(n * 0.1);
    while primal_converged == false
        temp_T = D - A_hat + (1/mu)*Y;
        temp_E = max( temp_T - lambda/mu,0) + min( temp_T + lambda/mu,0);

        %if choosvd(n, sv) == 1
        %    [U S V] = lansvd(D - temp_E + (1/mu)*Y, sv, 'L');
        %else
            %[U,S,V] = svd(D - temp_E + (1/mu)*Y, 'econ');
            [U,S,V] = svdecon(D - temp_E + (1/mu)*Y); % fastest
        %end

        diagS = diag(S);
        svp = length(find(diagS > 1/mu));
        if svp < sv
            sv = min(svp + 1, n);
        else
            sv = min(svp + round(0.05*n), n);
        end
        temp_A = U(:,1:svp)*diag(diagS(1:svp)-1/mu)*V(:,1:svp)';

        if norm(A_hat - temp_A, 'fro') < tolProj && norm(E_hat - temp_E, 'fro') < tolProj
            primal_converged = true;
        end
        A_hat = temp_A;
        E_hat = temp_E;
        primal_iter = primal_iter + 1;
        total_svd = total_svd + 1;
    end

    Z = D - A_hat - E_hat;
    Y = Y + mu*Z;
    mu = rho * mu;

    %% stop Criterion
    stopCriterion = norm(Z, 'fro') / dnorm;
    if stopCriterion < tol
        converged = true;
    end

    disp(['Iteration' num2str(iter) ' #svd ' num2str(total_svd) ' r(A) ' num2str(svp)...
        ' |E|_0 ' num2str(length(find(abs(E_hat)>0)))...
        ' stopCriterion ' num2str(stopCriterion)]);

    if ~converged && iter >= maxIter
        disp('Maximum iterations reached') ;
        converged = 1 ;
    end
end

if nargin == 5
    fclose(fid);
end


function [ff,f]=lpcar2ff(ar,np)
%LPCAR2FF LPC: Convert AR coefs to complex spectrum FF=(AR,NP)
%
%  Inputs: ar(nf,n)     AR coefficients, one frame per row
%          np           Size of output spectrum is np+1 [n]
%
% Outputs: ff(nf,np+1)  Complex spectrum from DC to Nyquist
%          f(1,np+1)    Normalized frequencies (0 to 0.5)
%
% For high speed make np equal to a power of 2

%      Copyright (C) Mike Brookes 1998-2014
%      Version: $Id: lpcar2ff.m 5026 2014-08-22 17:47:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nf,p1]=size(ar);
if nargin<2
    if nargout
        np=p1-1;
    else
        np=128;
    end
end
ff=(rfft(ar.',2*np).').^(-1);
f=(0:np)/(2*np);
if ~nargout
    subplot(2,1,2);
    plot(f,unwrap(angle(ff)));
    xlabel('Normalized frequency f/f_s');
    ylabel('Phase (rad)');
    subplot(2,1,1);
    plot(f,db(abs(ff)));
    xlabel('Normalized frequency f/f_s');
    ylabel('Gain (dB)');
    title('LPC Spectrum');
end


function lp_coefficients_test ( )

%*****************************************************************************80
%
%% LP_COEFFICIENTS_TEST tests LP_COEFFICIENTS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2014
%
%  Author:
%
%    John Burkardt
%
  n_max = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LP_COEFFICIENTS_TEST\n' );
  fprintf ( 1, ...
    '  LP_COEFFICIENTS: coefficients of Legendre polynomial P(n,x).\n' );
  fprintf ( 1, '\n' );

  for n = 0 : n_max

    [ o, c, f ] = lp_coefficients ( n );

    m = 1;
    e(1:o) = f(1:o) + 1;

    label = sprintf ( '  P(%d,x) = ', n );
    polynomial_print ( m, o, c, e, label );

   end

  return
end


function [ xd, yd, xdp, ydp ] = hermite_interpolant ( n, x, y, yp )

%*****************************************************************************80
%
%% HERMITE_INTERPOLANT sets up a divided difference table from Hermite data.
%
%  Discussion:
%
%    The polynomial represented by the divided difference table can be
%    evaluated by calling HERMITE_INTERPOLANT_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer N, the number of items of data
%    ( X(I), Y(I), YP(I) ).
%
%    Input, real X(N), the abscissas.
%    These values must be distinct.
%
%    Input, real Y(N), YP(N), the function and
%    derivative values.
%
%    Output, real XD(2*N), YD(2*N), the divided difference table
%    for the interpolant value.
%
%    Output, real XDP(2*N-1), YDP(2*N-1), the divided difference
%    table for the interpolant derivative.
%
  x = x ( : );
  y = y ( : );
  yp = yp ( : );
%
%  Copy the data:
%
  nd = 2 * n;
  xd = zeros ( nd, 1 );
  xd(1:2:nd-1) = x(1:n);
  xd(2:2:nd  ) = x(1:n);
%
%  Carry out the first step of differencing.
%
  yd = zeros ( nd, 1 );
  yd(1) = y(1);
  yd(3:2:nd-1) = ( y(2:n) - y(1:n-1) ) ./ ( x(2:n) - x(1:n-1) );
  yd(2:2:nd  ) = yp(1:n);
%
%  Carry out the remaining steps in the usual way.
%
  for i = 3 : nd
    for j = nd : -1 : i
      yd(j) = ( yd(j) - yd(j-1) ) / ( xd(j) - xd(j+1-i) );
    end
  end
%
%  Compute the difference table for the derivative.
%
  [ ndp, xdp, ydp ] = dif_deriv ( nd, xd, yd );

  return
end


function[varargout]=vshift(varargin)
% VSHIFT  Cycles the elements of an array along a specified dimension.
%
%   Y=VSHIFT(X,N,DIM) cycles the elements of X N places along dimension DIM.
%
%   Example: x=[1 2 3 4 5];
%            vshift(x,+1,2)=[2 3 4 5 1]
%            vshift(x,-1,2)=[5 1 2 3 4]
%
%   Note shifting by N and then by -N recovers the original array.
%
%   [Y1,Y2,...YN]=VSHIFT(X1,X2,...XN,N,DIM) also works.
%
%   VSHIFT(X1,X2,...XN,N,DIM); with no arguments overwrite the original
%   input variables.
%
%   Note that VSHIFT is similar to Matlab's CIRCSHIFT, but has the opposite
%   convection for positive and negative shifts.
%
%   ------------------------------------------------------------------
%   Y=VSHIFT(X,N,DIM,INDEX,DIM2) applies this shift selectively, only to
%   that subset of X obtained by indexing X with INDEX along DIM2, i.e.
%
%		    1 2      DIM2     DIMS(X)
%		    | |        |         |
%		  X(:,:, ... INDEX, ..., :)
%
%   is cycled N places along dimension DIM, but the remainder of X is not.
%   DIM and DIM2 cannot be the same.  The above extensions to multiple
%   output varibles work in this case as well.
%   ------------------------------------------------------------------
%
%   See also: VINDEX, CIRCSHIFT.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2001--2016 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
  vshift_test,return
end

%/********************************************************
%Sort out input arguments
nax=2;
if nargin>4
  if  length(varargin{end-3}(:))==1
     nax=4;
     dim2=(varargin{end});
     jj=varargin{end-1}(:);
     dim=varargin{end-2};
     n=varargin{end-3};
     if dim==dim2
       error('DIM and DIM2 cannot be the same.')
     end
     %n, jj, dim,dim2
  end
end

if nax==2
  dim=varargin{end};
  n=varargin{end-1};
end
%\********************************************************

for i=1:length(varargin)-nax
  if nax==2
    varargout{i}=vshift1(varargin{i},n,dim);
  else
    varargout{i}=vshift1(varargin{i},n,dim,jj,dim2);
  end
end
eval(to_overwrite(nargin-nax))

function[y]=vshift1(x,n,ndim,jj,ndim2)

N=size(x,ndim);
if n>0
    ii=[(n+1:N) (1:n)];
elseif n<0
    n=-n;
    ii=[(N-(n-1):N) (1:N-n)];
elseif n==0
    ii=(1:N);
end

if nargin==3
%   Same as using circshift... these are the same speed
    %array=zeros(max(ndim,numel(size(x))),1);
    %array(ndim)=-n;
    %ndim
 %   y=circshift(x,-n,ndim);
    y=vindex(x,ii,ndim);
else
    y1=vindex(x,jj,ndim2);
    y1=vindex(y1,ii,ndim);
    %vsize(x,y1,jj,ndim2);
    %x,y1,ii,jj,ndim2
    y=vindexinto(x,y1,jj,ndim2);
end

function[]=vshift_test
x=(1:10);
ans1=[(2:10) 1];
reporttest('VSHIFT col case', aresame(vshift(x,1,2),ans1))

x=(1:10)';
ans1=[10 (1:9)]';
reporttest('VSHIFT row case', aresame(vshift(x,-1,1),ans1))

clear x ans1
x(:,:,1)=[1 2; 3 4];
x(:,:,2)=2*[1 2; 3 4];
ans1(:,:,2)=x(:,:,1);
ans1(:,:,1)=x(:,:,2);
reporttest('VSHIFT mat case', aresame(vshift(x,1,3),ans1))

clear x ans1
x(:,:,1)=[1 2; 3 4];
x(:,:,2)=2*[1 2; 3 4];
ans1(:,:,1)=[3 2;1 4];
ans1(:,:,2)=2*[3 2;1 4];
reporttest('VSHIFT mat selective case one', aresame(vshift(x,1,1,1,2),ans1))

clear x ans1
x(:,:,1)=[1 2; 3 4];
x(:,:,2)=2*[1 2; 3 4];
ans1(:,:,1)=[3 4;1 2];
ans1(:,:,2)=2*[3 4;1 2];
reporttest('VSHIFT mat selective case two', aresame(vshift(x,1,1,1:2,2),ans1))


function [minCostTuples,gain]=assign3DBB(C,maximize,boundType,initMethod,maxIter,epsVal)
%%ASSIGN3DBB Solve the data fusion axial 3D assignment problem using a
%         branch-and-bound algorithm. Such problems are NP-hard and thus
%         cannot be solved in polynomial time, so this function can only
%         solve problems of a moderate size. The optimization problem being
%         solved is minimize (or maximize)
%         \sum_{i=1}^{n1}\sum_{j=1}^{n2}\sum_{k=1}^{n3}C_{i,j,k}*\rho_{i,j,k}
%         subject to
%         \sum_{i=1}^{n1}\sum_{j=1}^{n2}\rho_{i,j,k}<=1 for all k
%         \sum_{i=1}^{n1}\sum_{k=1}^{n3}\rho_{i,j,k}<=1 for all j
%         \sum_{j=1}^{n2}\sum_{k=1}^{n3}\rho_{i,j,k} =1 for all i
%         \rho_{i,j,k} = 0 or 1
%         assuming that n1<=n2<=n3, and C is and n1Xn2Xn3 cost matrix.
%         This is equivalent to the optimization problem
%         min (or max) sum_{i} C(i,phi_2(i),phi_3(i))
%         where phi_2 and phi_3 are length n1 arrangements of n2 and n3
%         items over which the minimization is performed. In general, it is
%         not required that n1<=n2<=n3 (for any other ordering, the
%         equality constraint is on the dimension with the fewest
%         elements), so the solution is given as tuples solving the problem
%         min (or max) sum_{i} C(tuple(1,i),tuple(2,i),tuple(3,i))
%         where the tuples satisfy all of the above constraints.
%
%INPUTS: C An n1Xn2Xn3 cost hypermatrix. C cannot contain any NaNs and 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. During minimization, there should be no elements
%          with -Inf cost and no elements with +Inf cost during
%          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.
% boundType This selects the bound to use for the branch and bound method.
%          These correspond to the method input of the assign3DLB function.
%          The default if this parameter is omitted or an empty matrix is
%          passed is 2.
% initMethod A parameter indicating how the branch-and-bound algorithm
%          should be initialized. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) Do not
%            use an initial estimate.
%          1 Use an initial solution (and lower bound) via algorithm 0 of
%            the assign3D function.
%  maxIter If initMethod=1, then this is the maximum number of iterations
%          to allow the initialization routine in assign3D to perform. If
%          initMethod~=1, then this parameter is ignored. The default if
%          omitted or an empty matrix is passed is 10.
%   epsVal If initMethod=1, then this is both the AbsTold and RelTol inputs
%          to assign3D to determine whether the function has converged in
%          terms of the relative duality gap. If initMethod~=1, then this
%          parameter is ignored. The default if omitted or an empty matrix
%          is passed is eps(1).
%
%OUTPUTS: tuples A 3Xn1 matrix where tuples(:,i) is the ith assigned tuple
%                in C as described above.
%           gain The cost value of the optimal assignment found. This is
%                the sum of the values of the assigned elements in C.
%
%The algorithm is similar to the branch-and bound procedure describes in
%[1], which uses the "classical" branching of [2]. There is a total of
%min([n1,n2,n3]) elements to choose out of the entire C matrix. Suppose
%that n1=min([n1,n2,n3]). Then, the tuples chosen will have the first index
%from 1 to n1 and the indices of the remaining two tuple components will
%vary. The branching here is on the tuple chosen for i=1 of C(i,j,k), then
%the next level is for i=2 until branching is done for i=n1. At each level
%of branching there are n2*n3 possibilities, as is illustrated in Fig. 1 of
%[1]. Each time a branch is taken, all tuples of (j,k) sharing any common
%tuples with the branch taken (and any previous branches taken) must be
%removed. At each level, lower bounds for all n2*n3 possible branches are
%computed. Those branches with lower bounds that are less than the best
%found solution are discarded.
%
%REFERENCES:
%[1] W. P. Pierskalla, "The multidimensional assignment problem,"
%    Operations Research, vol. 16, no. 2, pp. 422-431, Mar-Apr. 1968.
%[2] R. E. Burkard and R. Rudolf, "Computational investigations on 3-
%    dimensional axial assignment problems," Belgian Journal of Operations
%    Research, Statistics and Computer Science, vol. 32, no. 1-3, pp.
%    85-98, 1993.
%
%September 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(epsVal))
	epsVal=eps(1);
end

if(nargin<5||isempty(maxIter))
    maxIter=10;
end

if(nargin<4||isempty(initMethod))
	initMethod=0;
end

if(nargin<3||isempty(boundType))
    boundType=2;
end

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

%For some types of lower bounds, 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(:));
    C=-C+CDelta;
else
    CDelta=min(C(:));
    C=C-CDelta;
end

%The algorthm was implemented assuming that n1<=n2<=n3 for the
%dimensionality of C. If a matrix having a different arrangement of indices
%is passed, one could permute the indices to make the assumptions below
%hold.
nVals=size(C);
%Deal with trailing singleton dimensions.
if(length(nVals)==2)
    nVals=[nVals,1];
elseif(length(nVals)==1)
    nVals=[nVals,1,1];
end

if(length(nVals)<3||~(nVals(1)<=nVals(2)&&nVals(2)<=nVals(3)))
    error('It is required that size(C,1)<=size(C,2)<=size(C,3)')
end

n1=nVals(1);
n2=nVals(2);
n3=nVals(3);

%Get an initial estimate.
switch(initMethod)
    case 0%Do not use an initial estimate.
        q=-Inf;
        gain=Inf;
        minCostTuples=[];
    case 1
        subgradMethod=0;
        maximize=false;
        [minCostTuples,gain,q,exitCode]=assign3D(C,false,subgradMethod,maximize,maxIter,epsVal,epsVal);
        if(exitCode==-3||exitCode==-1)
            %If no valid assignment was found, then we just start the
            %algorithm without initialization. This most commonly occurs
            %when the problem is infeasible and no solution will be found
            %in the end anyway.
            q=-Inf;
            gain=Inf;
            minCostTuples=[];
        end
    otherwise
        error('Invalid initMethod specified.');
end

%If the initial approximation returned a suboptimal solution.
if(abs(gain-q)>=epsVal*gain)
    numLevels=n1;%The number of things to assign.

    %There are n1*n2*n3 total tuples. However, we define a level by the
    %value of the first index of the tuple. In the initial level, there are
    %n2*n3 possible tuples (fix the value of the first index of C and the
    %other two are free). In subsequent levels, there are
    %(n2-curLevel+1)*(n3-curLevel+1) possibilities.
    maxTuplesPerLevel=n2*n3;

    %Generate all possible tuples of the second and third coordinates that
    %can be assigned. The assignment of the first coordinate is the level
    %of the recursion below.
    origFreeTuples=genAllTuples([n2-1;n3-1],false)+1;

    %The list of tuples that are candidates in the current level.
    freeTuples=zeros(2,maxTuplesPerLevel,numLevels);
    %Save the tuples that could possibly be assigned in the first level.
    freeTuples(:,:,1)=origFreeTuples;

    %These values link the modified tuples at each level to the original
    %tuples. These are important, because the ordering of the tuples in the
    %lower levels will be changed.
    freeTupleOrigIdx=zeros(maxTuplesPerLevel,numLevels);
    freeTupleOrigIdx(:,1)=1:maxTuplesPerLevel;

    %This keeps track of how many tuples are being considered at each
    %level after eliminating those that conflict with prior assignments and
    %eliminating those whose lower bounds are too high.
    numFreeTuples=zeros(numLevels,1);
    %In the top level, all possibilities are initially considered.
    numFreeTuples(1)=maxTuplesPerLevel;

    %The list of tuples that were candidates when entering the current
    %level. In the current level, some tuples might be removed due to a low
    %cost at that level. However, when going to a higher level, all tuples
    %that do not conflict with assignments at lower levels must be
    %considered. Thus, this stores the value of the tuples in the current
    %level before pruning.
    freeTuplesEntering=zeros(2,maxTuplesPerLevel,numLevels);
    freeTuplesEntering(:,:,1)=origFreeTuples;

    %Like freeTupleOrigIdx, these link the values in freeTuplesEntering to
    %the original set of tuples.
    freeTuplesEnteringOrigIdx=zeros(maxTuplesPerLevel,numLevels);
    freeTuplesEnteringOrigIdx(:,1)=1:maxTuplesPerLevel;

    %This keeps track of how many tuples are being considered at each
    %level after eliminating those that conflict with prior assignments but
    %not eliminating anything where the lower bounds are too high.
    numFreeTuplesEntering=zeros(numLevels,1);
    %In the top level, all possibilities are considered.
    numFreeTuplesEntering(1)=maxTuplesPerLevel;

    %The tuple that is assigned at each level, after the indexation has
    %been changed to relate it to the current cost submatrix at that level.
    %We don't need to save the first index here, because it is just the
    %level number. We don't need to save the assigned tuple in the maximum
    %level, because we use it right away.
    assignedTuples=zeros(2,numLevels-1);

    %This links the assigned tuple to the index of the tuple in
    %origFreeTuples. It is simpler to use this than to try to reconstruct
    %the tuple from assignedTuples, where each tuple has been modified
    %based on assignments at lower levels. Unlike assignedTuples, this
    %needs to be numLevels in size.
    assignedTupleOrigIdx=zeros(numLevels,1);

    %The cumulative assigned cost at each level. The final cost is computed
    %at the top level and does not need to be stored in this.
    cumAssignedCost=zeros(numLevels-1,1);

    %Allocate space for the bounded total costs for assigning something at
    %each level. Each level is given by the value in the first index that
    %is assigned. For each first index, there are n2*n3 values. There are
    %no bounds for the deepest level, because the cost matrix can be
    %directly used.
    boundVals=zeros(maxTuplesPerLevel,numLevels-1);

    %This holds what was the minimum cost value before going to a higher
    %level of recursion. Thus, when returning to a particular level, if the
    %minimum cost value changed, one should check the previous bounds and
    %see if any of the candidate tuples should be thrown out and never
    %visited.
    prevMinCost=zeros(numLevels-1,1);

    %Allocate space for the marginal costs at each level (the cost
    %submatrices). We could just allocate this like
    %CostMatCur=zeros(n1,n2,n3,numLevels); and then easily address each
    %level with the final index. However, that wastes a lot of space,
    %because each level removes one element from every dimension. The first
    %matrix is n1Xn2Xn3 in size, the next is (n1-1)*(n2-1)*(n3-1) in size
    %and so on until the final level which is 1X(n2-n1+1)X(n3-n1+1) in
    %size. To save space, we shall allocate the minimum amount of space
    %required, as well as an offset array, so that we know where the
    %elements of the matrices start and end so that we can later consider
    %each matrix individually.
    costMatCurStartIdx=zeros(numLevels+1,1);
    costMatCurStartIdx(1)=1;
    for curLevel=2:(numLevels+1)
        offset=curLevel-2;
        costMatCurStartIdx(curLevel)=costMatCurStartIdx(curLevel-1)+(n1-offset)*(n2-offset)*(n3-offset);
    end
    %The final value in costMatCurStartIdx is one past the end of the
    %matrix.
    CostMatCur=zeros(costMatCurStartIdx(numLevels+1)-1,1);
    %Assign the marginal cost matrix of the first level.
    CostMatCur(costMatCurStartIdx(1):(costMatCurStartIdx(2)-1))=C(:);

    %Enter the recursion (unrolled).
    curLevel=1;
    increasingLevelNumber=true;
    while(curLevel>0)
        if(curLevel==numLevels)
            %If we have reached the top level, the final assignment is just
            %choosing the most likely free tuple in the current cost
            %matrix.

            minVal=Inf;
            minIdx=1;
            for curTuple=1:numFreeTuples(curLevel)
                i=1;
                j=freeTuples(1,curTuple,curLevel);
                k=freeTuples(2,curTuple,curLevel);
                %The dimensionality of the cost matrix at the current
                %level.
                dims=[n1-curLevel+1;n2-curLevel+1;n3-curLevel+1];
                %The index within the cost matrix at the current level.
                idx=sub2ind(dims,i,j,k);
                curCost=CostMatCur(costMatCurStartIdx(curLevel)+idx-1);

                if(curCost<minVal)
                    minVal=curCost;
                    minIdx=curTuple;
                end
            end
            if(numLevels>1)
                minVal=cumAssignedCost(curLevel-1)+minVal;
            end

            if(minVal<gain)%If a new global minimum has been found.
                gain=minVal;

                assignedTupleOrigIdx(curLevel)=freeTupleOrigIdx(minIdx,curLevel);

                minCostTuples=zeros(3,numLevels);
                for i=1:numLevels
                    minCostTuples(:,i)=[i;origFreeTuples(:,assignedTupleOrigIdx(i))];
                end
            end

            increasingLevelNumber=false;
            curLevel=curLevel-1;
            continue;
        elseif(increasingLevelNumber==true)
            %We have just entered this level, so we have to first compute
            %the lower bounds for the current level.

            curTuple=1;
            while(curTuple<=numFreeTuples(curLevel))
                i=1;
                j=freeTuples(1,curTuple,curLevel);
                k=freeTuples(2,curTuple,curLevel);

                %The i,j,k indices of the tuple are with respect to indices
                %in the shrunken cost matrix CostMatCur(i,j,k,curLevel),
                %not to the set of tuples with respect to the global cost
                %matrix C. Since we are picking off rows one at a time, i
                %will always be 1.

                %The dimensionality of the cost matrix at the current
                %level.
                dims=[n1-curLevel+1;n2-curLevel+1;n3-curLevel+1];
                %The index within the cost matrix at the current level.
                idx=sub2ind(dims,i,j,k);
                curCost=CostMatCur(costMatCurStartIdx(curLevel)+idx-1);

                %If the assignment is not allowed.
                if(~isfinite(curCost))
                    curTuple=curTuple+1;
                    continue;
                end

                %Create the cost matrix at the next level up by removing
                %the appropriate entry from each dimension of the current
                %cost matrix. First, we extract the matrix for the current
                %level.
                CCur=reshape(CostMatCur(costMatCurStartIdx(curLevel):(costMatCurStartIdx(curLevel+1)-1)),[n1-curLevel+1,n2-curLevel+1,n3-curLevel+1]);
                %The indices of the cost matrix one level up.
                spanNext=costMatCurStartIdx(curLevel+1):(costMatCurStartIdx(curLevel+2)-1);
                CostMatCur(spanNext)=CCur(2:(n1-curLevel+1),[1:(j-1),(j+1):(n2-curLevel+1)],[1:(k-1),(k+1):(n3-curLevel+1)]);

                if(curLevel>1)
                    boundVals(curTuple,curLevel)=cumAssignedCost(curLevel-1)+curCost+assign3DLB(reshape(CostMatCur(spanNext),[n1-curLevel,n2-curLevel,n3-curLevel]),boundType);
                else
                    boundVals(curTuple,1)=curCost+assign3DLB(reshape(CostMatCur(spanNext),[n1-curLevel,n2-curLevel,n3-curLevel]),boundType);
                end

                %If the bound is no better than the current best solution,
                %then remove that tuple as a contender in this level. We
                %just overwrite it with the last tuple, decriment the
                %number of tuples present and then revisit the tuple with
                %the same index (which will now be the last tuple).
                if(boundVals(curTuple,curLevel)>=gain)
                    CostMatCur(costMatCurStartIdx(curLevel)+idx-1)=Inf;

                    freeTuples(:,curTuple,curLevel)=freeTuples(:,numFreeTuples(curLevel),curLevel);
                    freeTupleOrigIdx(curTuple,curLevel)=freeTupleOrigIdx(numFreeTuples(curLevel),curLevel);
                    numFreeTuples(curLevel)=numFreeTuples(curLevel)-1;
                else
                    curTuple=curTuple+1;
                end
            end

            %If there are not enough tuples left to make a full assignment.
            %The assumption here is that numLevels>1, so if we are here,
            %there must be one tuple left to assign after assigning the one
            %in this level.
            if(numFreeTuples(curLevel)==0)
                increasingLevelNumber=false;
                curLevel=curLevel-1;
                continue;
            end

            %Sort the tuples by cost.
            [boundVals(1:numFreeTuples(curLevel),curLevel),idx]=sort(boundVals(1:numFreeTuples(curLevel),curLevel),'descend');
            freeTuples(:,1:numFreeTuples(curLevel),curLevel)=freeTuples(:,idx,curLevel);
            freeTupleOrigIdx(1:numFreeTuples(curLevel),curLevel)=freeTupleOrigIdx(idx,curLevel);

            %Record the current minimum cost value.
            prevMinCost(curLevel)=gain;
        end

        %If we are here, then either increasingLevelNumber=false and we are
        %backtracking, or increasingLevelNumber=true, and we have just
        %entered this level.
        if(increasingLevelNumber==false)
            %If we are backtracking, then we remove the last tuple visited
            %in this level from consideration.
            i=1;
            j=assignedTuples(1,curLevel);
            k=assignedTuples(2,curLevel);

            %The dimensionality of the cost matrix at the current
            %level.
            dims=[n1-curLevel+1;n2-curLevel+1;n3-curLevel+1];
            %The index within the cost matrix at the current level.
            idx=sub2ind(dims,i,j,k);
            %freeTuples(:,curLevel,numFreeTuples(curLevel),curLevel)
            %contains the last assigned tuple for this level. Here, we
            %eliminate it from further consideration by setting
            %CostMatCur(i,j,k,curLevel) to Inf and reducing the count of
            %the tuple by one.
            CostMatCur(costMatCurStartIdx(curLevel)+idx-1)=Inf;
            numFreeTuples(curLevel)=numFreeTuples(curLevel)-1;

            %If the last tuple visited leaves too few tuples left for a
            %full assignment, then go up another level.
            if(numFreeTuples(curLevel)==0)
                increasingLevelNumber=false;
                curLevel=curLevel-1;
                continue;
            end

            %Also, if the minimum bound changed, remove all tuples whose
            %bounds are larger than the new minimum.
            if(prevMinCost(curLevel)~=gain)
                prevMinCost(curLevel)=gain;

                %Find all of the tuples where the bound is no better than
                %the current best solution and remove them as contenders in
                %this level.

                %Remember that when entering this level, the largest bounds
                %are at the beginning of the boundVals array, because it
                %has been sorted in decreasing order. Thus, we have to find
                %the first index where the bound is <gain.

                idx=find(boundVals(1:numFreeTuples(curLevel),curLevel)<gain,1);

                %If this gets rid of all tuples.
                if(isempty(idx))
                    curLevel=curLevel-1;
                    continue;
                elseif(idx>1)
                    %If here, remove the tuples with bounds that are too
                    %large.
                    sel1=1:(numFreeTuples(curLevel)-idx+1);
                    sel2=idx:numFreeTuples(curLevel);
                    numFreeTuples(curLevel)=numFreeTuples(curLevel)-idx+1;

                    boundVals(sel1,curLevel)=boundVals(sel2,curLevel);

                    %Set the entries in the cost matrix associated with the
                    %tuples that are being removed to Inf, so they won't be
                    %assigned.
                    for curTuple=1:(idx-1)
                        i=1;
                        j=freeTuples(1,curTuple,curLevel);
                        k=freeTuples(2,curTuple,curLevel);
                        %The dimensionality of the cost matrix at the
                        %current level.
                        dims=[n1-curLevel+1;n2-curLevel+1;n3-curLevel+1];
                        %The index within the cost matrix at the current
                        %level.
                        idx=sub2ind(dims,i,j,k);
                        CostMatCur(costMatCurStartIdx(curLevel)+idx-1)=Inf;
                    end

                    freeTuples(:,sel1,curLevel)=freeTuples(:,sel2,curLevel);
                    freeTupleOrigIdx(sel1,curLevel)=freeTupleOrigIdx(sel2,curLevel);
                end
            end

            increasingLevelNumber=true;
        end

        %The tuple having the lowest cost is visited first.
        %Assign the tuple in this current level.
        assignedTuples(:,curLevel)=freeTuples(:,numFreeTuples(curLevel),curLevel);
        assignedTupleOrigIdx(curLevel)=freeTupleOrigIdx(numFreeTuples(curLevel),curLevel);

        %Add the cost of the assigned tuple to the cumulative assigned
        %cost.
        i=1;
        j=assignedTuples(1,curLevel);
        k=assignedTuples(2,curLevel);
        %The dimensionality of the cost matrix at the current
        %level.
        dims=[n1-curLevel+1;n2-curLevel+1;n3-curLevel+1];
        %The index within the cost matrix at the current level.
        idx=sub2ind(dims,i,j,k);
        curCost=CostMatCur(costMatCurStartIdx(curLevel)+idx-1);

        if(curLevel>1)
            cumAssignedCost(curLevel)=cumAssignedCost(curLevel-1)+curCost;
        else
            cumAssignedCost(curLevel)=curCost;
        end

        %Construct the cost matrix for the next level. This means removing
        %the entire row, column, etc. in each dimension that contains the
        %assigned tuple.
        CCur=reshape(CostMatCur(costMatCurStartIdx(curLevel):(costMatCurStartIdx(curLevel+1)-1)),[n1-curLevel+1,n2-curLevel+1,n3-curLevel+1]);
        %The indices of the cost matrix one level up.
        spanNext=costMatCurStartIdx(curLevel+1):(costMatCurStartIdx(curLevel+2)-1);
        CostMatCur(spanNext)=CCur(2:(n1-curLevel+1),[1:(j-1),(j+1):(n2-curLevel+1)],[1:(k-1),(k+1):(n3-curLevel+1)]);

        %Copy the tuples that can be considered for the next and subsequent
        %levels into freeTuples(:,:,curLevel+1) and
        %freeTuplesEntering(:,:,curLevel+1).
        %This draws tuples for the next level down from
        %freeTuplesEntering(:,:,curLevel), because freeTuples(:,:,curLevel)
        %may have had tuples removed only for curLevel due to bounding. At
        %the same time as the tuples are added, the coordinates of all
        %valid tuples are adjusted to deal with the removed elements of
        %CostMatCur.

        numFreeTuples(curLevel+1)=0;
        for curTuple=1:numFreeTuplesEntering(curLevel)
            if(any(freeTuplesEntering(:,curTuple,curLevel)==assignedTuples(:,curLevel)))
                continue;
            else
                numFreeTuples(curLevel+1)=numFreeTuples(curLevel+1)+1;

                %Free tuples are indexed with respect to the shrunken
                %matrix of costs in CostMatCur(:,:,:,curLevel+1). This
                %means that for index values > the assigned index, one must
                %be subtracted.
                freeTuples(:,numFreeTuples(curLevel+1),curLevel+1)=freeTuplesEntering(:,curTuple,curLevel)-(freeTuplesEntering(:,curTuple,curLevel)>assignedTuples(:,curLevel));
                freeTupleOrigIdx(numFreeTuples(curLevel+1),curLevel+1)=freeTuplesEnteringOrigIdx(curTuple,curLevel);
            end
        end
        numFreeTuplesEntering(curLevel+1)=numFreeTuples(curLevel+1);
        freeTuplesEntering(:,1:numFreeTuples(curLevel+1),curLevel+1)=freeTuples(:,1:numFreeTuples(curLevel+1),curLevel+1);
        freeTuplesEnteringOrigIdx(1:numFreeTuples(curLevel+1),curLevel+1)=freeTupleOrigIdx(1:numFreeTuples(curLevel+1),curLevel+1);

        %Go to the next level.
        curLevel=curLevel+1;
    end
end

%Adjust the gain for the initial offset of the cost matrix.
if(maximize==true)
    gain=-gain+CDelta*n1;
else
    gain=gain+CDelta*n1;
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 [pred] = stackedAEPredict(theta, inputSize, hiddenSize, numClasses, netconfig, data)

% stackedAEPredict: Takes a trained theta and a test data set,
% and returns the predicted labels for each example.

% theta: trained weights from the autoencoder
% visibleSize: the number of input units
% hiddenSize:  the number of hidden units *at the 2nd layer*
% numClasses:  the number of categories
% data: Our matrix containing the training data as columns.  So, data(:,i) is the i-th training example.

% Your code should produce the prediction matrix
% pred, where pred(i) is argmax_c P(y(c) | x(i)).

%% Unroll theta parameter

% We first extract the part which compute the softmax gradient
softmaxTheta = reshape(theta(1:hiddenSize*numClasses), numClasses, hiddenSize);

% Extract out the "stack"
stack = params2stack(theta(hiddenSize*numClasses+1:end), netconfig);

%% ---------- YOUR CODE HERE --------------------------------------
%  Instructions: Compute pred using theta assuming that the labels start
%                from 1.

depth = numel(stack);
z = cell(depth+1,1);
a = cell(depth+1, 1);
a{1} = data;

for layer = (1:depth)
  z{layer+1} = stack{layer}.w * a{layer} + repmat(stack{layer}.b, [1, size(a{layer},2)]);
  a{layer+1} = sigmoid(z{layer+1});
end

[~, pred] = max(softmaxTheta * a{depth+1});

% -----------------------------------------------------------

end

% You might find this useful
function sigm = sigmoid(x)
    sigm = 1 ./ (1 + exp(-x));
end


function [] = mci_compare_sensitivities (model,pars)
% Compare methods for sensitivity computation
% FORMAT [] = mci_compare_sensitivities (model,pars)
%
% model     'phase', 'nmm-r2p2'
% pars      vector indicating which sensitivities to plot
%           eg. [1,2,..,Np] (default) for all parameters
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_compare_sensitivities.m 6548 2015-09-11 12:39:47Z will $

[P,M,U,Y] = mci_compare_setup (model);

if nargin < 2 | isempty(pars)
    pars=[1:M.Np];
end
Np=length(pars);

disp(' ');
disp('Plot sensitivity to parameter changes as computed by:');
disp('(1) Forward equations based on Matlab''s ODE suite (red)');
disp('(2) Forward equations based on Sundials (blue)');

[G,sy_m,st] = spm_mci_sens (P,M,U);

[G,sy,st] = spm_mci_sens_sun (P,M,U);

if st==-1
    disp('Problem with integration');
    return
end

mci_plot_outputs(M,G);

hs=figure;
set(hs,'Name','Sensitivities');
k=1; lw=2;
for i=1:M.l,
    for p=1:Np,
        j=pars(p);
        subplot(M.l,Np,k);
        plot(M.t,squeeze(sy(:,i,j)),'LineWidth',lw);
        hold on
        plot(M.t,squeeze(sy_m(:,i,j)),'r','LineWidth',lw);
        grid on
        title (sprintf('dy(%d)/dp(%d)',i,j));
        xlabel('Time');
        k=k+1;
    end
end


function [ err ] = residual_KR( X1, X2, K1, K2, R)

% homopraphy matrix
H = K2*R/K1;
n = size(X1,2);

x1 = X1(1,:); y1 = X1(2,:);
x2 = X2(1,:); y2 = X2(2,:);

w = H(3,1)*x1 + H(3,2)*y1 + H(3,3);
x1_2 = (H(1,1)*x1 + H(1,2)*y1 + H(1,3)) ./ w;
y1_2 = (H(2,1)*x1 + H(2,2)*y1 + H(2,3)) ./ w;
err1_2 = [x1_2 - x2; y1_2 - y2];

err = reshape(err1_2,[2*n,1]);

end


function y = cumGamma(x, a, b)

% CUMGAMMA Cumulative distribution for gamma.
% FORMAT
% DESC computes the cumulative gamma distribution.
% ARG x : input value.
% RETURN p : output probability.
%
% SEEALSO :  gammainc, gamma
%
% COPYRIGHT : Neil D. Lawrence, 2008

% NDLUTIL

y = gammainc(x*b, a);


function varargout = interp1(varargin)
% bsarray/interp1: 1-D interpolation (table lookup)
% usage: YI = interp1(B,XI);
%    or: YI = interp1(B,XI,EXTRAPVAL);
%
% arguments:
%   B - bsarray object having tensorOrder 1. The positions of the
%       underlying data array are assumed to be X = s.*(1:N), where N is
%       the number of data points (i.e., max(get(B,'dataSize'))), and s
%       is the element spacing (i.e., get(B,'elementSpacing')).
%   XI - points at which to interpolate B.
%   EXTRAPVAL - value to return for points in XI that are outside the range
%       of X. Default EXTRAPVAL = NaN.
%
%   YI - the values of the underlying bsarray B evaluated at the points in
%       the array XI.
%

% author: Nathan D. Cahill
% email: ndcahill@gmail.com
% date: 18 April 2008

% parse input arguments
[b,xi,extrapval] = parseInputs(varargin{:});

% get flag to determine if basis functions are centred or shifted
m = double(get(b,'centred'));

% get number of data elements and coefficients, determine amount of padding
% that has been done to create coefficients
nData = max(get(b,'dataSize'));
n = max(get(b,'coeffsSize'));
padNum = (n-nData-1)/2;

% get the spacing between elements, and then construct vectors of the
% locations of the data and of the BSpline coefficients
h = get(b,'elementSpacing');
xCol = h.*((1-padNum):(nData+padNum+1))';
xDataCol = xCol((1+padNum):(end-padNum));

% turn evaluation points into a column vector, but retain original size so
% output can be returned in same size as input
siz_xi = size(xi);
xiCol = xi(:);
siz_yi = siz_xi;

% grab the BSpline coefficients
cMat = get(b,'coeffs');

% initialize some variables for use in interpolation
numelXi = length(xiCol);
yiMat = zeros(numelXi,1);
p = 1:numelXi;

% Find indices of subintervals, x(k) <= u < x(k+1),
% or u < x(1) or u >= x(m-1).
k = min(max(1+floor((xiCol-xCol(1))/h),1+padNum),n-padNum) + 1-m;
s = (xiCol - xCol(k))/h;

% perform interpolation
d = get(b,'degree');
for i=1:ceil((d+1)/2)
    yiMat(:) = yiMat(:) + cMat(k-i+m).*evalBSpline(s+i-(1+m)/2,d);
    yiMat(:) = yiMat(:) + cMat(k+i-1+m).*evalBSpline(s-i+(1-m)/2,d);
end
if ~m && mod(d,2)
    yiMat(:) = yiMat(:) + cMat(k-i-1).*evalBSpline(s+i+1/2,d);
end

% perform extrapolation
outOfBounds = xiCol<xDataCol(1) | xiCol>xDataCol(nData);
yiMat(p(outOfBounds)) = extrapval;

% reshape result to have same size as input xi
yi = reshape(yiMat,siz_yi);
varargout{1} = yi;

%% subfunction parseInputs
function [b,xi,extrapval] = parseInputs(varargin)

nargs = length(varargin);
error(nargchk(2,3,nargs));

% Process B
b = varargin{1};
if ~isequal(b.tensorOrder,1)
    error([mfilename,'parseInputs:WrongOrder'], ...
        'bsarray/interp1 can only be used with bsarray objects having tensor order 1.');
end

% Process XI
xi = varargin{2};
if ~isreal(xi)
    error([mfilename,'parseInputs:ComplexInterpPts'], ...
        'The interpolation points XI should be real.')
end

% Process EXTRAPVAL
if nargs > 2
    extrapval = varargin{3};
else
    extrapval = [];
end
if isempty(extrapval)
    extrapval = NaN;
end
if ~isscalar(extrapval)
    error([mfilename,':NonScalarExtrapValue'],...
        'EXTRAP option must be a scalar.')
end


function Cb2n = ch_roty(theta)
% 3D初等旋转， theta为旋转角度，rad
Cb2n = [cos(theta), 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];

end


function r = loggmpdf(X, model)
% Compute log pdf of a Gaussian mixture model.
% Written by Mo Chen (sth4nth@gmail.com).
mu = model.mu;
Sigma = model.Sigma;
w = model.weight;

n = size(X,2);
k = size(mu,2);
logRho = zeros(k,n);

for i = 1:k
    logRho(i,:) = loggausspdf(X,mu(:,i),Sigma(:,:,i));
end
r = logsumexp(bsxfun(@plus,logRho,log(w)'),1);

function y = loggausspdf(X, mu, Sigma)
d = size(X,1);
X = bsxfun(@minus,X,mu);
[U,p]= chol(Sigma);
if p ~= 0
    error('ERROR: Sigma is not PD.');
end
Q = U'\X;
q = dot(Q,Q,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(U)));   % normalization constant
y = -(c+q)/2;


function [b bbar sigeps]=panel3prior(Xibar,Xbar,yi,y,N,q)

% first obtain b, the mean group estimator
% estimate the first term
term1=sparse(q,q);
for ii=1:N
term1=term1+Xibar{ii,1}'*Xibar{ii,1};
end

% estimate the second term
term2=sparse(q,1);
for ii=1:N
term2=term2+Xibar{ii,1}'*yi(:,:,ii);
end

% obtain b
b=term1\term2;

% obtain bbar
bbar=repmat(b,N,1);

% obtain sigma_epsilon, the common residual variance term
eps=y-Xbar*bbar;
sigeps=var(eps);


function [x, y] = circle(senter, radius, n_points)

	%Funksjon brukes til å lage sirkler ved å bruke polarkoordinater

    s = senter;
    r = radius;
    n = n_points;

    theta = linspace(0,2*pi, n);
    rho = ones(1, n)*r;
    [x, y] = pol2cart(theta, rho);
    x = x + s(1);
    y = y + s(2);

end


function hypercube_grid_test03 ( )

%*****************************************************************************80
%
%% HYPERCUBE_GRID_TEST03 tests HYPERCUBE_GRID on a three dimensional example.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 August 2014
%
%  Author:
%
%    John Burkardt
%
  m = 3;

  a = [ -1.0, -1.0, -1.0 ];
  b = [ +1.0, +1.0, +1.0 ];
  c = [ 1, 1, 1 ];
  ns = [ 3, 3, 3 ];

  n = prod ( ns(1:m) );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERCUBE_GRID_TEST03\n' );
  fprintf ( 1, '  Create a grid using HYPERCUBE_GRID.\n' );
  fprintf ( 1, '  Use the same parameters in every dimension.\n' );
  fprintf ( 1, '  Spatial dimension M = %d\n', m );
  fprintf ( 1, '  Number of grid points N = %d\n', n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    NS     C      A         B\n' );
  fprintf ( 1, '\n' );
  for i = 1 : m
    fprintf ( 1, '  %4d  %4d  %4d  %8.4f  %8.4f\n', i, ns(i), c(i), a(i), b(i) );
  end

  x = hypercube_grid ( m, n, ns, a, b, c );
  r8mat_transpose_print ( m, n, x, '  Grid points:' );

  return
end


function W=calcKalmanGain(R,PzPred,otherInfo)
%%CALCKALMANGAIN Using the output of the Kalman filter measurement
%           prediction function KalmanMeasPred and the covariance
%           matrix of a measurement R, compute the gain of a Kalman filter.
%           This function can be useful for creating a gain based on some
%           type of "maximum" covariance matrix for a scene for purposes of
%           giving it to the calcMissedGateCov function to determine how
%           the covariance matrix of the target state prediction should be
%           increased for a missed-detection event when gating is
%           performed.
%
%INPUTS: R The zDimXzDim measurement covariance matrix.
%   PzPred The zDimXzDim covariance matrix of the measurement predicted by
%          the filter.
% otherInfo The structure returned by the KalmanMeasPred function that
%          includes various terms that can be reused.
%
%OUTPUTS: W The xDimXzDim gain matrix used in the filter.
%
%This function produces the gain in the same manner as is done in
%KalmanUpdateWithPred and KalmanUpdate. See the comments to those functions
%for more information.
%
%EXAMPLE:
%Here, we demonstrate that the gain used in a complete update is the same
%as that obtained using just the prediction via KalmanMeasPred and then
%calcKalmanGain for the gain without using the measurement z itself.
% xPred=[1e3;-2e3;100;200];
% PPred=[28,   3.5,    6,  8.5;
%      3.5,    23,  8.5,   11;
%        6,   8.5,   18, 13.5;
%      8.5,    11, 13.5,   13];
% z=1e3*[-5.498856156296510;
%        1.199241491470584];
% R=eye(2);
% H=[0, 4, 9, 8;
%    6, 3, 0, 6];
% [~,~,~,~,W]=KalmanUpdate(xPred,PPred,z,R,H);
% [~,PzPred,otherInfo]=KalmanMeasPred(xPred,PPred,H);
% W1=calcKalmanGain(R,PzPred,otherInfo);
% %One will see that the result below is true (1).
% all(W1(:)==W(:))
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

Pxz=otherInfo.Pxz;

Pzz=PzPred+R;
W=Pxz/Pzz;

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 [pnt, tri] = mesh_cube()

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

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

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

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


function [Rmean, Pmean, frq, Pmean2] = NormedSingleCurveLengthWindowed(x,winlen,shiftlen,lag,fs,nrmdegree)

beVerbose = false;

% shiftlen is winlen - overlaplen

M = floor((length(x) - winlen) /  shiftlen ) + 1;

Psum = zeros(1,2*lag+1);
Rsum = zeros(1,lag+1);

for ii = 1:M

    xseg = x((ii-1)*shiftlen+1:(ii-1)*shiftlen+winlen);

    [rx,px] = NormedSingleCurveLength(xseg,lag,fs,0,nrmdegree);

    Psum = Psum + px;

    Rsum = Rsum + rx;

    if beVerbose
        display(ii);
    end
end

Pmean = Psum ./ M;
Rmean = Rsum ./ M;

Pmean2 = abs(fft([fliplr(Rmean) Rmean(2:end)]));

frq = linspace(0,0.5*fs,lag+1);

end


function [dx, dy] = GradientSobel(img)

        Sv = [-1 -2 -1;...
               0  0  0;...
               1  2  1];
        Sh = [-1  0  1;
              -2  0  2;
              -1  0  1];
        dx=conv2(img, Sh,'valid');
        dy=conv2(img, Sv,'valid');
end


%SF_TET_P5 Fifth order Lagrange shape functions for tetrahedra (P5).
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_TET_P5( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates conforming fifth order P5 Lagrange shape functions on 3D tetrahedral elements
%   with values defined in the nodes, edges, faces, and cell center. XI are Barycentric coordinates.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar: 3              Number of space dimensions
%       n_vert      scalar: 4              Number of vertices per cell
%       i_dof       scalar: 1-56           Local basis function to evaluate
%       xi          array [4,1]            Local coordinates of evaluation point
%       aInvJac     [n,12]                 Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [4]                    Number of local degrees of freedom on
%                                          vertices, edges, faces, and cell interiors
%       xLDof       [4,n_ldof]             Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SF_TET_P1

% Copyright 2013-2022 Precise Simulation, Ltd.


function [OutImg,OutImgIdx] = PCA_output(InImg, InImgIdx, PatchSize, NumFilters, V)

ImgZ = length(InImg);
mag = (PatchSize-1)/2;
OutImg = cell(NumFilters*ImgZ,1);
cnt = 0;
for i = 1:ImgZ
    [ImgX, ImgY, NumChls] = size(InImg{i});
    img = zeros(ImgX+PatchSize-1,ImgY+PatchSize-1, NumChls);
    img((mag+1):end-mag,(mag+1):end-mag,:) = InImg{i};
    im = im2col_mean_removal(img,[PatchSize PatchSize]); % collect all the patches of the ith image in a matrix, and perform patch mean removal
    for j = 1:NumFilters
        cnt = cnt + 1;
        OutImg{cnt} = reshape(V(:,j)'*im,ImgX,ImgY);  % convolution output
    end
    InImg{i} = [];
end
OutImgIdx = kron(InImgIdx,ones(NumFilters,1));


function [logRS,logERS,V]=RSana(x,n,method,q)
%Syntax: [logRS,logERS,V]=RSana(x,n,method,q)
%____________________________________________
%
% Performs R/S analysis on a time series.
%
% logRS is the log(R/S).
% logERS is the Expectation of log(R/S).
% V is the V statistic.
% x is the time series.
% n is the vector with the sub-periods.
% method can take one of the following values
%  'Hurst' for the Hurst-Mandelbrot variation.
%  'Lo' for the Lo variation.
%  'MW' for the Moody-Wu variation.
%  'Parzen' for the Parzen variation.
% q can be either
%  a (non-negative) integer.
%  'auto' for the Lo's suggested value.
%
%
% References:
%
% Peters E (1991): Chaos and Order in the Capital Markets. Willey
%
% Peters E (1996): Fractal Market Analysis. Wiley
%
% Lo A (1991): Long term memory in stock market prices. Econometrica
% 59: 1279-1313
%
% Moody J, Wu L (1996): Improved estimates for Rescaled Range and Hurst
% exponents. Neural Networks in Financial Engineering, eds. Refenes A-P
% Abu-Mustafa Y, Moody J, Weigend A: 537-553, Word Scientific
%
% Hauser M (1997): Semiparametric and nonparametric testing for long
% memory: A Monte Carlo study. Empirical Economics 22: 247-271
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 1 Jan 2004.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % N is the time series length
   N=length(x);
end

if nargin<2 | isempty(n)==1
   n=1;
else
   % n must be either a scalar or a vector
   if min(size(n))>1
      error('n must be either a scalar or a vector.');
   end
   % n must be integer
   if n-round(n)~=0
       error('n must be integer.');
   end
   % n must be positive
   if n<=0
      error('n must be positive.');
   end
end

if nargin<4 | isempty(q)==1
   q=0;
else
    if q=='auto'
        t=autocorr(x,1);
        t=t(2);
        q=((3*N/2)^(1/3))*(2*t/(1-t^2))^(2/3);
    else
        % q must be a scalar
        if sum(size(q))>2
            error('q must be scalar.');
        end
        % q must be integer
        if q-round(q)~=0
            error('q must be integer.');
        end
        % q must be positive
        if q<0
            error('q must be positive.');
        end
    end
end

for i=1:length(n)

    % Calculate the sub-periods
    a=floor(N/n(i));

    % Make the sub-periods matrix
    X=reshape(x(1:a*n(i)),n(i),a);

    % Estimate the mean of each sub-period
    ave=mean(X);

    % Remove the mean from each sub-period
    cumdev=X-ones(n(i),1)*ave;

    % Estimate the cumulative deviation from the mean
    cumdev=cumsum(cumdev);

    % Estimate the standard deviation
    switch method
    case 'Hurst'
        % Hurst-Mandelbrot variation
        stdev=std(X);
    case 'Lo'
        % Lo variation
        for j=1:a
            sq=0;
            for k=0:q
                v(k+1)=sum(X(k+1:n(i),j)'*X(1:n(i)-k,j))/(n(i)-1);
                if k>0
                    sq=sq+(1-k/(q+1))*v(k+1);
                end
            end
            stdev(j)=sqrt(v(1)+2*sq);
        end
    case 'MW'
        % Moody-Wu variation
        for j=1:a
            sq1=0;
            sq2=0;
            for k=0:q
                v(k+1)=sum(X(k+1:n(i),j)'*X(1:n(i)-k,j))/(n(i)-1);
                if k>0
                    sq1=sq1+(1-k/(q+1))*(n(i)-k)/n(i)/n(i);
                    sq2=sq2+(1-k/(q+1))*v(k+1);
                end
            end
            stdev(j)=sqrt((1+2*sq1)*v(1)+2*sq2);
        end
    case 'Parzen'
        % Parzen variation
        if mod(q,2)~=0
            error('For the "Parzen" variation q must be dived by 2.');
        end
        for j=1:a
            sq1=0;
            sq2=0;
            for k=0:q
                v(k+1)=sum(X(k+1:n(i),j)'*X(1:n(i)-k,j))/(n(i)-1);
                if k>0 & k<=q/2
                    sq1=sq1+(1-6*(k/q)^2+6*(k/q)^3)*v(k+1);
                elseif k>0 & k>q/2
                    sq2=sq2+(1-(k/q)^3)*v(k+1);
                end
            end
            stdev(j)=sqrt(v(1)+2*sq1+2*sq2);
        end
    otherwise
        error('You should provide another value for "method".');
    end

    % Estiamte the rescaled range
    rs=(max(cumdev)-min(cumdev))./stdev;

    clear stdev

    % Take the logarithm of the mean R/S
    logRS(i,1)=log10(mean(rs));

    if nargout>1

        % Initial calculations fro the log(E(R/S))
        j=1:n(i)-1;
        s=sqrt((n(i)-j)./j);
        s=sum(s);

        % The estimation of log(E(R/S))
        logERS(i,1)=log10(s/sqrt(n(i)*pi/2));

        % Other estimations of log(E(R/S))
        %logERS(i,1)=log10((n(i)-0.5)/n(i)*s/sqrt(n(i)*pi/2));
        %logERS(i,1)=log10(sqrt(n(i)*pi/2));

    end

    if nargout>2
        % Estimate V
        V(i,1)=mean(rs)/sqrt(n(i));
    end

end


function [mi2] = yd22mi2(yd2)
% Convert area from square yards to square miles.
% Chad A. Greene 2012
mi2 = yd2*3.228305785124E-7;


clear all; close all;
I=imread('onion.png');
I=rgb2gray(I);
I=im2double(I);
LEN=25;
THETA=20;
PSF=fspecial('motion', LEN, THETA);
J=imfilter(I, PSF, 'conv', 'circular');
NSR=0;
K=deconvwnr(J, PSF, NSR);
figure;
subplot(131);  imshow(I);
subplot(132);  imshow(J);
subplot(133);  imshow(K);


function pass = test_ivp(varargin)

% This test solves the Van der Pol ODE in CHEBFUN using ode15s and ode45. It
% checks the solution with Matlab's inbuilt ode15s and ode45 solvers.

% Rodrigo Platte Jan 2009, modified by Asgeir Birkisson, modified by Nick Hale.

%% Test using Van der Pol:

% Test ode15s Using default tolerances (RelTol = 1e-3)
[t, y] = chebfun.ode15s(@vdp1, [0, 5], [2 ; 0]); % CHEBFUN solution
[tm, ym] = ode15s(@vdp1, [0, 5], [2 ; 0]);       % Matlab's solution
pass(1) = max(max(abs(ym - feval(y, tm)))) < 2e-2;

% Test ode45 Using default tolerances (RelTol = 1e-3)
[t, y] = chebfun.ode45(@vdp1, [0, 5], [2 ; 0]);  % CHEBFUN solution
[tm, ym] = ode45(@vdp1, [0, 5], [2 ; 0]);        % Matlab's solution
pass(2) = max(max(abs(ym - feval(y, tm)))) < 1e-2;

% Test ode113 with different tolerance (RelTol = 1e-6)
opts = odeset('RelTol', 1e-6);
[t, y] = chebfun.ode113(@vdp1, [0, 20], [2 ; 0], opts); % CHEBFUN solution
[tm, ym] = ode113(@vdp1, [0, 20], [2 ; 0], opts);       % Matlab's solution
pass(3) = max(max(abs(ym - feval(y,tm)))) < 1e-5;

%% Test some trivial complex-valued IVPs:

f = @(x, u) 1i*u;
d = [0, 1];
soln = exp(1i);

% Test ode15s:
y = chebfun.ode15s(f, d, 1);
pass(4) = abs(y(1) - soln) < 2e-2;

% Test ode45:
y = chebfun.ode45(f, d, 1);
pass(5) = abs(y(1) - soln) < 2e-2;

% Test ode113:
y = chebfun.ode113(f, d, 1);
pass(6) = abs(y(1) - soln) < 2e-2;

end


function [structdis,iMinusMu,mu,sigma]= divisiveNormalization(imdist)

    window = fspecial('gaussian',7,7/6);
    window = window/sum(sum(window));

    mu = filter2(window, imdist, 'same');
    mu_sq = mu.*mu;

    sigma = sqrt(abs(filter2(window, imdist.*imdist, 'same') - mu_sq));
    iMinusMu = (imdist-mu);
    structdis =iMinusMu./(sigma +1);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 Launch code for sinusoidal input                 %
%       code by Fabrizio Conso, university of pavia, student       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                launch code example                    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                       %
 format short;
 f_s=1;            % normalized sampling frequency
 f_in=0.4;         % normalized signal frequency
 f_bw=5;                    % DAC's bandwidth
 sr=2;                      % DAC's slew-rate
 nbit=12;                   % converter bits
 N=2^12;                    % fft points
 t=[1:(1/f_s):N*(1/f_s)];   % discrete time array
 bw=0.5;
 fin=antismear(f_in,f_s,N);
 for i=1:N         % cycle for collect ADC outputs
     input=0.5+0.5*sin(2*pi*fin*t(i));
     counter2=Approx_2(input,nbit,f_bw,sr,f_s);
     counter=Approx(input,nbit,f_bw,sr,f_s);
     y(i)=counter;
     z(i)=counter2;
 end

 z=z-mean(z);                % eliminating DC tone
 y=y-mean(y);                % eliminating DC tone
 w=gausswin(N);              % window
 f=fin/f_s;                  % Normalized signal frequency
 fB=N*(bw/f_s);              % Base-band frequency bins
 [snr2,ptot2]=calcSNR(z(1:N),f,fB,w',N);
 ptot2=ptot2-max(ptot2);
 [snr,ptot]=calcSNR(y(1:N),f,fB,w',N);
 ptot=ptot-max(ptot);

figure(1);
clf;
plot(linspace(0,f_s/2,N/2), ptot(1:N/2), 'r');
grid on;
title('Output PSD without correction')
xlabel('Frequency [Hz]')
ylabel('PSD [dB]')
axis([0.3 f_s/2 -120 0]);

figure(2);
clf;
plot(linspace(0,f_s/2,N/2), ptot2(1:N/2), 'r');
grid on;
title('Output PSD with correction')
xlabel('Frequency [Hz]')
ylabel('PSD [dB]')
axis([0.3 f_s/2 -120 0]);
%                                                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% sgwt_kernel_abspline3 : Monic polynomial / cubic spline / power law decay kernel
%
% function r = sgwt_kernel_abspline3(x,alpha,beta,t1,t2)
%
% defines function g(x) with g(x) = c1*x^alpha for 0<x<x1
% g(x) = c3/x^beta for x>t2
% cubic spline for t1<x<t2,
% Satisfying g(t1)=g(t2)=1
%
% Inputs :
% x : array of independent variable values
% alpha : exponent for region near origin
% beta : exponent decay
% t1, t2 : determine transition region
%
% Outputs :
% r - result (same size as x)

% This file is part of the SGWT toolbox (Spectral Graph Wavelet Transform toolbox)
% Copyright (C) 2010, David K. Hammond.
%
% The SGWT toolbox 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.
%
% The SGWT toolbox 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 the SGWT toolbox.  If not, see <http://www.gnu.org/licenses/>.

function r = sgwt_kernel_abspline3(x,alpha,beta,t1,t2)
  r=zeros(size(x));
  % compute spline coefficients
  % M a = v
  M=[[1 t1 t1^2 t1^3];...
     [1 t2 t2^2 t2^3];...
     [0 1 2*t1 3*t1^2];...
     [0 1 2*t2 3*t2^2]];
  %v=[t1^alpha ; t2^(-beta) ; alpha*t1^(alpha-1) ; -beta*t2^(-beta-1)];
  v=[1 ; 1 ; t1^(-alpha)*alpha*t1^(alpha-1) ; -beta*t2^(-beta-1)*t2^beta];
  a=M\v;

  r1=find(x>=0 & x<t1);
  r2=find(x>=t1 & x<t2);
  r3=find(x>=t2);
  r(r1)=x(r1).^alpha*t1^(-alpha);
  r(r3)=x(r3).^(-beta)*t2^(beta);

  x2=x(r2);
  r(r2)=a(1)+a(2)*x2+a(3)*x2.^2+a(4)*x2.^3;
%  tmp=polyval(flipud(a),x2);
%  keyboard


function sig_out = wsola_time_scaling(sig_in, fs, scale_factor, simil_method)
% sig_in - input signal
% fs - sampling frequency
% scale_factor - by what ratio to strech/compress the audio.
% simil_method - optional parameter either 'xcorr' or 'amdf' (default)
%
% from the paper by Werner VERHELST and Marc ROELANDS
% found here:
% http://www.etro.vub.ac.be/research/dssp/PUB_FILES/int_conf/ICASSP-1993.pdf

if nargin<4
	simil_method = 'amdf';
end

sf_split_margin = 3;
orig_len = length(sig_in);
% recursively treat scale factors too big or too small
if (scale_factor > sf_split_margin) || (scale_factor < 1/sf_split_margin)
    num_recur = ceil(abs((log(scale_factor)./log(sf_split_margin))));

    scale_factor_recur = scale_factor.^((num_recur-1)/num_recur);

    sig_in = wsola_time_scaling(sig_in, fs, scale_factor_recur, simil_method);

    scale_factor = scale_factor./(length(sig_in)./orig_len);
end

% consts
win_time = 0.020; %sec
overlap_ratio = 0.5;
max_err = min(0.005, win_time*overlap_ratio./scale_factor/2);

% lengths
win_len = ceil(win_time.*fs);
max_err_len = ceil(max_err.*fs);
step_len = floor(overlap_ratio.*win_len);

% vectors
win = hann(win_len);
% orig_scale = 1:length(sig_in);
new_scale = (1:round((length(sig_in).*scale_factor*2)))';
sig_out = zeros(size(new_scale));
win_out = zeros(size(new_scale));

cursor_in = 1;
cursor_out = 1;

while cursor_in<(length(sig_in)-win_len-max(step_len, step_len./scale_factor) - 2*max_err_len)...
                                    && cursor_out<(length(sig_out)-win_len)
    % input segments
    new_seg = sig_in(cursor_in:(cursor_in+win_len-1)).*win;
    new_seg_neihbour = sig_in((cursor_in+step_len):(cursor_in+step_len+win_len-1)).*win;
    % overlap add
    sig_out(cursor_out:(cursor_out+win_len-1)) = ...
            sig_out(cursor_out:(cursor_out+win_len-1))+...
            new_seg;
    % overlap add window normalization vec
    win_out(cursor_out:(cursor_out+win_len-1)) = ...
            win_out(cursor_out:(cursor_out+win_len-1))+...
            win;
    % move cursors
    cursor_out = cursor_out + step_len;
    cursor_in = cursor_in + round(step_len./scale_factor);
    % new candidate
    new_seg_cand = sig_in((cursor_in):(cursor_in+win_len-1)).*win;
    % similarity calc. Note: matlab apears to be using FFT for xcorr, and
    % so computes the whole thing (instead of just the center).
    % Writing own version of xcorr would probably make it faster.
    if strcmp(simil_method, 'xcorr')
        shift = max_xcorr_similarity(new_seg_neihbour, new_seg_cand, max_err_len);
    elseif strcmp(simil_method, 'amdf')
        shift = min_amdf_similarity(new_seg_neihbour, new_seg_cand, max_err_len);
    else
        return
    end
    % adjust cursor place
    cursor_in = cursor_in - shift;
end

% remove slack
sig_out(cursor_out:end) = [];
win_out(cursor_out:end) = [];

sig_out = sig_out./(win_out+eps); %normalize to remove possible modulations

end

    % cross correlation based similarity calculation
    function shift = max_xcorr_similarity(seg1, seg2, max_lag)
        [~, max_i] = max(xcorr(seg1, seg2, max_lag, 'unbiased'));
        shift = max_i - max_lag;
    end

    % amdf based similarity calculation
    function shift = min_amdf_similarity(seg1, seg2, max_lag)
        n = length(seg1);
        amdf = ones(1,2*max_lag-1);
        for lag=-max_lag:max_lag
            amdf(lag+max_lag+1) = sum(abs(seg2(max(1,(-lag+1)):min(n,(n-lag)))-...
                           seg1(max(1,(lag+1)):min(n,(n+lag))) ))/n;
        end
        [~, min_i] = min(amdf);
        shift = min_i - max_lag;
    end


function [bhat sigmahatb sigmahat]=panel1estimates(X,Y,N,n,q,k,T)

% obtain an estimate of the VAR coefficients
% initiate the betahat vector
betahat=[];
% initiate the sum of the beta_i vectors and sigma_i matrices
betasum=zeros(q,1);
sigmasum=zeros(n,n);
% loop over units
for ii=1:N
% obtain Yi and Xi
Xi=X(:,:,ii);
Yi=Y(:,:,ii);
% estimate the VAR coefficients for this unit
Bhati=(Xi'*Xi)\(Xi'*Yi);
betahat(:,:,ii)=Bhati(:);
% estimate the residuals for this unit
EPShati=Yi-Xi*Bhati;
EPShat(:,:,ii)=EPShati;
% estimate the variance-covariance matrix for this unit and add to summation
sigmahati=(1/(T-k-1))*EPShati'*EPShati;
sigmasum=sigmasum+sigmahati;
% add the betahat value to the sum
betasum=betasum+betahat(:,:,ii);
end

% estimate bhat and sigmahat
bhat=(1/N)*betasum;
sigmahat=(1/N)*sigmasum;

% eventually estimate sigmab, the variance covariance matrix of the bhat vector of coefficients
sigmahatb=zeros(q,q);
for ii=1:N
sigmahatb=(betahat(:,1,ii)-bhat)*(betahat(:,1,ii)-bhat)';
end
sigmahatb=(1/(N*(N-1)))*sigmahatb;


function wiener_iter(ns_file, es_file, iter_num)

%
%  Implements the basic iterative Wiener filtering algorithm [1].
%
%  Usage:  wiener_iter(noisyFile, outputFile,NumberOfIterations)
%
%         infile - noisy speech file in .wav format
%         outputFile - enhanced output file in .wav format
%         NumberOfIterations - number of iterations (recommended 2-4)
%
%  Example call:  wiener_iter('sp04_babble_sn10.wav','out_wien.wav',3);
%
%  References:
%   [1] Lim, J. and Oppenheim, A. V. (1978). All-pole modeling of degraded speech.
%       IEEE Trans. Acoust. , Speech, Signal Proc., ASSP-26(3), 197-210.
%
% Authors: Yi Hu and Philipos C. Loizou
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%-------------------------------------------------------------------------

if nargin<3
   fprintf('Usage: wiener_iter(noisyfile.wav,outFile.wav,NumIter) \n\n');
   return;
end

NF_SABSENT= 6;
%this is the number of speech-absent frames to estimate the initial
%noise power spectrum

[nsdata, Fs, bits]= wavread( ns_file);	%nsdata is a column vector

nwind= floor( 20* Fs/ 1000);	%this corresponds to 20ms window
if rem( nwind, 2)~= 0 nwind= nwind+ 1; end	%made window length even
noverlap= nwind/ 2;	w= hanning( nwind);	rowindex= ( 1: nwind)';
pred_order= 12;	%LPC order is set to 12
FFTlen=2*nwind;

%we assume the first NF_SABSENT frames are speech absent, we use them to estimate the noise power spectrum
noisedata= nsdata( 1: nwind* NF_SABSENT);	noise_colindex= 1+ ( 0: NF_SABSENT- 1)* nwind;
noisematrixdata = zeros( nwind, NF_SABSENT);
noisematrixdata( :)= noisedata( ...
   rowindex( :, ones(1, NF_SABSENT))+ noise_colindex( ones( nwind, 1), :)- 1);
noisematrixdata= noisematrixdata.* w( :, ones( 1, NF_SABSENT)) ;	%WINDOWING NOISE DATA
noise_r0= sum( sum( noisematrixdata.^ 2)/ nwind)/ NF_SABSENT;	%noise energy
noise_ps= mean( (abs( fft( noisematrixdata,FFTlen))).^ 2, 2);
%NOTE!!!! it is a column vector

nslide= nwind- noverlap;

x= nsdata( nwind* NF_SABSENT+ 1: end);	%x is to-be-enhanced noisy speech
nx= length( x);	ncol= fix(( nx- noverlap)/ nslide);
colindex = 1 + (0: (ncol- 1))* nslide;
if nx< (nwind + colindex(ncol) - 1)
   x(nx+ 1: nwind+ colindex(ncol) - 1) = ...
      rand( nwind+ colindex( ncol)- 1- nx, 1)* (2^ (-15));   % zero-padding
end

es_old= zeros( noverlap, 1);
%es_old is actually the second half of the previous enhanced speech frame,
%it is used for overlap-add

img=sqrt(-1);
for k= 1: ncol

   y= x( colindex( k): colindex( k)+ nwind- 1);
   y= y.* w;	%WINDOWING NOISY SPEECH DATA

   y_spec= fft(y,FFTlen);	y_specmag= abs( y_spec);	y_specang= angle( y_spec);
   %they are the frequency spectrum, spectrum magnitude and spectrum phase, respectively

   y_ps= y_specmag.^ 2;	%power spectrum of noisy speech

   %we must have a set of initial LPC coefficients to use for iterative wiener algorithm,
   %we get it from original noisy speech file

   lpc_coeffs= (lpc(y, pred_order))';
   %now we get initial lpc coefficients of all current speech frame

   for m= 1: pred_order+ 1
      %exp_matrix(m, :)= exp(-i* (m- 1)* ((1: nwind)- 1)* 2* pi/ nwind);
      exp_matrix(m, :)= exp(-img* (m- 1)* ((1: FFTlen)- 1)* 2* pi/FFTlen);
   end

   x_old_spec=y_spec;
   for n=1:iter_num

      xx= 1./ (abs( exp_matrix'* lpc_coeffs).^ 2);

      lpc_energy= mean( xx);

      tmp= y_ps- noise_ps;

      g= max( mean( tmp)./ lpc_energy, 1e-16);

      tmp1= g.* xx;
      h_spec= tmp1./ (tmp1+ noise_ps);  % Wiener filter

      es_tmpspec= x_old_spec.* h_spec;
      es_tmp= real( ifft( es_tmpspec,FFTlen));

      x_old_spec = fft(es_tmp, FFTlen);
      % ----
      if n~= iter_num
         lpc_coeffs= lpc( es_tmp, pred_order)';
      end
   end

   es_data( colindex( k): colindex( k)+ nwind- 1)= [es_tmp( 1: noverlap)+ es_old;...
         es_tmp( noverlap+ 1: nwind)];
   %overlap-add
   es_old= es_tmp( nwind- noverlap+ 1: nwind);
end

wavwrite( es_data, Fs, bits, es_file);


function[z1,z2,z3] = primtoriem(u1,u2,u3)
	% Transformation from primitive to Riemann invariants
global gamma
u = u2./u1; 			       c2 = gamma*(gamma-1)*(u3./u1 - 0.5*u.^2);
z2 = log((1/gamma)*c2.*u1.^(1-gamma)); c2 = sqrt(c2);
z1 = u - 2*c2/(gamma-1);	       z3 = u + 2*c2/(gamma-1);


function value = p05_f ( n, x )

%*****************************************************************************80
%
%% P05_F evaluates the function for problem p05.
%
%  Discussion:
%
%    Step function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), the evaluation points.
%
%    Output, real VALUE(N,1), the function values.
%
  value =       cos (  7.0 * x ) ...
        + 5.0 * cos ( 11.2 * x ) ...
        - 2.0 * cos ( 14.0 * x ) ...
        + 5.0 * cos ( 31.5 * x ) ...
        + 7.0 * cos ( 63.0 * x );

  return
end


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

%%
clear; close all; clc;

%% Syntax
% |M=checkerBoard3D(siz);|

%% Description
% This function creates a checkboard image of the size siz whereby elements
% are either black (0) or white (1). The first element is white.

%% Examples

%%
% Plot settings
fontSize=15;

%% Example creating a 2D checkerboard image

siz=[4 6]; %Image size
M=checkerBoard3D(siz); %Create checkerboard image

%%
% Plotting results

cFigure;
title('A 2D checkerboard pattern','FontSize',fontSize);
xlabel('X','FontSize',fontSize);ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;
imagesc(M);

colormap gray;
axis equal; view(2); axis tight;
set(gca,'FontSize',fontSize);
drawnow;

%% Example creating a 3D checkerboard image

siz=[6 6 3]; %Image size
M=checkerBoard3D(siz); %Create checkerboard image

%%
% Plotting results

[Fv,Vv,Cv]=im2patch(M,1:numel(M),'vb'); %Create patch data for plotting

cFigure; hold on;
title('A 3D checkerboard pattern','FontSize',fontSize);
gpatch(Fv,Vv,Cv);

camlight('headlight');
colormap gray;
axisGeom;
set(gca,'FontSize',fontSize);
drawnow;

%% Example, changing checkerboard block size

siz=[50 50]; %Image size
blockSize=10; %Block size in pixel units
M=checkerBoard3D(siz,blockSize); %Create checkerboard image

%%
% Plotting results

[Fv,Vv,Cv]=im2patch(M,1:numel(M),'vb'); %Create patch data for plotting

cFigure; hold on;
title('A 2D checkerboard pattern','FontSize',fontSize);
gpatch(Fv,Vv,Cv,'r',1,1);

colormap gray;
axisGeom; view(2);
set(gca,'FontSize',fontSize);
drawnow;

%%

siz=[12 12 6]; %Image size
blockSize=3; %Block size in pixel units
M=checkerBoard3D(siz,blockSize); %Create checkerboard image

%%
% Plotting results

[Fv,Vv,Cv]=im2patch(M,1:numel(M),'vb'); %Create patch data for plotting

cFigure; hold on;
title('A 3D checkerboard pattern','FontSize',fontSize);
gpatch(Fv,Vv,Cv,'r',1,1);

camlight('headlight');
colormap gray;
axisGeom;
set(gca,'FontSize',fontSize);
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


function [dist]=find3DNeighbourDists(mesh,scaling);
% function [dist]=find3DNeighbourDists(mesh,scaling);
% AUTHOR: WADE
% DATE : 062700
% PURPOSE : Return a list of the distances between each node and its neighbours.
% If there are n connections in the mesh, there should be n entries in distSQ
% See also find2DNeighboutDists - does the same thing with the 2d mesh.
% ARW 031201 - Now takes a scaling argument (equiv of dimdist for mrManDist)
% This scales distances along the appropriate matrix dimensions
% ARW 082702 - Now returns the abs distance not the squared one.
if (~exist('scaling'))
	scaling=[1 1 1];
end

spX=mesh.connectionMatrix;
spY=mesh.connectionMatrix;
spZ=mesh.connectionMatrix;
nVerts=length(mesh.connectionMatrix);

% What am I doing here?
 %   S = SPARSE(i,j,s,m,n,nzmax) uses the rows of [i,j,s] to generate an
 %   m-by-n sparse matrix with space allocated for nzmax nonzeros.  The
 %   two integer index vectors, i and j, and the real or complex entries
 %   vector, s, all have the same length, nnz, which is the number of
 %   nonzeros in the resulting sparse matrix S .  Any elements of s
 %   which have duplicate values of i and j are added together.

xI=sparse((1:nVerts),(1:nVerts),mesh.uniqueVertices(:,1),nVerts,nVerts)*scaling(1);
yI=sparse((1:nVerts),(1:nVerts),mesh.uniqueVertices(:,2),nVerts,nVerts)*scaling(2);
zI=sparse((1:nVerts),(1:nVerts),mesh.uniqueVertices(:,3),nVerts,nVerts)*scaling(3);
% So we're creating 3 diagonal sparse matrices.
% They contain the x,y,z coordinates of the uniqueVertices on their diagonals.

spX=spX*xI;
spY=spY*yI;
spZ=spZ*zI;

% What does this mean? If there's a connection at between nodes i and j, there will be a '1' in
% the conmat at i,j (and j,i...).
% The above multiplication means that all the '1's (connections) in column j will be
% replaced by the X, Y, or Z ordinates of the j'th node.

% Cols of sp(X,Y,Z) are now ordinates of corresponding vertices
spX=spX-spX'; % Smart eh? So the i,jth entry is the X ordinate of the j'th node. And the j,i'th entry is the X ordinate ofthe
              % ith node. So this subtraction calculates the X distance between them.
spY=spY-spY';
spZ=spZ-spZ';
dist=sqrt(spX.^2+spY.^2+spZ.^2); % And this just sums the squares of the X,Y and Z distances. Take the sqrt of this matrix to get the
                               % edge distances.


function [x_sh, y_sh]=shift_spectra(spectra, X,C);
% function [x_sh, y_sh]=shift_spectra(spectra, X,C);
%   Function to shift spectra with a 10x better accuracy than original data:
%       -if variable have a step in x-axis 0.1 than the spectra are interpolated
%        to having step of 0.01, the spectra are shifted and then converted back
%        to original number of points
%   This prepares data for multivariate analysis which is sensitive to
%   shift in data
%   Preferred over shift_spectra_or.m function
%
% spectra - matrix of spectra [nxm]
% X-variable (wavelength, mass, etc.)[n x 1]
% C - value at which a maximum of peak  is desired
% created by K.Artyushkova
% kartyush@unm.edu
%

%% converts spectra and X variable into 10X more channels than original
% data
[m,n]=size(spectra);
step=X(1)-X(2);
stepN=step/10*-1;
x = X(1):stepN:(X(m)+stepN);
x=x';
for i=1:n;
y(:,i)=spline(X,spectra(:,i),x);
end

%% finds the maximum for each spectra - its position and shift in number
% of points (S) from the most high value of maximum through the set of
% spectra
for i=1:n
    [k,r(i)]=max(y(:,i));
    xx(i)=x(r(i));
end
[q,s]=max(xx);
for i=1:n
    S(i)=xx(i)-xx(s);
end
stepN=stepN*-1;
S=double(single(1/stepN*abs(S)));
N=max(S);

%% combines all spectra into matrix yy. Now all spectra have the maximum at the
% same position of the variable
[mm,nn]=size(y);
for i=1:n
    Ss=S(i);
    yy(:,i)=y((Ss+1):(mm-(N-Ss)),i);
end

%% shifts X-axis to have a maximum at the desired value of C
X_x=x(1:(mm-(N-1)));
I = size(X_x);
shift=double(single((C-q)));
stepN=stepN*-1;
X_xx=(X_x(1)+shift):stepN:(X_x(I(1))+shift);
X_xx=X_xx';

%% converts data back to original number of points in X
mmm=round(I(1)/10);
for i=1:1:(mmm-1)
    yyy(i,:)=yy(i*10,:);
    X_xxx(i)=X_xx(i*10);
end

y_sh=yyy;
x_sh=X_xxx';
reverplot(x_sh, y_sh)