function c = tapas_hgf_categorical_norm_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF) for categorical inputs
% restricted to 3 levels, no drift, and no inputs at irregular intervals, in the absence of
% perceptual uncertainty.
%
% This model deals with the situation where an agent has to determine the probability of categorical
% outcomes. The tendencies of these outcomes are modeled as independent Gaussian random processes at
% the second level of the HGF. They are transformed into predictive probabilities at the first level
% by a softmax function (i.e., a logistic sigmoid). This amounts to the assumption that the
% probabilities are performing a Gaussian random walk in logit space. The volatility of all of
% these walks is determined by the same higher-level state x_3 in standard HGF fashion.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, & Stephan KE. (2011). A Bayesian foundation for individual
% learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% and elaborated in
%
% Mathys, C, Lomakina, EI, Daunizeau, J, Iglesias, S, Brodersen, KH, Friston, KJ, & Stephan, KE
% (2014). Uncertainty in perception and the Hierarchical Gaussian Filter. Frontiers in Human
% Neuroscience, 8:825.
%
% This file refers to CATEGORICAL inputs (Eqs 1-3 in Mathys et al., (2011)); for continuous inputs,
% refer to tapas_hgf_config.m, for binary inputs, refer to tapas_hgf_binary.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., omega). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., sigma2).
%
% Kappa and theta are estimated in 'logit-space' because bounding them above (in addition to
% their natural lower bound at zero) is an effective means of preventing the exploration of
% parameter regions where the assumptions underlying the variational inversion (cf. Mathys et
% al., 2011) no longer hold.
%
% 'Logit-space' is a logistic sigmoid transformation of native space with a variable upper bound
% a>0:
%
% logit(x) = ln(x/(a-x)); x = a/(1+exp(-logit(x)))
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin of x3 are arbitrary. This is the case if the observation model does not contain the
% representations mu3 and sigma3 from the third level. A choice of scale and origin is then
% implied by fixing the initial value mu3_0 of mu3 and either kappa or omega.
%
% Kappa and theta can be fixed to an arbitrary value by setting the upper bound to twice that
% value and the mean as well as the variance of the prior to zero (this follows immediately from
% the logit transform above).
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_categorical_plotTraj(est)
%
% where est is the stucture returned by fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu2_0      initial values of the mu2s
%         est.p_prc.sa2_0      initial values of the sigma2s
%         est.p_prc.mu3_0      initial value of mu3
%         est.p_prc.sa3_0      initial value of sigma3
%         est.p_prc.ka         kappa
%         est.p_prc.om         omega
%         est.p_prc.th         theta
%
%         est.traj.mu          mu
%         est.traj.sa          sigma
%         est.traj.muhat       prediction mean
%         est.traj.sahat       prediction variance
%         est.traj.v           inferred variances of random walks
%         est.traj.w           weighting factor of informational and environmental uncertainty at the 2nd level
%         est.traj.da          prediction errors
%         est.traj.ud          updates with respect to prediction
%         est.traj.psi         precision weights on prediction errors
%         est.traj.epsi        precision-weighted prediction errors
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate)
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_hgf_categorical_config', 'tapas_bayes_optimal_categorical_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior mean of kappa, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2014 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_categorical';

% Number of states
c.n_outcomes = 3;

% Upper bound for kappa and theta (lower bound is always zero)
c.kaub = 2;
c.thub = 0.1;

% Sufficient statistics of Gaussian parameter priors

% Initial mu2
c.mu2_0mu = repmat(tapas_logit(1/c.n_outcomes,1),1,c.n_outcomes);
c.mu2_0sa = zeros(1,c.n_outcomes);

% Initial sigma2
c.logsa2_0mu = repmat(log(1),1,c.n_outcomes);
c.logsa2_0sa = zeros(1,c.n_outcomes);

% Initial mu3
% Usually best kept fixed to 1 (determines origin on x3-scale).
c.mu3_0mu = 1;
c.mu3_0sa = 0;

% Initial sigma3
c.logsa3_0mu = log(0.1);
c.logsa3_0sa = 1;

% Kappa
% This should be fixed (preferably to 1) if the observation model
% does not use mu3 (kappa then determines the scaling of x3).
c.logitkamu = 0; % If this is 0, and
c.logitkasa = 0; % this is 0, and c.kaub = 2 above, then kappa is fixed to 1

% Omega
c.ommu =  -4;
c.omsa = 5^2;

% Theta
c.logitthmu = 0;
c.logitthsa = 2;

% Gather prior settings in vectors
c.priormus = [
    c.mu2_0mu,...
    c.logsa2_0mu,...
    c.mu3_0mu,...
    c.logsa3_0mu,...
    c.logitkamu,...
    c.ommu,...
    c.logitthmu,...
             ];

c.priorsas = [
    c.mu2_0sa,...
    c.logsa2_0sa,...
    c.mu3_0sa,...
    c.logsa3_0sa,...
    c.logitkasa,...
    c.omsa,...
    c.logitthsa,...
             ];

% Model function handle
c.prc_fun = @tapas_hgf_categorical;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_categorical_transp;

return;


function [val,idx] = of_RMSE(obs,sim,idx)
% of_RMSE Calculates the Root Mean Squared Error of simulated streamflow.
% Ignores time steps with negative flow values.

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

% In:
% obs       - time series of observations       [nx1]
% sim       - time series of simulations        [nx1]
% idx       - optional vector of indices to use for calculation, can be
%               logical vector [nx1] or numeric vector [mx1], with m <= n
%
% Out:
% val       - objective function value          [1x1]
% idx       - indices used for the calculation

%% Check inputs and select timesteps
if nargin < 2
    error('Not enugh input arguments')
end

if nargin < 3; idx = []; end
[sim, obs, idx] = check_and_select(sim, obs, idx);

%% Calculate metric
val = sqrt(mean((obs-sim).^2));
end


% Ambiguity function
%
% Computes the ambiguity function of the input signal. An analytic
% signal generator is called if the input signal is real. This version
% takes advantage of the property K(n,-m)=K*(n,m) to increase
% calculation speed.
%
% Usage:
%
%     af = ambf( signal, lag_res, window_length )
%
%
% Parameters:
%
%     af
%
%	  The computed ambiguity function representation. size(af) will
%	  return [a, b], where b is the next largest power of two above
%	  (2*window_length), and a is equal signal length.
%
%    signal
%
%	  Input one dimensional signal to be analysed. An analytic signal
%	  is required for this function, however, if signal is real, a
%	  default analytic transformer routine will be called from this
%	  function before computing the ambiguity function.
%
%    window_length
%
%	  The number of signal samples used for analysis.
%
%    lag_res
%
%	  The number of lag to skip between successive slices of
%	  the analysis.
%
%
%
%  See Also: analyt
%
% TFSAP 7.0
% Copyright Prof. B. Boashash
% Qatar University, Doha
% email: tfsap.research@gmail.com


function [um] = pm2um(pm)
% Convert length from picometers to micrometers (or microns).
% Chad A. Greene 2012
um = pm*1e-6;


function [sol, val] = gaDemo1Eval(sol,options)
% Demonstration evaluation function used in gademo1.
% f(x)=x+10sin(5x)+7cos(4x)
%
% function [val,sol] = gaDemo1Eval(sol,options)
%
% val - the fittness of this individual
% sol - the individual, returned to allow for Lamarckian evolution
% options - [current_generation]

% Binary and Real-Valued Simulation Evolution for Matlab
% Copyright (C) 1996 C.R. Houck, J.A. Joines, M.G. Kay
%
% C.R. Houck, J.Joines, and M.Kay. A genetic algorithm for function
% optimization: A Matlab implementation. ACM Transactions on Mathmatical
% Software, Submitted 1996.
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 1, 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. A copy of the GNU
% General Public License can be obtained from the
% Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

x=sol(1);
val = x + 10*sin(5*x)+7*cos(4*x);


function I = gcmi_model_cd(x, y, Ym)
% GCMI_MODEL_CD Gaussian-Copula Mutual Information between a continuous and a
%         discrete variable in bits based on ANOVA style model comparison.
%   I = gcmi_model_gd(x,y,Ym) returns the MI between the (possibly multidimensional)
%   continuous variable x and the discrete variable y.
%   For 1D x this is a lower bound to the mutual information.
%   Rows of x correspond to samples, columns to dimensions/variables.
%   (Samples first axis)
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%   See also: GCMI_MIXTURE_CD

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)~=2
    error('gcmi_model_cd: input array should be 2d')
end

if isvector(y)
    y = y(:);
else
    error('gcmi_model_cd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvar = size(x,2);

if size(y,1) ~= Ntrl
    error('gcmi_model_cd: number of trials do not match');
end

% check for repeated values
for xi=1:Nvar
    if length(unique(x(:,xi)))./Ntrl < 0.9
        warning('Input x has more than 10% repeated values.')
        break
    end
end

% check values of discrete variable
if min(y)~=0 || max(y)~=(Ym-1) || any(round(y)~=y)
    error('Values of discrete variable y are not correct')
end

% copula normalisation
cx = copnorm(x);
% parametric Gaussian MI
I = mi_model_gd(cx,y,Ym,true,true);


function fv= proc_rSquare(fv, varargin)
%PROC_RSQUARE - computes r^2 values (measure for discriminance)
%
%Synopsis:
% 	fv = proc_rSquare(fv, <opt>)
%
%Returns:
% FV_RVAL - data structure of squared biserial correlation coefficients
%  .x     - squared biserial correlation between each feature and the class label
%  .se    - standard error of atanh(r), if opt.Stats==1
%  .p     - p value of null hypothesis that there is zero
%           correlation between feature and class-label, if opt.Stats==1
%  .sgnlogp - contains the signed log10 p-value, if opt.Stats==1
%             if opt.Bonferroni==1, the p-value is multiplied by
%             fv_rval.corrfac
%  .sgnlogp - contains the signed log10 p-value, if opt.Stats==1
%           if opt.Bonferroni==1, the p-value is multiplied by
%           fv_rval.corrfac, cropped, and then logarithmized
%  .sigmask - binary array indicating significance at alpha level
%             opt.Alphalevel, if opt.Stats==1 and opt.Alphalevel > 0
%  .corrfac - Bonferroni correction factor (number of simultaneous tests),
%             if opt.Bonferroni==1
%
%Properties:
% 'TolerateNans': observations with NaN value are skipped
%    (nanmean/nanstd are used instead of mean/std). Deafult: 0
% 'ValueForConst': constant feauture dimensions are assigned this
%    value. Default: NaN.
% 'MulticlassPolicy': possible options: 'pairwise' (default),
%    'all-against-last', 'each-against-rest', or provide specified
%    pairs as an [nPairs x 2] sized matrix. ('specified_pairs' is obsolete)
% 'Stats' - if true, additional statistics are calculated, including the
%           standard error of atanh(r), the p-value for the null
%           Hypothesis that the correlation is zero,
%           and the "signed log p-value"
% 'Bonferroni' - if true, Bonferroni corrected is used to adjust p-values
%                and their logarithms
% 'Alphalevel' - if provided, a binary indicator of the significance to the
%                alpha level is returned for each feature in fv_rval.sigmask
%
%Description
% Computes the r^2 value for each feature. The r^2 value is a measure
% of how much variance of the joint distribution can be explained by
% class membership.
%
% See also proc_classmeanDiff, proc_rValues, proc_rSquareSigned
%
% 03-03 Benjamin Blankertz
% 09-2012 stefan.haufe@tu-berlin.de

if nargin==0,
  fv=proc_rValues; return
end

fv= proc_rValues(fv, varargin{:});
fv.x= fv.x.^2;
for cc= 1:length(fv.className),
  fv.className{cc}= ['r^2' fv.className{cc}(2:end)];
end
fv.yUnit= 'r^2';


function triangle01_monomial_integral_test ( )

%*****************************************************************************80
%
%% TRIANGLE01_MONOMIAL_INTEGRAL_TEST estimates integrals over the unit triangle.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/triangle_integrals/triangle01_monomial_integral_test.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE01_MONOMIAL_INTEGRAL_TEST\n' );
  fprintf ( 1, '  TRIANGLE01_MONOMIAL_INTEGRAL returns the integral Q of\n' );
  fprintf ( 1, '  a monomial X^I Y^J over the interior of the unit triangle.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   I   J         Q(I,J)\n' );

  for d = 0 : 5
    fprintf ( 1, '\n' );
    for i = 0 : d
      j = d - i;
      q = triangle01_monomial_integral ( i, j );
      fprintf ( 1, '  %2d  %2d  %14.6g\n', i, j, q );
    end
  end

  return
end


% Usage  [L, inliers] = ransacfitline(XYZ, t, feedback)
%
% Arguments:
%          XYZ - 3xNpts array of xyz coordinates to fit line to.
%          t   - The distance threshold between data point and the line
%                used to decide whether a point is an inlier or not.
%          feedback - Optional flag 0 or 1 to turn on RANSAC feedback
%                     information.
%
% Returns:.
%           V - Line obtained by a simple fitting on the points that
%               are considered inliers.  The line goes through the
%               calculated mean of the inlier points, and is parallel to
%               the principal eigenvector.  The line is scaled by the
%               square root of the largest eigenvalue.
%               This line is a n*2 matrix.  The first column is the
%               beginning point, the second column is the end point of the
%               line.
%           L - The two points in the data set that were found to
%               define a line having the most number of inliers.
%               The two columns of L defining the two points.
%           inliers - The indices of the points that were considered
%                     inliers to the fitted line.
%
% See also:  RANSAC, FITPLANE, RANSACFITPLANE

% Copyright (c) 2003-2006 Peter Kovesi and Felix Duvallet (CMU)
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% Aug  2006 - created ransacfitline from ransacfitplane
%             author: Felix Duvallet

function [V, L, inliers] = ransacfitparabola(XY, t, feedback)

    if nargin == 2
	feedback = 0;
    end

    [rows, npts] = size(XY);

    if rows ~= 2
        error('data is not 2D');
    end

    if npts < 2
        error('too few points to fit line');
    end

    s = 3;  % Minimum No of points needed to fit a line.

    fittingfn = @defineParabola;
    distfn    = @paraptdist;
    degenfn   = @isdegenerate;

    [L, inliers] = ransac(XY, fittingfn, distfn, degenfn, s, t, feedback);

    V = fitparabola(XY(:, inliers));

%------------------------------------------------------------------------
% Function to define a parabola given 3 data points as required by
% RANSAC.

function L = defineParabola(X)
    L = X;
%------------------------------------------------------------------------
% Function to calculate distances between a line and an array of points.
function [inliers, L] = paraptdist(L, X, t)
    p = polyfit( L(1, :), L(2, :), 2 );
    YEst = polyval(p, X(1, :) );
    d = YEst - X(2, :);
    inliers = find(abs(d) < t);

%------------------------------------------------------------------------
% Function to determine whether a set of 2 points are in a degenerate
% configuration for fitting a line as required by RANSAC.
% In this case two points are degenerate if they are the same point
% or if they are exceedingly close together.

function r = isdegenerate(X)
    %find the norm of the difference of the two points
    % this will be 0 iff the two points are the same (the norm of their
    % difference is zero)
    r0 = norm(X(:,1) - X(:,2)) < eps;
    r1 = norm(X(:,1) - X(:,3)) < eps;
    r = r0 | r1;


function [leftpt,leftcurve,rightpt,rightcurve]=slicesurf3(node,elem,p1,p2,p3,step,minangle)
%
% [leftpt,leftcurve,rightpt,rightcurve]=slicesurf3(node,elem,p1,p2,p3,step,minangle)
%
% Slice a closed surface by a plane and extract the landmark nodes along
% the intersection between p1 and p3, then output into 2 segments: between
% p2 to p1 (left half), and p2 to p3 (right half)
%
% author: Qianqian Fang (q.fang at neu.edu)
%
% input:
%    node: an N x 3 array defining the 3-D positions of the mesh
%    elem: an N x 3 interger array specifying the surface triangle indices;
%    p1: 3D position of the start on the curve-of-interest
%    p2: 3D position of the middle on the curve-of-interest
%    p3: 3D position of the end on the curve-of-interest
%    step: (optional) a percentage (0-100) specifying the spacing of the
%        output landmark nodes; step=20 means the landmarks on the left
%        curve are spaced as 20% of the total lengths of the left-half, and
%        those on the right-curve are spaced at 20% of the right-half,
%        starting from p2.
%    minangle: (optional) a positive minangle will ask this function to
%        call polylinesimplify to remove sharp turns on the curve.
%
% output:
%    leftpt: the equal-spaced landmark nodes on the left-half (p2-p1)
%            intersection curve; spacing between these nodes are
%            (step% * length of the curve between p2-p1)
%    leftcurve: all nodes on the left-half (p2-p1) intersection curve
%    rightpt: the equal-spaced landmark nodes on the right-half (p2-p3)
%            intersection curve; spacing between these nodes are
%            (step% * length of the curve between p2-p3)
%    rightcurve: all nodes on the left-half (p2-p1) intersection curve
%
% -- this function is part of brain2mesh toolbox (http://mcx.space/brain2mesh)
%    License: GPL v3 or later, see LICENSE.txt for details
%

fullcurve=slicesurf(node, elem, [p1;p2;p3]);
if(nargin>=7 && minangle>0)
    fullcurve=polylinesimplify(fullcurve,minangle);
end

[fulllen, fullcurve]=polylinelen(fullcurve, p1,p3,p2);

[leftlen,  leftcurve]=polylinelen(fullcurve, p2, p1);
if(nargin>=6)
    [idx, weight, leftpt]=polylineinterp(leftlen, sum(leftlen)*(step:step:(100-step*0.5))*0.01, leftcurve);
else
    leftpt=leftcurve;
end

if(nargout>2)
    [rightlen, rightcurve]=polylinelen(fullcurve, p2, p3);
    if(nargin>=6)
        [idx, weight, rightpt]=polylineinterp(rightlen, sum(rightlen)*(step:step:(100-step*0.5))*0.01, rightcurve);
    else
        rightpt=rightcurve;
    end
end


function [count, h, parent, post, L] = symbfact2 (A, mode, Lmode)	    %#ok
%SYMBFACT2  symbolic factorization
%
%   Analyzes the Cholesky factorization of A, A'*A, or A*A'.
%
%   Example:
%   count = symbfact2 (A)               returns row counts of R=chol(A)
%   count = symbfact2 (A,'col')         returns row counts of R=chol(A'*A)
%   count = symbfact2 (A,'sym')         same as symbfact2(A)
%   count = symbfact2 (A,'lo')          same as symbfact2(A'), uses tril(A)
%   count = symbfact2 (A,'row')         returns row counts of R=chol(A*A')
%
%   The flop count for a subsequent LL' factorization is sum(count.^2)
%
%   [count, h, parent, post, R] = symbfact2 (...) returns:
%
%       h: height of the elimination tree
%       parent: the elimination tree itself
%       post: postordering of the elimination tree
%       R: a 0-1 matrix whose structure is that of chol(A) for the symmetric
%           case, chol(A'*A) for the 'col' case, or chol(A*A') for the
%           'row' case.
%
%   symbfact2(A) and symbfact2(A,'sym') uses the upper triangular part of A
%   (triu(A)) and assumes the lower triangular part is the transpose of
%   the upper triangular part.  symbfact2(A,'lo') uses tril(A) instead.
%
%   With one to four output arguments, symbfact2 takes time almost proportional
%   to nnz(A)+n where n is the dimension of R, and memory proportional to
%   nnz(A).  Computing the 5th argument takes more time and memory, both
%   O(nnz(L)).  Internally, the pattern of L is computed and R=L' is returned.
%
%   The following forms return L = R' instead of R.  They are faster and take
%   less memory than the forms above.  They return the same count, h, parent,
%   and post outputs.
%
%   [count, h, parent, post, L] = symbfact2 (A,'col','L')
%   [count, h, parent, post, L] = symbfact2 (A,'sym','L')
%   [count, h, parent, post, L] = symbfact2 (A,'lo', 'L')
%   [count, h, parent, post, L] = symbfact2 (A,'row','L')
%
%   See also CHOL, ETREE, TREELAYOUT, SYMBFACT

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

error ('symbfact2 mexFunction not found!') ;


function S=smoothHeaviside(varargin)

switch nargin
    case 1
        x=varargin{1};
        k=6;
        r=0;
    case 2
        x=varargin{1};
        k=varargin{2};
        r=0;
    case 3
        x=varargin{1};
        k=varargin{2};
        r=varargin{3};
end
k=k*2;
S=exp(2*k*(x-r))./(1+exp(2*k*(x-r)));

%%
% _*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 yprime = decay1(t,y)
%DECAY1 Calculates the decay rates of Thorium 227 and Radium 223.
% Function DECAY1 Calculates the rates of change of Thorium 227
% and Radium 223 (yprime) for a given current concentration y.

% Define variables:
%   t         -- Time (in days)
%   y         -- Vector of current concentrations
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    03/15/07    S. J. Chapman        Original code

% Set decay constants.
lambda_th = 0.03710636;
lambda_ra = 0.0606428;

% Calculate rates of decay
yprime = zeros(2,1);
yprime(1) = -lambda_th * y(1);
yprime(2) = -lambda_ra * y(2) + lambda_th * y(1);


% ACWMF algorithm implementation
%function [y, noise_matrix] = ACWMF(I);
function output = ACWMF(I);

% Threshold parameters
%delta = [40 25 10 5];   %% for uniform impulse noise
delta = [55,40,25,15];  %% for salt and pepper noise
%delta = (delta1+delta2)/2;   %% for mixed impulse noise

x = double(I);
image_size = size(I);
B = im2col(padarray(x,[1 1],'symmetric','both'),[3 3],'sliding');

% Compute filter output
m = medfilt2(x,[3 3],'symmetric');

%Compute differences
d = abs(x-m);
d0 = d(:)';
clear d;
B(10,:) = x(:)';
B(11,:) = x(:)';
m1 = median(B);
d1 = abs(x(:)'-m1);
B(12,:) = x(:)';
B(13,:) = x(:)';
m1=median(B);
d2 = abs(x(:)'-m1);
B(14,:) = x(:)';
B(15,:) = x(:)';
m1=median(B);
d3 = abs(x(:)'-m1);
clear B;

% Compute MAD values
B_x = im2col(padarray(x,[1 1],'symmetric','both'),[3 3],'sliding');

for i = 1:9
    B(i,:) = abs(B_x(i,:) - m(:)');
end
MAD = median(B);

clear B;

% Compute threshold values
s = 0.1;
T1 = (MAD * s) + delta(1);
T2 = (MAD * s) + delta(2);
T3 = (MAD * s) + delta(3);
T4 = (MAD * s) + delta(4);

x2 = x(:);

% Detect noisy pixels
F = find((d0>T1)|(d1>T2)|(d2>T3)|(d3>T4));

%noise_matrix = zeros(image_size);
%noise_matrix(F) = 1;

% Replace noisy pixels
x2(F) = m(F);
clear F m d0 d1 d2 d3 T x;
output = uint8(col2im(x2,[1 1],image_size,'sliding'));


function y = logDirichlet(X, a)
% Compute log pdf of a Dirichlet distribution.
% Input:
%   X: d x n data matrix, each column sums to one (sum(X,1)==ones(1,n) && X>=0)
%   a: d x k parameter of Dirichlet
%   y: k x n probability density
% Output:
%   y: k x n probability density in logrithm scale y=log p(x)
% Written by Mo Chen (sth4nth@gmail.com).
X = bsxfun(@times,X,1./sum(X,1));
if size(a,1) == 1
    a = repmat(a,size(X,1),1);
end
c = gammaln(sum(a,1))-sum(gammaln(a),1);
g = (a-1)'*log(X);
y = bsxfun(@plus,g,c');


% fft_compress.m

% 作用：使用离散傅里叶变换对输入的 JPG 图片 image 按照指定的滤波比例 ratio 进行压缩
% 返回值：返回低通滤波之后的频率分布和压缩之后的图片
function [z, k] = fft_compress(image, ratio)

    % 对原 JPG 图片在三个颜色上分别做二维离散傅里叶变换，得到频率分布
    z(:,:,1) = fft2(image(:,:,1));
    z(:,:,2) = fft2(image(:,:,2));
    z(:,:,3) = fft2(image(:,:,3));

    % 获取图片的尺寸大小
    [a, b, ~] = size(image);

    % 低通滤波
    for i = 1 : a
        for j = 1 : b
            if (i + j > (a+b) * ratio)
                z(i, j, 1) = 0;
                z(i, j, 2) = 0;
                z(i, j, 3) = 0;
            end
        end
    end

    % 对过滤之后的结果在三个颜色上分别做进行二维反离散傅里叶变换
    k(:,:,1) = ifft2(z(:,:,1));
    k(:,:,2) = ifft2(z(:,:,2));
    k(:,:,3) = ifft2(z(:,:,3));

    % 类型转换，转换为 0-255 范围内的颜色值
    k = uint8(k);
end


function [in2] = mm22in2(mm2)
% Convert area from square millimeters to square inches.
% Chad A. Greene 2012
in2 = mm2*0.001550003100006;


%% Checking Solutions
% note remove the private setting from the opti class
clear all

%% Bounds
clc
f = [-1;-2;-3;-4];
lb = [0;0;0;0];
ub = [1;1;1;1];

O = opti('f',f,'bounds',lb,ub,'options',optiset('solver','clp'));
O.sol = [-1;-1;2;2];
O.ef = 1;

[status,msg] = checkSol(O)

%% LP General Constraints
clc
f = [-1;-2];
A = [1 0;0 1];
b = [0;0];
Aeq = [1 0; 0 1];
beq = [0.5;0.5];

O = opti('f',f,'ineq',A,b,'eq',Aeq,beq,'options',optiset('solver','cplex'));
O.sol = [1;1];
O.ef = 1;

[status,msg] = checkSol(O)

%% LP Row Constraints
clc
f = [-1;-2];
A = sparse([1 0;0 1;1 0;0 1;1 0;0 1]);
rl = [2;2;-Inf;-Inf;0.5;0.5];
ru = [Inf;Inf;0;0;0.5;0.5];

O = opti('f',f,'lin',A,rl,ru,'options',optiset('solver','clp'));
O.sol = [1;1];
O.ef = 1;

[status,msg] = checkSol(O)

%% QC Single Constraint
clc
f = [-1;-2];
Q = eye(2);
l = [-1;-2];

O = opti('H',zeros(2),'f',f,'qc',Q,l,-1.5,'options',optiset('solver','scip'));
O.sol = [1;1];
O.ef = 1;

[status,msg] = checkSol(O)

%% QC Multiple Constraints
clc
f = [-1;-2];
Q = {eye(2);eye(2)};
l = [-1 -1;-2 -3];

O = opti('H',zeros(2),'f',f,'qc',Q,l,[-1.5;-3],'options',optiset('solver','scip'));
O.sol = [1;1];
O.ef = 1;

[status,msg] = checkSol(O)

%% NLP General Constraints
clc
fun = @(x) x(1)+x(2);
nlcon = @(x) [x(1);x(2);x(1);x(2);x(1);x(2)];
nlrhs = [2;2;0;0;0.5;0.5];
nle = [1;1;-1;-1;0;0];

O = opti('fun',fun,'nlmix',nlcon,nlrhs,nle,'ndec',2,'options',optiset('solver','nlopt'));
O.sol = [1;1];
O.ef = 1;

[status,msg] = checkSol(O)

%% NLP Row Constraints
clc
fun = @(x) x(1)+x(2);
nlcon = @(x) [x(1);x(2);x(1);x(2);x(1);x(2)];
cl = [2;2;-Inf;-Inf;0.5;0.5];
cu = [Inf;Inf;0;0;0.5;0.5];

O = opti('fun',fun,'nl',nlcon,cl,cu,'ndec',2,'options',optiset('solver','ipopt'));
O.sol = [1;1];
O.ef = 1;

[status,msg] = checkSol(O)

%% Integer Constraints
clc
f = [-1;-2;-3;-4];
lb = [0;0;0;0];
ub = [1;1;1;1];

O = opti('f',f,'bounds',lb,ub,'int','ICCI','options',optiset('solver','cbc'));
O.sol = [-1.1;-1.5;2;2.1];
O.ef = 1;

[status,msg] = checkSol(O)

%% Binary Constraints
clc
f = [-1;-2;-3;-4];
lb = [0;0;0;0];
ub = [1;1;1;1];

O = opti('f',f,'bounds',lb,ub,'int','ICBI','options',optiset('solver','cbc'));
O.sol = [-1.1;-1.5;1.001;2.1];
O.ef = 1;

[status,msg] = checkSol(O)


clear all; close all;
I=imread('cameraman.tif');
I=im2double(I);
I=imnoise(I, 'salt & pepper', 0.05);
J=medfilt2(I, [3, 3]);
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(J);


function sphere_triangle_quad_test03 ( )

%*****************************************************************************80
%
%% SPHERE_TRIANGLE_TEST03 tests SPHERE01_TRIANGLE_QUAD_ICOS1C.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2014
%
%  Author:
%
%    John Burkardt
%
  e_test = [ ...
    0, 0, 0; ...
    1, 0, 0; ...
    0, 1, 0; ...
    0, 0, 1; ...
    2, 0, 0; ...
    0, 2, 2; ...
    2, 2, 2; ...
    0, 2, 4; ...
    0, 0, 6; ...
    1, 2, 4; ...
    2, 4, 2; ...
    6, 2, 0; ...
    0, 0, 8; ...
    6, 0, 4; ...
    4, 6, 2; ...
    2, 4, 8; ...
   16, 0, 0 ]';
  n_mc1 = 1000;
  n_mc2 = 10000;
  n_mc3 = 100000;

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_TRIANGLE_QUAD_TEST03\n' );
  fprintf ( 1, '  SPHERE01_TRIANGLE_QUAD_ICOS1C approximates the\n' );
  fprintf ( 1, '  integral of a function over a spherical triangle on\n' );
  fprintf ( 1, '  the surface of the unit sphere using a centroid rule.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We do not have an exact result, so we compare each\n' );
  fprintf ( 1, '  estimate to the final one.\n' );
%
%  Choose three points at random to define a spherical triangle.
%
  [ v1, seed ] = sphere01_sample ( 1, seed );
  [ v2, seed ] = sphere01_sample ( 1, seed );
  [ v3, seed ] = sphere01_sample ( 1, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Vertices of random spherical triangle:\n' );
  fprintf ( 1, '\n' );
  r8vec_transpose_print ( 3, v1, '  V1:' );
  r8vec_transpose_print ( 3, v2, '  V2:' );
  r8vec_transpose_print ( 3, v3, '  V3:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  FACTOR   N   RESULT\n' );

  for j = 1 : 17

    e(1:3) = e_test(1:3,j);

    e = polyterm_exponent ( 'SET', e );

    polyterm_exponent ( 'PRINT', e );

%   factor = 2 ^ 11;
    factor = 2 ^ 7;
    [ best, node_num ] = sphere01_triangle_quad_icos1c ( v1, v2, v3, factor, ...
      @polyterm_value_3d );

    factor = 1;
    for factor_log = 0 : 5

      [ result, node_num ] = sphere01_triangle_quad_icos1c ( v1, v2, v3, ...
        factor, @polyterm_value_3d );

      error = abs ( result - best );

      fprintf ( 1, '  %4d  %8d  %16.8g  %10.2e\n', ...
        factor, node_num, result, error );

      factor = factor * 2;

    end

  end

  return
end


function x=demean(x,dim)

% DEMEAN(X)
% Removes the Average or mean value.
%
% DEMEAN(X,DIM)
% Removes the mean along the dimension DIM of X.

if(nargin==1),
   dim = 1;
   if(size(x,1) > 1)
      dim = 1;
   elseif(size(x,2) > 1)
      dim = 2;
   end;
end;

dims = size(x);
dimsize = size(x,dim);
dimrep = ones(1,length(dims));
dimrep(dim) = dimsize;

x = x - repmat(mean(x,dim),dimrep);


%KLMS Karhunen Loeve Mapping, followed by scaling
%
%   [W,FRAC] = KLMS(A,N)
%   [W,N]    = KLMS(A,FRAC)
%
% INPUT
%   A            Dataset
%   N  or FRAC   Number of dimensions (>= 1) or fraction of variance (< 1)
%                to retain; if > 0, perform PCA; otherwise MCA. Default: N = inf.
%
% OUTPUT
%   W            Affine Karhunen-Loeve mapping
%   FRAC or N    Fraction of variance or number of dimensions retained.
%
% DESCRIPTION
% First a Karhunen Loeve Mapping is performed (i.e. PCA or MCA on the average
% prior-weighted class covariance matrix). The result is scaled by the mean
% class standard deviations. For N and FRAC, see KLM.
%
% Default N: select all ('pre-whiten' the average covariance matrix, i.e.
% orthogonalize and scale). The resulting mapping has a unit average
% covariance matrix.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, KLM, PCA

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: klms.m,v 1.2 2006/03/08 22:06:58 duin Exp $

function [w,truefrac] = klms(a,n)

		if (nargin < 2), n = []; end;
	if (nargin < 1) | (isempty(a))
		w = prmapping('klms',n);
		w = setname(w,'Scaled KL Mapping');
		return
	end

	[w,truefrac] = klm(a,n);        % Calculate KL mapping
	b = a*w;                        % Combine KL mapping with scaling on
	w = w*scalem(b,'c-variance');   % KL-mapped data
	w = setname(w,'Scaled KL Mapping');

	return


function accuracy = ClassifyDataset(dataset, labels, P, G)
% returns the accuracy of the model P and graph G on the dataset
%
% Inputs:
% dataset: N x 10 x 3, N test instances represented by 10 parts
% labels:  N x 2 true class labels for the instances.
%          labels(i,j)=1 if the ith instance belongs to class j
% P: struct array model parameters (explained in PA description)
% G: graph structure and parameterization (explained in PA description)
%
% Outputs:
% accuracy: fraction of correctly classified instances (scalar)
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

N = size(dataset, 1);
accuracy = 0.0;
calclabels = zeros(size(labels));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
if size(size(G),2) == 2
	G1 = G; G2 = G;
else
	G1 = reshape(G(:,:,1),10,2);
	G2 = reshape(G(:,:,2),10,2);
end

for i = 1:N
	data = reshape(dataset(i,:,:),10,3);
	log1 = log(P.c(1));
	log2 = log(P.c(2));
	for j = 1:10
		if G1(j,1) == 1   % construct g1 and g2 in the beginning from g
			%do shit
			theta1 = P.clg(j).theta(1,:);   theta2 = P.clg(j).theta(2,:);
			parent1 = data(G1(j,2),:);   parent2 = data(G2(j,2),:);
			mu_y1 = sum(theta1(1:4).*[1, parent1]);mu_y2 = sum(theta2(1:4).*[1, parent2]);
			mu_x1 = sum(theta1(5:8).*[1, parent1]);mu_x2 = sum(theta2(5:8).*[1, parent2]);
			mu_a1 = sum(theta1(9:12).*[1, parent1]);mu_a2 = sum(theta2(9:12).*[1, parent2]);
			log1 += lognormpdf(data(j,1),mu_y1,P.clg(j).sigma_y(1));
			log1 += lognormpdf(data(j,2),mu_x1,P.clg(j).sigma_x(1));
			log1 += lognormpdf(data(j,3),mu_a1,P.clg(j).sigma_angle(1));
			log2 += lognormpdf(data(j,1),mu_y2,P.clg(j).sigma_y(2));
			log2 += lognormpdf(data(j,2),mu_x2,P.clg(j).sigma_x(2));
			log2 += lognormpdf(data(j,3),mu_a2,P.clg(j).sigma_angle(2));

		else
			log1 += lognormpdf(data(j,1),P.clg(j).mu_y(1),P.clg(j).sigma_y(1));
			log1 += lognormpdf(data(j,2),P.clg(j).mu_x(1),P.clg(j).sigma_x(1));
			log1 += lognormpdf(data(j,3),P.clg(j).mu_angle(1),P.clg(j).sigma_angle(1));
			log2 += lognormpdf(data(j,1),P.clg(j).mu_y(2),P.clg(j).sigma_y(2));
			log2 += lognormpdf(data(j,2),P.clg(j).mu_x(2),P.clg(j).sigma_x(2));
			log2 += lognormpdf(data(j,3),P.clg(j).mu_angle(2),P.clg(j).sigma_angle(2));
		end
	end
	calclabels(i,:) = [log1,log2];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[temp,temp1] = max(calclabels,[],2);
[temp,temp2] = max(labels,[],2);
accuracy = sum(temp1==temp2)/N;
fprintf('Accuracy: %.2f\n', accuracy);


function x=windinfo(w,fs)
%V_WINDINFO window information and figures of merit X=(W,FS)
% Usage: (1) v_windinfo(v_windows('hamming',720,'ds'),720);         % plot hamming window info
%
%  Inputs:  W        is a vector containing the window
%           FS       is the sampling frequency (default=1)
%
% Outputs:  X.len         length of the window (samples)
%           X.nw          length of the window (samples)
%           X.ewgdelay    energy centroid delay from first sample (samples)
%           X.dcgain      DC gain (dB)
%           X.sidelobe    maximum sdelobe level in dB relative to DC gain
%           X.falloff     rate at which sidelobes decay (dB/octave)
%           X.enbw        equivalent noise bandwidth (*fs/len Hz)
%           X.scallop     scalloping loss (dB)
%           X.ploss       processing loss (dB)
%           X.wcploss     worst case processing loss (dB)
%           X.band3       3dB bandwidth (Hz)
%           X.band6       6 dB bandwidth (Hz)
%           X.band0       essential bandwidth (to first minimum) (Hz)
%           X.gain0       gain at first minimum (Hz)
%           X.olc50       50% overlap correction
%           X.olc75       75% overlap correction
%           X.cola        overlap factors giving constant overlap add (exlcuding multiples)
%           X.cola2       as X.cola but for squared window
%
% If no output argument is given, the window and frequency response
% will be plotted e.g. v_windinfo(v_windows('hamming',720,'ds'),720);
%
% To obtain the figures of merit listed in Table 1 of [1] set
% fs = length(W), multiply X.olc50 and X.olc75 by 100%. The "coherent gain
% listed in the table is 10^(x.dcgain/20)/(max(w)*length(w)).
%
%  [1]  F. J. Harris. On the use of windows for harmonic analysis with the
%       discrete fourier transform. Proc IEEE, 66 (1): 51-83, Jan. 1978.

%	   Copyright (C) Mike Brookes 2009-2014
%      Version: $Id: v_windinfo.m 6801 2015-09-12 09:30:42Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%

if nargin<2
    fs=1;
end
w=w(:);
nw=length(w);
x.len=nw/fs;
x.nw=nw;
% energy weighted group delay = centre of energy
x.ewgdelay=((1:nw)*w.^2/sum(w.^2)-1)/fs;
% now calculate spectrum
of=16;      % spectrum oversample factor must be even
nwo=of*nw;
f=rfft(w,nwo);
p=f.*conj(f);
% sidelobe attenuation is maximum peak (note DC peak at p(1) is not found)
[kp,vp]=v_findpeaks(p,'q');
[kt,vt]=v_findpeaks(-p,'q');
% dbpo=10*log10(vp(2:end)./vp(1:end-1))./log2((kp(2:end)-1)./(kp(1:end-1)-1)); % slope in dB/octave
if ~numel(kp)
    x.sidelobe=10*log10(min(p)/p(1));
else
    x.sidelobe=10*log10(max(vp)/p(1));
end
np=length(kp);
ipa=floor(np/4);
if ~ipa
    x.falloff=0;
else
    ipb=floor(np/2);
    x.falloff=10*log10(vp(ipb)/vp(ipa))/log2((ipb-1)/(ipa-1));
end
sumw2=sum(w.^2);
sumw=sum(w);
enbwbin=nw*sumw2/sumw^2;
x.enbw=enbwbin*fs/nw;
x.dcgain=20*log10(sumw);
% do linear interpolation in p() to find 3dB and 6dB points
p3=0.5*p(1);
i3=find(p<p3,1);
if ~numel(i3)
    x.band3=Inf;
    x.band6=Inf;
else
    x.band3=2*(i3-(p3-p(i3))/(p(i3-1)-p(i3))-1)/of*fs/nw;
    p6=0.25*p(1);
    i6=find(p<p6,1);
    x.band6=2*(i6-(p6-p(i6))/(p(i6-1)-p(i6))-1)/of*fs/nw;
end
% do linear interpolation in f() to find closest approach to the origin
if~numel(kt)
    x.band0=Inf;
    x.gain0=0;
else
    i0=floor(kt(1));
    df=f(i0+1)-f(i0);
    j0=-real(f(i0)*conj(df))/abs(df)^2;
    x.band0=2*(i0+j0-1)/of*fs/nw;
    p0=abs(f(i0)+j0*df)^2;
    if p0>0
        x.gain0=10*log10(p0/p(1));
    else
        x.gain0=-Inf;
    end
end
% overlap factors
i50=round(nw*0.5);
x.olc50=sum(w(1:nw-i50).*w(1+i50:nw))/sumw2;
i75=round(nw*0.25);
x.olc75=sum(w(1:nw-i75).*w(1+i75:nw))/sumw2;
% processing loss and scalloping loss
x.scallop=10*log10(p(1)/p(1+of/2));
x.ploss=10*log10(enbwbin);
x.wcploss=x.ploss+x.scallop;
co=zeros(1,nw);
co2=co;
w2=w.^2;
for i=1:nw
    if i*fix(nw/i)==nw  % check i is a factor of the window length
        if co(i)==0
            co(i)=all(abs(sum(reshape(w',nw/i,i),2)-i*mean(w))<max(abs(w))*1e-4);
            if co(i)
                co(2*i:i:nw)=-1; % ignore multiples
            end
        end
        if co2(i)==0
            co2(i)=all(abs(sum(reshape(w2',nw/i,i),2)-i*mean(w2))<max(w2)*1e-4);
            if co2(i)
                co2(2*i:i:nw)=-1; % ignore multiples
            end
        end
    end
end
co(co<0)=0;
co2(co2<0)=0;
x.cola=find(co);
x.cola2=find(co2);
%
% now plot it if no output arguments given
%
if ~nargout
    clf;
    subplot(212);
    nf=min(max(floor(2*max(x.band6,x.band0)*of*nw/fs)+1,of*8),length(p));
    ff=(0:nf-1)*fs/(of*nw);
    fqi=[x.enbw x.band3 x.band6]/2;
    if ff(end)>2000
        ff=ff/1000;
        fqi=fqi/1000;
        xlab='kcyc/L';
    else
        xlab='cyc/L';
    end
    dbn=20*log10(x.nw); % window width in dB
    dbrange=min(100,-1.5*x.sidelobe);
    dd=10*log10(max(p(1:nf),p(1)*0.1^(dbrange/10)));
    ffs=[0 ff(end)];
    dbs=repmat(x.dcgain+x.sidelobe,1,2);
    ffb=[0 fqi(1) fqi(1)];
    dbb=[dd(1) dd(1) dd(1)-dbrange];
    ff3=[0 fqi(2) fqi(2)];
    db3=[dd(1)+db(0.5)/2 dd(1)+db(0.5)/2 dd(1)-dbrange];
    ff6=[0 fqi(3) fqi(3)];
    db6=[dd(1)+db(0.5) dd(1)+db(0.5) dd(1)-dbrange];
    area(ffb,dbb-dbn,max(dd)-dbrange-dbn,'facecolor',[1 0.7 0.7]);
    hold on
    plot(ffs,dbs-dbn,':k',ff3,db3-dbn,':k',ff6,db6-dbn,':k',ffb,dbb-dbn,'r',ff,dd-dbn,'b');
    legend(['Equiv Noise BW = ' sprintsi(x.enbw,-2) 'cyc/L'],['Max sidelobe = ' sprintf('%.0f',x.sidelobe) ' dB'],['-3 & -6dB BW = ' sprintf('%.2g',(x.band3)) ' & ' sprintf('%.2g',(x.band6)) ' cyc/L']);
    hold off
    axis([0 ff(end) max(dd)-dbrange-dbn max(dd)+2-dbn]);
    ylabel('Gain/N (dB)');
    xlabel(sprintf('Freq (%s)',xlab));
    %
    % Now plot the window itself
    %
    subplot(211);
    tax=(0:nw-1)/fs-x.ewgdelay;
    area(tax,w,'FaceColor',[0.7 0.7 1]);
    ylabel('Window');
    xlabel('Time/L');
    dtax=(tax(end)-tax(1))*0.02;
    axv=[tax(1)-dtax tax(end)+dtax min(0,min(w)) max(w)*1.05];
    texthvc(tax(end),max(w),sprintf('N=%d',nw),'rtk');
    if length(x.cola)>3
        tcola=sprintf(',%d',x.cola(1:3));
        tcola=[tcola ',...'];
    else
        tcola=sprintf(',%d',x.cola);
    end
    if length(x.cola2)>3
        tcola2=sprintf(',%d',x.cola2(1:3));
        tcola2=[tcola2 ',...'];
    else
        tcola2=sprintf(',%d',x.cola2);
    end
    texthvc(tax(1),max(w),sprintf('COLA=%s\nCOLA^2=%s',tcola(2:end),tcola2(2:end)),'ltk');
    axis(axv);
end


function pdf = pdf_discrete_value ( x_num, x, y, v_num, v )

%*****************************************************************************80
%
%% PDF_DISCRETE_VALUE evaluates the PDF of a discrete histogram.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer V_NUM, the number of sample values.
%
%    Input, real V(V_NUM,1), the sample values.
%
%    Input, integer X_NUM, the number of values in the discrete histogram.
%
%    Input, real X(X_NUM,1), the histogram data values.
%
%    Input, real Y(X_NUM,1), the normalized histogram values.
%
%    Output, real PDF(V_NUM,1), the values of the discrete PDF.
%
  pdf = zeros ( v_num, 1 );
%
%  Seek bracket intervals for each sample point.
%
  b(1:v_num,1) = r8vec_bracket6 ( x_num, x, v_num, v );
%
%  Sample points outside the interval are ignored.
%
  i = find ( b ~= -1 );
%
%  Linearly interpolate values within the interval.
%
  l = b(i);
  r = l + 1;
%
% pdf(i) = (  ( x(r) - v(i)        ) .* y(l)   ...
%          +  (        v(i) - x(l) ) .* y(r) ) ...
%          ./ ( x(r)        - x(l) );

  for i = 1 : v_num
    if ( b(i) ~= -1 )
      l = b(i);
      r = l + 1;
      pdf(i) = (  ( x(r) - v(i)        ) .* y(l)   ...
               +  (        v(i) - x(l) ) .* y(r) ) ...
               ./ ( x(r)        - x(l) );
    end
  end

  return
end


function isContained=rectContainedInRect(rectMin1,rectMax1,rectMin2,rectMax2)
%RECTCONTAINEDINRECT Determine whether an axis-aligned rectangle (or a
%                    more general axis-aligned hyperrectangle if the number
%                    of dimensions is not two) is completely engulfed by
%                    another rectangle. This does not indicate which
%                    rectangle is contained in the other.
%
%INPUTS: rectMin1 A kX1 or 1Xk vector of the lower bounds of each of the
%                 dimensions of the first k-dimensional hyperrectangle.
%        rectMax1 A kX1 or 1Xk vector of the upper bounds of the first
%                 hyperectangle.
%        rectMin2 A kX1 or 1Xk vector of the lower bounds of the second
%                 hyperrectangle.
%        rectMax2 A kX1 or 1Xk vector of the upper bounds of the second
%                 hyperrectangle.
%
%OUTPUTS: isContained A boolean value that is true if one hyperrectangle
%             is completely contained in the other and is false otherwise.
%
%December 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

k=length(rectMin1);

isContained=true;
for curIdx=1:k
    if(rectMin1(curIdx)<rectMin2(curIdx))
        isContained=false;
        break;
    end

    if(rectMax1(curIdx)>rectMax2(curIdx))
       isContained=false;
       break;
    end
end

if(isContained==true)
    return;
end

isContained=true;
for curIdx=1:k
    if(rectMin2(curIdx)<rectMin1(curIdx))
        isContained=false;
        return;
    end

    if(rectMax2(curIdx)>rectMax1(curIdx))
       isContained=false;
       return;
    end
end
end

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


function [sigma,dkm,g,r]=noiseest(dlogCr,dlogr,method)
%Syntax: [sigma,dkm,g,r]=noiseest(dlogCr,dlogr,method)
%_____________________________________________________
%
% Calculates the noise standard deviation from the derivative of the
% Correlation Integral. Requires the auxilary function "dkmminusg".
%
% sigma is the noise standard deviation.
% dkm is the empirical effect of noise on the Correlation Integral.
% g is the theoritical effect of noise on the Correlation Integral.
% r is the range.
% dlogCr is the derivative of the logCr.
% dlogr is the log(range).
% method can take one of the folloing values:
%  'full' for Schreiber's full minimization
%  'mod' for Leontitsis et al. modified minimization
%
%
% References:
%
% Schreiber T (1993): Determination of the noise level of chaotic time
% series. Physical Review E 48(1): R13-R16
%
% Leontitsis A, Pange J., Bountis T. (2003): Large noise level estimation.
% International Journal of Bifurcation and Chaos 13(8): 2309-2313
%
%
% 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
%
% June 15, 2001.

% dlogCr and dlogr must have the same number of rows
if size(dlogCr,1)~=size(dlogr,1)
   error('dlogCr and dlogr must have the same number of rows.');
end

r=10.^dlogr;
options=optimset('Display','off');
for i=2:size(dlogCr,2)
   dkm(:,i-1)=(dlogCr(:,i)-dlogCr(:,1))/(i-1);
   sigma(i-1)=fminbnd(@dkmminusg,r(1),r(end),options,dkm(:,i-1),r,method);
end
z=r./2./sigma(1);
g=2.*z.*exp(-z.^2)./sqrt(pi)./erf(z);

function error=dkmminusg(s,dkm,r,method)
%Syntax: error=dkmminusg(s,dkm,r,method)
%_______________________________________
%
% Auxilary function for "noiseest". Calculates the  error between the dkm
% and function "g" given a noise standard deviation (s) and a vector of
% range values (r).
%
% 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
%
% June 15, 2001.

% Calculate the auxilary variable z
z=r./2./s;

% Calpculate the g fumction
g=2*z.*exp(-z.^2)/sqrt(pi)./erf(z);

switch method
    case 'full'
        % The ordinary (low noise) calculation
        error=norm(dkm-g);
    case 'mod'
        % The modified (large noise) estimation
        if any(dkm<g)==1
            i=find(dkm<g);
            error=sum(abs(dkm(i)-g(i)));
        else
            error=norm(dkm-g,-inf);
        end
   otherwise
       error('You should provide another value for method.');
end


%
% y = ndetrend(x,dim,type,breaks)
%
% NDETREND removes a trend from an N-dimensional array <x> along a given
% dimension <dim>.
%
%       x: array to be detrended
%     dim: dimension along which to detrend, default is first nonsingleton
%    type: linear detrend (1) or demean (0), default is linear.
%  breaks: breakpoint indices for a piecewise linear trend
%
% See also DETREND, MEAN

%   Based on DETREND.M
%   Copyright 1984-2004 The MathWorks, Inc.
%   Modified by Bill Winter May 2006
function x = ndetrend(x,dim,o,b)
siz = size(x);                                      % array size
if nargin < 2, dim = find(siz > 1,1); end           % default: nonsingleton
N = siz(dim);                                       % dimension length
a = ones(N,1);                                      % constant
if nargin < 3 || o                                  % default: linear
    if nargin < 4, b = []; end                      % default: no breaks
    b = unique([1;b(:)]);                           % breaks unique
    b(b < 1 | b > N-1) = [];                        % breaks within array
    l = length(b);                                  % number linear pieces
    a = [zeros(N,l) a];                             % preallocate linear
    M = N - b;                                      % length of linear piece
    for k = 1:l, a(1+b(k):N,k) = (1:M(k))'/M(k); end% linear pieces
end
if length(siz) > 2
    if dim ~= 1                                     % permute 1 with dim
        per = 1:length(siz);
        per([1 dim]) = [dim 1];
        siz([1 dim]) = siz([dim 1]);
        x = builtin('permute',x,per);
    end
    for k = 1:prod(siz(3:end)), x(:,:,k) = x(:,:,k) - a*(a\x(:,:,k));end
    if dim ~= 1, x = builtin('permute',x,per);end   % depermute 1 with dim
elseif dim == 2, x = x - (x/a')*a';                 % right-regress
elseif dim == 1, x = x - a*(a\x);                   % left-regress
end


function cinh_values_test ( )

%*****************************************************************************80
%
%% CINH_VALUES_TEST demonstrates the use of CINH_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CINH_VALUES_TEST:\n' );
  fprintf ( 1, '  CINH_VALUES stores values of\n' );
  fprintf ( 1, '  the Hyperbolic Cosine Integral function CINH(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            CINH(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = cinh_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


%converts frequency to mel
%>
%> @param fInHz: frequency
%> @param cModel: 'Fant','Shaughnessy', or 'Umesh'
%>
%> @retval mel pitch value
% ======================================================================
function [mel] = ToolFreq2Mel(fInHz, cModel)

    if (nargin < 2)
        cModel = 'Fant';
    end

    % set function handle
    hPitchFunc = str2func (['aca' cModel '_I']);

    mel = hPitchFunc(fInHz);
end

% Fant
function [mel] = acaFant_I(f)
    mel = 1000 * log2(1 + f/1000);
end

% Shaughnessy
function [mel] = acaShaughnessy_I(f)
    mel = 2595 * log10(1 + f/700);
end

% Umesh
function [mel] = acaUmesh_I(f)
    mel = f./(2.4e-4*f + 0.741);
end


function A = TriArea3D(X1,X2,X3)

% Xi n*3 are the three points of triangles where n is the number of
% triangle

x1 = X1(:,1);  y1 = X1(:,2);   z1 = X1(:,3);
x2 = X2(:,1);  y2 = X2(:,2);   z2 = X2(:,3);
x3 = X3(:,1);  y3 = X3(:,2);   z3 = X3(:,3);
A = 1/2*(((x1-x3).*(y2-y1) - (x1-x2).*(y3-y1)).^2 + ...
    ((y1-y3).*(z2-z1) - (y1-y2).*(z3-z1)).^2 + ...
    ((z1-z3).*(x2-x1) - (z1-z2).*(x3-x1)).^2).^(1/2);

A = abs(A);

end


function [features,F,M] = lpc2spec(lpcas, nout)
% [features,F,M] = lpc2spec(lpcas,nout)
%    Convert LPC coeffs back into spectra
%    nout is number of freq channels, default 17 (i.e. for 8 kHz)
% 2003-04-11 dpwe@ee.columbia.edu  part of rastamat

if nargin < 2
  nout = 17;
end

[rows, cols] = size(lpcas);
order = rows - 1;

gg = lpcas(1,:);
aa = lpcas./repmat(gg,rows,1);

% Calculate the actual z-plane polyvals: nout points around unit circle
zz = exp((-j*[0:(nout-1)]'*pi/(nout-1))*[0:order]);

% Actual polyvals, in power (mag^2)
features =  ((1./abs(zz*aa)).^2)./repmat(gg,nout,1);

F = zeros(cols, floor(rows/2));
M = F;

for c = 1:cols;
  aaa = aa(:,c);
  rr = roots(aaa');
  ff = angle(rr');
%  size(ff)
%  size(aaa)
  zz = exp(j*ff'*[0:(length(aaa)-1)]);
  mags = sqrt(((1./abs(zz*aaa)).^2)/gg(c))';

  [dummy, ix] = sort(ff);
  keep = ff(ix) > 0;
  ix = ix(keep);
  F(c,1:length(ix)) = ff(ix);
  M(c,1:length(ix)) = mags(ix);
end


% Chapter 8 - Planar Systems.
% Program_8b - Phase Portrait (Fig. 8.8(a)).
% Copyright Birkhauser 2013. Stephen Lynch.

% Phase portrait of a linear system of ODE's.
% IMPORTANT - Program_8a is vectorfield.m.
clear;
% sys=inline('[2*x(1)+x(2);x(1)+2*x(2)]','t', 'x');
sys = @(t,x) [2*x(1)+x(2);x(1)+2*x(2)];
vectorfield(sys,-3:.25:3,-3:.25:3)
     hold on
     for x0=-3:1.5:3
         for y0=-3:1.5:3
            [ts,xs] = ode45(sys,[0 5],[x0 y0]);
            plot(xs(:,1),xs(:,2))
         end
     end
     for x0=-3:1.5:3
         for y0=-3:1.5:3
            [ts,xs] = ode45(sys,[0 -5],[x0 y0]);
            plot(xs(:,1),xs(:,2))
         end
     end
     hold off
axis([-3 3 -3 3])
fsize=15;
set(gca,'XTick',-3:1:3,'FontSize',fsize)
set(gca,'YTick',-3:1:3,'FontSize',fsize)
xlabel('x(t)','FontSize',fsize)
ylabel('y(t)','FontSize',fsize)
hold off

% End of Program_8b.


function IIDAnalysis(Dates,Data)
% this function performs simple invariance (i.i.d.) tests on a time series
% 1. it checks that the variables are identically distributed by looking at the
%    histogram of two subsamples
% 2. it checks that the variables are independent by looking at the 1-lag scatter plot
% under i.i.d. the location-dispersion ellipsoid should be a circle
% see "Risk and Asset Allocation"-Springer (2005), by A. Meucci

% plot time series

figure
h=plot(Dates,Data,'.');
xlim([Dates(1) Dates(end)])
datetick('x','mmmyy','keeplimits','keepticks');
grid on

% test "identically distributed hypothesis": split observations into two sub-samples and plot histogram
Sample_1=Data(1:round(length(Data)/2));
Sample_2=Data(round(length(Data)/2)+1:end);
num_bins_1=round(5*log(length(Sample_1)));
num_bins_2=round(5*log(length(Sample_2)));
X_lim=[min(Data)-.1*(max(Data)-min(Data)) max(Data)+.1*(max(Data)-min(Data))];
[n1,xout1]=hist(Sample_1,num_bins_1);
[n2,xout2]=hist(Sample_2,num_bins_2);

figure
subplot('Position',[0.03 .59 .44 .35])
h1=bar(xout1,n1,1);
set(h1,'FaceColor',[.7 .7 .7],'EdgeColor','k')
set(gca,'ytick',[],'xlim',X_lim,'ylim',[0 max(max(n1),max(n2))])
grid off

subplot('Position',[.53 .59 .44 .35])
h2=bar(xout2,n2,1);
set(h2,'FaceColor',[.7 .7 .7],'EdgeColor','k');
set(gca,'ytick',[],'xlim',X_lim,'ylim',[0 max(max(n1),max(n2))]);
grid off

% test "independently distributed hypothesis": scatter plot of observations at lagged times
subplot('Position',[.28 .01 .43 .43])
X=Data(1:end-1);
Y=Data(2:end);
h3=plot(X,Y,'.');
grid off
axis equal
set(gca,'xlim',X_lim,'ylim',X_lim);

m=mean([X Y])';
S=cov([X Y]);
TwoDimEllipsoid(m,S,2,0,0);


function pass = test_coeffs2vals(pref)
% test various coeffs2vals, vals2coeffs, vals2vals, and coeffs2coeffs codes.
if ( nargin == 0 )
    pref = chebfunpref();
end

tol = 1000*eps;

f = chebfun(@exp, pref);
N = length(f);
c_leg = legcoeffs(f);
c_cheb = chebcoeffs(f);
v_leg = f(legpts(N));
v_cheb1 = f(chebpts(N,1));
v_cheb2 = f(chebpts(N,2));

pass(1) = norm(c_leg - chebcoeffs2legcoeffs(c_cheb), inf) < tol;
pass(2) = norm(c_leg - legvals2legcoeffs(v_leg), inf) < tol;
pass(3) = norm(c_leg - chebvals2legcoeffs(v_cheb1, 1), inf) < tol;
pass(4) = norm(c_leg - chebvals2legcoeffs(v_cheb2, 2), inf) < tol;

pass(5) = norm(c_cheb - legcoeffs2chebcoeffs(c_leg), inf) < tol;
pass(6) = norm(c_cheb - legvals2chebcoeffs(v_leg), inf) < tol;
pass(7) = norm(c_cheb - chebvals2chebcoeffs(v_cheb1, 1), inf) < tol;
pass(8) = norm(c_cheb - chebvals2chebcoeffs(v_cheb2, 2), inf) < tol;

pass(9) = norm(v_leg - legcoeffs2legvals(c_leg), inf) < tol;
pass(10) = norm(v_leg - chebcoeffs2legvals(c_cheb), inf) < tol;
pass(11) = norm(v_leg - chebvals2legvals(v_cheb1, 1), inf) < tol;
pass(12) = norm(v_leg - chebvals2legvals(v_cheb2, 2), inf) < tol;

pass(13) = norm(v_cheb1 - legcoeffs2chebvals(c_leg, 1), inf) < tol;
pass(14) = norm(v_cheb1 - chebcoeffs2chebvals(c_cheb,1), inf) < tol;
pass(15) = norm(v_cheb1 - legvals2chebvals(v_leg, 1), inf) < tol;
pass(16) = norm(v_cheb1 - chebvals2chebvals(v_cheb2, 2, 1), inf) < tol;

pass(17) = norm(v_cheb2 - legcoeffs2chebvals(c_leg, 2), inf) < tol;
pass(18) = norm(v_cheb2 - chebcoeffs2chebvals(c_cheb,2), inf) < tol;
pass(19) = norm(v_cheb2 - legvals2chebvals(v_leg, 2), inf) < tol;
pass(20) = norm(v_cheb2 - chebvals2chebvals(v_cheb1, 1, 2), inf) < tol;

end


function [xPred,SPred,exitCode]=sqrtCDEKFPred(xPrev,SPrev,a,D,AJacob,tPrev,tPred,RKOptions)
%%SQRTCDEKFPRED Predict forward a Gaussian state estimate through time when
%               the evolution of the state is described by a
%               continuous-time stochastic diferential equation using a
%               square root continuous-discrete extended Kalman filter.
%               This uses a Gaussian approximation throughout the entire
%               prediction, allowing for a solution based on deterministic
%               differential equations. The differential equations are
%               integrated forward using the RKAdaptiveOverRange function,
%               an explicit Runge-Kutta method, which can take a number of
%               options.
%
%INPUTS: xPrev The xDim X 1 state estimate at time tPrior.
%        SPrev The xDim X xDim square root state covariance matrix at time
%              tPrior.
%            a The drift function in the continuous-time stochastic
%              dynamic model. It takes the state and a time variable as its
%              arguments.
%            D The diffusion function in the continuous-time stochastic
%              dynamic model. It takes the state and a time variable as its
%              arguments.
%       AJacob The derivative wrt x of the drift function in the
%              continuous-time stochastic dynamic model. It takes the
%              state and a time variable as its arguments. If an empty
%              matrix is passed, then AJacob will be found using
%              numerical differentiation via the numDiff function with
%              default parameters.
%        tPrev The time of xPrev and SPrev.
%        tPred The time to which xPrev and SPrev should be predicted.
%    RKOptions An optional structure whose components have the same name
%              and meaning as the corresponding components in the
%              RKAdaptiveOverRange function. The possible compnents of
%              the RKOptions function (with default values if omitted)
%              are
%              RKOptions.initStepSize: (tPred-tPrev)/RKOptions.maxSteps
%              RKOptions.order:           5
%              RKOptions.solutionChoice:  0
%              RKOptions.RelTol:          1e-3.
%              RKOptions.AbsTol:          1e-6
%              RKOptions.maxSteps:        1024
%              If an empty matrix is passed in place of RKOptions, then
%              only the default values will be used.
%
%OUTPUTS: xPred The xDim X 1 predicted state estimate. If the
%               RKAdaptiveOverRange function failed, which might occur,
%               for example, if the maximum number of allowed steps is
%               too small, then an empty matrix is returned.
%         SPred The xDim X xDim predicted square root state covariance
%               matrix, or an empty matrix if the RKAdaptiveOverRange
%               failed.
%      exitCode The exit code from the RKAdaptiveOverRange function. This
%               is zero if the integration was a success. Otherwise, the
%               value identifies what the problem was. See the
%               RKAdaptiveOverRange function for more details.
%
%The algorithm is modified from the mean-covariance Runge-Kutta (MC-RK4)
%method described in [1]. Other methods in this paper are criticized in
%Section III of [2].
%
%Parts of the calculation for the derivative of the square root state
%covariance matrix are taken from Section VI of [3].
%
%REFERENCES:
%[1] P. Frogerais., J. Bellanger, and L. Senhadji. "Various ways to compute
%    the continuous-discrete extended Kalman filter," IEEE
%    Transactions on Automatic Control, vol. 57, no. 4, pp. 1000-1004,
%    2012.
%[2] G. Y. Kulikov and M. V. Kulikova. "Accurate numerical implementation
%    of the continuous-discrete extended Kalman filter," IEEE Transactions
%    on Automatic Control, vol. 59, no. 1, pp. 273-279, Jan. 2014.
%[3] D. F. Crouse, "Basic tracking using nonlinear continuous-time dynamic
%    models," IEEE Aerospace and Electronic Systems Magazine, vol. 30, no.
%    2, Part II, pp. 4-41, Feb. 2015.
%
%March 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=length(xPrev);

    if(nargin<8)
        RKOptions=[];
    end
    if(isempty(AJacob))
        AJacob=@(x,t)numDiff(x,@(y) a(y,t),xDim);
    end

    %The default options for the RKAdaptiveOverRange function. The
    %initStepSize default option is not set until the other options are
    %read, since the default depends on the value of maxSteps.
    initStepSize=[];
    order=5;
    solutionChoice=0;
    RelTol=1e-3;
    AbsTol=1e-6;
    maxSteps=1024;
    if(~isempty(RKOptions))
        if(isfield(RKOptions,'initStepSize'))
            initStepSize=RKOptions.initStepSize;
        end
        if(isfield(RKOptions,'order'))
            order=RKOptions.order;
        end
        if(isfield(RKOptions,'solutionChoice'))
            solutionChoice=RKOptions.solutionChoice;
        end
        if(isfield(RKOptions,'RelTol'))
            RelTol=RKOptions.RelTol;
        end
        if(isfield(RKOptions,'AbsTol'))
            AbsTol=RKOptions.AbsTol;
        end
        if(isfield(RKOptions,'maxSteps'))
            maxSteps=RKOptions.maxSteps;
        end
    end

    if(isempty(initStepSize))
        initStepSize=(tPred-tPrev)/maxSteps;
    end

    %The zeros in S are not given to the differential equation solver.
    %lowerTriEls holds the indices of the lower-triangular elements.
    lowerTriEls=tril(true(xDim,xDim));
    [xVals,~,~,exitCode]=RKAdaptiveOverRange([xPrev;SPrev(lowerTriEls)],[tPrev tPred],@f,initStepSize,0,order,solutionChoice,RelTol,AbsTol,maxSteps);
    if(isempty(xVals))%If the RKAdaptiveOverRange function failed.
        xPred=[];
        SPred=[];
        return;
    end

    xSVec=xVals(:,end);
    xPred=xSVec(1:xDim,end);
    SEls=xSVec((xDim+1):end,end);
    SPred=zeros(xDim,xDim);
    SPred(lowerTriEls)=SEls;

    function dVal=f(x,t)
        %This function provides the derivatives of the state and the
        %lower-triangular square root covariance matrix at a particular
        %time.
        xCur=x(1:xDim);
        SElsCur=x((xDim+1):end);
        %Only the nonzero elements of S were passed.
        SCur=zeros(xDim,xDim);
        SCur(lowerTriEls)=SElsCur;
        PCur=SCur*SCur';

        dx=a(xCur,t);

        dPoints=D(xCur,t);
        F=AJacob(xCur,t);
        dP=PCur*F'+F*PCur'+dPoints*dPoints';
        SInv=pinv(SCur);
        dS=SCur*triLower(SInv*dP*SInv');

        dVal=[dx;dS(lowerTriEls)];
    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.


%  Figure 10.70      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_87.m is a script to generate Fig. 10.70 the linear RTP response
% LQG method with internal model
% RTP chamber demo Example
clf;
a=[-0.068208813989939 0.014929776357245 0.000000065782442;...
   0.045809672136430 -0.118134773528570 0.021802006306129;...
   0.000000433637498 0.046839194867347 -0.100884399149872];
b3=[0.37873508304055 0.110575403544586 0.022912893467038;...
   0.000000002974222 0.449046982554739 0.073572900808161;...
   0.000000027324116 0.000702342292121 0.417797228783230];
c3=eye(3,3);
d=0*eye(3,3);
sysp=ss(a,b3,c3,d);
[eol]=eig(a);
[zol]=tzero(sysp);
[zol22]=tzero(a,b3(:,2),c3(2,:),d(2,2));
% Combine 3 lamps into a single actuator
b=b3(:,1)+b3(:,2)+b3(:,3);
% Select center temperature
c=c3(2,:)
aa=[zeros(1,1) c;zeros(3,1) a];
bb=[zeros(1,1);b];
%
% weight temperature differences
q1hat=[eye(1,1) zeros(1,3);0 2 -1 -1; 0 -1 2 -1; 0 -1 -1 2]+1e-6*eye(4,4);
q1hat=diag([1 10 10 10])*q1hat;
q2hat=eye(1,1);
[kk,sric,ee]=lqr(aa,bb,q1hat,q2hat);
k1=kk(1,1);
ko=kk(:,2:4);
acl=[a-b*ko b;-k1*c zeros(1,1)];
bcl=[zeros(3,1);k1];
ccl=[c zeros(1,1)];
dcl=zeros(1,1);
[ecl]=eig(acl);
[zcl]=tzero(acl,bcl,ccl,dcl);
%CL DC gain
[cldcgain]=dcl-ccl*inv(acl)*bcl;
%CL Step Response
%[y,t]=step(acl,bcl,ccl,dcl);
%s1y=plot(y(:,:,1));
%grid;
%pause;
%Control effort
%cclu=[-ko eye(1,1)];
%dclu=[0];
%[uu,t]=step(acl,bcl,cclu,dclu);
%su=plot(uu(:,:,1));
%grid;
%pause
%Step in all channels
%t=0:.1:100;
%u=[25*ones(1,251) 25*ones(1,500)0*ones(1,250)];
%u10=[u'];
%sysc1=ss(acl,bcl,ccl,dcl);
%[yy1,t]=lsim(sysc1,u10,t);
%plot(t,yy1)
%grid
%pause;
%cclu=[-ko eye(1,1)];
%dclu=[0];
%syscl2=ss(acl,bcl,cclu,dclu);
%[uu1,t]=lsim(syscl2,u10,t);
%su=plot(uu1(:,:,1));
%grid;
%pause
% Estimator design
qe=eye(1,1);
re=0.001*eye(1,1);
[ll,pp,el]=lqe(a,b,c,qe,re);
ac=zeros(1,1);
bc=-k1;
cc=eye(1,1);
dc=-ko;
acle=[a, b*cc, -b*ko;
   bc*c, ac, zeros(1,3);
   ll*c, b*cc, a-ll*c-b*ko];
bcle=[zeros(3,1);-bc;zeros(3,1)];
ccle=[c, zeros(1,4)];
dcle=zeros(1,1);
[ecle]=eig(acle);
[zcle]=tzero(acle,bcle,ccle,dcle);
dcgain=dcle-ccle*inv(acle)*bcle;
t=0:.1:100;
R=[0:.1:25, 25*ones(1,500), 0*ones(1,250)];
R11=[R'];
syscl=ss(acle,bcle,ccle,dcle);
[yy,t]=lsim(syscl,R11,t);
plot(t,yy,'--');
grid on;
hold on;
plot(t,R11,'-');
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.70 (a) Internal model controller: temperature tracking response');
%legend('y')
pause;
hold off;
cclu=[zeros(1,3), eye(1,1), -ko];
dclu=zeros(1,1);
syscu=ss(acle,bcle,cclu,dclu);
[uuu,t]=lsim(syscu,R11,t);
plot(t(1:753,:),uuu(1:753,:));
hold on;
plot(t(752:818,:),0*ones(67,3));
hold on;
plot(t(753:818,:),uuu(753:818,:),'--');
hold on;
plot(t(819:1001,:),uuu(819:1001,:));
xlabel('Time (sec)');
ylabel('Lamp voltage');
title('Fig. 10.70 (b) Internal model controller: control effort');
legend('u')
grid;
hold off;


function [qt1,qdt1,qddt1,time1] = interpola3(delta_t,qi,qf,t)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%INTERPOLADOR DE 3ER ORDEN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
amax = 2; %rad/s/s
wmax = 1; %rad/s
t=[0 t];

q=[qi qf];

[qt1,qdt1,qddt1,time1,k1]=interpola3orden(q,  [0 wmax], [0 amax], t, delta_t);

function [q_t, qd_t, qdd_t, time, k]=interpola3orden(q, qd, qdd, t, delta_t)

A=[1 t(1) t(1)^2 t(1)^3;
   1 t(2) t(2)^2 t(2)^3;
   0   1  2*t(1) 3*t(1)^2;
   0   0   2     6*t(1)];
k=inv(A)*[q(1) q(2) qd(1) qdd(1)]';

time=t(1):delta_t:t(2);
q_t = k(1) + k(2)*time + k(3)*time.^2 + k(4)*time.^3;
qd_t= k(2)*ones(1,length(time)) + 2*k(3)*time + 3*k(4)*time.^2;
qdd_t=2*k(3)*ones(1,length(time)) + 6*k(4)*time;


function [Recognized_index filename] = Recognition(Test_image, Mean, A, eigenfaces)

ProjectedImages = [];
Train_Number = size(eigenfaces,2);
for i = 1 : Train_Number
    temp = eigenfaces'*A(:,i);
    ProjectedImages = [ProjectedImages temp];
end

% Extract the PCA features from test image

temp = Test_image;

[irow icol] = size(temp);
InImage = reshape(temp',irow*icol,1);
Difference = double(InImage)-Mean;
ProjectedTestImage = eigenfaces'*Difference;

% Calculate Euclidean distances
% Test image is supposed to have minimum distance with its corresponding
% image in the training database.

dist = [];
for i = 1 : Train_Number
    q = ProjectedImages(:,i);
    temp = ( norm( ProjectedTestImage - q ) )^2;
    dist = [dist temp];
end

[dist_min , Recognized_index] = min(dist);

 filename=sprintf('s%d.1.tif', Recognized_index);
   disp(['matched image is ',filename]);

subplot(1,2,1); imshow(Test_image);
title('Test Image');
subplot(1,2,2); imshow(filename);
title('Matched Image')

end


function y=PSNR_RGB(X,Y)

% Y= PSNR_RGB(X,Y)
% Computes the Peak Signal to Noise Ratio for two RGB images
% Class input : double [0,1] ,
% july ,25, 2012
% KHMOU Youssef

if size(X)~=size(Y)
    error('The images must have the same size');
end

%if ~isa(X,'double')
%   X=double(X)./255.00;
%end
%if  ~isa(Y,'double')
%    Y=double(Y)./255.00;
%end

% begin
d1=max(X(:));
d2=max(Y(:));
d=max(d2);
sigma=mean2((X-Y).^2);

y=10*log10((d.^2)./sigma);


%THIS FUMCTION ILLUSTRATES HOW TO USE THE USER-DEFINED ALGORITHM
function [AH,XH] = user_alg5(Y,r,X)
%
% The example of implementing the user-defined algorithm (this is the Lee-Seung algorithm based on the KL divergence)
%
% INPUTS:
% Y - mixed signals (matrix of size [m by T])
% r - number of estimated signals
% X - true source signals
%
% OUTPUTS
% AH - estimated mixing matrix (matrix of size [m by r])
% XH - estimated source signals (matrix of size [r by T])
%
% #########################################################################
% Initialization
[m,T]=size(Y);
Y(Y <=0) = eps; % this enforces the positive value in the data
AH=rand(m,r);
XH=rand(r,T);

IterNo = 1000; % number of alternating steps

% Iterations
for k = 1:IterNo
    XH = XH.*(AH'*(Y./(AH*XH + eps)));
    AH = AH.*((Y./(AH*XH + eps))*XH')./repmat(sum(XH,2)',m,1);
    AH = AH*diag(1./(sum(AH,1) + eps));
end


function M = shapefitfactory(VJt)
% Linear manifold structure for optimization over the ShapeFit search space
%
% function M = shapefitfactory(VJt)
%
% Input: VJt is a matrix of size dxn, such that VJt * ones(n, 1) = 0.
%
% Returns M, a structure describing the Euclidean space of d-by-n matrices
% equipped with the standard Frobenius distance and associated trace inner
% product, as a manifold for Manopt. Matrices on M, denoted by T, have size
% dxn and obey T*ones(n, 1) = 0 (centered columns) and <VJt, T> = 1, where
% <A, B> = Trace(A' * B).
%
% See this paper: http://arxiv.org/abs/1506.01437
% ShapeFit: Exact location recovery from corrupted pairwise directions, 2015
% Paul Hand, Choongbum Lee, Vladislav Voroninski
%
% See also: shapefit_smoothed

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, June 18, 2015.
% Contributors:
% Change log:
%
%   Jan. 25, 2017 (NB):
%       M.tangent = M.proj now, instead of being identity. This is notably
%       necessary so that checkgradient will pick up on gradients that do
%       not lie in the appropriate tangent space.
%
%   Jan. 4, 2021 (NB):
%       Changes for compatibility with Octave 6.1.0.

    [d, n] = size(VJt);

    M.name = @() sprintf('ShapeFit space of size %d x %d', d, n);

    M.dim = @() d*n - d - 1;

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) norm(x-y, 'fro');

    M.typicaldist = @() sqrt(d*n);

    VJt_normed = VJt / norm(VJt, 'fro');
    M.proj = @(T, U) projection(U, VJt_normed);
    function PU = projection(U, VJt_normed)
        % Center the columns
        PU = bsxfun(@minus, U, mean(U, 2));
        % Remove component along VJt
        % Note: these two actions can be executed separately, without
        % interference, owing to VJt having centered columns itself.
        PU = PU - (VJt_normed(:)'*U(:))*VJt_normed;
    end

    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @(x, eg, eh, d) projection(eh, VJt_normed);

    M.tangent = M.proj;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

    M.log = @(x, y) y-x;

    M.hash = @(x) ['z' hashmd5(x(:))];

    M.randvec = @(x) randvec(VJt, VJt_normed);
    function u = randvec(VJt, VJt_normed)
        u = projection(randn(size(VJt)), VJt_normed);
        u = u / norm(u, 'fro');
    end

    % We exploit the fact that VJt_normed belongs to the manifold
    M.rand = @() VJt_normed + randn(1) * randvec(VJt, VJt_normed);

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(d, n);

    M.transp = @(x1, x2, d) d;

    M.pairmean = @(x1, x2) .5*(x1+x2);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [d, n]);
    M.vecmatareisometries = @() true;

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script for surface smoothing
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% preparation
% user must add the path of iso2mesh to matlab path list
% addpath('../');

% user need to add the full path to .../iso2mesh/bin directory
% to windows/Linux/Unix PATH environment variable

%% load the sample data
load rat_head.mat

% volimage is a volumetric image such as an X-ray or MRI image
% A,b are registration matrix and vector, respectively
%% perform mesh generation

[node,face]=v2s(volimage,0.5,2,'cgalmesh');

face=face(:,1:3);

p0=min(node);
p1=max(node);

rownum=3;
colnum=4;
figure;
subplot(rownum,colnum,1);

plotmesh(node,face(:,1:3));
if(~isoctavemesh)
	title({'Laplacian+HC Smoothing Test','no smoothing'});
else
	title('Laplacian+HC - no smoothing');
end
axis equal;
set(gca,'xlim',[p0(1),p1(1)],'ylim',[p0(2),p1(2)],'zlim',[p0(3),p1(3)])

%=========================================================
% apply Laplacian+HC smoothing
%=========================================================

n1=node;
for i=1:rownum*colnum-1
  n1=sms(n1,face(:,1:3),1,0.5); % apply Laplacian+HC mesh smoothing
  subplot(rownum,colnum,i+1);
  plotmesh(n1,face(:,1:3));
  title(['iter=' num2str(i)]);
  axis equal;
  set(gca,'xlim',[p0(1),p1(1)],'ylim',[p0(2),p1(2)],'zlim',[p0(3),p1(3)])
end

%=========================================================
% apply Laplacian smoothing
%=========================================================

figure;
subplot(rownum,colnum,1);

plotmesh(node,face(:,1:3));
if(~isoctavemesh)
        title({'Laplacian Smoothing Test','no smoothing'});
else
        title('Laplacian - no smoothing');
end
axis equal;
set(gca,'xlim',[p0(1),p1(1)],'ylim',[p0(2),p1(2)],'zlim',[p0(3),p1(3)])

conn=meshconn(face(:,1:3),size(node,1));

n1=node;
for i=1:rownum*colnum-1
  n1=smoothsurf(n1,[],conn,1,0.5,'laplacian');
  subplot(rownum,colnum,i+1);
  plotmesh(n1,face(:,1:3));
  title(['iter=' num2str(i)]);
  axis equal;
  set(gca,'xlim',[p0(1),p1(1)],'ylim',[p0(2),p1(2)],'zlim',[p0(3),p1(3)])
end

%=========================================================
% apply Low-pass smoothing
%=========================================================

figure;
subplot(rownum,colnum,1);

plotmesh(node,face(:,1:3));
if(~isoctavemesh)
        title({'Low-pass Smoothing Test','no smoothing'});
else
        title('Low-pass - no smoothing');
end
axis equal;
set(gca,'xlim',[p0(1),p1(1)],'ylim',[p0(2),p1(2)],'zlim',[p0(3),p1(3)])

conn=meshconn(face(:,1:3),size(node,1));

n1=node;
for i=1:rownum*colnum-1
  n1=smoothsurf(n1,[],conn,1,0.5,'lowpass');
  subplot(rownum,colnum,i+1);
  plotmesh(n1,face(:,1:3));
  title(['iter=' num2str(i)]);
  axis equal;
  set(gca,'xlim',[p0(1),p1(1)],'ylim',[p0(2),p1(2)],'zlim',[p0(3),p1(3)])
end


function psi_test ( )

%*****************************************************************************80
%
%% PSI_TEST tests R4_PSI and R8_PSI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PSI_TEST:\n' );
  fprintf ( 1, '  Test PSI_VALUES, R4_PSI, R8_PSI.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X         PSI(X)\n' );
  fprintf ( 1, '                    R4_PSI(X)         Diff\n' );
  fprintf ( 1, '                    R8_PSI(X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = psi_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_psi ( single ( x ) );
    fx3 = r8_psi ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function varargout = gradientFD(f, dn, dim, deriv_order, accuracy_order)
%GRADIENTFD Calculate the gradient using a finite-difference method.
%
% DESCRIPTION:
%       gradientFD calculates the gradient of an n-dimensional input matrix
%       using the finite-difference method. For one-dimensional inputs, the
%       gradient is always computed along the non-singleton dimension. For
%       higher dimensional inputs, the gradient for singleton dimensions is
%       returned as 0. For elements in the centre of the grid, the gradient
%       is computed using centered finite-differences. For elements on the
%       edge of the grid, the gradient is computed using forward or
%       backward finite-differences. The order of accuracy of the
%       finite-difference approximation is controlled by accuracy_order
%       (default = 2). The calculations are done using sparse
%       multiplication, so the input matrix is always cast to double
%       precision.
%
% USAGE:
%       fx = gradientFD(f, dx)
%       fx = gradientFD(f, dx, [], deriv_order)
%       fx = gradientFD(f, dx, [], deriv_order, accuracy_order)
%       fn = gradientFD(f, dn, dim)
%       fn = gradientFD(f, dn, dim, deriv_order, accuracy_order)
%       [fx, fy] = gradientFD(f, dn)
%       [fx, fy] = gradientFD(f, dn, [], deriv_order, accuracy_order)
%       [fx, fy, fz, ...] = gradientFD(f, dn)
%       [fx, fy, fz, ...] = gradientFD(f, dn, [], deriv_order, accuracy_order)
%
% INPUTS:
%       f           - matrix or vector to find the gradient of
%       dn          - array of values for the grid point spacing in each
%                     dimension. If a value for dim is given, dn is the
%                     spacing in dimension dim.
%
% OPTIONAL INPUTS:
%       dim             - optional input to specify a single dimension over
%                         which to compute the gradient for n-dimension
%                         input functions
%       deriv_order     - order of the derivative to compute, e.g., use 1
%                         to compute df/dx, 2 to compute df^2/dx^2, etc.
%                         (default = 1)
%       accuracy_order  - order of accuracy for the finite difference
%                         coefficients. Because centered differences are
%                         used, this must be set to an integer multiple of
%                         2 (default = 2)
%
% OUTPUTS:
%       fx, fy, ... - gradient in the each dimension, where x corresponds
%                     to dim = 1, y corresponds to dim = 2 etc
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 24th August 2012
%       last update - 3rd September 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also getFDMatrix, gradient, gradientSpect

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

% % display warning if input is not double precision
% if ~isa(f, 'double')
%     disp('gradientFD: converting input to double precision');
% end

% force input to be in double precision (sparse operations are only
% supported in double or logical formats)
f = double(f);

% get size of the input function
sz = size(f);

% check dimension of input
f_dims = numDim(f);

% check for user input for accuracy_order
if nargin < 5 || isempty(accuracy_order)
    accuracy_order = 2;
elseif rem(accuracy_order, 2)
    error('Input for accuracy_order must be an integer multiple of 2');
end

% check for user input for deriv_order
if nargin < 4 || isempty(deriv_order)
    deriv_order = 1;
elseif ~((deriv_order > 0) && (deriv_order == round(deriv_order)))
    error('Input for deriv_order must be an integer > 0');
end

% check for user input for dim
if (nargin < 3) || isempty(dim)

    % check if input is 1D
    if f_dims == 1
        % only compute gradient over required dimension
        if sz(1) == 1
            dim_array = 2;
        else
            dim_array = 1;
        end
    else
        % otherwise compute gradient over all dimensions
        dim = 0;
        dim_array = 1:f_dims;

        % check for the correct number of dn values
        if length(dn) ~= length(sz)
            error([num2str(length(sz)) ' values for dn must be specified for a ' num2str(length(sz)) '-dimensional input matrix']);
        end
    end
else
    dim_array = dim;
end

% only allow 1, 2, or 3D inputs (only these cases are implemented)
if f_dims > 3
    error('Input for f must have 1, 2, or 3 dimensions');
end

% if input is 1D, only allow it to be a row or column vector
if (f_dims == 1) && (length(sz) > 2)
    error('1D inputs for f must be row or column vectors');
end

% if input is 2D, only allow it to be a regular matrix
if (f_dims == 2) && (length(sz) > 2)
    error('2D inputs for f must be of size m x n');
end

% if input is 2D or 3D, check dim input isn't bigger than the matrix size
if (f_dims > 1) && (dim > f_dims)
    error('Input for dim cannot be greater than the number of dimensions of f');
end

% set output argument index
argout_index = 1;

% loop through required dimensions
for dim_index = dim_array

    % set dimension
    dim = dim_index;

    % check if a single dn value is given, if not, extract the required
    % value
    if numel(dn) ~= 1
        dn_val = dn(dim);
    else
        dn_val = dn;
    end

    % get the grid size along the specified dimension, or the longest
    % dimension if 1D
    if nargout == 1 && (max(sz) == prod(sz))
        [Nx, dim] = max(sz);
    else
        Nx = sz(dim);
    end

    % get the finite-difference matrix
    FDM = getFDMatrix(Nx, dn_val, deriv_order, accuracy_order);

    % compute derivatives of 1D or 2D matrix
    if ndims(f) < 3
        switch dim

            % derivative along dimension 1 (columns)
            case 1
                varargout{argout_index} = FDM * f;

            % derivative along dimension 2 (rows)
            case 2
                varargout{argout_index} = f * FDM.';

        end

    % compute derivatives of 3D matrix
    else
        switch dim

            % derivative along dimension 1
            case 1

                % preallocate output matrix
                varargout{argout_index} = zeros(size(f));

                % loop through z-plane
                for dim3_index = 1:sz(3)
                    varargout{argout_index}(:, :, dim3_index) = FDM * f(:, :, dim3_index);
                end

            % derivative along dimension 2
            case 2

                % preallocate output matrix
                varargout{argout_index} = zeros(size(f));

                % rotate FDM
                FDM = FDM.';

                % loop through z-plane
                for dim3_index = 1:sz(3)
                    varargout{argout_index}(:, :, dim3_index) = f(:, :, dim3_index) * FDM;
                end

            % derivative along dimension 3
            case 3

                % preallocate output matrix
                varargout{argout_index} = zeros(size(f));

                % rotate FDM
                FDM = FDM.';

                % loop through x-plane
                for dim1_index = 1:sz(1)
                    varargout{argout_index}(dim1_index, :, :) = squeeze(f(dim1_index, :, :)) * FDM;
                end

        end

    end

    % increment output argument index
    argout_index = argout_index + 1;

end


% VL_IMINTEGRAL  Compute integral image
%   J = VL_IMINTEGRAL(I) calculates the integral image J of the image
%   I.  I must a matrix with DOUBLE, SINGLE, UINT32, or INT32 storage
%   class. J is given by
%
%    J(i,j) = sum(I(1:i,1:j)).
%
%   J has the same size as I and the same storage class.
%
%   Example::
%     The following identity holds:
%       VL_IMINTEGRAL(ONES(3)) = [ 1 2 3 ;
%                                  2 4 6 ;
%                                  3 6 9 ]
%
%   See also: VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


% Modelling Power Law Absorption Example
%
% This example describes the characteristics of the absorption and
% dispersion encapsulated by the k-Wave simulation functions
%
% For a more detailed discussion of the absorption model used in k-Wave,
% see Treeby, B. E. and Cox, B. T., "Modeling power law absorption and
% dispersion for acoustic propagation using the fractional Laplacian," J.
% Acoust. Soc. Am., vol. 127, no. 5, pp. 2741-2748, 2010.
%
% author: Bradley Treeby
% date: 4th February 2011
% last update: 24th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

% modify this parameter to run the different examples
example_number = 1;
% 1: Simulation using kspaceSecondOrder
% 2: Simulation using kspaceFirstOrder1D with default CFL
% 3: Simulation using kspaceFirstOrder1D with CFL = 0.05

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 1024;      % number of grid points in the x (row) direction
x = 12.8e-3;    % grid size in the x direction [m]
dx = x/Nx;      % grid point spacing in the x direction [m]
kgrid = makeGrid(Nx, dx);

% define the properties of the propagation medium
medium.sound_speed = 1500;  % [m/s]

% define time array
t_end = 4e-6;
if example_number < 3
    [kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed, [], t_end);
else
    [kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed, 0.05, t_end);
end

% create a spatial delta pulse
source_pos = Nx/4;              % [grid points]
source.p0 = zeros(Nx, 1);
source.p0(source_pos) = 1;

% define the sensor positions
source_sensor_dist = 0.5e-3;    % [m]
sensor_sensor_dist = 1e-3;      % [m]
sensor_pos_1 = source_pos + round(source_sensor_dist/dx);
sensor_pos_2 = source_pos + round((source_sensor_dist + sensor_sensor_dist)/dx);

% calculate discrete distance between the sensor positions
d = (sensor_pos_2 - sensor_pos_1)*dx;   % [m]
d_cm = d*100;

% index where the relative dispersion is defined
f_index = 30;

% create a Binary sensor mask
sensor.mask = zeros(Nx, 1);
sensor.mask(sensor_pos_1) = 1;
sensor.mask(sensor_pos_2) = 1;

% preallocate the storage variables
attenuation = zeros(3,  floor(length(kgrid.t_array)/2) + 1);
attenuation_th = zeros(3, floor(length(kgrid.t_array)/2) + 1);
cp = zeros(3, floor(length(kgrid.t_array)/2) + 1);
cp_kk = zeros(3, floor(length(kgrid.t_array)/2) + 1);

for loop = 1:3

    % define the absorption properties of the propagation medium
    switch loop
        case 1
            medium.alpha_coeff = 0.5;
            medium.alpha_power = 1.1;
        case 2
            medium.alpha_coeff = 0.25;
            medium.alpha_power = 1.5;
        case 3
            medium.alpha_coeff = 0.1;
            medium.alpha_power = 1.9;
    end

    % run the simulation without visualisation
    if example_number == 1
        sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor, 'PlotSim', false);
    else
        sensor_data = kspaceFirstOrder1D(kgrid, medium, source, sensor, 'PlotSim', false);
    end

    % calculate the amplitude and phase spectrum at the two sensor
    % positions
    [f, as1, ps1] = spect(sensor_data(1, :), 1/dt);
    [f, as2, ps2] = spect(sensor_data(2, :), 1/dt);

    % calculate the attenuation from the amplitude spectrums
    attenuation(loop, :) = -20*log10(as2./as1)./d_cm;

    % calculate the corresponding theoretical attenuation in dB/cm
    attenuation_th(loop, :) = medium.alpha_coeff.*(f./1e6).^medium.alpha_power;

    % calculate the dispersion (dependence of the sound speed on frequency)
    % from the phase spectrums
    cp(loop, :) = 2*pi.*f.*d./(unwrap(ps1) - unwrap(ps2));

    % calculate the corresponding theoretical dispersion using the
    % Kramers-Kronig relation for power law absorption
    cp_kk(loop, :) = powerLawKramersKronig(2*pi*f, 2*pi*f(f_index), cp(loop, f_index), db2neper(medium.alpha_coeff, medium.alpha_power), medium.alpha_power);
end

% =========================================================================
% VISUALISATION
% =========================================================================

% set downsampling factor so there is sufficient space between plot markers
ds = 8;

% plot the attenuation
figure;
f_max = 50;
plot(f(1:ds:end)./1e6, attenuation(:, 1:ds:end), 'ko', f./1e6, attenuation_th, 'k-');
set(gca, 'XLim', [0 f_max]);
box on;
xlabel('Frequency [MHz]');
ylabel('\alpha [dB/cm]');

% label the plots
text(40, 160, 'y = 1.9');
text(40, 90, 'y = 1.5');
text(40, 40, 'y = 1.1');

% plot the dispersion
figure
plot(f(1:ds:end)./1e6, cp(:, 1:ds:end), 'ko', f./1e6, cp_kk, 'k-');
set(gca, 'XLim', [0 f_max]);
box on;
xlabel('Frequency [MHz]');
ylabel('C_p [m/s]');

% label the plots
text(40, 1517, 'y = 1.1');
text(40, 1509, 'y = 1.5');
text(40, 1500, 'y = 1.9');


function determ = rectangle_adj_determinant ( row_num, col_num )

%*****************************************************************************80
%
%% RECTANGLE_ADJ_DETERMINANT returns the determinant of the RECTANGLE_ADJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ROW_NUM, COL_NUM, the number of rows and
%    columns in the rectangle.
%
%    Output, real DETERM, the determinant.
%

%
%  If ROW_NUM == 1 or COL_NUM == 1 we have a case of the LINE_ADJ matrix.
%
  if ( row_num == 1 )

         if ( mod ( row_num, 4 ) == 1 )
      determ =   0.0;
    elseif ( mod ( row_num, 4 ) == 2 )
      determ = - 1.0;
    elseif ( mod ( row_num, 4 ) == 3 )
      determ =   0.0;
    elseif ( mod ( row_num, 4 ) == 0 )
      determ = + 1.0;
    end

  elseif ( col_num == 1 )

         if ( mod ( col_num, 4 ) == 1 )
      determ =   0.0;
    elseif ( mod ( col_num, 4 ) == 2 )
      determ = - 1.0;
    elseif ( mod ( col_num, 4 ) == 3 )
      determ =   0.0;
    elseif ( mod ( col_num, 4 ) == 0 )
      determ = + 1.0;
    end
%
%  Otherwise, we can form at least one square, hence a null vector,
%  hence the matrix is singular.
%
  else

    determ = 0.0;

  end

  return
end


function [dWx,dWy] = vl_dwaffine(x,y)
% VL_DWAFFINE  Derivative of an affine warp
%   [DWX,DWY]=VL_DWAFFINE(X,Y) returns the derivative of the 2-D affine
%   warp [WX; WY] = [A T] [X; Y] with respect to the parameters A,T
%   computed at points X,Y.
%
%   See also: VL_WAFFINE(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% dW = [ kron(x',I) I ]
%    |
%    = [ x1  0  x2  0 1 0 ]
%      [  0 x1   0 x2 0 1 ]

z = zeros(length(x(:)),1) ;
o =  ones(length(x(:)),1) ;

dWx = [ x(:) z      y(:) z      o z ] ;
dWy = [ z    x(:)   z    y(:)   z o ] ;