function newval=meshinterp(fromval,elemid,elembary,fromelem)
%
% newval=meshinterp(fromval,elemid,elembary,fromelem)
%
% Interpolate nodal values from the source mesh to the target mesh based on
% a linear interpolation
%
% author: Qianqian Fang (q.fang at neu.edu)
%
% input:
%	 fromval: values defined at the source mesh nodes, the row or column
%	          number must be the same as the source mesh node number, which
%	          is the same as the elemid length
%	 elemid: the IDs of the source mesh element that encloses the nodes of
%            the target mesh nodes; a vector of length of target mesh node
%            count; elemid and elembary can be generated by calling
%
%           [elemid,elembary]=tsearchn(node_src, elem_src, node_target);
%
%           note that the mapping here is inverse to that in meshremap()
%
%	 elembary: the bary-centric coordinates of each target mesh nodes
%	         within the source mesh elements, sum of each row is 1, expect
%	         3 or 4 columns (or can be N-D)
%    fromelem: the element list of the source mesh
%
%
% output:
%	 newval: a 2D array with rows equal to the target mesh nodes (nodeto),
%            and columns equals to the value numbers defined at each source
%            mesh node
% example:
%
%    [n1,f1,e1]=meshabox([0 0 0],[10 20 5],1); % target mesh
%    [n2,f2,e2]=meshabox([0 0 0],[10 20 5],2); % src mesh
%    [id, ww]=tsearchn(n2,e2,n1);              % project target to src mesh
%    value_src=n2(:,[2 1 3]);             % create dummy values at src mesh
%    newval=meshinterp(value_src,id, ww, e2);
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(size(fromval,1)==1)
    fromval=fromval(:);
end

idx=find(~isnan(elemid));

allval=reshape(fromval(fromelem(elemid(idx),:),:),length(idx),size(elembary,2),size(fromval,2));
tmp=cellfun(@(x) sum(elembary(idx,:).*x,2), num2cell(allval,[1 2]),'UniformOutput',false);
newval=size(length(elemid),size(fromval,2));
newval(idx,:)=squeeze(cat(3,tmp{:}));


function x = pvand ( n, alpha, b )

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

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

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

  return
end


function [ r, seed ] = r4_uniform_01 ( seed )

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

  seed = floor ( seed );

  seed = mod ( seed, 2147483647 );

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

  k = floor ( seed / 127773 );

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

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

  r = seed * 4.656612875E-10;

  return
end


function a = conex1_inverse ( alpha )

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

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

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

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

  return
end


%% Vehicle Articulated Linear
% Linear articulated bicycle model with 4 degrees of freedom.
%
%
%
%
%
%% Sintax
% |dx = _VehicleModel_.Model(t,states,tspan)|
%
% |dx = _VehicleModel_.MassMatrix(t,states,tspan)|
%
%% Arguments
% The following table describes the input arguments:
%
%

%
 <tt>t</tt>	 Time

%
 <tt>states</tt>	 Model state variables: [XT YT PSI PHI VT ALPHAT dPSI dPHI]

%
 <tt>tspan</tt>	 Time span

%

%
%% See Also
%
% <https://www.mathworks.com/matlabcentral/fileexchange/58683-vehicle-dynamics-lateral Home>
%


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

clear variables
close all

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

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

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

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

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

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

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

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

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

VBA_getSubplots ();


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

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

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

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

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

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

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


%BOXFILTER  Blurs an image using the box filter
%
%     dst = cv.boxFilter(src)
%     dst = cv.boxFilter(src, 'OptionName',optionValue, ...)
%
% ## Input
% * __src__ input image.
%
% ## Output
% * __dst__ output image of the same size and type as `src`.
%
% ## Options
% * __DDepth__ the output image depth (-1 to use `class(src)`). Default -1.
%   See cv.filter2D for details.
% * __KSize__ blurring kernel size. Default [5,5]
% * __Anchor__ anchor point `[x,y]`; default value [-1,-1] means that the
%   anchor is at the kernel center.
% * __Normalize__ flag, specifying whether the kernel is normalized by its
%   area or not. default true
% * __BorderType__ border mode used to extrapolate pixels outside of the
%   image. See cv.copyMakeBorder. Default 'Default'
%
% The function smooths an image using the kernel:
%
%     K = alpha * ones(KSize)
%
% where:
%
%             | 1/prod(KSize)  when Normalize=true
%     alpha = |
%             | 1              otherwise
%
% Unnormalized box filter is useful for computing various integral
% characteristics over each pixel neighborhood, such as covariance matrices
% of image derivatives (used in dense optical flow algorithms, and so on). If
% you need to compute pixel sums over variable-size windows, use cv.integral.
%
% See also: cv.blur, cv.bilateralFilter, cv.GaussianBlur, cv.medianBlur,
%  cv.integral, cv.sqrBoxFilter, imfilter, imboxfilt
%


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

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

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

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

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

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

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

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


function cdf_discrete_test ( )

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

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

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

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

  return
end


function att = Cbn2att(Cbn);
%-------------------------------------------------------
% Yudan Yi, May 26, 2005
%-------------------------------------------------------
% attitude: roll, pitch and heading
att(1) = atan2(Cbn(3,2),Cbn(3,3));
att(2) = asin(-Cbn(3,1));
att(3) = atan2(Cbn(2,1),Cbn(1,1));
att = att(:);
return;


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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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


function cdf = student_noncentral_cdf ( x, idf, d )

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

  f = idf;

  if ( idf == 1 )

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

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

  elseif ( idf <= a_max )

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

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

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

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

    ak = 1.0;

    for k = 2 : 2 : idf - 2

      fk = k;

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

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

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

      ak = 1.0 / ( ak * fk );

    end

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

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

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

    cdf2 = normal_01_cdf ( temp );
    cdf = cdf2;

  end

  return
end


%MDL_TWOLINK_SYM Create symbolic model of a simple 2-link mechanism
%
% MDL_TWOLINK_SYM is a script that creates the workspace variable twolink
% which describes in symbolic form the kinematic and dynamic
% characteristics of a simple planar 2-link mechanism moving in the
% xz-plane, it experiences gravity loading.  The symbolic parameters are:
%  - link lengths: a1, a2
%  - link masses: m1, m2
%  - link CoMs in the link frame x-direction: c1, c2
%  - gravitational acceleration: g
%  - joint angles: q1, q2
%  - joint angle velocities: qd1, qd2
%  - joint angle accelerations: qdd1, qdd2
%
% Notes::
% - It is a planar mechanism operating in the vertical plane and is
%   therefore affected by gravity (unlike mdl_planar2 in the horizontal
%   plane).
% - Gear ratio is 1 and motor inertia is 0.
% - Link inertias Iyy1, Iyy2 are 0.
% - Viscous and Coulomb friction is 0.
%
% References::
%  - Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition).
%
% See also mdl_puma560, mdl_stanford, SerialLink.

% MODEL: generic, planar, dynamics, 2DOF, symbolic, standard_DH

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

syms a1 a2 g real
syms c1 c2 m1 m2 real

%syms Iyy1 Iyy2 b1 b2 real
Iyy1 = 0;
Iyy2 = 0;
b1 = 0;
b2 = 0;

twolink = SerialLink([
    Revolute('d', 0, 'a', a1, 'alpha', 0, 'm', m1, 'r', [c1 0 0], 'I', [0 Iyy1 0], 'B', b1, 'G', 1, 'Jm', 0, 'standard')
    Revolute('d', 0, 'a', a2, 'alpha', 0, 'm', m2, 'r', [c2 0 0], 'I', [0 Iyy2 0], 'B', b2, 'G', 1, 'Jm', 0, 'standard')
    ], ...
    'name', 'two link', ...
    'comment', 'from Spong, Hutchinson, Vidyasagar');
twolink = twolink.sym();
twolink.gravity = [0; 0; g];
twolink.base = trotx(pi/2);

syms q1 q2 q1d q2d q1dd q2dd real


function [ratio, ratiominus, ratioplus, proba_post, yt] = classification_evaluation(bnet, BDT, class)
% Computes the classification ratio of a bnet structure on a test dataset BDT
% [ratio ratiominus ratioplus] = classification_evaluation(bnet, BDT, class)
%
% [ratiominus rationplus] is the 95 percent confident interval.
% results are in percentage [0 100].
%
%  francois.olivier.c.h@gmail.com
%

    [proba_post,engine] = inference(bnet, mat_to_bnt(BDT), class);
               [tmp yt] = max(proba_post, [],2);
                  [N L] = size(BDT);
                  count = length(find(BDT(class,:)==yt'));
                  ratio = 100*count/L;
[ratiominus, ratioplus] = confiance(ratio,L);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [I, J] = confiance(t, N)
% Compute the 95 percent confident interval
%
% see Y. Bennani and F. Bossaert,
%     Predictive neural networks for traffic disturbance detection in the telephone network
%     In Proceedings of IMACS-CESA 1996, Lille, France.

Z    = 1.96; % this value for the 95 percent confident interval
T    = t/100;
tmp  = (Z*Z)/N;
D    = 1+tmp;
N1   = T+tmp/2;
tmp2 = T*(1-T)/N + tmp/(4*N);
N2   = Z*sqrt(tmp2);
I    = 100*(N1-N2)/D;
J    = 100*(N1+N2)/D;


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

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

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

    temp1 = 0.0;
    temp2 = i / nsub;

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

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

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

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

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

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

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

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

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

       end

    end

  end

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

  return
end


function varargout = lambda_min(varargin)
% LAMBDA_MIN Returns largest eigenvalue of Hermitian matrix.
%
% r = LAMBDA_MIN(X)
%
% See also LAMBDA_MAX

switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)
        error(nargchk(1,1,nargin));
        X = varargin{1};
        [n,m] = size(X);
        if ~ishermitian(X)
            error('LAMBDA_MIN can only be applied on Hermitian matrices');
        else
            varargout{1} = min(real(eig(X)));
        end

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        error(nargchk(1,1,nargin));
        X = varargin{1};
        [n,m] = size(X);
        if ~ishermitian(X)
            error('LAMBDA_MIN can only be Hermitian matrices');
        else
            varargout{1} = yalmip('define',mfilename,varargin{:});
        end

    case 'char' % YALMIP sends 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2}; % Second arg is the extended operator variable
            X = varargin{3}; % Third arg and above are the args user used when defining t.
            varargout{1} = (t*eye(size(X,1)) <= X);
            varargout{2} = struct('convexity','concave','monotonicity','none','definiteness','none','model','graph');
            varargout{3} = X;
        else
            varargout{1} = [];
            varargout{2} = [];
            varargout{3} = [];
        end
    otherwise
end


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

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

%% Check input validity

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

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

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

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

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

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

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

%% Compute dihedral angle for each edge

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

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

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

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

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

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

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

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


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


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

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

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

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

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

end


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

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

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


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

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


function prob_test084 ( )

%*****************************************************************************80
%
%% TEST084 tests GOMPERTZ_CDF, GOMPERTZ_CDF_INV, GOMPERTZ_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST084\n' );
  fprintf ( 1, '  For the Gompertz PDF:\n' );
  fprintf ( 1, '  GOMPERTZ_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  GOMPERTZ_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  GOMPERTZ_PDF evaluates the PDF;\n' );

  a = 2.0;
  b = 3.0;

  check = gompertz_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST084 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =       %14f\n', a );
  fprintf ( 1, '  PDF parameter B =       %14f\n', b );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ x, seed ] = gompertz_sample ( a, b, seed );

    pdf = gompertz_pdf ( x, a, b );

    cdf = gompertz_cdf ( x, a, b );

    x2 = gompertz_cdf_inv ( cdf, a, b );

    fprintf ( 1, ' %14f  %14f  %14f  %14f\n', x, pdf, cdf, x2 );

  end

  return
end


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

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

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

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

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

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

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

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

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


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

%% base triangle

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

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

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

%% Equliateral triangles

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

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

%% Compute centroid of each triangle

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

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

function tri = createTriangle(p1, p2)

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

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

tri = [p1; p2; p3];


%% Find Image Rotation and Scale Using Automated Feature Matching
% This example shows how to automatically align two images that differ by a
% rotation and a scale change. It closely parallels another example titled
% <matlab:showdemo('RotationFitgeotransExample') Find Image Rotation and Scale>.
% Instead of using a manual approach to register the two images, it
% utilizes feature-based techniques found in the Computer Vision System
% Toolbox(TM) to automate the registration process.
%
% In this example, you will use |detectSURFFeatures| and
% |vision.GeometricTransformEstimator| System object to recover rotation
% angle and scale factor of a distorted image. You will then transform the
% distorted image to recover the original image.

% Copyright 1993-2014 The MathWorks, Inc.

function recovered = homographyAlignment(original, distorted, verbose)
% clc; clear; close all

%% Step 1: Read Image
% Bring an image into the workspace.
if isempty(original)
    original = imread('00001.jpg');
end
original_c = original;
original = rgb2gray(original);
if verbose
    imshow(original);
    text(size(original,2),size(original,1)+15, ...
        'Image courtesy of Massachusetts Institute of Technology', ...
        'FontSize',7,'HorizontalAlignment','right');
end

%% Step 2: Resize and Rotate the Image
if isempty(distorted)
    % scale = 0.7;
    % J = imresize(original, scale); % Try varying the scale factor.
    %
    % theta = 30;
    % distorted = imrotate(J,theta); % Try varying the angle, theta.
    distorted = rgb2gray(imread('00003.jpg'));
end
distorted_c = distorted;
distorted = rgb2gray(distorted);
if verbose
    figure, imshow(distorted)
end

%%
% You can experiment by varying the scale and rotation of the input image.
% However, note that there is a limit to the amount you can vary the scale
% before the feature detector fails to find enough features.

%% Step 3: Find Matching Features Between Images
% Detect features in both images.
ptsOriginal  = detectSURFFeatures(original);
ptsDistorted = detectSURFFeatures(distorted);

%%
% Extract feature descriptors.
[featuresOriginal,   validPtsOriginal]  = extractFeatures(original,  ptsOriginal);
[featuresDistorted, validPtsDistorted]  = extractFeatures(distorted, ptsDistorted);

%%
% Match features by using their descriptors.
indexPairs = matchFeatures(featuresOriginal, featuresDistorted);

%%
% Retrieve locations of corresponding points for each image.
matchedOriginal  = validPtsOriginal(indexPairs(:,1));
matchedDistorted = validPtsDistorted(indexPairs(:,2));

%%
% Show point matches. Notice the presence of outliers.
if verbose
    figure;
    showMatchedFeatures(original,distorted,matchedOriginal,matchedDistorted);
    title('Putatively matched points (including outliers)');
end

%% Step 4: Estimate Transformation
% Find a transformation corresponding to the matching point pairs using the
% statistically robust M-estimator SAmple Consensus (MSAC) algorithm, which
% is a variant of the RANSAC algorithm. It removes outliers while computing
% the transformation matrix. You may see varying results of the
% transformation computation because of the random sampling employed by the
% MSAC algorithm.
[tform, inlierDistorted, inlierOriginal, status] = estimateGeometricTransform(...
    matchedDistorted, matchedOriginal, 'projective');
if status
    recovered = original_c;
    return;
end

%%
% Display matching point pairs used in the computation of the
% transformation matrix.
if verbose
    figure;
    showMatchedFeatures(original,distorted, inlierOriginal, inlierDistorted);
    title('Matching points (inliers only)');
    legend('ptsOriginal','ptsDistorted');
end

%% Step 5: Solve for Scale and Angle
% Use the geometric transform, TFORM, to recover
% the scale and angle. Since we computed the transformation from the
% distorted to the original image, we need to compute its inverse to
% recover the distortion.
%
%  Let sc = scale*cos(theta)
%  Let ss = scale*sin(theta)
%
%  Then, Tinv = [sc -ss  0;
%                ss  sc  0;
%                tx  ty  1]
%
%  where tx and ty are x and y translations, respectively.
%

%%
% Compute the inverse transformation matrix.
Tinv  = tform.invert.T;

ss = Tinv(2,1);
sc = Tinv(1,1);
scale_recovered = sqrt(ss*ss + sc*sc);
theta_recovered = atan2(ss,sc)*180/pi;

%%
% The recovered values should match your scale and angle values selected in
% *Step 2: Resize and Rotate the Image*.

%% Step 6: Recover the Original Image
% Recover the original image by transforming the distorted image.
outputView = imref2d(size(original_c));
recovered = zeros(size(original_c));

[h,w,l] = size(original_c);
[mx,my] = meshgrid(1:w,1:h);
tmp = [mx(:)';my(:)';ones(1,h*w)];
tmp = (Tinv)'*tmp;
X = reshape(tmp(1,:)./tmp(3,:),h,w);
Y = reshape(tmp(2,:)./tmp(3,:),h,w);
recovered = ba_interp2(single(distorted_c),X,Y,'linear');

% for i=1:l
%     recovered(:,:,i) = interp2(mx,my,distorted_c(:,:,i),X,Y,'linear',0);
% end

% for ic = 1:size(original_c,3)
%     recovered(:,:,ic)  = imwarp(squeeze(distorted_c(:,:,ic)),tform,'OutputView',outputView,'FillValues',0,'SmoothEdges',true);
% end

%%
% Compare |recovered| to |original| by looking at them side-by-side in a montage.
if verbose
    figure, imshowpair(original_c,recovered,'montage')
%%
% The |recovered| (right) image quality does not match the |original| (left)
% image because of the distortion and recovery process. In particular, the
% image shrinking causes loss of information. The artifacts around the edges are
% due to the limited accuracy of the transformation. If you were to detect
% more points in *Step 4: Find Matching Features Between Images*,
% the transformation would be more accurate. For example, we could have
% used a corner detector, |detectFASTFeatures|, to complement the SURF
% feature detector which finds blobs. Image content and image size also
% impact the number of detected features.

    displayEndOfDemoMessage(mfilename)
end


function trans = translation3d(varargin)
%TRANSLATION3D return 4x4 matrix of a 3D translation.
%
%   usage :
%   TRANS = translation3d(DX, DY, DZ);
%   return the translation corresponding to DX and DY.
%   The returned matrix has the form :
%   [1 0 0 DX]
%   [0 1 0 DY]
%   [0 0 1 DZ]
%   [0 0 0  1]
%
%   TRANS = translation3d(VECT);
%   return the translation corresponding to the given vector [x y z].
%
%
%   See also:
%   vectors3d, transforms3d, transformPoint, rotation

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2004-04-06
% Copyright 2004 INRA - TPV URPOI - BIA IMASTE

% deprecation warning
warning('geom3d:deprecated', ...
    [mfilename ' is deprecated, use ''createTranslation3d'' instead']);

% call current implementation
trans = createTranslation3d(varargin{:});


function value = p12_f ( dim_num, point_num, x )

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

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

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

  return
end


function [n, xout] = histx(varargin)
% A wrapper for hist that picks the "ideal" number of bins to use if
% unspecified.
%
% N = HISTX(Y) bins the elements of Y into M equally spaced containers and
% returns the number of elements in each container.  If Y is a matrix, HIST
% works down the columns.  The value of M is the middle (median) value of
% the optimal number of bins calculated using the Freedman-Diaconis, Scott
% and Sturges methods.  See below for further information on these methods.
%
% N = HISTX(Y,M), where M is a scalar, uses M bins.  (This is identical to
% N = HIST(Y,M).)
%
% N = HISTX(Y, X), where X is a vector, returns the distribution of Y among
% bins with centers specified by X. The first bin includes data between
% -inf and the first center and the last bin includes data between the last
% bin and inf. Note: Use HISTC if it is more natural to specify bin edges
% instead.  (This is identical to N = HIST(Y,X).)
%
% N = HISTX(Y, METHOD), where METHOD is a string chooses the number of bins
% as follows:
%
% 'fd': The Freedman-Diaconis method, based upon the inter-quartile range
% and number of data, is used.
% 'scott': The Scott method, based upon the sample standard deviation and
% nnumber of data, is used.
% 'sturges': The Sturges method, based upon the number of data, is used.
% 'middle': All three methods are tried, and the middle value is used.
%
% N = HISTX(Y, [], MINIMUM), where MINIMUM is a numeric scalar, defines the
% smallest acceptable number of bins.
%
% N = HISTX(Y, [], [], MAXIMUM), where MAXIMUM is a numeric scalar, defines the
% largest acceptable number of bins.
%
% HISTX(...) without output arguments produces a histogram bar plot of the
% results. The bar edges on the first and last bins may extend to cover the
% min and max of the data unless a matrix of data is supplied.
%
% HISTX(Y, 'all') produces three histograms in a new figure window, drawn
% with the number of bins calculated using each of the three methods.
%
% HISTX(AX, ...) plots into AX instead of GCA.
%
% Notes:
% 1. When the method chosen is 'all', any axes inputted will be ignored
% with a warning.  Likewise for output arguments.
%
% 2. References for the methods can be found in the help page for
% CALCNBINS.
%
% Examples:
% y = randn(10000,1);
% histx(y)
% histx(y, 'all')
%
% See also: CALCNBINS, HIST.
%
% $ Author: Richard Cotton $		$ Date: 2008/08/28 $    $ Version 1.1 $

% Check there is at least one input, and see if the first input is an axis.
error(nargchk(1,inf,nargin,'struct'));
[cax,args,nargs] = axescheck(varargin{:});

plotall = false;
y = args{1};

% If the number of bins wasn't specified, calculate it using calcnbins.
if nargs < 4 || isempty(args{4})
   args{4} = []; % this also sets missing arguments before this positon to [].
end

if ~isempty(args{2}) && isnumeric(args{2})
   nbins = args{2};
else
   nbins = calcnbins(y, args{2}, args{3}, args{4});
   if ischar(args{2}) && strcmpi(args{2}, 'all')
      plotall = true;
   end
end

% Plot the histogram(s)
if plotall
   if nargout > 0
      warning('histx:ignoreargout', 'No arguments will be returned when the method is "all".');
   end
   if ~isempty(cax)
      warning('histx:ignoreaxes', 'The input axes are ignored when the method is "all".');
   end
   n = [];
   xout = [];
   figure();
   subplot(3, 1, 1); hist(y, nbins.fd);
   title('Freedman-Diaconis'' method');
   subplot(3, 1, 2); hist(y, nbins.scott);
   title('Scott''s method');
   subplot(3, 1, 3); hist(y, nbins.sturges);
   title('Sturges'' method');
else
   if nargout==0
      if isempty(cax)
         hist(y, nbins);
      else
         hist(cax, y, nbins);
      end
   else
      if isempty(cax)
      [n, xout] = hist(y, nbins);
   else
      [n, xout] = hist(cax, y, nbins);
      end
   end
end


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

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

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

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

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

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

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

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

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

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


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


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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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


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

%%
clear; close all; clc;

%% Syntax
% |[Vc,R]=circlefit(V);|

%% Description
% This function returns the centre Vc and radius R for a circle fitted to
% the input point set V. The input can be 2D or 3D but the output centre is
% always a point in 3D space.

%% Examples
%

%%
% Plot settings
markerSize=30;
lineWidth=3;

%%
% Create input circle

n=50; % Number of points on the circle
r=2; % True radius
t=linspace(0,2*pi,n+1)'; t=t(1:end-1); % Angles
V_true=r.*[cos(t) sin(t) zeros(size(t))]; %Circle true coordinates
V=V_true+r/20*randn(size(V_true));

%%
% Fit a circle

[Vc,R]=circlefit(V);

nf=250;
r=2;
t=linspace(0,2*pi,nf+1)'; t=t(1:end-1);
Vf=Vc+R.*[cos(t) sin(t) zeros(size(t))];

%%
% Visualize result

cFigure; hold on;
hp1=plotV(V,'k.','MarkerSize',markerSize);
hp2=plotV(V_true,'g.-','MarkerSize',markerSize,'LineWidth',lineWidth);
hp3=plotV(Vf,'r-','LineWidth',lineWidth);
legend([hp1 hp2 hp3],{'Noisy input data','True circle','Fitted circle'},'Location','NorthEastOutside')
axisGeom; view(2);
drawnow;

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


function f = p08_f ( m, n, x )

%*****************************************************************************80
%
%% P08_F evaluates the objective function for problem 08.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Marcin Molga, Czeslaw Smutnicki,
%    Test functions for optimization needs.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of arguments.
%
%    Input, real X(M,N), the arguments.
%
%    Output, real F(N), the function evaluated at the arguments.
%
  f = zeros ( n, 1 );

  y = r8vec_indicator ( m );
  y(1:m,1) = y(1:m,1) + 1.0;

  for j = 1 : n
    f(j) = sum ( abs ( x(1:m,j) ).^y(1:m) );
  end

  return
end


function [easting,northing]=latlon2eastingsNorthings(sourcelat, sourcelon, lat, lon)
% Convert lat,lon to eastings,northings with sourcelat, sourcelon as origin
% inputs are in degrees, output is in metres relative to origin
% [easting,northing]=latlon2eastingsNorthings(sourcelat, sourcelon, lat, lon)
deg2m = deg2km(1) * 1000;
for c=1:length(lat)
    e = distance(lat(c), lon(c), lat(c), sourcelon) * deg2m;
    easting(c) = e * sign(lon(c)-sourcelon);
    n = distance(lat(c), lon(c), sourcelat, lon(c)) * deg2m;
    northing(c) = n * sign(lat(c)-sourcelat);
end


function [filtered_outpt] = coax_simulator(Samples,rate,t,f,inpt,Zr, ...
    L,b,a,Ur,Er,cond,var_coax,T0,cutoff,ord_but,considerBeta)

format long;

%% ########################### Input Signal ############################
INPT=fftshift(fft(inpt));

%% ########################## Coaxial Cable ############################
H  = coaxTF(f,Zr,L,b,a,Ur,Er,cond,considerBeta);
FRESH_OUTPT = INPT.*H;
fresh_outpt = real(ifft(ifftshift(FRESH_OUTPT)));

%% #################### Noise from Coaxial Cable #######################
AWGNoise = sqrt(var_coax)*randn(size(t));
noisy_out = fresh_outpt + AWGNoise;

%% ########################### Low Pass Filter #########################
    filtrTF = buttLPF(f,cutoff,ord_but);                %Butterworth LPF

%% ##################### Thermal Noise of receiver #####################
k = 1.38e-23;   % Boltzmann's Const. (W/K-Hz)
noise_SD = sqrt(k*T0*cutoff*Zr);
noise_variance = noise_SD^2;
thermal_noise = noise_SD*randn(size(t));
outpt = noisy_out + thermal_noise;

%% ##################### Output Calculation ############################
OUTPT = fftshift(fft(outpt));
FILTERED_OUTPT = OUTPT.*filtrTF;
filtered_outpt = real(ifft(ifftshift(FILTERED_OUTPT)));


function ival = c8mat_is_unitary ( m, n, a )

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

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

  for i = 1 : n

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

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

    for j = i + 1 : n

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

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

    end

  end

  ival = 1;

  return
end


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

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

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

end


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

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

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

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

% Parse Input Arguments
if nargin ~= 1

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

    for k = 2:2:nargin

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

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

            case 'GrayLimits'

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


function indx = i4vec_indexed_heap_d ( n, a, indx )

%*****************************************************************************80
%
%% I4VEC_INDEXED_HEAP_D creates a descending heap from an indexed I4VEC.
%
%  Discussion:
%
%    An I4VEC is a vector of I4's.
%
%    An indexed I4VEC is an I4VEC of data values, and an I4VEC of N indices,
%    each referencing an entry of the data vector.
%
%    The function adjusts the index vector INDX so that, for 1 <= J <= N/2,
%    we have:
%      A(INDX(2*J))   <= A(INDX(J))
%    and
%      A(INDX(2*J+1)) <= A(INDX(J))
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf,
%    Combinatorial Algorithms for Computers and Calculators,
%    Academic Press, 1978,
%    ISBN: 0-12-519260-6,
%    LC: QA164.N54.
%
%  Parameters:
%
%    Input, integer N, the size of the index array.
%
%    Input, integer A(*), the data vector.
%
%    Input, integer INDX(N), the index array.
%    Each entry of INDX must be a valid index for the array A.
%
%    Output, integer INDX(N), the indices have been reordered into
%    a descending heap.
%

%
%  Only nodes N/2 down to 1 can be "parent" nodes.
%
  for i = floor ( n / 2 ) : - 1 : 1
%
%  Copy the value out of the parent node.
%  Position IFREE is now "open".
%
    key = indx(i);
    ifree = i;

    while ( 1 )
%
%  Positions 2*IFREE and 2*IFREE + 1 are the descendants of position
%  IFREE.  (One or both may not exist because they exceed N.)
%
      m = 2 * ifree;
%
%  Does the first position exist?
%
      if ( n < m )
        break
      end
%
%  Does the second position exist?
%
      if ( m + 1 <= n )
%
%  If both positions exist, take the larger of the two values,
%  and update M if necessary.
%
        if ( a(indx(m)) < a(indx(m+1)) )
          m = m + 1;
        end

      end
%
%  If the large descendant is larger than KEY, move it up,
%  and update IFREE, the location of the free position, and
%  consider the descendants of THIS position.
%
      if ( a(indx(m)) <= a(key) )
        break
      end

      indx(ifree) = indx(m);
      ifree = m;

    end
%
%  Once there is no more shifting to do, KEY moves into the free spot IFREE.
%
    indx(ifree) = key;

  end

  return
end


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

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

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

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

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

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

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

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


function [fx,dF_dX,dF_dTheta] = f_Lorenz(Xt,Theta,ut,inF)
% Lorenz chaotic evolution function

deltat = inF.deltat;

x       = Xt;
rho     = Theta(1);
sigma   = Theta(2);
beta    = Theta(3);

fx      = zeros(3,1);
fx(1)   = sigma.*(x(2)-x(1));
fx(2)   = x(1).*(rho-x(3))-x(2);
fx(3)   = x(1).*x(2) - beta.*x(3);
fx      = deltat.*fx + x;

J       = zeros(3,3);
J(1,:)  = [-sigma,sigma,0];
J(2,:)  = [rho-x(3),-1,-x(1)];
J(3,:)  = [x(2),x(1),-beta];
dF_dX   = deltat.*J' + eye(3);

dF_dTheta = zeros(3,3);
dF_dTheta(1,:)      = [0,x(1),0];
dF_dTheta(2,:)      = [x(2)-x(1),0,0];
dF_dTheta(3,:)      = [0,0,-x(3)];
dF_dTheta           = deltat.*dF_dTheta;


function c = tapas_softmax_mu3_wld_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the softmax observation model for multinomial responses with phasic
% volatility exp(mu3) as the decision temperature and parameters accounting for win- and
% loss-distortion of state values.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2018 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;

% Is the decision based on predictions or posteriors? Comment as appropriate.
c.predorpost = 1; % Predictions
%c.predorpost = 2; % Posteriors

% Model name
c.model = 'softmax_mu3_wld';

% Sufficient statistics of Gaussian parameter priors

% Win-distortion
c.la_wdmu = 0;
c.la_wdsa = 2^-2;

% Loss-distortion
c.la_ldmu = 0;
c.la_ldsa = 2^-2;

% Gather prior settings in vectors
c.priormus = [
    c.la_wdmu,...
    c.la_ldmu,...
         ];

c.priorsas = [
    c.la_wdsa,...
    c.la_ldsa,...
         ];

% Model filehandle
c.obs_fun = @tapas_softmax_mu3_wld;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_softmax_mu3_wld_transp;

return;


function [V,F,VC,FC] = image_mesh(filename,m)
  % IMAGE_MESH Given a path to .png file, create a mesh within the α>½ region of
  % the image with triangles placed economically so that per-vertex colors will
  % look reasonable.
  %
  % Input:
  %   filename  path to .png file
  %   m  number of output triangles
  % Outputs:
  %   V  #V by 2 list of 2D vertex positions
  %   F  #F by 3 list of triangle indices into V
  %   VC  #V by 3 list of vertex colors
  %   FC  #F by 3 list of face colors
  %
  % See also: bwmesh
  %

  [im,M,A] = imread(filename);
  if ~isempty(M)
      im = ind2rgb(im,M);
  end
  if isempty(A)
    A = ones(size(im,1),size(im,2));
  end
  [V,F] = bwmesh(A,'SmoothingIters',2,'Tol',0.2);
  %[V,F] = bwmesh(A);
  [X,Y] = meshgrid((1:size(im,2))-0.5,(size(im,1):-1:1)-0.5);

  for c = 1:3
    V(:,2+c) = interp2(X,Y,im2double(im(:,:,c)),V(:,1),V(:,2));
  end

  % weird trick to get better quality elements. Based on Delaunay lifting idea.
  %V(:,end+1) = (V(:,1).^2+V(:,2).^2)*0.001;

  [V,F] = decimate_libigl(V,F,m,'Method','qslim');

  VC = V(:,3:5);
  V = V(:,1:2);

  if nargout>3
    BC = barycenter(V,F);
    FC = zeros(size(F,1),3);
    for c = 1:3
      FC(:,c) = interp2(X,Y,im2double(im(:,:,c)),BC(:,1),BC(:,2));
    end
  end
end


% demo for linear decomposition

clear all
close all

k = 5; % number of simulated components
n = 15; % 1st dim of data matrix
p = 7; % 2nd dim of data matrix

A0 = randn(n,k);
B0 = randn(k,p);
Y = A0*B0;
y = Y+randn(size(Y));

[A,B,out,posterior] = VBA_LinDecomp(y,k);

hf = figure('color',[1 1 1]);
ha = subplot(2,2,1,'parent',hf);
plot(ha,A,A0,'.')
ha = subplot(2,2,2,'parent',hf);
plot(ha,B',B0','.')
ha = subplot(2,2,3,'parent',hf);
bar(corr(A,A0),'parent',ha)
ha = subplot(2,2,4,'parent',hf);
bar(corr(B',B0'),'parent',ha)


function EO = gaborconvolve_BW(im, nscale, norient, minWaveLength, sigmaOnf, mult)
%---------------------------------------------------
% This code gives the logGabor decomposition
% of input image 'im', 'nacale' is the number
% of scale, 'norient' is the number of
% orientations and 'EO' is the logGabor
% decomposed output. EO is a cell of
% dimension nscale times norient

% This code has used codes from author: Peter Kovesi
% Department of Computer Science & Software Engineering
% The University of Western Australia
% pk@cs.uwa.edu.au  www.cs.uwa.edu.au/~pk
% May 2001
%---------------------------------------------------

[rows cols] = size(im);

% Author: Peter Kovesi
% Department of Computer Science & Software Engineering
% The University of Western Australia
% pk@cs.uwa.edu.au  www.cs.uwa.edu.au/~pk
%
% May 2001

dThetaOnSigma   = 1.5;

imagefft = fft2(im);                 % Fourier transform of image
EO = cell(nscale, norient);          % Pre-allocate cell array

% Pre-compute some stuff to speed up filter construction

x = ones(rows,1) * (-cols/2 : (cols/2 - 1))/(cols/2);
y = (-rows/2 : (rows/2 - 1))' * ones(1,cols)/(rows/2);
radius = sqrt(x.^2 + y.^2);       % Matrix values contain *normalised* radius from centre.
radius(round(rows/2+1),round(cols/2+1)) = 1; % Get rid of the 0 radius value in the middle
                                             % so that taking the log of the radius will
                                             % not cause trouble.

% Precompute sine and cosine of the polar angle of all pixels about the
% centre point

theta = atan2(-y,x);              % Matrix values contain polar angle.
                                  % (note -ve y is used to give +ve
                                  % anti-clockwise angles)
sintheta = sin(theta);
costheta = cos(theta);
clear x; clear y; clear theta;      % save a little memory

thetaSigma = pi/norient/dThetaOnSigma;  % Calculate the standard deviation of the
                                        % angular Gaussian function used to
                                        % construct filters in the freq. plane.
% The low pass filter
lp = fftshift(lowpassfilter([rows,cols],.45,10));   % Radius .4, 'sharpness' 10

% The main loop...

for o = 1:norient,                   % For each orientation.
%   fprintf('Processing orientation %d \n', o);
  angl = (o-1)*pi/norient;           % Calculate filter angle.
  wavelength = minWaveLength;        % Initialize filter wavelength.

  % Pre-compute filter data specific to this orientation
  % For each point in the filter matrix calculate the angular distance from the
  % specified filter orientation.  To overcome the angular wrap-around problem
  % sine difference and cosine difference values are first computed and then
  % the atan2 function is used to determine angular distance.

  ds = sintheta * cos(angl) - costheta * sin(angl);     % Difference in sine.
  dc = costheta * cos(angl) + sintheta * sin(angl);     % Difference in cosine.
  dtheta = abs(atan2(ds,dc));                           % Absolute angular distance.
  spread = exp((-dtheta.^2) / (2 * thetaSigma^2));      % Calculate the angular filter component.

  for s = 1:nscale,                  % For each scale.

    % Construct the filter - first calculate the radial filter component.
    fo = 1.0/wavelength;                  % Centre frequency of filter.
    rfo = fo/0.5;                         % Normalised radius from centre of frequency plane
                                          % corresponding to fo.
    logGabor = exp((-(log(radius/rfo)).^2) / (2 * log(sigmaOnf)^2));
    logGabor(round(rows/2+1),round(cols/2+1)) = 0; % Set the value at the center of the filter
                                                   % back to zero (undo the radius fudge).

    filter = fftshift(logGabor .* lp .* spread); % Multiply by the angular spread to get the filter
                                           % and swap quadrants to move zero frequency
                                           % to the corners.

    % Do the convolution, back transform, and save the result in EO
    EO{s,o} = ifft2(imagefft .* filter);

    wavelength = wavelength * mult;       % Finally calculate Wavelength of next filter
  end                                     % ... and process the next scale

end  % For each orientation

end