function [zCart,RCart]=ruv2CartStdRefracCubature(zRUVBiased,SR,useHalfRange,zTx,zRx,M,Ns,xi,w,ce,rE,spherCent,xMax)
%%RUV@CARTSTDREFRACCUBATURE Use cubature integration to approximate the
%              moments of measurements converted from refraction-corrupted
%              bistatic r-u-v coordinates into Cartesian coordinates using
%              a standard exponential atmospheric model. For a two-way
%              monostatic conversion, set zTx=[0;0;0]; to make the
%              transmitter and receiver collocated.
%
%INPUTS: z A 3XnumMeas matrix of numMeas vectors to convert. Each has
%          elements [r;u;v], where r is the bistatic range from the
%          transmitter to the target to the receiver, and u and v are
%          direction cosines.
%       SR The 3X3XnumMeas lower-triangular square roots of the measurement
%          covariance matrices for the measurements. If all of the matrices
%          are the same, then this can just be a single 3X3 matrix.
% useHalfRange A boolean value specifying whether the bistatic range value
%          should be divided by two. This normally comes up when operating
%          in monostatic mode, so that the range reported is a one-way
%          range. The default if this parameter is not provided (or an
%          empty matrix is provided) is false.
%      zTx The 3X1 [x;y;z] location vector of the transmitter in global
%          Cartesian coordinates.  If this parameter is omitted or an empty
%          matrix is passed, then the receiver is placed at the origin.
%      zRx The 3X1 [x;y;z] location vector of the receiver in global
%          Cartesian coordinates. If this parameter is omitted or an empty
%          matrix is passed, then the receiver is placed at the origin.
%        M A 3X3 rotation matrix to go from the alignment of the global
%          coordinate system to the local alignment fo the receiver. The z
%          vector of the local coordinate system of the receiver is the
%          pointing direction of the receiver. If this matrix is omitted,
%          then the identity matrix is used.
%       Ns The atmospheric refractivity reduced to the reference sphere.
%          Note that the refractivity is (n-1)*1e6, where n is the index
%          of refraction. The function reduceStdRefrac2Spher can be used
%          to reduce a refractivity to the surface of a reference
%          ellipsoid. This function does not allow different
%          refractivities to be used as the transmitter and receiver. If
%          this parameter is omitted or an empty matrix is passed, a
%          default value of 313 is used.
%       xi A 3 X numCubaturePoints matrix of cubature points for the
%          numeric integration. If this and the next parameter are omitted
%          or empty matrices are passed, then fifthOrderCubPoints is used
%          to generate cubature points.
%        w A numCubaturePoints X 1 vector of the weights associated with
%          the cubature points.
%       ce The optional decay constant of the exponential model. The
%          refractivity N at height h is N=Ns*exp(-ce*(h-h0)) where h0 is
%          the reference height (in this function, the height of the
%          reference ellipsoid surface is used). ce is related to the
%          change in refractivity at an elevation of 1km based on the
%          refractivity at sea level as
%          ce=log(Ns/(Ns+DeltaN))/1000;%Units of inverse meters.
%          where the change in refractivity for a change in elevation of
%          1km is DeltaN=-multConst*exp(expConst*Ns); In [1], standard
%          values for the two constants are expConst=0.005577; and
%          multConst=7.32; If ce is omitted or an empty matrix is passed,
%          the value based on the standard model is used.
% rE,spherCent The radius of the Earth to use for the spherical Earth
%           approximation used in the model and also the offset between the
%           global model and the local spherical model. It is assumed that
%           zC,zTx,and zRx are all given in the global model and will need
%           to be transformed to the local model to the used. If rE is
%           omitted or an empty matrix is passed, then the default of
%           [rE,spherCent]=osculatingSpher4LatLon(Cart2Ellipse(zRx)) is
%           used. The defaults here mean that a WGS-84 reference ellipsoid
%           is approximated by the local osculating sphere.
%     xMax This function traces the ray to a maximum displacement in the
%          local tangent plane prior (or vertically for nearly vertical
%          directions) to attempting to find a particular range. This is an
%          optional parameter specifying the maximum distance to trace the
%          ray. The default if this parameter is omitted or an empty matrix
%          is passed is 1000e3 (1000km).
%
%OUTPUTS: zCart The approximate means of the PDF of the measurements
%               in global Cartesian [x;y;z] coordinates. This is a
%               3XnumMeas matrix.
%         RCart The approximate 3X3XnumMeas covariance matrices of the
%               PDF of the Cartesian converted measurements. This is a
%               3X3XnumMeas hypermatrix.
%
%The basic cubature conversion approach is detailed in [1] and [2]. The
%standard exponential measurement model is from [3] and the model is
%discussed in more detail in the comments to ruv2CartStdRefrac.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using 3D monostatic and bistatic
%    measurements in refractive environments," IEEE Aerospace and
%    Electronic Systems Magazine, vol. 29, no. 8, Part II, pp. 54-75, Aug.
%    2014.
%[2] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[3] B. R. Bean and G. D. Thayer, CRPL Exponential Reference Atmosphere.
%    Washington, D.C.: U. S. Department of Commerce, National Bureau of
%    Standards, Oct. 1959. [Online]. Available:
%    http://digicoll.manoa.hawaii.edu/techreports/PDF/NBS4.pdf
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=size(zRUVBiased,2);

if(nargin<3||isempty(useHalfRange))
    useHalfRange=false;
end

if(nargin<4||isempty(zTx))
   zTx=zeros(3,1);
end

if(nargin<5||isempty(zRx))
   zRx=zeros(3,1);
end

if(nargin<6||isempty(M))
   M=eye(3,3);
end

if(nargin<7||isempty(Ns))
   Ns=313;
end

if(nargin<8||isempty(xi))
    [xi,w]=fifthOrderCubPoints(3);
end

if(nargin<10||isempty(ce))
    expConst=0.005577;
    multConst=7.32;

    %The change in refractivity at an elevation of 1km based on the
    %refractivity on the surface of the Earth.
    DeltaN=-multConst*exp(expConst*Ns);
    ce=log(Ns/(Ns+DeltaN))/1000;%Units of inverse meters.
end

if(nargin<11||isempty(rE))
    %Use the radius of the Earth that is the radius of the osculating
    %sphere at the location of the observed. This will be the radius used
    %in the local spherical Earth approximation for computing atmospheric
    %refraction. This uses the WGS-84 reference ellipsoid.
    [rE,spherCent]=osculatingSpher4LatLon(Cart2Ellipse(zRx));
end

if(nargin<13||isempty(xMax))
    xMax=1000e3;%1000 kilometer assumed maximum x displacement.
end

if(size(SR,3)==1)
    SR=repmat(SR,[1,1,numMeas]);
end

zCart=zeros(3,numMeas);
RCart=zeros(3,3,numMeas);
for curMeas=1:numMeas
    %Transform the cubature points to match the given Gaussian.
    cubPoints=transformCubPoints(xi,zRUVBiased(:,curMeas),SR(:,:,curMeas));

    %Convert all of the points into Cartesian space
    cubPoints=ruv2CartStdRefrac(cubPoints,useHalfRange,zTx,zRx,M,Ns,ce,rE,spherCent,xMax);

    %Extract the first two moments of the transformed points.
    [zCart(:,curMeas),RCart(:,:,curMeas)]=calcMixtureMoments(cubPoints,w);
end
end

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


%compute speed in the central-head direction
function [data,units]=compute_velch(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velch=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    % just a little faster
    velch{i} = trx(larva).vx_cm.*cos(trx(larva).centralheadang(1,1:end-1)) + ...
      trx(larva).vy_cm.*sin(trx(larva).centralheadang(1,1:end-1));
    % velch{1,i}=trx(larva).velmag_ctr.*(cos(trx(larva).velang).*cos(trx(larva).centralheadang(1,1:end-1))+sin(trx(larva).velang).*sin(trx(larva).centralheadang(1,1:end-1)));
end

units=parseunits('mm/s');
data=velch;


function y=renyi_entro(DATA,q)
  [M,N]=size(DATA);
       y=zeros(1,N);
       for n=1:N
           y(1,n)=log(sum(DATA(:,n).^q))/(1-q);
       end


classdef LRN < dagnn.ElementWise
  properties
    param = [5 1 0.0001/5 0.75]
  end

  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = vl_nnnormalize(inputs{1}, obj.param) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, param, derOutputs)
      derInputs{1} = vl_nnnormalize(inputs{1}, obj.param, derOutputs{1}) ;
      derParams = {} ;
    end

    function obj = LRN(varargin)
      obj.load(varargin) ;
    end
  end
end


function [out] = infiltration_8(S,Smax,fin)
%infiltration_8

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

% Flux function
% ------------------
% Description:  Infiltration into storage is equal the inflow when current
%               storage is under the maximum storage,
%               and zero when storage reaches maximum capacity
% Constraints:  f <= fin
% @(Inputs):
%               S    - current storage [mm]
%               Smax - maximum storage [mm]
%               fin  - size of incoming flux [mm/d]

out = (S < Smax) .* fin;

end


function [pindx, pval] = peakdetect3(dat, threshold, mindist)

% PEAKDETECT3 detects peaks above a certain threshold in single-channel data
%
% Use as
%   [pindx, pval] = peakdetect3(dat, threshold, mindist)
%
% See also PEAKDETECT, PEAKDETECT2

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

% threshold the data
tr = dat>threshold;

% the derivative of the data changes its sign at the peak
td = diff(dat);
td = td(1:(end-1))>0 & td(2:end)<0;
td = [0 td 0];

pindx = find(td & tr);

if nargin>2 && length(pindx)>0
  % find the peaks that are too close to each other
  pd = [inf diff(pindx)];
  pindx = pindx(pd>mindist);
end

if nargout>1
  pval = dat(pindx);
end


function cycle_brent_test05 ( )

%*****************************************************************************80
%
%% CYCLE_BRENT_TEST05 tests CYCLE_BRENT for F5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CYCLE_BRENT_TEST05\n' );
  fprintf ( 1, '  Test CYCLE_BRENT for F5().\n' );
  fprintf ( 1, '  f5(i) = mod ( 16383 * i + 1, 65536 ).\n' );

  x0 = 1;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Starting argument X0 = %d\n', x0 );

  [ lam, mu ] = cycle_brent ( @f5, x0 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reported cycle length is %d\n', lam );
  fprintf ( 1, '  Expected value is 8\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reported distance to first cycle element is %d\n', mu );
  fprintf ( 1, '  Expected value is 0\n' );

  i = 0;
  x0 = 1;
  fprintf ( 1, '  %d  %d\n', i, x0 );
  for i = 1 : 10
    x0 = f5 ( x0 );
    fprintf ( 1, '  %d  %d\n', i, x0 );
  end

  return
end


function determ = daub12_determinant ( n )

%*****************************************************************************80
%
%% DAUB12_DETERMINANT returns the determinant of the DAUB12 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  determ = - 1.0;

  return
end


function y = logsumexp( varargin )

%LOGSUMEXP    log(sum(exp(x))).
%   LOGSUMEXP(X) = LOG_SUM_EXP(X) = LOG(SUM(EXP(X)). We have replaced this
%   function with LOG_SUM_EXP to better match our function naming
%   conventions. Please start using it instead.

warning( 'CVX:Renamed', [ ...
    'The function "logsumexp" has been renamed "log_sum_exp". Please start\n', ...
    'using the new name. The old name will be removed in a future release.' ], 1 );

y = log_sum_exp( varargin{:} );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function d = dot(sF1, sF2)
% inner product
%
% Syntax
%   sF = dot(sF1, sF1)
%
% Input
%  sF1, sF2   - @S2FunHarmonic
%
% Output
%  sF - @S2FunHarmonic
%

bw = min(sF1.bandwidth,sF2.bandwidth);

sF1.bandwidth = bw;
sF2.bandwidth = bw;

d = sum(sF1.fhat .* conj(sF2.fhat));

end


function [ha,hf,lr] = unwrapVBvolatileOTO(posterior,out)
% OTO: recovers posterior sufficient statistics from VB inversion

if isempty(posterior)
    ha =[];
    return
end

[n,n_t] = size(posterior.muX);
if n==5

    ka = out.options.inF.lev2* VBA_sigmoid(posterior.muTheta(1), 'scale', out.options.inF.kaub);
    om = posterior.muTheta(2);
    mux = posterior.muX([2,4],:);
    sx = exp(posterior.muX([3,5],:));

    hf = figure('color',[1 1 1]);
    ha = subplot(2,2,1,'parent',hf,'ygrid','on','xlim',[1,n_t]);
    plotUncertainTimeSeries(mux,sx,1:n_t,ha);
    title(ha,'Learner''s belief estimate')
    legend(ha,{'x2 = outcome probability [LOG space]','x3 = expected volatility [LOG space]'})
    box(ha,'off')

    ha(2) = subplot(2,2,2,'parent',hf,'nextplot','add','ygrid','on','xlim',[1,n_t]);
    plot(ha(2),VBA_sigmoid(mux(1,:)));
    mv = exp(ka*mux(2,:)+om);
    plot(ha(2),mv,'g');

    % plot belief about outcome probability
    er1 = mux(1,:) - sqrt(sx(1,:));
    er2 = mux(1,:) + sqrt(sx(1,:));
    yp = [VBA_sigmoid(er2),fliplr(VBA_sigmoid(er1))];
    xp = [1:size(mux,2),fliplr(1:size(mux,2))];
    hfi = fill(xp,yp,'r','facecolor','b','edgealpha',0,'facealpha',0.25,'parent',ha(2));
    ev1 = exp(ka*(mux(2,:)-sqrt(sx(2,:)))+om);
    ev2 = exp(ka*(mux(2,:)+sqrt(sx(2,:)))+om);
    yp = [ev2,fliplr(ev1)];
    hfi = fill(xp,yp,'r','facecolor','g','edgealpha',0,'facealpha',0.25,'parent',ha(2));
    legend(ha(2),{'outcome probability','volatility'})
    box(ha(2),'off')

    x = posterior.muX;
    x(3,:) = exp(x(3,:));
    x(5,:) = exp(x(5,:));
    s1h = VBA_sigmoid(x(2,:)).*(1-VBA_sigmoid(x(2,:))); % likelihood precision
    s2h = x(3,:) + exp(ka*x(4,:)+om); % 2nd-level prediction variance
    lr = 1./(s2h.^-1 + s1h); % posterior variance
    ha(3) = subplot(2,2,3,'parent',hf,'nextplot','add','ygrid','on','xlim',[1,n_t]);
    plot(ha(3),lr);
    title(ha(3),'LEARNING RATE')
    box(ha(3),'off')

else

    ka = out.options.inF.lev2*VBA_sigmoid(posterior.muTheta(1),'scale',out.options.inF.kaub);
    om = posterior.muTheta(2);
    mux = posterior.muX([2,4,7,9],:);
    sx = exp(posterior.muX([3,5,8,10],:));

    hf = figure('color',[1 1 1]);
    ha = subplot(2,2,1,'parent',hf,'ygrid','on','xlim',[1,n_t]);
    plotUncertainTimeSeries(mux,sx,1:n_t,ha);
    title(ha,'Learner''s belief estimate (log-space)')
    legend(ha,{'1st cue: outcome probability','1st cue: expected volatility','2nd cue: outcome probability','2nd cue: expected volatility'})
    box(ha,'off')

    ha(2) = subplot(2,2,2,'parent',hf,'nextplot','add','ygrid','on','xlim',[1,n_t]);
    mr = mux([1,3],:);
    mv = exp(ka*mux([2,4],:)+om);
    col = getColors(4);
    for i=1:2
        plot(ha(2),VBA_sigmoid(mr(i,:)),'color',col(2*(i-1)+1,:));
        plot(ha(2),mv(i,:),'color',col(2*(i-1)+2,:));
    end
    er1 = mux([1,3],:) - sqrt(sx([1,3],:));
    er2 = mux([1,3],:) + sqrt(sx([1,3],:));
    ev1 = exp(ka*(mux(2,:)-sqrt(sx(2,:)))+om);
    ev2 = exp(ka*(mux(2,:)+sqrt(sx(2,:)))+om);
    for i=1:2
        yp = [VBA_sigmoid(er2(i,:)),fliplr(VBA_sigmoid(er1(i,:)))];
        xp = [1:size(mux,2),fliplr(1:size(mux,2))];
        hfi = fill(xp,yp,'r','facecolor',col(2*(i-1)+1,:),'edgealpha',0,'facealpha',0.25,'parent',ha(2));
        yp = [ev2,fliplr(ev1)];
        hfi = fill(xp,yp,'r','facecolor',col(2*(i-1)+2,:),'edgealpha',0,'facealpha',0.25,'parent',ha(2));
    end
    legend(ha(2),{'1st cue: outcome probability','1st cue: expected volatility','2nd cue: outcome probability','2nd cue: expected volatility'})
    box(ha(2),'off')
    title(ha(2),'Learner''s belief estimate')

    x = posterior.muX;
    x([3,5,8,10],:) = exp(x([3,5,8,10],:));
    s1h = VBA_sigmoid(x([2,7],:)).*(1-VBA_sigmoid(x([2,7],:))); % likelihood precision
    s2h = x([3,8],:) + exp(ka*x([4,9],:)+om); % 2nd-level prediction variance
    lr = 1./(s2h.^-1 + s1h); % posterior variance
    ha(3) = subplot(2,2,3,'parent',hf,'nextplot','add','ygrid','on','xlim',[1,n_t]);
    plot(ha(3),lr');
    title(ha(3),'LEARNING RATES')
    box(ha(3),'off')

end

X = [posterior.muX];
C = corrcoef(X');
ha(4) = subplot(2,2,4,'parent',hf);
imagesc(C,'parent',ha(4))
axis(ha(4),'square')
colorbar('peer',ha(4))
set(ha(4),'clim',[-1,1])
title(ha(4),'STATES'' CORRELATION')

VBA_getSubplots ();


function [Hdraw,HvarsDraw,phi_Hdraw,Adraw,h0,Kh0]=sampleH_update(yData,Psi,A,B,phi_H,HvarsOld,priorValues,dataValues,h0,Kh0)
% this function samples the stochastic covariance matrix H. The steps are
% (1) the states s, (2) the log variances, (3) the phi parameters, and (4)
% the matrix A.
%yData=YData
%Psi = PsiDraw_prop; %draw for the local mean
%A=Adraw;            %draw for the A matrix (constant part of H=VCV_VAR),
%H=A^-1*Lambda*A^-1'
%B = Bdraw; %VAR coefficients
%phi_H =phi_Hdraw; %volatitilty of innovations in random walk for the elemnts of Lambda (stochastic volatility parameters)
%HvarsOld = HvarsDraw; %H from previous iteration
%% Initialize
offset_c=priorValues.offset_c; %constant for log transformation

[T,M]=size(yData);
n=M;
p = size(B,1)/M; %lags of the B vector that has dimensions MxM*p (no constant), each column is one regression

% obtain prior data
%startMeanVector=priorValues.mean_ln_h0;  %log mean of the initial state (variance scaling parameters) as residuals from an AR(4) in the training sample
%startVarVector=priorValues.var_ln_h0;    %variance of the initial state for the elements of lambda

% obtain prior data
b0=priorValues.mean_ln_h0;  %log mean of the initial state (variance scaling parameters) as residuals from an AR(4) in the training sample
a0=eye(M)/priorValues.var_ln_h0(1,1);    %variance of the initial state for the elements of lambda

priorPhi_H=priorValues.phi_h;            %centering parameter for the inverse gamma of the variance of the innovations governing the random walk for lambda
priorD_H  =priorValues.d_h;              %scaling parameter for the inverse gamma distribution for the variance of the innovations governing the random walk for lambda

S_h = priorPhi_H*ones(n,1);
nu_h = priorD_H*ones(n,1);

%% Prepare Data

Y_Psi=yData-Psi;  %subtract the local mean
Y_Psi(1:p,:)=yData(1:p,:)-ones(p,1)*mean(yData(1:p,:)); %also generate initial conditions for the construction of the lagmatrix
% X_Psi = lagmatrix(Y_Psi,1:p); %create RHS of the VAR part
X_Psi = bear.lagx(Y_Psi,p-1);
X_Psi = X_Psi(1:end-1,:);
% X_Psi = X_Psi(p+1:end,:);     %remove the first p rows of RHS
Y_Psi=Y_Psi(p+1:end,:);       %and do so for LHS

E=Y_Psi-X_Psi*B;              %VAR residuals
Escaled=E*A';                 %transform such that VAR residuals have variance Lambda

Hvars=zeros(T,M);             %sampled states (diagonal elements of lambda)
%phi_Hdraw=zeros(1,M);         %variance of the random walk process governing the elements of lambda

%% Sample (States,Vars,phi)

for i=1:M
    % prepare for each i
    residsTemp=Escaled(:,i);
    yStar=log(residsTemp.^2+offset_c); %transform scaled residuals
    phi=phi_H(i); %previous draw for the variance of this particular variable
    lnSigma2=log(HvarsOld(p+1:T,i)); % note log => h = ln sigma2

    startMean=h0(i); %variable specific mean of the initial condition for the  volatility
    startVar=Kh0(i,i);   %variable specific variance of the disturbances of the random walk for the volatility process

    % sample states
    [yStarAdj, Ht] = bear.statesMix(yStar,lnSigma2);

    % sample log variances
    [logVarsDraw_Hi]=bear.KF_CKsimSV(yStarAdj,Ht,phi,startMean,startVar);
    Hvars(:,i)=exp([zeros(1,p) logVarsDraw_Hi']');

%     % sample phi
%     [phiDraw_Hi]=samplePhi(logVarsDraw_Hi,priorD_H,priorPhi_H);
%     phi_Hdraw(i)=phiDraw_Hi;
end

  %sample phi
%   for kk=1:n
%    [phiDraw_Hi]=samplePhi([h0(kk,1); log(Hvars(p+1:end,kk))],priorD_H,priorPhi_H);
%    phi_Hdraw(1,kk)=phiDraw_Hi;
%   end

for kk=1:n
logSVseries=[h0(kk,1); log(Hvars(p+1:end,kk))];
  %% Initialize
T1=length(logSVseries);
vDiffSV=(logSVseries(2:T1)-logSVseries(1:T1-1));

%% posteror parameter values
a1=priorD_H*priorPhi_H+sum(vDiffSV.^2); %scale parameter
b1=priorD_H+(T1-1); %shape parameter

%% Take a draw form the posterior distribution
gammaDraw=bear.grandn(b1/2,2/a1);
phiDrawkk=1/gammaDraw;
phi_Hdraw(1,kk)=phiDrawkk;
end
    % sample phi

%diff = (log(Hvars(p+1:end,:)) - [h0'; log(Hvars(p+1:end-1,:))]).^2;
%phi_Hdraw = 1./gamrnd(nu_h + size(diff,1)/2, 1./(S_h + sum(diff)'/2))';
%phi_Hdraw = 1./gamrnd(S_h + size(diff,1)/2, 1./((nu_h.*S_h) + sum(diff)'/2))';

%
% % sample the initial value
Kh0 = a0 + sparse(1:n,1:n,1./phi_Hdraw');
h0_hat = Kh0\(a0*b0 + log(Hvars(p+1,:)')./phi_Hdraw');
h0 = h0_hat + chol(Kh0,'lower')'\randn(n,1);

    %h0 = priorValues.mean_ln_h0; %initialize initial variance
    %Kh0 = diag(priorValues.var_ln_h0); %initialize initial variance

%% Sample A
[Adraw]=bear.sampleA_H(yData,Psi,B,Hvars,T,priorValues,dataValues);
AdrawInv=Adraw\eye(M);

%% Construct H
Hdraw=zeros(M,M,T);

for t=1:T
    Hdraw(:,:,t)=AdrawInv*diag(Hvars(t,:))*AdrawInv';
end

HvarsDraw=Hvars;

end


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the TWSC algorithm for additive white Gaussian noise
% noise removal.
%
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk / nankaimathxujun@gmail.com
%          The Hong Kong Polytechnic University
%
% Please refer to the following paper if you find this code helps:
%
% @article{TWSC_ECCV2018,
% 	author = {Jun Xu and Lei Zhang and David Zhang},
% 	title = {A Trilateral Weighted Sparse Coding Scheme for Real-World Image Denoising},
% 	journal = {ECCV},
% 	year = {2018}
% }
%
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------
clear;
Original_image_dir  =    'cleanimages/'; % put clean images in this folder
Sdir = regexp(Original_image_dir, '\', 'split');
fpath = fullfile(Original_image_dir, '*.png');
im_dir  = dir(fpath);
im_num = length(im_dir);

method = 'TWSC';
write_MAT_dir = [method '/Results_AWGN/'];
write_sRGB_dir  = [write_MAT_dir method '/'];
if ~isdir(write_sRGB_dir)
    mkdir(write_sRGB_dir);
end
for nSig = [15 25 35 50 75]
    %% Parameters
    Par.innerIter = 2;
    Par.win = 30;
    Par.lambda1 = 0;
    Par.ps = 8;
    Par.outerIter = 10;
    Par.step = 3;
    Par.nlspini = 90;
    Par.nlspgap = 10;
    if 0 < nSig <= 20
        Par.outerIter = 8;
        Par.delta = .07;
        Par.nlspini = 70;
        Par.lambda2 = .9;
    elseif 20 < nSig <= 30
        Par.delta = .06;
        Par.lambda2 = .76;
    elseif 30 < nSig <= 40
        Par.delta = .07;
        Par.lambda2 = .78;
    elseif 40 < nSig <= 60
        Par.nlspini = 120;
        Par.nlspgap = 15;
        Par.delta = .05;
        Par.lambda2 = .72;
    elseif 60 < nSig <= 80
        Par.ps = 9;
        Par.outerIter = 14;
        Par.step = 4;
        Par.nlspini = 140;
        Par.delta = .05;
        Par.lambda2 = .68; % .66
    else
        disp('Please tune the above parameters by yourself, thanks!');
    end
    % record all the results in each iteration
    Par.PSNR = zeros(Par.outerIter, im_num, 'double');
    Par.SSIM = zeros(Par.outerIter, im_num, 'double');
    T512 = [];
    T256 = [];
    for i = 1:im_num
        Par.nlsp = Par.nlspini;  % number of non-local patches
        Par.image = i;
        Par.nSig = nSig/255;
        Par.I =  im2double( imread(fullfile(Original_image_dir, im_dir(i).name)) );
        S = regexp(im_dir(i).name, '\.', 'split');
        randn('seed',0);
        Par.nim =   Par.I + Par.nSig*randn(size(Par.I));
        fprintf('%s :\n',im_dir(i).name);
        PSNR =   csnr( Par.nim*255, Par.I*255, 0, 0 );
        SSIM      =  cal_ssim( Par.nim*255, Par.I*255, 0, 0 );
        fprintf('The initial value of PSNR = %2.4f, SSIM = %2.4f \n', PSNR,SSIM);
        time0 = clock;
        [im_out, Par]  =  TWSC_Sigma_AWGN(Par);
        if size(Par.I,1) == 512
            T512 = [T512 etime(clock,time0)];
            fprintf('Total elapsed time = %f s\n', (etime(clock,time0)) );
        elseif size(Par.I,1) ==256
            T256 = [T256 etime(clock,time0)];
            fprintf('Total elapsed time = %f s\n', (etime(clock,time0)) );
        end
        im_out(im_out>1)=1;
        im_out(im_out<0)=0;
        % calculate the PSNR
        Par.PSNR(Par.outerIter, Par.image)  =   csnr( im_out*255, Par.I*255, 0, 0 );
        Par.SSIM(Par.outerIter, Par.image)      =  cal_ssim( im_out*255, Par.I*255, 0, 0 );
        imname = sprintf([write_sRGB_dir method '_nSig' num2str(nSig) '_oIte' num2str(Par.outerIter) '_iIte' num2str(Par.innerIter) '_ps' num2str(Par.ps) '_step' num2str(Par.step) '_nlspini' num2str(Par.nlspini) '_nlspgap' num2str(Par.nlspgap) '_delta' num2str(Par.delta) '_l1' num2str(Par.lambda1) '_l2' num2str(Par.lambda2) '_' im_dir(i).name]);
        imwrite(im_out,imname);
        fprintf('%s : PSNR = %2.4f, SSIM = %2.4f \n',im_dir(i).name, Par.PSNR(Par.outerIter, Par.image),Par.SSIM(Par.outerIter, Par.image)     );
    end
    PSNR = Par.PSNR(end,:);
    mPSNR=mean(PSNR,2);
    SSIM = Par.SSIM(end,:);
    mSSIM=mean(SSIM,2);
    mT512 = mean(T512);
    sT512 = std(T512);
    mT256 = mean(T256);
    sT256 = std(T256);
    fprintf('The average PSNR = %2.4f, SSIM = %2.4f. \n', mPSNR,mSSIM);
    name = sprintf([write_MAT_dir method '_nSig' num2str(nSig) '_oIte' num2str(Par.outerIter) '_iIte' num2str(Par.innerIter) '_ps' num2str(Par.ps) '_step' num2str(Par.step) '_nlspini' num2str(Par.nlspini) '_nlspgap' num2str(Par.nlspgap) '_delta' num2str(Par.delta) '_l1' num2str(Par.lambda1) '_l2' num2str(Par.lambda2) '.mat']);
    save(name,'nSig','PSNR','SSIM','mPSNR','mSSIM','mT512','sT512','mT256','sT256');
end


classdef CEC2010_F4 < PROBLEM
% <single> <real> <constrained>
% CEC'2010 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% R. Mallipeddi and P. N. Suganthan, Problem definitions and evaluation
% criteria for the CEC 2010 competition on constrained real-parameter
% optimization, Nanyang Technological University, Singapore, 2010.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2010.mat'),'Data');
            obj.O = Data{4};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 50;
            obj.upper    = zeros(1,obj.D) + 50;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = max(Z,[],2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = abs(mean((Z.*cos(sqrt(abs(Z)))),2)) - 1e-4;
            PopCon(:,2) = abs(sum((Z(:,1:end/2-1)-Z(:,2:end/2)).^2,2)) - 1e-4;
            PopCon(:,3) = abs(sum((Z(:,end/2+1:end-1).^2-Z(:,end/2+2:end)).^2,2)) - 1e-4;
            PopCon(:,4) = abs(sum(Z,2)) - 1e-4;
        end
    end
end


function SO3F = approximation(nodes, y, varargin)
% computes a least square problem to get an approximation
%
% Syntax
%   SO3F = SO3FunHarmonic.approximation(SO3Grid, f)
%   SO3F = SO3FunHarmonic.approximation(SO3Grid, f,'constantWeights')
%   SO3F = SO3FunHarmonic.approximation(SO3Grid, f, 'bandwidth', bandwidth, 'tol', TOL, 'maxit', MAXIT, 'weights', W)
%
% Input
%  SO3Grid - rotational grid
%  f       - function values on the grid (maybe multidimensional)
%
% Options
%  constantWeights  - uses constant normalized weights (for example if the nodes are constructed by equispacedSO3Grid)
%  bandwidth        - maximum degree of the Wigner-D functions used to approximate the function
%  tol              - tolerance for lsqr
%  maxit            - maximum number of iterations for lsqr
%  weights          - weight w_n for the nodes (default: Voronoi weights)
%

if isa(nodes,'orientation')
  SRight = nodes.CS; SLeft = nodes.SS;
  if nodes.antipodal
    nodes.antipodal = 0;
    varargin{end+1} = 'antipodal';
  end
else
  [SRight,SLeft] = extractSym(varargin);
  nodes = orientation(nodes,SRight,SLeft);
end

% make points unique
s = size(y);
y = reshape(y, length(nodes), []);
[nodes,~,ind] = unique(nodes(:),'tolerance',0.02);

% take the mean over duplicated nodes
for k = 1:size(y,2)
  yy(:,k) = accumarray(ind,y(:,k),[],@nanmean); %#ok<AGROW>
end

y = reshape(yy, [length(nodes) s(2:end)]);

tol = get_option(varargin, 'tol', 1e-6);
maxit = get_option(varargin, 'maxit', 50);

% TODO: What bandwidth?
bw = get_option(varargin, 'bandwidth', min(dim2deg(length(nodes)*2),getMTEXpref('maxSO3Bandwidth')));

W = get_option(varargin, 'weights');
if check_option(varargin,'constantWeights')
  W = 1/length(nodes);
elseif isempty(W)
  % TODO: calcVoronoiVolume is bad estimated
  [~,i,~] = unique(quaternion(nodes));
  nodes=nodes(i); y=y(i,:);
  W = calcVoronoiVolume(nodes);
else
  W = accumarray(ind,W);
end

W = sqrt(W(:));

b = W.*y;

% create plan
% SO3FunHarmonic([1;1]).eval(nodes,'createPlan','nfsoft')
% SO3FunHarmonic.quadrature(nodes,1,'createPlan','nfsoft','bandwidth',bw)

% least squares solution
for index = 1:size(y,2)
  [fhat(:, index),flag] = lsqr( ...
    @(x, transp_flag) afun(transp_flag, x, nodes, W,bw), b(:, index), tol, maxit);
end

% kill plan
% SO3FunHarmonic.quadrature(1,'killPlan','nfsoft')
% SO3FunHarmonic(1).eval(1,'killPlan','nfsoft')

SO3F = SO3FunHarmonic(fhat,SRight,SLeft);

end

function y = afun(transp_flag, x, nodes, W,bw)

if strcmp(transp_flag, 'transp')

  x = x.*W;
%   F = SO3FunHarmonic.quadrature(nodes,x,'keepPlan','nfsoft','bandwidth',bw);
  F = SO3FunHarmonic.quadrature(nodes,x,'bandwidth',bw);
  y = F.fhat;

elseif strcmp(transp_flag, 'notransp')

  F = SO3FunHarmonic(x,nodes.CS,nodes.SS);
  F.bandwidth = bw;
%   y = F.eval(nodes,'keepPlan','nfsoft');
  y = F.eval(nodes);
  y = y.*W;

end

end

% Possibly split the quadrature and eval functions. Therefore do the
% precomputations before the lsqr-Method.

% TODO: Try Cross-Vallidation, if there are to much points
% TODO: Possibly use the Round2ClenshawCurtis-function (see quadrature) or delete it


function saveOBBs(obblist,filename)

fid = fopen(filename,'wt');

for i = 1:size(obblist,1)
    p = obblist(i,:);

    center = p(1:3);
    dir_1 = p(4:6);
    dir_2 = p(7:9);
    lengths = p(10:12);

    dir_1 = dir_1/norm(dir_1);
    dir_2 = dir_2/norm(dir_2);
    dir_3 = cross(dir_1,dir_2);
    dir_3 = dir_3/norm(dir_3);
    cornerpoints = zeros(8,3);

    d1 = 0.5*lengths(1)*dir_1;
    d2 = 0.5*lengths(2)*dir_2;
    d3 = 0.5*lengths(3)*dir_3;
    cornerpoints(1,:) = center-d1-d2-d3;
    cornerpoints(2,:) = center-d1+d2-d3;
    cornerpoints(3,:) = center+d1-d2-d3;
    cornerpoints(4,:) = center+d1+d2-d3;
    cornerpoints(5,:) = center-d1-d2+d3;
    cornerpoints(6,:) = center-d1+d2+d3;
    cornerpoints(7,:) = center+d1-d2+d3;
    cornerpoints(8,:) = center+d1+d2+d3;

    for j = 1:size(cornerpoints,1)
        fprintf(fid,'v  %f  %f  %f  \n',cornerpoints(j,1),cornerpoints(j,2),cornerpoints(j,3));
    end
end

N = 8;
for i = 1:size(obblist,1)
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+6,(i-1)*N+4,(i-1)*N+2);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+6,(i-1)*N+8,(i-1)*N+4);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+8,(i-1)*N+7,(i-1)*N+4);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+7,(i-1)*N+3,(i-1)*N+4);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+7,(i-1)*N+5,(i-1)*N+3);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+2,(i-1)*N+1);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+6,(i-1)*N+2);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+4,(i-1)*N+3,(i-1)*N+1);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+2,(i-1)*N+4,(i-1)*N+1);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+8,(i-1)*N+6);
    fprintf(fid,'f  %d  %d  %d\n',(i-1)*N+5,(i-1)*N+7,(i-1)*N+8);
end

fclose(fid);
% plot3([cornerpoints(1,1),cornerpoints(2,1)],[cornerpoints(1,2),cornerpoints(2,2)],[cornerpoints(1,3),cornerpoints(2,3)],col);hold on;
% plot3([cornerpoints(1,1),cornerpoints(3,1)],[cornerpoints(1,2),cornerpoints(3,2)],[cornerpoints(1,3),cornerpoints(3,3)],col);hold on;
% plot3([cornerpoints(2,1),cornerpoints(4,1)],[cornerpoints(2,2),cornerpoints(4,2)],[cornerpoints(2,3),cornerpoints(4,3)],col);hold on;
% plot3([cornerpoints(3,1),cornerpoints(4,1)],[cornerpoints(3,2),cornerpoints(4,2)],[cornerpoints(3,3),cornerpoints(4,3)],col);hold on;
% plot3([cornerpoints(5,1),cornerpoints(6,1)],[cornerpoints(5,2),cornerpoints(6,2)],[cornerpoints(5,3),cornerpoints(6,3)],col);hold on;
% plot3([cornerpoints(5,1),cornerpoints(7,1)],[cornerpoints(5,2),cornerpoints(7,2)],[cornerpoints(5,3),cornerpoints(7,3)],col);hold on;
% plot3([cornerpoints(6,1),cornerpoints(8,1)],[cornerpoints(6,2),cornerpoints(8,2)],[cornerpoints(6,3),cornerpoints(8,3)],col);hold on;
% plot3([cornerpoints(7,1),cornerpoints(8,1)],[cornerpoints(7,2),cornerpoints(8,2)],[cornerpoints(7,3),cornerpoints(8,3)],col);hold on;
% plot3([cornerpoints(1,1),cornerpoints(5,1)],[cornerpoints(1,2),cornerpoints(5,2)],[cornerpoints(1,3),cornerpoints(5,3)],col);hold on;
% plot3([cornerpoints(2,1),cornerpoints(6,1)],[cornerpoints(2,2),cornerpoints(6,2)],[cornerpoints(2,3),cornerpoints(6,3)],col);hold on;
% plot3([cornerpoints(3,1),cornerpoints(7,1)],[cornerpoints(3,2),cornerpoints(7,2)],[cornerpoints(3,3),cornerpoints(7,3)],col);hold on;
% plot3([cornerpoints(4,1),cornerpoints(8,1)],[cornerpoints(4,2),cornerpoints(8,2)],[cornerpoints(4,3),cornerpoints(8,3)],col);hold on;

end


function ADE = estimate_average_displacement_error(displacementField1,displacementField2,mask)
% ESTIMATE_AVERAGE_DISPLACEMENT_ERROR Estimates the average displacement error
%
% ADE = estimate_average_displacement_error(displacementField1,displacementField2,mask)
%
% INPUT ARGUMENTS
% displacementField1    - Displacement field 1
% displacementField2    - Displacement field 2
% mask                  - Mask, to remove certain points in the data
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% ADE                   - Average displacement error between the two
%                         provided displacement fields

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

dims = length(displacementField1);
numberOfElements = sum(mask(:) ~= 0);
switch dims
    case 1
        x_error = mask.*(displacementField1{1}-displacementField2{1});
        ADE = sum(sqrt(x_error(:).^2))/numberOfElements;
    case 2
        x_error = mask.*(displacementField1{1}-displacementField2{1});
        y_error = mask.*(displacementField1{2}-displacementField2{2});
        ADE = sum(sqrt(x_error(:).^2 + y_error(:).^2))/numberOfElements;
    case 3
        x_error = mask.*(displacementField1{1}-displacementField2{1});
        y_error = mask.*(displacementField1{2}-displacementField2{2});
        z_error = mask.*(displacementField1{3}-displacementField2{3});
        ADE = sum(sqrt(x_error(:).^2 + y_error(:).^2 + z_error(:).^2))/numberOfElements;
    otherwise
        error('Not implemented for dimensions higher than three.')
end


function [B, U] = compressPCAH(X, PCAHparam)
% Input:
%          X: n*d n is the number of samples, d is the dimension of feature
%          PCAHparam:
%                              PCAHparam.nbits---encoding length
%                              PCAHparam.pcaW---hashing function
% Output:
%          B: compacted binary code
%          U: binary code

U = X*PCAHparam.pcaW;
B = compactbit(U>0);
U = (U>0);


function [P,Pi] = computePandPi(Dir_alpha,Dir2d_alpha)

K = size(Dir2d_alpha,2);
if length(size(Dir2d_alpha))==3
    Q = length(size(Dir2d_alpha));
else
    Q = 1;
end
P = zeros(K,K,Q);
if Q==1, Pi = zeros(1,K);
else, Pi = zeros(K,Q);
end
for i = 1:Q
    for j = 1:K
        PsiSum = psi(sum(Dir2d_alpha(j,:,i)));
        for k = 1:K
            P(j,k,i) = exp(psi(Dir2d_alpha(j,k,i))-PsiSum);
        end
        P(j,:,i) = P(j,:,i) ./ sum(P(j,:,i));
    end
    if Q==1
        PsiSum = psi(sum(Dir_alpha));
        for k = 1:K
            Pi(k) = exp(psi(Dir_alpha(k))-PsiSum);
        end
        Pi = Pi ./ sum(Pi);
    else
        PsiSum = psi(sum(Dir_alpha(:,i)));
        for k = 1:K
            Pi(k,i) = exp(psi(Dir_alpha(k,i))-PsiSum);
        end
        Pi(:,i) = Pi(:,i) ./ sum(Pi(:,i));
    end
end

end


function ic = check_incircle_edge(vertex, face, edge)

% check_incicle_edge - compute "empty circle" property for a set of edges
%
%   ic = check_incicle_edge(vertex,face, edge);
%
%   ic(i)==1 if edge(:,i) is delaunay valid (boundary or empty circles or non convex).
%   It thus should be flipped if ic(i)==0.
%
%   Copyright (c) 2008 Gabriel Peyre

if nargin<3
    edge = compute_edges(face);
end

n = size(edge, 2);
e2f = compute_edge_face_ring(face);

m = size(e2f,1);
f1 = full( e2f(edge(1,:)+(edge(2,:)-1)*m) );
f2 = full( e2f(edge(2,:)+(edge(1,:)-1)*m) );

if not( isempty( find(f1==0 & f2==0) ) )
    warning('Problem with triangulation');
end

ic = ones(n,1);
isconvex = ones(n,1);

I = find(f1>0 & f2>0);

f1 = f1(I); f2 = f2(I);

f1 = face(:,f1);
f2 = face(:,f2);

edge = edge(:,I);
n = length(I);

points1 = find_other(f2, edge(1,:), edge(2,:));
ic1 = check_incircle(vertex, f1, points1 );

points2 = find_other(f1, edge(1,:), edge(2,:));
ic2 = check_incircle(vertex, f2, points2 );

ic(I) = ic1 & ic2;

% check for convexity
v = vertex(:,points2) - vertex(:,points1);
u1 = vertex(:,edge(1,:)) - vertex(:,points1);
u2 = vertex(:,edge(2,:)) - vertex(:,points1);
A = zeros(2,2,n); B = zeros(2,2,n);
A(:,1,:) = reshape(v, [2 1 n]);
A(:,2,:) = reshape(u1, [2 1 n]);
B(:,1,:) = reshape(v, [2 1 n]);
B(:,2,:) = reshape(u2, [2 1 n]);
isconvex(I) = ( det3(A) .* det3(B) ) < 0;
% non convex should not be flipped
ic(isconvex==0) = 1;

%%
function f = find_other(f,a,b)

u = f - repmat(a, [3 1]); f(u==0) = 0;
u = f - repmat(b, [3 1]); f(u==0) = 0;
if sum( sum(f==0)~=2 )>0
    error('Problem with triangulation');
end
f = sum(f);


report_this_filefun(mfilename('fullpath'));

bz2 = []; z = [];
p1 = []; p1b=[];p99b=[];p50b=[];pmib=[];pmab=[];
p99 = []; zr =[];
p50 = [];pma=[]; pmi=[];
niv = 1:0.2:4;

tdiff = round((teb - t0b)*365/par1);

def = {'100','300'};
tit ='Random Zmax calculation';
prompt={'Number of events in each window?', 'Number of random samples drawn? (Grid-size)'};

ni2 = inputdlg(prompt,tit,1,def);
l = ni2{1};
nil = str2double(l);
l = ni2{2};
no = str2double(l);

%ni = str2double(prmptdlg('Number of events in each window?','100'));
%no = str2double(prmptdlg('Number of random samples drawn ?','30'));
iwl = iwl*365/par1;
zr = (-15:0.1:15)*0;
wai = waitbar(0,' Please Wait ...  ');
set(wai,'NumberTitle','off','Name','Makegrid  -Percent done');;
n0 = no;
na = no;

for iwl = 1.0:0.2:4
    iwl
    iwl = iwl*365/par1;
    zr = 0;
    na = n0; no = n0;
    while na+1000 > 1000;
        if na > 1000; na = 1000; end
        l = ceil(rand([ni na])*a.Count);
        [cumu, xt] = hist(reshape(a(l,3),ni,na),(t0b:par1/365:teb));
        for j = 2:tdiff-iwl
            cu = [cumu(1:j-1,:) ; cumu(j+iwl+1:length(cumu(:,1)),:)];
            mean1 = mean(cu);
            mean2 = mean(cumu(j:j+iwl,:));
            var1 = (std(cu)).^2;;
            var2 = (std(cumu(j:j+iwl,:)).^2);
            as = (mean1 - mean2)./(sqrt(var1/(length(cumu(:,1))-iwl)+var2/iwl));
            p1 = [p1 prctile2(as,1)];
            p99 = [p99 prctile2(as,99)];
            p50 = [p50 prctile2(as,50)];
            si =  [ si std(as)];
            pma = [pma max(as)];
            pmi = [pmi min(as)];
            [tmp, tmp2] = hist(as,-15:0.1:15);
            zr = [zr + tmp];
        end     % for j
        p1 = [p1 prctile2(as,1)];
        p99 = [p99 prctile2(as,99)];
        p50 = [p50 prctile2(as,50)];
        pma = [pma max(as)];
        pmi = [pmi min(as)];
        no = no - 1000;
        na = no;
    end % while na

    p1b = [p1b mean(p1) ];
    p99b = [p99b mean(p99) ];
    p50b = [p50b mean(p50) ];
    pmab = [pmab max(pma) ];
    pmib = [pmib min(pmi) ];
    z = [z , zr'];
    p1 = []; p99 = []; p50 = [];pma=[]; pmi=[];
    waitbar((iwl*par1/365)/max(niv));

end  % for iwl

close(wai)
figure
pl =plot(niv,p1b,'b')
hold on
set(pl,'LineWidth',2.0)
pl =plot(niv,p99b,'b')
set(pl,'LineWidth',2.0)
pl =plot(niv,p50b,'r')
pl =plot(niv,pmab,'g--')
set(pl,'LineWidth',2.0)
pl =plot(niv,pmib,'g--')
set(pl,'LineWidth',2.0)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
grid
xlabel('Windowlength in [years]')
ylabel('Range of z')
title(['ni  =  ' num2str(ni) ' events, ' num2str(n0) ' random samples'])

matdraw

figure
pcolor(z)
shading flat
colormap(jet)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
xlabel('Windowlength in [years]')
ylabel('Range of z')
title(['ni  =  ' num2str(ni) 'events, ' num2str(n0) ' random samples'])

matdraw


function [sig,a] = spm_shoot_blur(t,prm,its,sig)
% A function for blurring ("smoothing") tissue probability maps
% FORMAT [sig,a_new] = spm_shoot_blur(t,prm,its,sig)
%     t   - sufficient statistics
%     prm - regularisation parameters (1,1,1, 0.01,0.02,1)
%     its - max no. iterations (12)
%     sig - optional starting estimates
%
%     sig - "smoothed" average
%     a   - parameters
%
% The core of this procedure is described in:
%     John Ashburner & Karl J. Friston.
%     "Computing Average Shaped Tissue Probability Templates"
%     NeuroImage, In Press, Accepted Manuscript, Available online 24 December 2008
%
% However, there is an additional modification such that the the null space
% of the parameters is rotated out.
%________________________________________________________
% (c) Wellcome Trust Centre for NeuroImaging (2009)

% John Ashburner
% $Id: spm_shoot_blur.m 7387 2018-08-03 15:13:57Z john $

d   = [size(t),1,1,1];
if nargin<3, its = 16;                         end % Maximum no. iterations
if nargin<2, prm = [1,1,1, 0.01 0.02 1];       end % Default regularisation
rits = [1 1]; % No. cycles and no. relaxation iterations

W    = zeros([d(1:3) round(((d(4)-1)*d(4))/2)],'single'); % 2nd derivatives
gr   = zeros([d(1:3),d(4)-1],'single');                   % 1st derivatives

% Re-organise sufficient statistics to a form that is easier to work with
t    = max(t,eps('single')*1000);
s    = sum(t,4);
for k=1:d(4)
    t(:,:,:,k) = t(:,:,:,k)./s;
end
maxs   = max(s(:)); % Used for scaling the regularisation
prm(4) = prm(4)+maxs*d(4)*1e-6;

% Only d(4)-1 fields need to be estimated because sum(a,4) = 0.  This matrix
% is used to rotate out the null space
R     = null(ones(1,d(4)));

% Initial starting estimates (if sig is passed)
a     = zeros([d(1:3),d(4)-1],'single');
if nargin>=4
    for z=1:d(3) % Loop over planes
        sz = sig(:,:,z,:);
        sz = min(max(sz,0),1);
        sz(~isfinite(sz)) = 1/d(4);
        sz = squeeze(log(double(sz*(1-d(4)*1e-3)+1e-3)));
        for j1=1:(d(4)-1)
            az = zeros(d(1:2));
            for j2=1:d(4)
                az = az + R(j2,j1)*sz(:,:,j2); % Note the rotation
            end
            a(:,:,z,j1) = az;
        end
        clear sz az
    end
end

for i=1:its

    ll  = 0;
    for z=1:d(3) % Loop over planes

        % Compute softmax for this plane
        sig = double(reshape(sftmax(a(:,:,z,:),R),[d(1:2),d(4)]));

        % -ve log likelihood of the likelihood
        ll  = ll - sum(sum(sum(log(sig).*reshape(t(:,:,z,:),[d(1:2),d(4)]),3).*s(:,:,z)));

        % Compute first derivatives (d(4)-1) x 1
        grz = sig - double(reshape(t(:,:,z,:),[d(1:2),d(4)]));
        for j1=1:(d(4)-1)
            gr(:,:,z,j1) = 0;
            for j2=1:d(4)
                gr(:,:,z,j1) = gr(:,:,z,j1) + R(j2,j1)*grz(:,:,j2); % Note the rotation
            end
            gr(:,:,z,j1) = gr(:,:,z,j1).*s(:,:,z);
        end

        % Compute d(4) x d(4) matrix of second derivatives at each voxel.
        % These should be positive definate, but rounding errors may prevent this.
        % Regularisation is included to enforce +ve definateness.
        wz = zeros([d(1:2),d(4),d(4)]);
        for j1=1:d(4)
            wz(:,:,j1,j1) =   (1-sig(:,:,j1)).*sig(:,:,j1).*s(:,:,z);
            for j2=1:(j1-1)
                wz(:,:,j1,j2) = -sig(:,:,j1) .*sig(:,:,j2).*s(:,:,z);
                wz(:,:,j2,j1) = wz(:,:,j1,j2);
            end
        end

        % First step of rotating 2nd derivatives to (d(4)-1) x (d(4)-1)
        % by R'*W*R
        wz1 = zeros([d(1:2),d(4),d(4)-1]);
        for j1=1:d(4)
            for j2=1:(d(4)-1)
                tmp = zeros(d(1:2));
                for j3=1:d(4)
                    tmp = tmp + wz(:,:,j1,j3)*R(j3,j2);
                end
                wz1(:,:,j1,j2) = tmp;
            end
        end

        % Second step of rotating 2nd derivatives to (d(4)-1) x (d(4)-1)
        % by R'*W*R
        wz = zeros([d(1:2),d(4)-1,d(4)-1]);
        for j1=1:(d(4)-1)
            for j2=1:(d(4)-1)
                tmp = zeros(d(1:2));
                for j3=1:d(4)
                    tmp = tmp + R(j3,j1)*wz1(:,:,j3,j2);
                end
                wz(:,:,j1,j2) = tmp;
            end
        end

        % First pull out the diagonal of the 2nd derivs
        for j1=1:d(4)-1
            W(:,:,z,j1) = wz(:,:,j1,j1);% + maxs*sqrt(eps('single'))*d(4)^2;
        end

        % Then pull out the off diagonal parts (note that matrices are symmetric)
        jj = d(4);
        for j1=1:d(4)-1
           for j2=(j1+1):(d(4)-1)
               W(:,:,z,jj) = wz(:,:,j2,j1);
               jj = jj+1;
           end
        end
    end

    % ss1 and ss2 are for examining how close the 1st derivatives are to zero.
    % At convergence, the derivatives from the likelihood term should match those
    % from the prior (regularisation) term.
    ss1 = sum(sum(sum(sum(gr.^2))));
    gr1 = spm_field('vel2mom',a,prm);        % 1st derivative of the prior term
    ll1 = 0.5*sum(sum(sum(sum(gr1.*a)))); % -ve log probability of the prior term
    gr  = gr + gr1;                       % Combine the derivatives of the two terms
    ss2 = sum(sum(sum(sum(gr.^2))));      % This should approach zero at convergence
    mx  = max(max(max(sum(gr.^2,4))));

    fprintf('%2d %8.4f %8.4f %8.4f %g\n', i, ll/prod(d(1:3)),ll1/prod(d(1:3)), (ll+ll1)/prod(d(1:3)), (ss2)/prod(d(1:3)));

    reg = double(0.01*sqrt(mx)*d(4));
   %reg = double(0.1*sqrt(ss2/prod(d(1:3))));
    a   = a - spm_field(W,gr,[prm(1:3) prm(4)+reg prm(5:6) rits]); % Gauss-Newton update

    if ss2/ss1<1e-4, break; end        % Converged?
end

sig = sftmax(a,R);
%________________________________________________________

%________________________________________________________
function sig = sftmax(a,R)
% Softmax function

d     = [size(a) 1 1 1];
sig   = zeros([d(1:3),d(4)+1],'single');
trunc = log(realmax('single')*(1-eps('single'))/(d(4)+1));

for j=1:size(a,3) % Loop over planes

    % Rotate the null-space back in to the data
    aj  = double(reshape(a(:,:,j,:),[d(1:2),d(4)]));
    sj  = zeros([d(1:2),d(4)+1]);
    for j1=1:d(4)+1
        sj(:,:,j1) = 0;
        for j2=1:d(4)
            sj(:,:,j1) = sj(:,:,j1) + R(j1,j2)*aj(:,:,j2);
        end
    end

    % Compute softmax
    sj = min(max(sj,-trunc),trunc);
    sj = exp(sj)+eps('single')*(d(4)+1);
    s  = sum(sj,3);
    for i=1:d(4)+1
        sig(:,:,j,i) = single(sj(:,:,i)./s);
    end

end
%________________________________________________________


function [ml] = yd32ml(yd3)
% Convert volume from cubic yards to milliliters.
% Chad Greene 2012
ml = yd3*764554.85798;


%% Import Script for ODF Data
%
% This script was automatically created by the import wizard. You should
% run the whoole script or parts of it in order to import your data. There
% is no problem in making any changes to this script.

%% Specify Crystal and Specimen Symmetries

% crystal symmetry
CS = {crystal symmetry};

% specimen symmetry
SS = {specimen symmetry};

% plotting convention
setMTEXpref('xAxisDirection',{xAxisDirection});
setMTEXpref('zAxisDirection',{zAxisDirection});

%% Specify File Names

% path to files
pname = {path to files};

% which files to be imported
fname = {file names};

%% Import the Data

% specify kernel
psi = {kernel name}('halfwidth',{halfwidth});

% load the ODF into the variable odf
odf = ODF.load(fname,CS,SS,{method},'kernel',psi,'resolution',{resolution},...
  'interface',{interface},{options});

%% Correct Data

rot = rotation.byEuler({phi1},{Phi},{phi2});
odf = rotate(odf,rot);


 function mse = mri_exp_errs(arg, Eh, Llist, ti, zmap)
%function mse = mri_exp_errs(arg, Eh, Llist, ti, zmap)
% evaluate errors of MRI exponential approximations for a range of L values
% see mri_exp_approx()
% in
%	arg	cell	to be passed to mri_exp_approx
%	Eh	[M,K]	K=N or K<<N
%	Llist	[L,1]
%	ti	[M,1]	time samples
%	zmap	[N,1]	rate map
% out
%	mse	[L,K]
%
% Copyright 2004-7-5, Jeff Fessler, The University of Michigan

if nargin < 5, ir_usage, end

nL = length(Llist);
mse = zeros(nL, ncol(Eh));
for ll=1:nL
	L = Llist(ll);
	ticker(arg{1}, ll, nL)
	if size(Eh, 2) == length(zmap) % this one pigs memory
		[B C] = mri_exp_approx(ti, zmap, L, arg{:});
	else % histogram-sized C
		[B C] = mri_exp_approx(ti, zmap, L, 'ctest', 1, arg{:});
	end
	mse(ll,:) = mean(abs(Eh - B * C).^2);
end


function quatProd = quatMult(quat1, quat2)
%
% Multiplies two quaternions using the {i,j,k,1} convention
%
    if( size(quat1,1) ~= 4 || size(quat1,2) ~= 1 ...
        || size(quat2,1) ~= 4 || size(quat2,2) ~= 1 )
        error('Input quaternions must be 4x1');
    end

    quatProd = quatLeftComp(quat1) * quat2;
end


%% Permutation test example
%
% A simple example of running a permutation test to determine the
% signifance of classification accuracies
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% Set the number of permutations
niter=1000;

%% Define dataset, classifier, partitioner
config=cosmo_config();
data_path=fullfile(config.tutorial_data_path,'ak6','s01');

data_fn=fullfile(data_path,'glm_T_stats_perrun.nii');
mask_fn=fullfile(data_path,'vt_mask.nii');
ds=cosmo_fmri_dataset(data_fn,'mask',mask_fn,...
                        'targets',repmat(1:6,1,10),...
                        'chunks',floor(((1:60)-1)/6)+1);

% remove constant features
ds=cosmo_remove_useless_data(ds);

% Only consider four classes (otherwise the classifier does extremily well)
ds=cosmo_slice(ds,ds.sa.targets<=4);

classifier=@cosmo_classify_nn;

% for more speed, just do odd-even partitioning
partitions=cosmo_oddeven_partitioner(ds);

%% compute classification accuracy of the original data
[pred, acc]=cosmo_crossvalidate(ds, classifier, partitions);

%% prepare for permutations
acc0=zeros(niter,1); % allocate space for permuted accuracies
ds0=ds; % make a copy of the dataset

%% for _niter_ iterations, reshuffle the labels and compute accuracy
% Use the helper function cosmo_randomize_targets
% >@@>
for k=1:niter
    ds0.sa.targets=cosmo_randomize_targets(ds);
    [foo, acc0(k)]=cosmo_crossvalidate(ds0, classifier, partitions);
end
% <@@<

p=sum(acc<acc0)/niter;
fprintf('%d permutations: accuracy=%.3f, p=%.4f\n', niter, acc, p);

bins=0:10/niter:1;
h=histc(acc0,bins);
bar(bins,h)
hold on
line([acc acc],[0,max(h)])
hold off
title(sprintf('acc=%.3f',acc))


function p_sensor = directionalResponse(kgrid, sensor, sensor_mask_index, p_k)
%DIRECTIONALRESPONSE    Apply directivity to the sensor measurements.
%
% DESCRIPTION:
%       directionalResponse takes a pressure field in k-space, p_k,
%       multiplies by a directivity function, and outputs the grid points
%       indicated in sensor.mask.
%
% USAGE:
%       p_sensor = directionalResponse(kgrid, sensor, p_k)
%
% INPUTS:
%       kgrid       - k-Wave grid structure returned by makeGrid
%
%       sensor      - k-Wave sensor structure containing the following
%                     fields:
%
%           .directivity_angle
%                   - a matrix with the same structure as
%                     sensor.mask that allocates a directivity angle to
%                     each sensor element as defined in sensor.mask. The
%                     angles are in radians:
%                     0 = max sensitivity in y direction (up/down)
%                     pi/2 or -pi/2 = max sensitivity in x direction (left/right)
%
%       	.directivity_unique_angles
%                   - list of the unique directivity angles returned by
%                     sensor.directivity_unique_angles =
%                     unique(sensor.directivity_angle(sensor.mask == 1));
%
%       	.directivity_pattern
%                   - a text string with currently only one
%                     option. 'pressure' indicates that the directional
%                     response should be of the kind due to spatial
%                     averaging over a sensor surface, so a sinc function
%                     in 2D.
%
%       	.directivity_size
%                   - the directivity pattern used is what
%                     would be the directivity if the sensor were this
%                     length (width). The larger this is the more
%                     directional the response.
%
%       	.directivity_wavenumbers
%                   - this is set to [kgrid.ky(:)'; kgrid.kx(:)']. It is
%                     precomputed to allow data casting, as kgrid.kx (etc)
%                     are computed on the fly.
%
%       sensor_mask_index
%                   - indices of the active sensor elements in sensor.mask
%
%       p_k         - the acoustic pressure field in the k-space domain.
%
% Currently works for binary sensor_mask, but not when sensor_mask is given
% as Cartesian coordinates. Also, currently works only in 2D.
%
% ABOUT:
%       author: Ben Cox and Bradley Treeby
%       date: 21st January 2010
%       last update: 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

% sensor mask indices
Ns = length(sensor_mask_index);

% pre-allocate p_sensor vector
p_sensor = zeros(Ns, 1, class(sensor.directivity_angle));

% loop over number of unique angles
for loop = 1:length(sensor.directivity_unique_angles);

   % get current angle
   theta = sensor.directivity_unique_angles(loop);

   % find which of the sensors have this directivity
   indices = find(sensor.directivity_angle(sensor_mask_index) == theta);

   switch sensor.directivity_pattern
      case 'pressure'
         % calculate magnitude of component of wavenumber along sensor face
         k_tangent = reshape([cos(theta) -sin(theta)]*sensor.directivity_wavenumbers, kgrid.Nx, kgrid.Ny);
         directionality = fftshift(sinc(k_tangent*sensor.directivity_size/2));
      case 'gradient'
         % calculate magnitude of component of wavenumber normal to the sensor face
         k_normal = reshape([sin(theta), cos(theta)]*[kgrid.ky(:)'; kgrid.kx(:)'], kgrid.Nx, kgrid.Ny);
         temp = k_normal./kgrid.k;
         temp(kgrid.k==0) = 0;
         directionality = fftshift(temp);
      otherwise
         error('Unsupported directivity pattern')
   end

   % apply the directivity response to the pressure field (in k-space)
   p_directivity = real(ifft2(p_k.*directionality));

   % pick out the response at the sensor points
   p_sensor(indices) = p_directivity(sensor_mask_index(indices));

end


% rf_scaleGrad.m
% Jamie Near, McGill University 2020.
%
% USAGE:
% RF_out=rf_scaleGrad(RF_in,scale);
%
% DESCRIPTION:
% Scale the gradient amplitude of a gradient modulated pulse by a fixed factor.
%
% INPUTS:
% RF_in     = Input RF pulse definition structure.  Must be a gradient
%             modulated pulse.
% scale     = Scaling factor by which you would like to multiply the
%             existing gradient waveform.
%
% OUTPUTS:
% RF_out    = Output rf waveform following the scaling of gradient
%             waveform.

function RF_out=rf_scaleGrad(RF_in,scale);

if ~isstruct(RF_in)
    error('ERROR:  the input RF pulse must be in structure format.  Try using rf_readwaveform to convert it!  Aborting.  ');
end

if ~RF_in.isGM
    error('ERROR:  the input RF pulse must be a gradient modulated pulse.  ABORTING!');
end

newWaveform=RF_in.waveform;
newWaveform(:,4)=newWaveform(:,4)*scale;

RF_out=RF_in;
RF_out.waveform=newWaveform;
RF_out.tthk=RF_in.tthk/scale;


%compute tail speed in the central-head direction
function [data,units]=compute_veltailch(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
veltailch=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    veltailch{1,i}=trx(larva).velmagtail.*(cos(trx(larva).velangtail).*cos(trx(larva).centralheadang(1,1:end-1))+sin(trx(larva).velangtail).*sin(trx(larva).centralheadang(1,1:end-1)));
end

units=parseunits('mm/s');
data=veltailch;


function failed_eeg_leadfield_units

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_convert_units ft_datatype_sens ft_convert_vol_sens ft_compute_leadfield current_dipole

[pnt, tri] = mesh_sphere(162);
sel = find(pnt(:,3)>0);

elec = [];
elec.pnt = pnt(sel,:) .* 10; % distributed on a 10 cm sphere
% elec.tra = eye(length(sel));
for i=1:length(sel)
  elec.label{i} = sprintf('electrode%d', i);
end
elec.unit = 'cm';

mesh = [];
mesh(1).pnt = pnt * 10.0; % in cm
mesh(1).tri = tri;
mesh(2).pnt = pnt *  9.5;
mesh(2).tri = tri;
mesh(3).pnt = pnt *  9.0;
mesh(3).tri = tri;

elec = ft_datatype_sens(elec);
elec = ft_convert_units(elec, 'm');
mesh = ft_convert_units(mesh, 'm');

vol0 = [];
vol0.type = 'infinite_currentdipole';
vol0.unit = 'm';

cfg = [];
cfg.method = 'singlesphere';
vol1 = ft_prepare_headmodel(cfg, mesh(1)); % only pass the outermost surface from the mesh

cfg = [];
cfg.method = 'concentricspheres';
cfg.conductivity = [1 1 1]; % functionally identical to a single sphere
vol2 = ft_prepare_headmodel(cfg, mesh);

cfg = [];
cfg.method = 'bemcp';
cfg.conductivity = [1 1 1]; % functionally identical to a single sphere
vol3 = ft_prepare_headmodel(cfg, mesh);

cfg = [];
cfg.method = 'openmeeg';
cfg.conductivity = [1 1 1]; % functionally identical to a single sphere
vol4 = ft_prepare_headmodel(cfg, mesh);

dip = [0 0 0.08]; % in meter

% this is to make a selection of the MEG channels
[vol0, elec] = ft_prepare_vol_sens(vol0, elec);
[vol1, elec] = ft_prepare_vol_sens(vol1, elec);
[vol2, elec] = ft_prepare_vol_sens(vol2, elec);
[vol3, elec] = ft_prepare_vol_sens(vol3, elec);
[vol4, elec] = ft_prepare_vol_sens(vol4, elec);

lf0 = ft_compute_leadfield(dip, elec, vol0);
lf1 = ft_compute_leadfield(dip, elec, vol1);
lf2 = ft_compute_leadfield(dip, elec, vol2);
lf3 = ft_compute_leadfield(dip, elec, vol3);
lf4 = ft_compute_leadfield(dip, elec, vol4);

n0 = norm(lf0);
n1 = norm(lf1);
n2 = norm(lf2);
n3 = norm(lf3);
n4 = norm(lf4);
% these should be relatively close to two, due to the mirror sources for the boundary condition
assert(abs(n1/n0-2)<0.3);
assert(abs(n2/n0-2)<0.3);
assert(abs(n3/n0-2)<0.3);
assert(abs(n3/n0-2)<0.3);
assert(abs(n4/n0-2)<0.3);

figure
ft_plot_dipole(dip, [1 0 0], 'unit', 'm');
ft_plot_headmodel(vol3, 'edgecolor', 'none', 'facealpha', 0.2);
ft_plot_sens(elec);
ft_plot_topo3d(elec.chanpos, lf3(:,1), 'facealpha', 0.6);


function [T,P]=fnipals(X,w,T)

%FNIPALS nipals algorithm for PCA
%
% function [T,P]=fnipals(X,w,T)
%
% 'fnipals.m'
%
% This algorithm requires the presence of:
% 'missmean.m'
%
% ----------------------------------------------------
%        Find eigenvectors according to NIPALS
% ----------------------------------------------------
%
% [T,P]=fnipals(X,w,T);
% [T,P]=fnipals(X,w);
%
% T is found so that X = T*P', s.t ||T||=1 and T'T=I
%
% X        : The matrix to be decomposed.
% w        : Number of factors to extract.
%            If w is high (perhaps>20) consider using SVD.
% T        : Initial guess of the solution, optional.
%            If T is not specified, a little time will
%            be used on finding orthogonal random
%            starting values.
%
% You may want to calculate P afterwards by typing 'P=X*T'.
% Note that the T returned is orthonormal.
% Calculation of P is left of this implementation to save FLOP's.
% It handles missing values NaNs (very dispersed, less than 15%)
% If the problem is small enough you would prefer the SVD rather
% than NIPALS for finding T. NIPALS may be inaccurate when
% extracting too many factors, i.e., many more than the rank
% of X.

%scalar ConvLim WarnLim ItMax a b i

% $ Version 1.01 $ Date 18. June 1998 $ Not compiled $

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% 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., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

ConvLim=1e-12;
WarnLim=1e-4;
ConvLimMiss=100*ConvLim;
ItMax=100;

filename='fnipals.m';

[a b]=size(X);

if (w>a | w>b) | w<1,
    help(filename);
    error(['Error in ' filename ': Number of factors to extract is invalid!'])
end;

np=isnan(X);
MissingExist=any(np);

if ~exist('T'),
    T=orth(randn(a,w));
end;

if exist('P'),
    P=[];
end;

if ~MissingExist
    if (size(T) == [a w]),
        if a>b,
            P=X'*T;
            l2=Inf;
            Z=X'*X;
            for i=1:w,
                p=P(:,i);
                d=1;
                it=0;
                while (d>ConvLim) & (it<ItMax),
                    it=it+1;
                    p=Z*p;
                    l1=sqrt(p'*p);
                    p=p/l1;
                    d=(l1-l2)^2;
                    l2=l1;
                end;
                P(:,i)=sqrt(l1)*p;
                Z=Z-P(:,i)*P(:,i)';
                WarnLim=sqrt(l1)/1000;
                if it>=ItMax & d>WarnLim,
                    disp('FNIPALS, High-X: Iterated up to the ItMax limit!')
                    disp('FNIPALS, High-X: The solution has not converged!')
                end;
            end;
            T=X*P;
        else
            P=[];
            l2=Inf;
            Z=X*X';
            for i=1:w,
                t=T(:,i);
                d=1;
                it=0;
                while (d>ConvLim) & (it<ItMax),
                    it=it+1;
                    t=Z*t;
                    l1=sqrt(t'*t);
                    t=t/l1;
                    d=(l1-l2).^2;
                    l2=l1;
                end;
                T(:,i)=sqrt(l1)*t;
                Z=Z-T(:,i)*T(:,i)';
                WarnLim=sqrt(l1)/1000;
                if it>=ItMax & d>WarnLim,
                    disp('FNIPALS, Wide-X: Iterated up to the ItMax limit!')
                    disp('FNIPALS, Wide-X: The solution has not converged!')
                end;
            end;
        end;
        T=gsm(T);
    else
        error(['Error in ' filename ': Number of factors to extract is invalid!'])
    end;
else
    MissIdx=find(np);
    [i j]=find(np);
    mnx=missmean(X)/2;
    mny=missmean(X')/2;
    n=size(i,1);
    for k=1:n,
        i_i=i(k);
        j_j=j(k);
        X(i_i,j_j) = mny(i_i) + mnx(j_j);
    end;
    mnz=(missmean(mnx)+missmean(mny))/2;

    ssmisold=sum(sum( X(MissIdx).^2 ));
    sstotold=sum(sum( X.^2 ));
    ssrealold=sstotold-ssmisold;
    iterate=1;
    while iterate

        if (size(T) == [a w]),
            if a>b,
                P=X'*T;
                l2=Inf;
                Z=X'*X;
                for i=1:w,
                    p=P(:,i);
                    d=1;
                    it=0;
                    while (d>ConvLim) & (it<ItMax),
                        it=it+1;
                        p=Z*p;
                        l1=sqrt(p'*p);
                        p=p/l1;
                        d=(l1-l2)^2;
                        l2=l1;
                    end;
                    P(:,i)=sqrt(l1)*p;
                    Z=Z-P(:,i)*P(:,i)';
                    WarnLim=sqrt(l1)/1000;
                    if it>=ItMax & d>WarnLim,
                        disp('FNIPALS, High-X: Iterated up to the ItMax limit!')
                        disp('FNIPALS, High-X: The solution has not converged!')
                    end;
                end;
                T=X*P;
            else
                P=[];
                l2=Inf;
                Z=X*X';
                for i=1:w,
                    t=T(:,i);
                    d=1;
                    it=0;
                    while (d>ConvLim) & (it<ItMax),
                        it=it+1;
                        t=Z*t;
                        l1=sqrt(t'*t);
                        t=t/l1;
                        d=(l1-l2).^2;
                        l2=l1;
                    end;
                    T(:,i)=sqrt(l1)*t;
                    Z=Z-T(:,i)*T(:,i)';
                    WarnLim=sqrt(l1)/1000;
                    if it>=ItMax & d>WarnLim,
                        disp('FNIPALS, Wide-X: Iterated up to the ItMax limit!')
                        disp('FNIPALS, Wide-X: The solution has not converged!')
                    end;
                end;
            end;
            T=gsm(T);
        else
            error(['Error in ' filename ': Number of factors to extract is invalid!'])
        end;

        P=X'*T;
        Xm=T*P';
        X(MissIdx)=Xm(MissIdx);
        ssmis=sum(sum( Xm(MissIdx).^2 ));
        sstot=sum(sum( X.^2 ));
        ssreal=sstot-ssmis;
        if abs(ssreal-ssrealold)<ConvLim*ssrealold & abs(ssmis-ssmisold)<ConvLimMiss*ssmisold,
            iterate=0;
        end;
        ssrealold=ssreal;
        ssmisold=ssmis;
    end;
end;
T=gsm(T);


function binary = matlab_real_to_binary_string ( x )

%*****************************************************************************80
%
%% MATLAB_REAL_TO_BINARY_STRING converts a MATLAB real number to a binary string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the MATLAB real number.
%
%    Output, string BINARY, the binary string.
%

%
%  Convert X to a hexadecimal string.
%
  hex = matlab_real_to_hex_string ( x );
%
%  Convert hexadecimal to binary.
%
  binary = hex_to_binary_string ( hex );

  return
end


classdef nnloss < nntest
  properties (TestParameter)
    loss = {...
      'classerror', 'log', 'softmaxlog', 'mhinge', 'mshinge', ...
      'binaryerror', 'binarylog', 'logistic', 'hinge'}
    weighed = {false, true}
  end

  properties
    x
  end

  methods
    function [x,c,dzdy,instanceWeights] = getx(test,loss)
      numClasses = 3 ;
      numAttributes = 5 ;
      numImages = 3 ;
      w = 5 ;
      h = 4 ;
      switch loss
        case {'log', 'softmaxlog', 'mhinge', 'mshinge', 'classerror'}
          % multiclass
          instanceWeights = test.rand(h,w, 'single') / test.range / (h*w) ;
          c = randi(numClasses, h,w,1,numImages) ;
          c = test.toDevice(c) ;
        otherwise
          % binary
          instanceWeights = test.rand(h,w, numAttributes, 'single') / test.range / (h*w*numAttributes) ;
          c = sign(test.randn(h,w,numAttributes, numImages)) ;
      end
      c = single(c) ;
      switch loss
        case {'log'}
          x = test.rand(h,w, numClasses, numImages, 'single') / test.range * .60 + .20 ;
          x = bsxfun(@rdivide, x, sum(x,3)) ;
        case {'binarylog'}
          x = test.rand(h,w, numAttributes, numImages, 'single') / test.range * .60 + .20 ;
        case {'softmaxlog'}
          x = test.randn(h,w, numClasses, numImages, 'single') / test.range ;
        case {'mhinge', 'mshinge', 'classerror'}
          x = test.randn(h,w, numClasses, numImages, 'single') / test.range ;
        case {'hinge', 'logistic', 'binaryerror'}
          x = test.randn(h,w, numAttributes, numImages, 'single') / test.range ;
      end
      dzdy = test.randn(1,1) / test.range ;
    end
  end

  methods (Test)
    function nullcategories(test, loss, weighed)
      [x,c,dzdy,instanceWeights] = test.getx(loss) ;
      % make a number of categories null
      c(:) = c(:) .* (test.randn(numel(c),1) > 0) ;
      opts = {'loss',loss} ;
      if weighed, opts = {opts{:}, 'instanceWeights', instanceWeights} ; end
      y = vl_nnloss(x,c,[],opts{:}) ;
      dzdx = vl_nnloss(x,c,dzdy,opts{:}) ;
      test.der(@(x) vl_nnloss(x,c,[],opts{:}), x, dzdy, dzdx, 0.001, -5e-1) ;
    end

    function convolutional(test, loss, weighed)
      [x,c,dzdy,instanceWeights] = test.getx(loss) ;
      opts = {'loss',loss} ;
      if weighed, opts = {opts{:}, 'instanceWeights', instanceWeights} ; end
      y = vl_nnloss(x,c,[],opts{:}) ;
      dzdx = vl_nnloss(x,c,dzdy,opts{:}) ;
      test.der(@(x) vl_nnloss(x,c,[],opts{:}), x, dzdy, dzdx, 0.001, -5e-1) ;
    end

  end
end


function [propulsionModel] = definePropulsionModel(quadRotorParams,plotflag)
% [propulsionModel] = definePropulsionModel(quadrotorParams)
%
% Defines a propulsion parameter plant model for use with dynBodyFrame.
% Supports any number of thrusters, n.
%
% First row of 'thrustLocations','thrustAxes','isSpinDirectionCCW' are
% associated with the first motor.
% Second row of 'thrustLocations','thrustAxes','isSpinDirectionCCW', are associate with the second
% motor, and so forth.
%
% Following parameters are the same for each thruster
%   (i.e.assumes same properties of prop-motor combination at all locations):
%       maxThrust, maxRPM, maxTorque, d_prop
%
% Inputs:
%   quadRotorParams [struct] parameter struct with the following fields
%       .thrustLocations = [n x 3] [m] location of motors
%       .thrustAxes = [n x 3] [unitvectors] direction of thrust axis
%       .isSpinDirectionCCW = [n x 1] [bool] if set
%       .maxThrust [n x 1] thrust at 100% throttle (N)
%       .maxRPM [n x 1] RPM at 100% throttle (RPM)
%       .maxTorque [n x 1] torque at 100% throttle (Nm)
%       .d_prop [n x 1] propeller diameter (m)
%
% Output:
%   quadRotorModel = [struct] with n elements, one for each motor.
%
% Written by Conrad McGreal 2020

% give shorter name
p = quadRotorParams ;

%% Compute propulsion system coefficients
% See: https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html
% air density during propulsion data collection (kg/m^3)
% (used to determine propulsion system coefficients).
rho = 1.225 ;

%% Assign to struct
propulsionModel = struct() ; % initialize output struct

for i=1:size(p.thrustLocations,1)
    propulsionModel(i).thrustAxis = p.thrustAxes(i,:) ;      % [port, nose, top] % nose faces north when body and world axis are aligned (world is "East North Up")
    propulsionModel(i).thrustLocation = p.thrustLocations(i,:) ;
    propulsionModel(i).isSpinDirectionCCW = p.isSpinDirectionCCW(i,:) ;
    propulsionModel(i).maxRPM = p.maxRPM(i) ;
    propulsionModel(i).maxTorque = p.maxTorque(i) ;
    propulsionModel(i).d_prop = p.d_prop(i) ;

    % compute coefficients.
    C_t = p.maxThrust(i) / (rho * (p.maxRPM(i)/60)^2 * p.d_prop(i)^4) ; % thrust coefficient
    propulsionModel(i).C_t = C_t ;

    C_q = p.maxTorque(i) / (rho * (p.maxRPM(i)/60)^2 * p.d_prop(i)^5) ; % torque coefficient
    propulsionModel(i).C_q = C_q ;

end

% plot propulsion model, if desired
if ~exist('plotflag','var')
else
    if plotflag
        showPropulsionModel(propulsionModel) ;
    end
end


function nI= upSmpIm(I,new_imgSize,filtS)

if(~exist('filtS','var'))
  filtS=1;
end
if (filtS==2)
  filt=[1,4,6,4,1]/8;
end
if (filtS==1)
  filt=[1,2,1]/2;
end
if (filtS==0)
  filt=[1];
end

id=floor((new_imgSize(1)-size(I,1)*2+1)/2);
iu=ceil((new_imgSize(1)-size(I,1)*2+1)/2);
jd=floor((new_imgSize(2)-size(I,2)*2+1)/2);
ju=ceil((new_imgSize(2)-size(I,2)*2+1)/2);

nI=zeros(new_imgSize(1)+2*filtS,new_imgSize(2)+2*filtS,size(I,3),size(I, ...
                                                  4));
nI(id+filtS+1:2:end-iu-filtS,jd+filtS+1:2:end-ju-filtS,:,:)=I;
nI(id+filtS-1:-2:1,:,:,:)=repmat(nI(id+filtS+1,:,:,:),ceil((id+filtS-1)/2),1);

nI(end-iu-filtS+2:2:end,:,:,:)=repmat(nI(end-iu-filtS,:,:,:),ceil((iu+filtS-1)/2),1);
nI(:,jd+filtS-1:-2:1,:,:)=repmat(nI(:,jd+filtS+1,:,:),1,ceil((jd+filtS-1)/2));
nI(:,end-ju-filtS+2:2:end,:,:)=repmat(nI(:,end-ju-filtS,:,:),1,ceil((ju+filtS-1)/2));

for i=1:size(nI,3)
  for j=1:size(nI,4)
    nI(:,:,i,j)=conv2(filt,filt',nI(:,:,i,j),'same');
  end
end

nI=nI(filtS+1:end-filtS,filtS+1:end-filtS,:,:);


function linplus_test577 ( )

%*****************************************************************************80
%
%% TEST577 tests R8SS_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 9;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST577\n' );
  fprintf ( 1, '  For a symmetric skyline storage matrix,\n' );
  fprintf ( 1, '  R8SS_INDICATOR computes an indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );

  [ na, diag, a ] = r8ss_indicator ( n );

  r8ss_print ( n, na, diag, a, '  The R8SS indicator matrix:' );

  return
end


function ThrowFcn(~, ~, game, FinishBtn, SelectBtn)

%% GET OFFENDER/DEFENDER DATA
offender = get(game.OffenseTxt, 'UserData');
defender = get(game.DefenseTxt, 'UserData');

offOcc = get(game.board(offender).Patch, 'UserData');
defOcc = get(game.board(defender).Patch, 'UserData');   % Defending state may already have been occupied in this turn

%% CHECK WHETHER DICE WERE THROWN AT THE APPROPRIATE MOMENT
if offOcc(1) == defOcc(1)   % same occupant
    msg = msgbox('You already invaded this state');
    uiwait(msg)
    return
end

%% NUMBER OF TROOPS
Noff = get(game.board(offender).Text, 'UserData');

%% GET NUMBER OF TROOPS THE OFFENDER WANTS TO SEND
while true
    N = str2double(inputdlg('Number of offending troops', 'How many troops?', 1, {'1'}));

    if isempty(N) || isnan(N)
        return
    end
    if N > Noff - 1
        uiwait(msgbox(sprintf('Maximum number of offending troops is %d', Noff - 1)))
        continue
    end
    if N <= 0
        uiwait(msgbox('Send at least 1 unit into battle'))
        continue
    end

    break
end

%% NO FINISH / RESELECT DURING BATTLE
set(FinishBtn, 'Enable', 'inactive')
set(SelectBtn, 'Enable', 'inactive')

%% NUMBER OF DICE TO BE USED
M(1) = N * (N <= 3) + 3 * (N > 3);

%% RESULT
res1 = round(rand(1,M(1)) * 5) + 1;
for i = 1:M(1)
    imshow(sprintf('dice/%d.jpg', res1(i)), 'Parent', game.dice(i));
end
for i = M(1) + 1 : 3
    imshow('dice/7.jpg', 'Parent', game.dice(i));
end

%% UNSET OFFENSE THROWBUTTON, SET DEFENSE
set(game.ThrowBtnOff, 'Enable', 'off')
set(game.ThrowBtnDef, 'Enable', 'on', 'Callback', {@ThrowFcn2, game, res1, N, M, FinishBtn, SelectBtn});
set(game.txtH, 'String', 'Defender may throw the dice')


function [InitialObservation,StockSaved] = myResetFunction(trainData)
% Copyright 2020 The MathWorks, Inc.

%% init
[total_step,n_step] = size(trainData);
init_invest = 20000;

%% reset the environment
cur_step = 1;
stock_price = trainData(cur_step, :);

%randomize stock holdings at the beginning
Ir = rand(1);

%% Init cash in and cash invested
cash_in_hand = round(init_invest*Ir);
invest_cash = init_invest-cash_in_hand;

stock_init = rand(1,3);
stock_init = stock_init/sum(stock_init);
stock_init_buy = invest_cash.*stock_init;

%% Save it in shared structure
StockSaved.stock_owned  = round(stock_init_buy./stock_price{:,:});
StockSaved.cash_in_hand = cash_in_hand;

%% Init Indicators
Ind1 = zeros(1,3);
Ind2 = Ind1;
Ind3 = Ind1;
Ind4 = Ind1;

%% Init price differentiation ratio
diffPricesBought = zeros(1,3);
%% Init first state and buffer
StockSaved.State = [StockSaved.stock_owned,diffPricesBought,StockSaved.cash_in_hand,Ind1,Ind2,Ind3,Ind4];
StockSaved.last7 = zeros(7,3);
StockSaved.last7(1,:) = stock_price{:,:};
StockSaved.prevBoughtPrices = Ind1;
StockSaved.profits = [];
%% Adding parameters to the structure saved
StockSaved.total_step = total_step;
StockSaved.n_step = n_step;
StockSaved.cur_step = cur_step;
StockSaved.cur_val = [];

%% Updating initial Observation
InitialObservation = StockSaved.State;


function hf_out = lhs_operation(hf, samplesf, reg_filter, sample_weights)

% This is the left-hand-side operation in Conjugate Gradient

% Get sizes
num_features = length(hf);
filter_sz = zeros(num_features,2);
for k = 1:num_features
    filter_sz(k,:) = [size(hf{k},1), size(hf{k},2)];
end
[~, k1] = max(filter_sz(:,1));  % Index for the feature block with the largest spatial size
block_inds = 1:num_features;
block_inds(k1) = [];
output_sz = [size(hf{k1},1), 2*size(hf{k1},2)-1];

% Compute the operation corresponding to the data term in the optimization
% (blockwise matrix multiplications)
%implements: A' diag(sample_weights) A f

% sum over all features and feature blocks
sh = mtimesx(samplesf{k1}, permute(hf{k1}, [3 4 1 2]), 'speed');    % assumes the feature with the highest resolution is first
pad_sz = cell(1,1,num_features);
for k = block_inds
    pad_sz{k} = (output_sz - [size(hf{k},1), 2*size(hf{k},2)-1]) / 2;

    sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) = ...
        sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) + mtimesx(samplesf{k}, permute(hf{k}, [3 4 1 2]), 'speed');
end

% weight all the samples
sh = bsxfun(@times,sample_weights,sh);

% multiply with the transpose
hf_out = cell(1,1,num_features);
hf_out{k1} = permute(conj(mtimesx(sh, 'C', samplesf{k1}, 'speed')), [3 4 2 1]);
for k = block_inds
    hf_out{k} = permute(conj(mtimesx(sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end), 'C', samplesf{k}, 'speed')), [3 4 2 1]);
end

% compute the operation corresponding to the regularization term (convolve
% each feature dimension with the DFT of w, and the tramsposed operation)
% add the regularization part
% hf_conv = cell(1,1,num_features);
for k = 1:num_features
    reg_pad = min(size(reg_filter{k},2)-1, size(hf{k},2)-1);

    % add part needed for convolution
    hf_conv = cat(2, hf{k}, conj(rot90(hf{k}(:, end-reg_pad:end-1, :), 2)));

    % do first convolution
    hf_conv = convn(hf_conv, reg_filter{k});

    % do final convolution and put toghether result
    hf_out{k} = hf_out{k} + convn(hf_conv(:,1:end-reg_pad,:), reg_filter{k}, 'valid');
end

end


function T = multiply_pots(T1, T2)
% MULTIPLY_POTS Multiply a pair of dpots together pointwise (cgpot)
% T = multiply_pots(pots)

ddom = myunion(T1.ddom, T2.ddom);
cdom = myunion(T1.cdom, T2.cdom);
dom = myunion(ddom, cdom);
ns = zeros(1, max(dom));
ns(T1.ddom) = T1.dsizes;
ns(T2.ddom) = T2.dsizes;
ns(T1.cdom) = T1.csizes;
ns(T2.cdom) = T2.csizes;

T = cgpot(ddom, cdom, ns);
T = multiply_by_pot(T, T1);
T = multiply_by_pot(T, T2);


function aux_FMD(params, hParentFigure)
% function aux_FMD(params, hParentFigure);
%-------------------------------------------
% Plot determination of Mc for a grid point using  calc_McCdf_plot
%
% Incoming variables:
% params        : all variables
% hParentFigure : Handle of the parent figure
%
% J.Woessner, woessner@seismo.ifg.ethz.ch
% last update: 23.01.03

% Get the axes handle of the plotwindow
axes(sv_result('GetAxesHandle', hParentFigure, [], guidata(hParentFigure)));
hold on;
% Select a point in the plot window with the mouse
[fX, fY] = ginput(1);
disp(['X: ' num2str(fX) ' Y: ' num2str(fY)]);
% Plot a small circle at the chosen place
plot(fX,fY,'ok');

% Get closest gridnode for the chosen point on the map
[fXGridNode fYGridNode,  nNodeGridPoint] = calc_ClosestGridNode(params.mPolygon, fX, fY);
plot(fXGridNode, fYGridNode, '*r');
hold off;

% Get the data for the grid node
mNodeCatalog_ = params.mCatalog(params.caNodeIndices{nNodeGridPoint}, :);

% Start calculation
%[mResult, fMls, fMc, fMu, fSigma, mDatPredBest, vPredBest] = plot_McEMR(mNodeCatalog_, 0.1)
[mResult, fMls, fMc, fMu, fSigma, mDatPredBest, vPredBest] =plot_McCdfnormal(mNodeCatalog_, 0.1)
%plot_McCdf2(mNodeCatalog_, 0.1);


function [varargout]=subQuadCatmullClark(varargin)

% function [Fs,Vs,Cs,CV]=subQuadCatmullClark(F,V,n,fixBoundaryOpt)
% ------------------------------------------------------------------------
%
%
%
% Change log:
% 2022/04/28 Minor array (Vn) allocation improvement
% ------------------------------------------------------------------------

%% parse input

switch nargin
    case 2
        F=varargin{1};
        V=varargin{2};
        n=1;
        fixBoundaryOpt=0;
    case 3
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=0;
    case 4
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=varargin{4};
end

C=(1:1:size(F,1))'; %Face colors or indices

%%

if n>0
    for qIter=1:1:n
        M=patchConnectivity(F,V,{'ev','ef','vf','ve','vv'});

        edgeVertexMat=M.edge.vertex;
        edgeFaceMat=M.edge.face;

        vertexFaceMat=M.vertex.face;
        vertexEdgeMat=M.vertex.edge;
        vertexVertexMat=M.vertex.vertex;

        numPoints = size(V,1);
        numEdges = size(edgeVertexMat,1);

        % Get indices of the three edges associated with each face
        A = sparse(edgeVertexMat(:,1),edgeVertexMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
        A = max(A,A'); %Copy symmetric

        %Indices for A matrix
        indA_12=F(:,1)+(F(:,2)-1)*numPoints;
        indA_23=F(:,2)+(F(:,3)-1)*numPoints;
        indA_34=F(:,3)+(F(:,4)-1)*numPoints;
        indA_41=F(:,4)+(F(:,1)-1)*numPoints;

        %Get indices for vertex array
        indV_12=full(A(indA_12));
        indV_23=full(A(indA_23));
        indV_34=full(A(indA_34));
        indV_41=full(A(indA_41));

        indV_mid=(1:1:size(F,1))'+numPoints+size(edgeVertexMat,1);

        %Create faces array
        Fs=[[F(:,1)  indV_12 indV_mid indV_41];...
            [indV_12 F(:,2)  indV_23 indV_mid];...
            [indV_mid indV_23 F(:,3)  indV_34 ];...
            [indV_41 indV_mid indV_34 F(:,4) ]];

        %Face centre points
        Vm=patchCentre(F,V);

        %Create vertex arrays

        Vne=(V(edgeVertexMat(:,1),:)+ V(edgeVertexMat(:,2),:))/2;

        if size(edgeFaceMat,2)==1
            logicBoundaryEdges=true(size(edgeFaceMat,1),1);
        else
            logicBoundaryEdges=edgeFaceMat(:,2)==0;
        end

        V_F_mean=zeros(size(V));
        logicValid=vertexFaceMat>0;
        X=nan(size(vertexFaceMat));
        for q=1:1:size(V,2)
            X(logicValid)=Vm(vertexFaceMat(logicValid),q);
            V_F_mean(:,q)=mean(X,2,'omitnan');
        end

        V_E_mean=zeros(size(V));
        logicValid=vertexEdgeMat>0;
        X=nan(size(vertexEdgeMat));
        for q=1:1:size(V,2)
            X(logicValid)=Vne(vertexEdgeMat(logicValid),q);
            V_E_mean(:,q)=mean(X,2,'omitnan');
        end

        N=sum(logicValid,2);
        Vv=(V_F_mean+2*V_E_mean+(N-3).*V)./N;

        Vn=zeros(numEdges,size(V,2));
        if nnz(logicBoundaryEdges)>0
            %Use normal mid-edge nodes for boundary edges
            Vn(logicBoundaryEdges,:)=Vne(logicBoundaryEdges,:);
            indBoundaryVertices=unique(edgeVertexMat(logicBoundaryEdges,:));
            if fixBoundaryOpt==1
                %Replace boundary nodes with original
                Vv(indBoundaryVertices,:)=V(indBoundaryVertices,:);
            else
                vvm=vertexVertexMat(indBoundaryVertices,:);
                logicCheck=~ismember(vvm,indBoundaryVertices);
                vvm(logicCheck)=0;
                vvm=sort(vvm,2,'descend');
                vvm=vvm(:,[1 2]);

                Vv(indBoundaryVertices,:)= 6/8*V(indBoundaryVertices,:) + 1/8*(V(vvm(:,1),:)+V(vvm(:,2),:));
            end

        end

        Vn(~logicBoundaryEdges,:)=(V(edgeVertexMat(~logicBoundaryEdges,1),:)+V(edgeVertexMat(~logicBoundaryEdges,2),:)+...
            Vm(edgeFaceMat(~logicBoundaryEdges,1),:) + Vm(edgeFaceMat(~logicBoundaryEdges,2),:) )/4;

        Vs = [Vv; Vn; Vm]; %Join point sets

        if qIter>1
            CV=[CV; 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];
        else
            CV=[zeros(size(V,1),1); 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];
        end

    %Override input for looping
    C=repmat(C,[size(Fs,1)/size(F,1),1]);
    F=Fs;
    V=Vs;
    end

else
    CV=zeros(size(V,1),1);
end

varargout{1}=F;
varargout{2}=V;
varargout{3}=C;
varargout{4}=CV;

%%
% _*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 [ahm, AHM_rf, AHM_sf, AHM_sk, AHM_sc, AHM_u, AHM_rho] = ...
    retroProjAhmPntFromPinHoleOnRob(Rf, Sf, Sk, Sc, u, n)

% RETROPROJAHMPNTFROMPINHOLEONROB Retro-project ahm from pinhole on robot.
%
%   AHM = RETROPROJAHMPNTFROMPINHOLEONROB(RF, SF, SK, SC, U, N) gives the
%   retroprojected AHM in World Frame from an observed pixel U. RF and SF
%   are Robot and Sensor Frames, SK and SD are camera calibration and
%   distortion correction parameters. U is the pixel coordinate and N is
%   the non-observable inverse depth. AHM is a 7-vector :
%     AHM = [X Y Z U V W IDepth]'
%
%   [AHM, AHM_rf, AHM_sf, AHM_k, AHM_c, AHM_u, AHM_n] = ... returns the
%   Jacobians wrt RF.x, SF.x, SK, SC, U and N.
%
%   See also INVPINHOLEAHM, FROMFRAMEAHM.

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

% Frame World -> Robot  :  Rf
% Frame Robot -> Sensor :  Sf

if(isempty(Sc))
    % AHM in Sensor Frame
    [ahms, AHMS_u, AHMS_rho, AHMS_sk] = invPinHoleAhm(u,n,Sk) ;
else
    % AHM in Sensor Frame
    [ahms, AHMS_u, AHMS_rho, AHMS_sk, AHMS_sc] = invPinHoleAhm(u,n,Sk,Sc) ;
end

[ahmr, AHMR_sf, AHMR_ahms] = fromFrameAhm(Sf,ahms);
[ahm , AHM_rf , AHM_ahmr]  = fromFrameAhm(Rf,ahmr);

AHM_ahms = AHM_ahmr*AHMR_ahms;
AHM_sk   = AHM_ahms*AHMS_sk ;
AHM_sf   = AHM_ahmr*AHMR_sf;

if(isempty(Sc))
    AHM_sc = [] ;
else
    AHM_sc = AHM_ahms*AHMS_sc ;
end

AHM_u = AHM_ahms*AHMS_u ;
AHM_rho = AHM_ahms*AHMS_rho ;

end

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


% VL_TEST_ECONOMIC_RELU
function vl_test_economic_relu()

x = randn(11,12,8,'single');
w = randn(5,6,8,9,'single');
b = randn(1,9,'single') ;

net.layers{1} = struct('type', 'conv', ...
                       'filters', w, ...
                       'biases', b, ...
                       'stride', 1, ...
                       'pad', 0);
net.layers{2} = struct('type', 'relu') ;

res = vl_simplenn(net, x) ;
dzdy = randn(size(res(end).x), 'like', res(end).x) ;
clear res ;

res_ = vl_simplenn(net, x, dzdy) ;
res__ = vl_simplenn(net, x, dzdy, [], 'conserveMemory', true) ;

a=whos('res_') ;
b=whos('res__') ;
assert(a.bytes > b.bytes) ;
vl_testsim(res_(1).dzdx,res__(1).dzdx,1e-4) ;
vl_testsim(res_(1).dzdw{1},res__(1).dzdw{1},1e-4) ;
vl_testsim(res_(1).dzdw{2},res__(1).dzdw{2},1e-4) ;


function h=ref_pfilt(f,g,a)
%REF_PFILT  Reference pfilt handling structs
%   Usage:  h=pfilt(f,g);
%           h=pfilt(f,g,a,dim);
%
%   `pfilt(f,g)` applies the filter *g* to the input *f*. If *f* is a
%   matrix, the filter is applied along each column.
%
%   `pfilt(f,g,a)` does the same, but downsamples the output keeping only
%   every a'th sample (starting with the first one).
%
%   `pfilt(f,g,a,dim)` filters along dimension dim. The default value of
%   [] means to filter along the first non-singleton dimension.
%
%   The filter *g* can be a vector, in which case the vector is treated
%   as a zero-delay FIR filter.
%
%   The filter *g* can be a cell array. The following options are
%   possible:
%
%     * If the first element of the cell array is the name of one of the
%       windows from |firwin|, the whole cell array is passed onto
%       |firfilter|.
%
%     * If the first element of the cell array is `'bl'`, the rest of the
%     cell array is passed onto |blfilter|.
%
%     * If the first element of the cell array is `'pgauss'`, `'psech'`,
%       the rest of the parameters is passed onto the respective
%       function. Note that you do not need to specify the length *L*.
%
%   The coefficients obtained from filtering a signal *f* by a filter *g* are
%   defined by
%
%   ..          L-1
%      c(n+1) = sum f(l+1) * g(an-l+1)
%               l=0
%
%   .. math:: c\left(n+1\right)=\sum_{l=0}^{L-1}f\left(l+1\right)g\left(an-l+1\right)
%
%   where $an-l$ is computed modulo $L$.
%
%   See also: pconv

if nargin<3
    a=1;
end;

[L,W]=size(f);

l=(0:L-1).'/L;
if isstruct(g)
    if isfield(g,'h')
        g_time=circshift(postpad(g.h,L),g.offset).*exp(2*pi*1i*(round(g.fc*L/2))*l);
        G=fft(g_time);
    elseif isfield(g,'H')
        G=circshift(postpad(g.H(L),L),g.foff(L)).*exp(-2*pi*1i*round(g.delay)*l);
    else
       error('%s: Unknown filter definition.',upper(mfilename));
    end;

else
    G=fft(fir2long(g,L));
end;

if numel(a) > 1
    % This is possibly a fractional subsampling

    afrac = a(1)/a(2);
    if a(1) ~= L
        error('%s: The length in a(1) is not equal to L.',upper(mfilename));
    end
    N = L/afrac;
    if abs(N-round(N))>1e-5
        error('%s: Output length is not integer.',upper(mfilename));
    else
        N = round(N);
    end

    foff = g.foff(L);

    h=zeros(N,W,assert_classname(f));
    for w=1:W
        h(:,w) = blfilt(f(:,w),G,N,foff,afrac);

        if isstruct(g) && isfield(g,'realonly') && g.realonly
            G2 = involute(G);
            supp = numel(g.H(L));
            foff2 = -L+mod(L-foff-supp,L)+1;
            h(:,w) = (h(:,w) + blfilt(f(:,w),G2,N,foff2,afrac))/2;
        end;

    end;

else
    % This is regular subsampling case
    if isstruct(g) && isfield(g,'realonly') && g.realonly
        G=(G+involute(G))/2;
    end;

    N=L/a;
    h=zeros(N,W,assert_classname(f));
    for w=1:W
        F=fft(f(:,w));
        h(:,w)=ifft(sum(reshape(F.*G,N,a),2))/a;
    end;
end

function h = blfilt(f,G,N,foff,afrac)
L = size(f,1);

align = foff - floor(foff/N)*N;

F = circshift(fft(f).*G,-foff);

F = postpad(F,ceil(L/N)*N);

F = circshift(F,align);

F = sum(reshape(F,N,numel(F)/N),2);

h = ifft(F)/afrac;