function lik = lik_gaussian(varargin)
%LIK_GAUSSIAN  Create a Gaussian likelihood structure
%
%  Description
%    LIK = LIK_GAUSSIAN('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a Gaussian likelihood structure in which the named
%    parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_GAUSSIAN(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood function structure with the named
%    parameters altered with the specified values.
%
%    Parameters for Gaussian likelihood function [default]
%      sigma2       - variance [0.1]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%      n            - number of observations per input (See using average
%                     observations below)
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%    Using average observations
%    The lik_gaussian can be used to model data where each input vector is
%    attached to an average of varying number of observations. That is, we
%    have input vectors x_i, average observations y_i and sample sizes n_i.
%    Each observation is distributed
%
%        y_i ~ N(f(x_i), sigma2/n_i)
%
%    The model is constructed as lik_gaussian('n', n), where n is the same
%    length as y and collects the sample sizes.
%
%  See also
%    GP_SET, PRIOR_*, LIK_*

% Internal note: Because Gaussian noise can be combined
% analytically to the covariance matrix, lik_gaussian is internally
% little between lik_* and gpcf_* functions.
%
% Copyright (c) 2007-2017 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_GAUSSIAN';
  ip.addOptional('lik', [], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>=0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('n',[], @(x) isreal(x) && all(x>0));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Gaussian';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Gaussian')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sigma2',ip.UsingDefaults)
    lik.sigma2 = ip.Results.sigma2;
  end
  if init || ~ismember('n',ip.UsingDefaults)
    lik.n = ip.Results.n;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    lik.p.sigma2=ip.Results.sigma2_prior;
  end
  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_gaussian_pak;
    lik.fh.unpak = @lik_gaussian_unpak;
    lik.fh.ll = @lik_gaussian_ll;
    lik.fh.llg = @lik_gaussian_llg;
    lik.fh.llg2 = @lik_gaussian_llg2;
    lik.fh.llg3 = @lik_gaussian_llg3;
    lik.fh.lp = @lik_gaussian_lp;
    lik.fh.lpg = @lik_gaussian_lpg;
    lik.fh.cfg = @lik_gaussian_cfg;
    lik.fh.tiltedMoments = @lik_gaussian_tiltedMoments;
    lik.fh.trcov  = @lik_gaussian_trcov;
    lik.fh.trvar  = @lik_gaussian_trvar;
    lik.fh.predy = @lik_gaussian_predy;
    lik.fh.siteDeriv = @lik_gaussian_siteDeriv;
    lik.fh.recappend = @lik_gaussian_recappend;
  end

end

function [w, s, h] = lik_gaussian_pak(lik)
%LIK_GAUSSIAN_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_GAUSSIAN_PAK(LIK) takes a likelihood structure LIK
%    and combines the parameters into a single row vector W.
%    This is a mandatory subfunction used for example in energy
%    and gradient computations.
%
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_GAUSSIAN_UNPAK

  w = []; s = {}; h=[];
  if ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s; 'log(gaussian.sigma2)'];
    h = [h 0];
    % Hyperparameters of sigma2
    [wh, sh, hh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s; sh];
    h = [h hh];
  end

end

function [lik, w] = lik_gaussian_unpak(lik, w)
%LIK_GAUSSIAN_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_GAUSSIAN_UNPAK(W, LIK) takes a likelihood structure
%    LIK and extracts the parameters from the vector W to the LIK
%    structure. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%    Assignment is inverse of
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_GAUSSIAN_PAK

  if ~isempty(lik.p.sigma2)
    lik.sigma2 = exp(w(1));
    w = w(2:end);

    % Hyperparameters of sigma2
    [p, w] = lik.p.sigma2.fh.unpak(lik.p.sigma2, w);
    lik.p.sigma2 = p;
  end
end

function logLik = lik_gaussian_ll(lik, y, f, ~)
%LIK_GAUSSIAN_LL    Log likelihood
%
%  Description
%    E = LIK_GAUSSIAN_LL(LIK, Y, F, Z) takes a likelihood data
%    structure LIK, incedence counts Y, expected counts Z, and
%    latent values F. Returns the log likelihood, log p(y|f,z).
%    This subfunction is needed when using Laplace approximation
%    or MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria (DIC, WAIC)
%    computations.
%
%  See also
%    LIK_GAUSSIAN_LLG, LIK_GAUSSIAN_LLG3, LIK_GAUSSIAN_LLG2, GPLA_E

  s2 = lik.sigma2;
  r2 = (f-y).^2;
  logLik =  sum(-0.5 * r2./s2 - 0.5*log(s2) - 0.5*log(2*pi));

end

function llg = lik_gaussian_llg(lik, y, f, param, ~)
%LIK_GAUSSIAN_LLG    Gradient of the log likelihood
%
%  Description
%    G = LIK_GAUSSIAN_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z
%    and latent values F. Returns the gradient of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    'param' or 'latent'. This subfunction is needed when using
%    Laplace approximation or MCMC for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    LIK_GAUSSIAN_LL, LIK_GAUSSIAN_LLG2, LIK_GAUSSIAN_LLG3, GPLA_E

switch param
    case 'param'
        % there is also correction due to the log transformation
        s2 = lik.sigma2;
        r2 = (f-y).^2;
        llg =  0.5.* sum( r2./s2 -1 );

    case 'latent'
        s2 = lik.sigma2;
        r = f-y;
        llg =  - r./s2 ;
end
end

function llg2 = lik_gaussian_llg2(lik, y, f, param, ~)
%LIK_GAUSSIAN_LLG2  Second gradients of the log likelihood
%
%  Description
%    G2 = LIK_GAUSSIAN_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z,
%    and latent values F. Returns the Hessian of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. G2 is a vector with diagonal elements of the
%    Hessian matrix (off diagonals are zero). This subfunction
%    is needed when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.

%
%  See also
%    LIK_GAUSSIAN_LL, LIK_GAUSSIAN_LLG, LIK_GAUSSIAN_LLG3, GPLA_E

switch param
    case 'latent'
        s2 = lik.sigma2;
        llg2 =  - ones(size(y))./s2 ;
    case 'latent+param'
        % there is also correction due to the log transformation
        s2 = lik.sigma2;
        r = f-y;
        llg2 =  r./s2 ;

end
end

function llg3 = lik_gaussian_llg3(lik, y, f, param, z)
%LIK_GAUSSIAN_LLG3  Third gradients of the log likelihood
%
%  Description
%    G3 = LIK_GAUSSIAN_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z
%    and latent values F and returns the third gradients of the
%    log likelihood with respect to PARAM. At the moment PARAM
%    can be only 'latent'. G3 is a vector with third gradients.
%    This subfunction is needed when using Laplace approximation
%    for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_GAUSSIAN_LL, LIK_GAUSSIAN_LLG, LIK_GAUSSIAN_LLG2, GPLA_E, GPLA_G

switch param
    case 'latent'
        llg3 = zeros(size(y));
    case 'latent2+param'
        % there is also correction due to the log transformation
        s2 = lik.sigma2;
        llg3 = ones(size(y))./s2 ;
end
end

function lp = lik_gaussian_lp(lik)
%LIK_GAUSSIAN_LP  Evaluate the log prior of likelihood parameters
%
%  Description
%    LP = LIK_T_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters.
%    This subfunctions is needed when there are likelihood
%    parameters.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_G, GP_E

  lp = 0;

  if ~isempty(lik.p.sigma2)
    likp=lik.p;
    lp = likp.sigma2.fh.lp(lik.sigma2, likp.sigma2) + log(lik.sigma2);
  end
end

function lpg = lik_gaussian_lpg(lik)
%LIK_GAUSSIAN_LPG  Evaluate gradient of the log prior with respect
%                  to the parameters.
%
%  Description
%    LPG = LIK_GAUSSIAN_LPG(LIK) takes a Gaussian likelihood
%    function structure LIK and returns LPG = d log (p(th))/dth,
%    where th is the vector of parameters. This subfunction is
%    needed when there are likelihood parameters.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_E, GP_G

  lpg = [];

  if ~isempty(lik.p.sigma2)
    likp=lik.p;

    lpgs = likp.sigma2.fh.lpg(lik.sigma2, likp.sigma2);
    lpg = lpgs(1).*lik.sigma2 + 1;
    if length(lpgs) > 1
      lpg = [lpg lpgs(2:end)];
    end
  end
end

function DKff = lik_gaussian_cfg(lik, x, x2)
%LIK_GAUSSIAN_CFG  Evaluate gradient of covariance with respect to
%                 Gaussian noise
%
%  Description
%    Gaussian likelihood is a special case since it can be
%    analytically combined with covariance functions and thus we
%    compute gradient of covariance instead of gradient of likelihood.
%
%    DKff = LIK_GAUSSIAN_CFG(LIK, X) takes a Gaussian likelihood
%    function structure LIK, a matrix X of input vectors and
%    returns DKff, the gradients of Gaussian noise covariance
%    matrix Kff = k(X,X) with respect to th (cell array with
%    matrix elements). This subfunction is needed only in Gaussian
%    likelihood.
%
%    DKff = LIK_GAUSSIAN_CFG(LIK, X, X2) takes a Gaussian
%    likelihood function structure LIK, a matrix X of input
%    vectors and returns DKff, the gradients of Gaussian noise
%    covariance matrix Kff = k(X,X) with respect to th (cell
%    array with matrix elements). This subfunction is needed
%    only in Gaussian likelihood.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_E, GP_G

  DKff = {};
  if ~isempty(lik.p.sigma2)
      if isempty(lik.n)
          DKff{1}=lik.sigma2;
      else
          n=size(x,1);
          DKff{1} = sparse(1:n, 1:n, lik.sigma2./lik.n, n, n);
      end
  end
end

function DKff  = lik_gaussian_ginput(lik, x, t, g_ind, gdata_ind, gprior_ind, varargin)
%LIK_GAUSSIAN_GINPUT  Evaluate gradient of likelihood function with
%                     respect to x.
%
%  Description
%    DKff = LIK_GAUSSIAN_GINPUT(LIK, X) takes a likelihood
%    function structure LIK, a matrix X of input vectors and
%    returns DKff, the gradients of likelihood matrix Kff =
%    k(X,X) with respect to X (cell array with matrix elements).
%    This subfunction is needed only in Gaussian likelihood.
%
%    DKff = LIK_GAUSSIAN_GINPUT(LIK, X, X2) takes a likelihood
%    function structure LIK, a matrix X of input vectors and
%    returns DKff, the gradients of likelihood matrix Kff =
%    k(X,X2) with respect to X (cell array with matrix elements).
%    This subfunction is needed only in Gaussian likelihood.
%
%  See also
%    LIK_GAUSSIAN_PAK, LIK_GAUSSIAN_UNPAK, LIK_GAUSSIAN_E, GP_G

end

function C = lik_gaussian_trcov(lik, x)
%LIK_GAUSSIAN_TRCOV  Evaluate training covariance matrix
%                    corresponding to Gaussian noise
%
%  Description
%    C = LIK_GAUSSIAN_TRCOV(GP, TX) takes in covariance function
%    of a Gaussian process GP and matrix TX that contains
%    training input vectors. Returns covariance matrix C. Every
%    element ij of C contains covariance between inputs i and j
%    in TX. This subfunction is needed only in Gaussian likelihood.
%
%  See also
%    LIK_GAUSSIAN_COV, LIK_GAUSSIAN_TRVAR, GP_COV, GP_TRCOV

  [n, m] =size(x);
  n1=n+1;

  if isempty(lik.n)
      C = sparse(1:n,1:n,ones(n,1).*lik.sigma2,n,n);
  else
      C = sparse(1:n, 1:n, lik.sigma2./lik.n, n, n);
  end

end

function C = lik_gaussian_trvar(lik, x)
%LIK_GAUSSIAN_TRVAR  Evaluate training variance vector
%                    corresponding to Gaussian noise
%
%  Description
%    C = LIK_GAUSSIAN_TRVAR(LIK, TX) takes in covariance function
%    of a Gaussian process LIK and matrix TX that contains
%    training inputs. Returns variance vector C. Every element i
%    of C contains variance of input i in TX. This subfunction is
%    needed only in Gaussian likelihood.
%
%
%  See also
%    LIK_GAUSSIAN_COV, GP_COV, GP_TRCOV

  [n, m] =size(x);
  if isempty(lik.n)
      C=repmat(lik.sigma2,n,1);
  else
      C=lik.sigma2./lik.n(:);
  end

end

function [lpy, Ey, Vary] = lik_gaussian_predy(lik, Ef, Varf, yt, zt)
%LIK_Gaussian_PREDY    Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_POISSON_PREDY(LIK, EF, VARF YT, ZT)
%    Returns also the predictive density of YT, that is
%        p(yt | y,zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the incedence counts YT, expected counts ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_POISSON_PREDY(LIK, EF, VARF, YT, ZT)
%    takes a likelihood structure LIK, posterior mean EF and
%    posterior variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  lpy = [];
  Ey = Ef;
  EVary = lik.sigma2;
  VarEy = Varf;
  Vary = EVary + VarEy;
  if numel(yt) ~= 0
      lpy = norm_lpdf(yt,Ey,sqrt(Vary));
  end
end

function [logM_0, m_1, sigm2hati1] = lik_gaussian_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
%LIK_PROBIT_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_PROBIT_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY) takes a likelihood structure LIK, class labels Y, index
%    I and cavity variance S2 and mean MYY. Returns the zeroth
%    moment M_0, mean M_1 and variance M_2 of the posterior
%    marginal (see Rasmussen and Williams (2006): Gaussian
%    processes for Machine Learning, page 55). This subfunction
%    is needed when using EP for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    GPEP_E

%   m_1=myy_i;
%   sigm2hati1=sigm2_i;
%   logM_0=zeros(size(y));

s2 = lik.sigma2;
tau = 1./s2 + 1./sigm2_i;
w = (y(i1)./s2 + myy_i./sigm2_i)./tau;
%Zi = 1 ./( sqrt( 2.*pi.*(s2+sigm2_i) ) ) .* exp( -0.5*(y(i1)-myy_i).^2./(s2+sigm2_i) );

m_1 = w;
sigm2hati1 = 1./tau;
%logM_0 = log(Zi)
logM_0 = -0.5*log( 2.*pi) - 0.5*log( (s2+sigm2_i) )  + ( -0.5*(y(i1)-myy_i).^2./(s2+sigm2_i) );

end

function [g_i] = lik_gaussian_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_NEGBIN_SITEDERIV  Evaluate the expectation of the gradient
%                      of the log likelihood term with respect
%                      to the likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_NEGBIN_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIK, incedence counts Y, expected
%    counts Z, index I and cavity variance S2 and mean MYY.
%    Returns E_f [d log p(y_i|f_i) /d a], where a is the
%    likelihood parameter and the expectation is over the
%    marginal posterior. This term is needed when evaluating the
%    gradients of the marginal likelihood estimate Z_EP with
%    respect to the likelihood parameters (see Seeger (2008):
%    Expectation propagation for exponential families). This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods and there are likelihood parameters.
%
%  See also
%    GPEP_G

s2 = lik.sigma2;
tau = 1/s2 + 1/sigm2_i;
w = (y(i1)/s2 + myy_i/sigm2_i)/tau;
%Zi = 1/( sqrt(2*pi*(s2+sigm2_i)) )*exp( -0.5*(y(i1)-myy_i)^2/(s2+sigm2_i) );

%g_i = 0.5*( (1/tau + w.^2 -2*w*y(i1) + y(i1).^2 ) /s2 - 1)/s2 * s2;
g_i = 0.5*( (1/tau + w.^2 -2*w*y(i1) + y(i1).^2 ) /s2 - 1);

end

function reclik = lik_gaussian_recappend(reclik, ri, lik)
%RECAPPEND  Record append
%
%  Description
%    RECLIK = LIK_GAUSSIAN_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood function record structure RECLIK, record index RI
%    and likelihood function structure LIK with the current MCMC
%    samples of the parameters. Returns RECLIK which contains all
%    the old samples and the current samples from LIK. This
%    subfunction is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC and GP_MC -> RECAPPEND

  if nargin == 2
    % Initialize the record
    reclik.type = 'Gaussian';

    % Initialize the parameters
    reclik.sigma2 = [];
    reclik.n = [];

    % Set the function handles
    reclik.fh.pak = @lik_gaussian_pak;
    reclik.fh.unpak = @lik_gaussian_unpak;
    reclik.fh.lp = @lik_gaussian_lp;
    reclik.fh.lpg = @lik_gaussian_lpg;
    reclik.fh.ll= @lik_gaussian_ll;
    reclik.fh.llg = @lik_gaussian_llg;
    reclik.fh.llg2 = @lik_gaussian_llg2;
    reclik.fh.llg3 = @lik_gaussian_llg3;
    reclik.fh.tiltedMoments = @lik_gaussian_tiltedMoments;
    reclik.fh.siteDeriv = @lik_gaussian_siteDeriv;
    reclik.fh.cfg = @lik_gaussian_cfg;
    reclik.fh.trcov  = @lik_gaussian_trcov;
    reclik.fh.trvar  = @lik_gaussian_trvar;
    reclik.fh.predy = @lik_gaussian_predy;
    reclik.fh.recappend = @lik_gaussian_recappend;

    reclik.p=[];
    reclik.p.sigma2=[];
    if ~isempty(ri.p.sigma2)
      reclik.p.sigma2 = ri.p.sigma2;
    end
  else
    % Append to the record
    likp = lik.p;

    % record sigma2
    reclik.sigma2(ri,:)=lik.sigma2;
    if isfield(likp,'sigma2') && ~isempty(likp.sigma2)
      reclik.p.sigma2 = likp.sigma2.fh.recappend(reclik.p.sigma2, ri, likp.sigma2);
    end
    % record n if given
    if isfield(lik,'n') && ~isempty(lik.n)
      reclik.n(ri,:)=lik.n(:)';
    end
  end
end


close all;
clear all;
clc;

data_file_path = 'bin/ra_mmv_phase_transition_snr_40db_s_8.mat';
options.export = true;
options.export_dir = 'bin';
options.export_name = 'ra_mmv_snr_40_db_s_8';
options.chosen_ks = [2, 4, 8, 16, 32, 64];
options.subtitle = 'Rank Aware, SNR=40dB s=8';
spx.pursuit.PhaseTransitionAnalysis.print_results(data_file_path, ...
    'CoSaMP', options);


function C = spm_mesh_isoline(M, T, t)
% Compute isolines on a triangular mesh
% FORMAT C = spm_mesh_isoline(M, T, t)
% M   - a GIfTI object or patch structure
% T   - [vx1] data vector
% t   - isovalue [Default: 0]
%
% C   - struct array of isolines with fields 'xdata', 'ydata', 'zdata' and
%       'isopen'
%__________________________________________________________________________
%
% M = gifti(fullfile(spm('Dir'),'canonical','cortex_20484.surf.gii'));
% M = export(M,'patch');
% M = spm_mesh_inflate(M);
% T = randn(size(M.vertices,1),1);
% T = spm_mesh_smooth(M,T,100);
% H = spm_mesh_render('Disp',M);
% H = spm_mesh_render('Overlay',H,T);
% hold on
% t = linspace(min(T),max(T),20);
% for i=1:numel(t)
%   C = spm_mesh_isoline(M,T,t(i));
%   for j=1:numel(C)
%     plot3(C(j).xdata,C(j).ydata,C(j).zdata,'k-');
%   end
% end
%__________________________________________________________________________
% Copyright (C) 2019 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_mesh_isoline.m 7618 2019-06-17 12:29:46Z guillaume $

if nargin < 3, t = 0; end

C = spm_mesh_contour(M,struct('T',T,'t',t));


function inpC = regCorrIntGradWiener(inp, Int, Noise);
% regCorrIntGradWiener - corrects for the intensity gradient applying a
%                     wiener-like filtering.
%
%    inpC = regCorrIntGradWiener(inp, Int, Noise);
%
% INPUT:
%   inp   - original inplanes
%   Int   - estimated intensity
%   Noise - estimated power-spatial distribution of the noise
%
% Oscar Nestares - 5/99
%

% minimum noise
sigma2 = mean(Noise(:))/2;

% correction
inpC = inp.*Int ./ (Int.^2 + Noise + sigma2);


function c = integrate(c)

%INTEGRATE   integrates each trace.
%
% C = INTEGRATE(C) integrates each trace. Prior to integrating, the trend
% and mean of each trace is removed. Depending on the application, it is
% recommended that users also consider applying a gentle high pass filter
% to remove unwanted low frequencies that can dominate the integrated
% waveforms.

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

c.W = detrend(c.W);
c.W = demean(c.W);
c.W = integrate(c.W);


% Example_4_PRVI: Mesh the west North Atlantic Ocean with high resolution
% around Puerto Rico and US Virgin Islands.
% User beware: this is a very intensive mesh generation example, takes
% around 1 hr.
clc; clearvars

addpath('..')
addpath(genpath('../utilities/'))
addpath(genpath('../datasets/'))
addpath(genpath('../m_map/'))

%% The constant parameters for all domains
wl        = 30;         % elements to resolve M2 wavelength
dt        = 0;          % use automatic timestep
grade     = 0.25; 		% mesh grade in decimal percent.
R         = -5;    		% number of elements to resolve feature width.
slp       = 15;         % 2*pi/number of elements to resolve slope
fl        = -50;        % use filter equal to Rossby radius divided by 50

%%  For relatively coarse resolution west North Atlantic Ocean
bbox = [-100 -53   	    % lon_min lon_max
         5    52.5];    % lat_min lat_max
min_el = 1000;  		% minimum resolution in meters.
max_el = 10e3;       	% maximum resolution in meters.

coastline = 'GSHHS_f_L1';
dem       = 'SRTM15+.nc';
gdat{1} = geodata('shp',coastline,...
                  'dem',dem,...
                  'bbox',bbox,...
                  'h0',min_el);

fh{1} = edgefx('geodata',gdat{1},...
               'fs',R,...
               'wl',wl,...
               'slp',slp,'fl',fl,...
               'max_el',max_el,...
               'dt',dt,...
               'g',grade);

%% For High Resolution around Puerto Rico and US Virgin Islands
coastline = ["pr_1s_0m_contour","usvi_0m_contour","sj_0contour_closed"];

dems      = ["pr_1s.nc","usvi_1_mhw_2014.nc", "san_juan_19_prvd02_2015.nc"];

for ii = 1:length(dems)
    % use same parameters as coarse mesh, just change min_el
    if ii == length(dems)
        min_el    = 10;  % minimum resolution in meters.
    else
        min_el    = 30;  % minimum resolution in meters.
    end
    % bbox is taken automatically from the DEM
    gdat{ii+1} = geodata('shp',coastline{ii},...
                         'dem',dems{ii},...
                         'h0',min_el);
    fh{ii+1} = edgefx('geodata',gdat{ii+1},...
                      'fs',R,...
                      'wl',wl,...
                      'slp',slp,'fl',fl,...
                      'max_el',max_el,...
                      'dt',dt,...
                      'g',grade);
end

%% Pass your edgefx class objects along with some meshing options
%% and build the mesh...
% (note that the nested edgefxs will be smoothed together with this call)
mshopts = meshgen('ef',fh,'bou',gdat,'plot_on',1,'itmax',50,...
                  'proj','lam');

% now build the mesh with your options and the edge function.
mshopts = mshopts.build;

% Get out the msh class from meshgen
m = mshopts.grd;

%% Interpolate on the bathy and gradients (automatically loops over all data)
m = interp(m,gdat,'mindepth',1); % interpolate bathy to the mesh with minimum depth of 1 m

%% Make the nodestrings
m = make_bc(m,'auto',gdat{1}); % make the nodestring boundary conditions

%% Plot and save the msh class object/write to fort.14
plot(m,'type','bd'); % plot triangulation with boundary conditions
plot(m,'type','b');  % plot the bathy
plot(m,'type','reso','colormap',[10 0 10e3]) % plot the resolution
% Save as a msh class
save('PRVI_msh.mat','m');
% Write an ADCIRC fort.14 compliant file to disk.
write(m,'PRVI_mesh')


function Image_out=Intensity_adjust(Image_in);
%This code extends the intensity limits of  an image from [low_in; high_in]
%to [0;255]. It is most convenient for applications where operations such
%as edge detection or black to white transformations are to be performed
%on a sequence of images. Threshold values are fixed for different images.
%
%   CLASS SUPPORT
%   -------------
%   The input is an RGB image.
%
%
%   Syntaxes:
%   Image_out=Intensity_adjust('sky.jpg');
%   The output is an RGB image of the same size as the initial image.
%   This code is written by:
%                         Nassim Khaled
%                         American University of Beirut
%                         Research and Graduate Assistant
%
%  Developed under Matlab 7                       Date:  April,2005

a=imread(Image_in);
s=rgb2gray(a);
b=double(s);
low_in=min(b(:))/255;
high_in=max(b(:))/255;
Image_out = imadjust(a,[low_in; high_in],[0;1]);


%--- help for statespace/forecast ---
%
% FORECAST Forecast states and observations of state-space models
%
%  Syntax:
%
%    [Y,YMSE,X,XMSE] = forecast(Mdl,numPeriods,Y0)
%    [Y,YMSE,X,XMSE] = forecast(Mdl,numPeriods,name,value,...)
%
%  Description:
%
%    Generate multiple-period forecasts for observation vector y(t) and state
%    vector x(t) for a general state-space model (SSM) of the form:
%
%    State equation:       x(t) = A(t) * x(t-1) + B(t) * u(t)
%    Observation equation: y(t) = C(t) * x(t)   + D(t) * e(t)
%
%    where u(t) and e(t) are uncorrelated, unit-variance white noise vector
%    processes. The length of x(t), y(t), u(t), and e(t) is m, n, k, and h,
%    respectively.
%
%    Additionally, forecast uncertainties are also generated.
%
%  Input Arguments:
%
%    Mdl - A state-space model, as created by the SSM constructor or
%      SSM/ESTIMATE method.
%
%    numPeriods - Positive, scalar, integer specifying the forecast horizon.
%
%    Y0 - Observed response data to be forecasted. For time-invariant models
%      in which the length of each observation vector (n) is the same, Y0 is
%      a T-by-n matrix. For time-varying models in which the length of the
%      observation vector changes, Y0 is a T-by-1 cell array in which each
%      element contains a time-varying n-element vector of observations, y(t),
%      associated with the corresponding period. The last observation is the
%      most recent.
%
%  Optional Input Name/Value Pairs:
%
%    'A' Cell vector of forecasted state transition matrices in which each
%        element is a matrix corresponding to a period in the forecast horizon
%        at time t = 1,2,...,numPeriods. If the length of the state vector
%        x(t) is constant, then each element is a square m-by-m matrix;
%        however, if the length of x(t) changes, then some elements are
%        non-square matrices. The length of the cell array must be at least
%        numPeriods, and any elements beyond the forecast horizon are ignored.
%        By default, the last coefficient of the input SSM model Mdl is used
%        for all future periods.
%
%    'B' Cell vector of forecasted state disturbance loading matrices in which
%        each element is a matrix corresponding to a period in the forecast
%        horizon at time t = 1,2,...,numPeriods. If the lengths of the state
%        vector x(t) and disturbance vector u(t) are constant, then each
%        element is an m-by-k matrix; however, if the length of x(t) or u(t)
%        changes, then the elements are matrices of various sizes. The length
%        of the cell array must be at least numPeriods, and any elements
%        beyond the forecast horizon are ignored. By default, the last
%        coefficient of the input SSM model Mdl is used for all future periods.
%
%    'C' Cell vector of forecasted measurement sensitivity matrices in which
%        each element is a matrix corresponding to a period in the forecast
%        horizon at time t = 1,2,...,numPeriods. If the lengths of the
%        observation vector y(t) and state vector x(t) are constant, then each
%        element is an n-by-m matrix; however, if the length of y(t) or x(t)
%        changes, then the elements are matrices of various sizes. The length
%        of the cell array must be at least numPeriods, and any elements
%        beyond the forecast horizon are ignored. By default, the last
%        coefficient of the input SSM model Mdl is used for all future periods.
%
%    'D' Cell vector of forecasted observation innovation matrices in which
%        each element is a matrix corresponding to a period in the forecast
%        horizon at time t = 1,2,...,numPeriods. If the lengths of the
%        observation vector y(t) and innovation vector e(t) are constant, then
%        each element is an n-by-h matrix; however, if the length of y(t) or
%        e(t) changes, then the elements are matrices of various sizes. The
%        length of the cell array must be at least numPeriods, and any
%        elements beyond the forecast horizon are ignored. By default, the
%        last coefficient of the input SSM model Mdl is used for all future
%        periods.
%
%    'Predictors0' T-by-d matrix of common predictor variables used to
%                  include a regression component in the observation equation.
%                  Observations at time t are deflated such that
%
%                  [y(t) - z(t)*b] = C * x(t) + D * e(t)
%
%                  where z(t) is a vector of predictor variables and b is
%                  the regression coefficient vector (see below). The default
%                  is an empty matrix (no regression component)
%
%    'PredictorsF' numPeriods-by-d matrix of common predictor variables used to
%                  include a regression component in the observation equation.
%
%    'Beta'        d-by-n matrix of regression coefficients associated with
%                  predictors (see above).
%
%  Output Arguments:
%
%    Y - Point forecasts of observations, E[y(t)|y(t-1),...,y(1)], for
%      t = 1,2,...,numPeriods. For time-invariant models in which the length
%      of each observation vector (n) is the same, this is a numPeriods-by-n
%      matrix. For time-varying models in which the length of the observation
%      vector changes, this is a numPeriods-by-1 cell array in which each
%      element contains a time-varying n-element vector of forecasts associated
%      with the corresponding period.
%
%    YMSE - Forecast error variances of future observations,
%      Cov[y(t)|y(t-1),...,y(1)], for t = 1,2,...,numPeriods. For
%      time-invariant models in which the length of each observation vector
%      (n) is the same, this is a numPeriods-by-n matrix. For time-varying
%      models in which the length of the observation vector changes, this is
%      a numPeriods-by-1 cell array in which each element contains a
%      time-varying n-element vector of forecast error variances associated
%      with the corresponding period.
%
%    X - Point forecasts of states, E[x(t)|y(t-1),...,y(1)], for t = 1,2,...,
%      numPeriods. For time-invariant models in which the length of each state
%      vector (m) is the same, this is a numPeriods-by-m matrix. For
%      time-varying models in which the length of the state vector changes,
%      this is a numPeriods-by-1 cell array in which each element contains a
%      time-varying m-element vector of forecasts associated with the
%      corresponding period.
%
%    XMSE - Forecast error variances of future states,
%      Cov[x(t)|y(t-1),...,y(1)], for t = 1,2,...,numPeriods. For
%      time-invariant models in which the length of each state vector (m) is
%      the same, this is a numPeriods-by-m matrix. For time-varying models
%      in which the length of the state vector changes, this is a
%      numPeriods-by-1 cell array in which each element contains a time-varying
%      m-element vector of forecast error variances associated with the
%      corresponding period.
%
%  See also SSM, FILTER, SMOOTH, SIMULATE, ESTIMATE.
%
%    Other functions named forecast
%
%       abstvar/forecast         garch/forecast
%       arima/forecast           generic/forecast
%       conjugateblm/forecast    gjr/forecast
%       customblm/forecast       regARIMA/forecast
%       diffuseblm/forecast      semiconjugateblm/forecast
%       egarch/forecast          varm/forecast
%       empiricalblm/forecast    vecm/forecast
%


function kern = polyardKernParamInit(kern)

% POLYARDKERNPARAMINIT POLYARD kernel parameter initialisation.
% The automatic relevance determination version of the polynomial
% kernel is included for completeness, but its use is generally not
% recommended.
%
%  k(x_i, x_j) = sigma2*(w*x_i'*A*x_j+b)^d
%
% The kernel parameters are sigma2 (kern.variance), w
% (kern.weightVariance), b (kern.biasVariance), A
% (kern.inputScales) a diagonal matrix of input scales and d
% (kern.degree). Only gradients of the first four are provided for
% kernel optimisation, it is assumed that polynomial degree would
% be set by hand.
%
% The kernel is not recommended as it is badly behaved when the
% w*x_i'*A*x_j + b has a magnitude greater than one.
%
% SEEALSO : polyKernParamInit
%
% FORMAT
% DESC initialises the automatic relevance determination polynomial
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% KERN

kern.weightVariance = 1;
kern.biasVariance = 1;
kern.variance = 1;
% These parameters are restricted to lie between 0 and 1.
kern.inputScales = 0.999*ones(1, kern.inputDimension);
kern.nParams = 3 + kern.inputDimension;

kern.degree = 2;

kern.transforms(1).index = [1 2 3];
kern.transforms(1).type = optimiDefaultConstraint('positive');
kern.transforms(2).index = [4:kern.nParams];
kern.transforms(2).type = optimiDefaultConstraint('zeroone');

kern.isStationary = false;


function [X0,isYout] = VBA_spm_Xadjust(SPMfile,VOIfile,varthresh)
% gets the effects of no interest, wrt to which the data has been adjusted
% [X0,isYout] = spm_Xadjust(SPMfile,VOIfile,varthresh)
% IN:
%   - SPMfile: name of the SPM file
%   - VOIfile: name of the VOI file
%   - varthresh: threshold for PCA on X0 (fraction of explained variance).
%   Default is 0.95.
% OUT:
%   - X0: confounds matrix
%   - isYout: vector of indices of scans that were effectively removed from
%   the GLM using scan-nulling regressors included in the original
%   confounds matrix.

try, varthresh; catch, varthresh = 0.95; end

load(SPMfile)
load(VOIfile)

if isequal(xY.Ic,0)
    X0 = [];
    isYout = [];
    return
end
Fc = SPM.xCon(xY.Ic);
ind = sum(abs(Fc.c),2)==0;
X0 = SPM.xX.X(:,ind);

% remove scan-nulling regressors
n0 = size(X0,2);
remove = [];
isYout = [];
for i=1:n0
    X0i = X0(:,i) - X0(1,i);
    i1 = find(X0i~=0);
    tmp = zeros(size(X0,1),1);
    tmp(i1) = 1;
    if isequal(X0i,tmp)
        remove = [remove;i];
        isYout = [isYout,i1];
    end
end
X0 = X0(:,setdiff(1:size(X0,2),remove));
X0 = bsxfun(@minus,X0,mean(X0,1));
X0 = bsxfun(@rdivide,X0,std(X0,[],1));
if varthresh < 1
    [u,s,v] = svd(X0);
    s2 = diag(s).^2;
    ev = cumsum(s2./sum(s2));
    it = find(ev>=varthresh, 1 );
    X0 = [u(:,1:it),ones(size(X0,1),1)./sqrt(size(X0,1))];
end

return

% the following reproduces the code in spm_regions.m:

% sX = SPM.xX.xKXs;
% X0 = sX.X*(eye(spm_sp('size',sX,2)) - spm_sp('xpx-',sX)*sf_H(Fc,sX));
%
% function H = sf_H(Fc,sX)
% if sf_ver(Fc) > 1,
%    hsqr = sf_Hsqr(Fc,sX);
%    H    = hsqr' * hsqr;
% else
%    H = Fc.c * pinv(Fc.X1o' * Fc.X1o) * Fc.c';
% end
%
% function hsqr = sf_Hsqr(Fc,sX)
% if sf_ver(Fc) > 1,
%    hsqr = spm_sp('ox',spm_sp('set',Fc.X1o.ukX1o))' * spm_sp('cukx',sX);
% else
%    hsqr = spm_sp('ox',spm_sp('set',Fc.X1o))'*spm_sp('x',sX);
% end
%
% function v = sf_ver(Fc)
% if isstruct(Fc.X0), v = 2; else v = 1; end


function gauss_seidel_test ( )

%*****************************************************************************80
%
%% GAUSS_SEIDEL_TEST tests the GAUSS_SEIDEL library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2011
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the GAUSS_SEIDEL library.\n' );

  gauss_seidel_test01 ( );
  gauss_seidel_test02 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function component = partition(model, solver, algorithm)
% swiftcc++ and fastcc++ augment swiftcc and fastcc by this preprocess
%
% USAGE:
%
%    component = partition(model, solver, algorithm)
%
% INPUTS:
%    model:        the metabolic network reconstruction
%                    * .S - the associated sparse stoichiometric matrix
%                    * .lb - feasible flux distribution lower bound
%                    * .ub - feasible flux distribution uppper bound
%                    * .rxns - cell array of reaction abbreviations
%                    * .rev - the 0-1 indicator vector of the reversible reactions
%    solver:       the LP solver to be used; the currently available options
%                  are 'gurobi', 'linprog', and 'cplex' with the default value
%                  of 'linprog'. It fallbacks to the COBRA LP solver interface
%                  if another supported solver is called.
%    algorithm:    the backend algorithm to be utilized between 'swift' and 'fast'
%
% OUTPUT:
%    component:    the index set of the reactions constituting the maximum
%                  flux consistent metabolic subnetwork
%
% NOTE:
%
%    requires bioinformatics toolbox
%
% .. Authors:
%       - Mojtaba Tefagh, Stephen P. Boyd, 2019, Stanford University

    assert(license('test', 'bioinformatics_toolbox') == 1, ...
        'The required Bioinformatics toolbox is not available!');
    S = model.S;
    rev = model.rev;
    lb = model.lb;
    ub = model.ub;
    c = model.c;
    rxns = model.rxns;
    [m, n] = size(S);

    %% constructing the directed graph
    DG = zeros(m+1);
    for i = 1:n
        head = S(:, i) > 0;
        if ~any(head)
            head = m+1;
        end
        tail = S(:, i) < 0;
        if ~any(tail)
            tail = m+1;
        end
        DG(head, tail) = 1;
        if rev(i)
            DG(tail, head) = 1;
        end
    end

    %% finding strongly or weakly connected components in the graph
    DG = sparse(DG);
    [~, C] = graphconncomp(DG, 'Directed', true);
    C = C(1:end-1);

    %% finding weakly connected components in the graph
    if range(C) == 0
        [~, C] = graphconncomp(max(DG, DG.'), 'Directed', false);
        C = C(1:end-1);
    end

    %% partitioning the metabolic network
    component = zeros(n, 1);
    for i = 1:n
        v = C(S(:, i) ~= 0);
        component(i) = all(v == v(1))*v(1);
    end
    if range(component) == 0
        component = zeros(n, 1);
        if strcmp(algorithm, 'fast')
            component(fastcc(model, getCobraSolverParams('LP', 'feasTol')*100)) = 1;
        elseif strcmp(algorithm, 'swift')
            component(swiftcc(S, rev, solver)) = 1;
        end
    else
        newcomponent = zeros(n, 1);
        for i = unique(component).'
            core = component == i;
            if sum(core) > 1
                model.S = S(:, core);
                model.rev = rev(core);
                model.lb = lb(core);
                model.ub = ub(core);
                model.c = c(core);
                model.rxns = rxns(core);
                newcomponent(core) = partition(model, solver, algorithm);
            end
        end
        component = newcomponent;
    end
end


% FILTER_FREQ Calculate center frequencies and bandwidths
%
% Usage
%    [psi_xi, psi_bw, phi_bw] = FILTER_FREQ(filt_opt)
%
% Input
%    filt_opt (struct): The parameters defining the filter bank.
%
% Output
%    psi_xi (numeric): The center frequencies of the wavelet filters.
%    psi_bw (numeric): The bandwidths of the wavelet filters.
%    phi_bw (numeric): The bandwidth of the lowpass filter.
%
% Description
%    Called by WAVELET_1D and WAVELET_LAYER_1D, this function provides the
%    center frequency and bandwidth of a mother wavelet, whose parameters
%    are specified in filt_opt. It operates as a mere disjunction between
%    SPLINE_FREQ_1D, and MORLET_FREQ_1D.
%
% See also
%   MORLET_FREQ_1D, SPLINE_FREQ_1D, WAVELET_1D, WAVELET_LAYER_1D

function [psi_xi, psi_bw, phi_bw] = filter_freq(filter_options)
	if strcmp(filter_options.filter_type,'spline_1d') || ...
        strcmp(filter_options.filter_type,'selesnick_1d')
		[psi_xi,psi_bw,phi_bw] = dyadic_freq_1d(filter_options);
	elseif strcmp(filter_options.filter_type,'morlet_1d') || ...
		strcmp(filter_options.filter_type,'gabor_1d')
		[psi_xi,psi_bw,phi_bw] = morlet_freq_1d(filter_options);
	else
		error('Unknown filter type ''%s''', filter_options.filter_type);
	end
end


function mmse(filename,outfile,SPU)

%
%  Implements the MMSE algorithm [1].
%
%  Usage:  mmse(noisyFile, outputFile, SPU)
%
%         infile - noisy speech file in .wav format
%         outputFile - enhanced output file in .wav format
%         SPU  - if 1, includes speech-presence uncertainty
%                if 0, doesnt include speech-presence uncertainty
%
%
%  Example call:  mmse('sp04_babble_sn10.wav','out_mmse.wav',1);
%
%  References:
%   [1] Ephraim, Y. and Malah, D. (1985). Speech enhancement using a minimum
%       mean-square error log-spectral amplitude estimator. IEEE Trans. Acoust.,
%       Speech, Signal Process., ASSP-23(2), 443-445.
%
% Authors: Philipos C. Loizou
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%-------------------------------------------------------------------------

if nargin<3
    fprintf('Usage: mmse(infile.wav,outfile.wav,SPU) \n');
    fprintf('where SPU=1 - includes speech presence uncertainty\n');
    fprintf('      SPU=0 - does not includes speech presence uncertainty\n\n');
    return;
end;

if SPU~=1 & SPU~=0
    error('ERROR: SPU needs to be either 1 or 0.');
end

[x, Srate, bits]= wavread( filename);

% =============== Initialize variables ===============

len=floor(20*Srate/1000); % Frame size in samples
if rem(len,2)==1, len=len+1; end;
PERC=50; % window overlap in percent of frame size
len1=floor(len*PERC/100);
len2=len-len1;

win=hanning(len);  % define window
win = win*len2/sum(win);  % normalize window for equal level output

% Noise magnitude calculations - assuming that the first 6 frames is noise/silence
%
nFFT=2*len;
j=1;
noise_mean=zeros(nFFT,1);
for k=1:6
    noise_mean=noise_mean+abs(fft(win.*x(j:j+len-1),nFFT));
    j=j+len;
end
noise_mu=noise_mean/6;
noise_mu2=noise_mu.^2;

%--- allocate memory and initialize various variables

k=1;
img=sqrt(-1);
x_old=zeros(len1,1);
Nframes=floor(length(x)/len2)-1;
xfinal=zeros(Nframes*len2,1);

% --------------- Initialize parameters ------------
%
k=1;
aa=0.98;
eta= 0.15;
mu=0.98;
c=sqrt(pi)/2;
qk=0.3;
qkr=(1-qk)/qk;
ksi_min=10^(-25/10); % note that in Chap. 7, ref. [17], ksi_min (dB)=-15 dB is recommended

%===============================  Start Processing =======================================================
%
for n=1:Nframes

    insign=win.*x(k:k+len-1);

    %--- Take fourier transform of  frame
    %
    spec=fft(insign,nFFT);
    sig=abs(spec); % compute the magnitude
    sig2=sig.^2;

    gammak=min(sig2./noise_mu2,40);  % posteriori SNR
    if n==1
        ksi=aa+(1-aa)*max(gammak-1,0);
    else
        ksi=aa*Xk_prev./noise_mu2 + (1-aa)*max(gammak-1,0);
        % decision-direct estimate of a priori SNR
        ksi=max(ksi_min,ksi);  % limit ksi to -25 dB
    end

    log_sigma_k= gammak.* ksi./ (1+ ksi)- log(1+ ksi);
    vad_decision= sum( log_sigma_k)/nFFT;
    if (vad_decision< eta) % noise only frame found
        noise_mu2= mu* noise_mu2+ (1- mu)* sig2;
    end
    % ===end of vad===

    vk=ksi.*gammak./(1+ksi);
    [j0,err]=besseli(0,vk/2);
    [j1,err2]=besseli(1,vk/2);
    if any(err) | any(err2)
        fprintf('ERROR! Overflow in Bessel calculation in frame: %d \n',n);
    else
        C=exp(-0.5*vk);
        A=((c*(vk.^0.5)).*C)./gammak;
        B=(1+vk).*j0+vk.*j1;
        hw=A.*B;
    end

    % --- estimate speech presence probability
    %
    if SPU==1
        evk=exp(vk);
        Lambda=qkr*evk./(1+ksi);
        pSAP=Lambda./(1+Lambda);
       sig=sig.*hw.*pSAP;
    else
        sig=sig.*hw;
    end

    Xk_prev=sig.^2;  % save for estimation of a priori SNR in next frame

    xi_w= ifft( sig .* exp(img*angle(spec)),nFFT);

    xi_w= real( xi_w);

    xfinal(k:k+ len2-1)= x_old+ xi_w(1:len1);
    x_old= xi_w(len1+ 1: len);

    k=k+len2;

end
%========================================================================================

wavwrite(xfinal,Srate,16,outfile);


function [results] = computeQualityMetrics(image)

    % Anisotrophy Test
    [gray,rgb] = biqaa.blindimagequality(image,8,6,0,'degree');
    results.biqaa_gray = gray;
    results.biqaa_rgb = rgb;

    % BIQI Test
    results.biqi = biqi.biqi(image);

    % Bliinds2 Test
    results.bliinds = bliinds2.bliinds2_score(image);

    % IQVG Test
    addpath('/Users/dsoellinger/Documents/git/uni/Matlab Toolbox/cjlin1_svnlib/libsvm/matlab');
    results.iqvg = iqvg.IQVG(image);
    rmpath('/Users/dsoellinger/Documents/git/uni/Matlab Toolbox/cjlin1_svnlib/libsvm/matlab');

    % NIQE Test
    results.niqe = niqe.niqe(image);

    % DIVINE Test
    addpath('/Users/dsoellinger/Documents/git/uni/Matlab Toolbox/gregfreeman_libsvm/libsvm/matlab');
    results.divine = divine.divine(image);
    rmpath('/Users/dsoellinger/Documents/git/uni/Matlab Toolbox/gregfreeman_libsvm/libsvm/matlab');

    % BRISQUE Test
    % Requires libsvm 3.22. Make sure that it is installed.
    results.brisque = brisque.brisquescore(image);

end


% Copyright 2013 Oliver Johnson, Srikanth Patala
%
% This file is part of MisorientationMaps.
%
%     MisorientationMaps 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.
%
%     MisorientationMaps 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 MisorientationMaps.  If not, see <http://www.gnu.org/licenses/>.

function rgb = colormap432(v)

% this is to adjust to the "correct" fundamental sector
sR = sphericalRegion([vector3d(0,1,-1),vector3d(1,-1,0),vector3d.Z]);
cs = crystalSymmetry('m-3m');
v = project2FundamentalRegion(v,cs,sR.center);

pts = reshape(double(v),[],3);

kmax = sqrt(2)-1;
theta = atan2(pts(:,3),pts(:,2));
t2ind2 = find(pts(:,1) > 1/3 & theta > atan((1-2*pts(:,1))./(pts(:,1))));

tempvar2 = pts(t2ind2,2).*(1-pts(t2ind2,1));
tempvar3 = pts(t2ind2,1).*(pts(t2ind2,2)+pts(t2ind2,3))./(tempvar2 + 1*(tempvar2 == 0));
pts(t2ind2,:) = [pts(t2ind2,1) tempvar3.*pts(t2ind2,2) tempvar3.*pts(t2ind2,3)];
g1 = rotvec2mat([1 0 0 -3*pi/8]);
pts = pts*g1;
pts = [pts(:,1) - kmax pts(:,2) pts(:,3)];

tempvar = (1 + pts(:,2).*tan(pi/8)./(pts(:,3) + 1*(pts(:,3)==0)));
pts = [pts(:,1) pts(:,2).*tempvar pts(:,3).*tempvar];

%%%%% Def Step 3 %%%%%
pts = [pts(:,1) pts(:,2)*cos(pi/8)/tan(pi/8) (pts(:,3) - pts(:,1)./cos(pi/8))];

%%%%% Def Step 4 %%%%%%
theta = atan2(-pts(:,1),pts(:,2));
pts = [pts(:,1).*(sin(theta) + abs(cos(theta))) ...
    pts(:,2).*(sin(theta) + abs(cos(theta))) pts(:,3)];

%%%% Def Step 5 %%%%%
theta = atan2(-pts(:,1),pts(:,2));
rad = hypot(pts(:,2),pts(:,1));
pts = [-rad.*sin(2*theta) rad.*cos(2*theta) pts(:,3)];
kmax = (sqrt(2)-1);tempk = cos(pi/8)/tan(pi/8);
pts(:,1) = pts(:,1)/kmax;pts(:,2) = pts(:,2)/kmax;pts(:,3) = pts(:,3)*tempk;
g1 = rotvec2mat([0 0 1 -pi/6]);
pts = pts*g1;

% S= hsvconereg(pts);
initS = hsvconereg(pts);
g1 = rotvec2mat([0 1 0 pi]);
initS = initS*g1;
initS = [initS(:,1) + 1 initS(:,2) initS(:,3)+1];
rgb = [initS(:,2) initS(:,3) initS(:,1)];
rgb(rgb > 1 & rgb - 1 <= eps) = 1;
rgb(rgb < 0 & rgb >= -10*eps) = 0;

end

% testing code
%v = 0.2*equispacedS2Grid('points',1000);

%rgb = colormap432(v);
%plot(v,rgb)


disp('Loading iris data');
X = load('data/iris.mtx');
y = load('data/iris.truth');

disp('Running ITML');
num_folds = 2;
knn_neighbor_size = 4;
acc = CrossValidateKNN(y, X, @(y,X) MetricLearningAutotuneKnn(@ItmlAlg, y, X), num_folds, knn_neighbor_size);

disp(sprintf('kNN cross-validated accuracy = %f', acc));


function [same,sloc] = setset2(iset,jset)
%SETSET2 a (much) faster variant of ISMEMBER for edge lists.
%   [IN] = SETSET2(ISET,JSET) returns an I-by-1 array IN,
%   with IN(K) = TRUE if ISET(K,:) is present in JSET. This
%   routine is essentially an optimised ISMEMBER variant de-
%   signed for processing lists of edge indexing. ISET is an
%   I-by-2 array of "query" edges, JSET is a J-by-2 array of
%   edges to test against.
%
%   See also ISMEMBER

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 29/01/2017

%---------------------------------------------- basic checks
    if ( ~isnumeric(iset) || ~isnumeric(jset) )
        error('setset2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(iset) ~= +2 || ndims(jset) ~= +2)
        error('setset2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(iset,2)~= +2 || size(jset,2)~= +2)
        error('setset2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%---------------------------------------------- set v1 <= v2
    iset = sort(iset,2) ;
    jset = sort(jset,2) ;

%-- this is the slow, but easy-to-undertsand version of what
%-- is happening here...

  % if (nargout == +1)
  % same = ismember(iset,jset,'rows') ;
  % else
  % [same,sloc] = ...
  %        ismember(iset,jset,'rows') ;

%-- as above, the 'ROWS' based call to ISMEMBER can be sped
%-- up by casting the edge lists (i.e. pairs of UINT32 valu-
%-- es) to DOUBLE, and performing the sorted queries on vec-
%-- tor inputs!
    if (nargout == +1)
    same       = ismember( ...
        iset*[2^31;1], jset*[2^31;1]) ;
    else
   [same,sloc] = ismember( ...
        iset*[2^31;1], jset*[2^31;1]) ;
    end

end


function dataout=modeDRAEC_BSS(nummics, numrefs, datain)
%
% Perform dr and aec together, then bss.
% nummics:              no. of mic channels
% numrefs:              no. of reference channels
% datain:               input data
% dataout:              output data
%

%% perform stft
addpath('stft2');
% fft size
fftsize=512;
stftshift=fftsize/2;

M=nummics;
R=numrefs;
N=M;

Xtf=cell(M+R, 1);
for m=1:M+R
    Xtf{m}=stft(datain(:, m), stftshift, fftsize, false);
end
[K, T]=size(Xtf{1});

Ytf=cell(M, 1);
for m=1:M
    Ytf{m}=zeros(K, T);
end

%% params
% dr filter length
DR_FLEN=5;
% forgetting factor for dr and aec
DRAEC_FORGET=0.999;
% forgetting factor for bss
BF_FORGET=0.999;
% the shape parameter of the source prior
GAMMA=0.2;
%
% used to keep stable
%
VAR_BIAS=0.01;
STABLE_EPS=1e-3;
DRAEC_DIAGLOAD=1e-6;
BF_DIAGLOAD=1e-6;

%% space for dr and aec
% current mic data backup
Miccurrent=zeros(K, M);
% ref and delayed mic data
draecfsize=R+M*DR_FLEN;
Refmicdelay=zeros(K, draecfsize);

% mic-ref correlation
Cxr=cell(K, 1);
for k=1:K
    Cxr{k}=zeros(M, draecfsize);
end

% reference auto correlation
Crr=cell(K, 1);
for k=1:K
    Crr{k}=zeros(draecfsize, draecfsize);
end

% reverb and echo path
REPath=cell(K, 1);
for k=1:K
    REPath{k}=zeros(M, draecfsize);
end

%% space for bss
% the weighted correlation matrices
C1=cell(K, 1);
C2=cell(K, 1);
for k=1:K
    C1{k}=STABLE_EPS*eye(M, M);
    C2{k}=STABLE_EPS*eye(M, M);
end

% demixing matrices
Demix=cell(K, 1);
for k=1:K
    Demix{k}=eye(N, M);
end

%% perform iteration
for tau=1:T
    %% perform dr and aec together
    % direct nearend and early reverberation
    Early=zeros(K, M);

    %
    % shift in new data
    %
    % shift old mic data back
    Refmicdelay=circshift(Refmicdelay, M, 2);
    % shift in delayed mic data
    Refmicdelay(:, R+1:R+M)=Miccurrent;

    % mic data backup
    for m=1:M
        Miccurrent(:, m)=Xtf{m}(:, tau);
    end

    % shift in reference data
    for r=1:R
        Refmicdelay(:, r)=Xtf{M+r}(:, tau);
    end

    for k=1:K
        % calculate late reverberation and echo
        ref=Refmicdelay(k, :).';
        late=REPath{k}*ref;

        % direct nearend and early reverberation
        mic=Miccurrent(k, :).';
        early=mic-late;
        % output data
        Early(k, :)=early.';

        %
        % calculate nonlinearity
        %
        xsq=abs(mic).^2;
        ysq=abs(early).^2;

        phi=0;
        for m=1:M
            if ysq(m)<=xsq(m)
                phi=phi+ysq(m);
            else
                phi=phi+xsq(m);
            end
        end

        phi=(1-DRAEC_FORGET)*(phi+VAR_BIAS)^((GAMMA-2)/2);

        % update mic ref correlation
        Cxr{k}=DRAEC_FORGET*Cxr{k}+phi*(mic*ref');

        % update ref auto-correlation
        Crr{k}=DRAEC_FORGET*Crr{k}+phi*(ref*ref');

        % update echo and reverb path
        REPath{k}=Cxr{k}/(Crr{k}+DRAEC_DIAGLOAD*eye(draecfsize, draecfsize));
    end

    %% perform bss
    Bssout=zeros(K, M);

    %
    % calculate nonlinearity
    %
    phi1=0;
    phi2=0;

    for k=1:K
        x=Early(k, :).';
        y=Demix{k}*x;
        % output data
        Bssout(k, :)=y.';

        phi1=phi1+abs(y(1))^2;
        phi2=phi2+abs(y(2))^2;
    end

    phi1=(1-BF_FORGET)*(phi1+VAR_BIAS)^((GAMMA-2)/2);
    phi2=(1-BF_FORGET)*(phi2+VAR_BIAS)^((GAMMA-2)/2);

    % update the demixing matrices
    for k=1:K
        %
        % accumulate the weighted correlation
        %
        x=Early(k, :).';
        C1{k}=BF_FORGET*C1{k}+phi1*(x*x');
        C2{k}=BF_FORGET*C2{k}+phi2*(x*x');

        % solve gev problem
        D=heig2(BF_DIAGLOAD, C2{k}, C1{k});
        Demix{k}=D;
    end

    for m=1:M
        Ytf{m}(:, tau)=Bssout(:, m);
    end
end

%% perform istft and output signal
dataout=zeros(dataLength(T, stftshift, fftsize ), N);
for n=1:N
    dataout(:, n)=istft(Ytf{n}, stftshift, false);
end

end


classdef prtFeatSelLlnn < prtFeatSel
    % prtFeatSelLLnn  Local Learning based feature selection
    %
    %   FEATSEL = prtFeatSelLlnn returns a local learning based feature selection
    %   object.
    %
    %    FEATSEL = prtFeatSelLlnn(PROPERTY1, VALUE1, ...) constructs a
    %    prttFeatSelExhaustive object FEATSEL with properties as specified by
    %    PROPERTY/VALUE pair
    %
    %    A prtFeatSelExhaustive object has the following properties:
    %
    %    selectedFeatures       - The indices of the features selected,
    %                             a read-only parameter, found by training.
    %    verbosePlot            - Toggles plotting on/off during training
    %    nMaxIterations         - The maximum number of iterations
    %    normalizedWeightCutOff - The weight threshold for keeping a feature
    %
    %    The following features are settable, and are related to the
    %    training algorithm. Please see reference for further information.
    %
    %    kernelSigma
    %    sparsnessLambda
    %    vGradNMaxSteps
    %    vGradInitStepSize
    %    vGradChangeThreshold
    %    vGradNMaxStepSizeChanges
    %    weightChangeThreshold
    %
    %   Reference:
    %   http://www.computer.org/portal/web/csdl/doi/10.1109/TPAMI.2009.190
    %
    %   A prtFeatSelExhaustive object inherits the TRAIN and RUN methods
    %   from prtClass.
    %
    %   Example:
    %
    %   dataSet = prtDataGenSpiral;   % Create a 2 dimensional data set
    %   nNoiseFeatures = 100;      % Append 100 irrelevant features
    %   dataSet = prtDataSetClass(cat(2,dataSet.getObservations,randn([dataSet.nObservations, nNoiseFeatures])), dataSet.getTargets);
    %   featSel = prtFeatSelLlnn('verbosePlot',true);  % Create the feature
    %                                                  % selection object.
    %   featSel.nMaxIterations = 10;                   % Set the max # of
    %                                                  % iterations.
    %   featSel = featSel.train(dataSet);              % Train
    %
    %   See Also:  prtFeatSelStatic, prtFeatSelSfs, prtFeatSelExhaustive

    properties (SetAccess=private)
        % Required by prtAction
        name = 'Local Learning Nearest Neighbor'
        nameAbbreviation = 'LLNN'
    end

    properties

        kernelSigma = 1;
        sparsnessLambda = 2;
        vGradNMaxSteps = 100;
        vGradInitStepSize = 1;
        vGradChangeThreshold = 1e-4;
        vGradNMaxStepSizeChanges = 12;
        weightChangeThreshold = 0.01;
        nMaxIterations = 25;         % The maximum number of iterations
        normalizedWeightCutOff = 0.05;  % The weight threshold to include a feature.

        % Learned
        weights = [] ;           % The weights of each feature
        selectedFeatures = [];   % The selected features

        verbosePlot = false;     % Toggles plotting on/off during training
    end

    methods

        % Constructor %%
        function Obj = prtFeatSelLlnn(varargin)

            % Allow for string, value pairs
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end
    end

    methods (Access=protected,Hidden=true)

        % Train %%
        function Obj = trainAction(Obj,DS)

            % Remove stuff from data set, too many calls to getObservations
            X = DS.getObservations();
            Y = DS.getTargetsClassInd();

            vExpDistanceFunction = @(v,zBar)exp(-zBar*(v.^2));

            vFitnessFunction = @(v,cExpDistance)sum(log(1+cExpDistance)) + Obj.sparsnessLambda*sum(v.^2);

            w = ones(DS.nFeatures,1);
            wOld = w;
            wChanges = nan(Obj.nMaxIterations,1);
            for iter = 1:Obj.nMaxIterations

                % Find zBar
                % Eq: 3-5
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

                cSigma = Obj.kernelSigma*sqrt(sum((w./max(w))>Obj.normalizedWeightCutOff)); % Modify kernel to the "dimensionality" imposed by w

                zBar = zeros(DS.nObservations, DS.nFeatures);
                for iSamp = 1:DS.nObservations

                    % L1 Distance from this point in each dim
                    cD = abs(bsxfun(@minus,X(iSamp,:),X));
                    cD(iSamp,:) = inf;

                    % Label of this point
                    cY = Y(iSamp);

                    % Kernel mapped distance to this point
                    cK = exp(-sum(bsxfun(@times,cD,w(:)'),2)/cSigma); % Hard coded L1 Distance

                    % Calculate the prob of nearest miss and nearst hit
                    % Eqs: 4, 5
                    cKMiss = cK;
                    cKMiss(Y==cY) = 0; % Cant be a miss if you are the same type
                    cKMiss(iSamp) = 0; % Cant be your own miss
                    cKMiss = cKMiss./sum(cKMiss);

                    cKHit = cK;
                    cKHit(Y~=cY) = 0; % Cant be a hit if you are a different type
                    cKHit(iSamp) = 0; % Cant be your own hit
                    cKHit = cKHit./sum(cKHit);

                    % Eq: 3
                    cD(iSamp,:) = 0; % inf*0 = nan
                    zBar(iSamp,:) = sum(bsxfun(@times,cD, cKMiss),1) - sum(bsxfun(@times,cD, cKHit),1);

                end
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

                % Optimize v
                % Eqs: 9, 10
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                v = sqrt(w);

                cExpDistance = vExpDistanceFunction(v,zBar);
                vScore = vFitnessFunction(v,cExpDistance); % Eq: 9

                vScoreOld = vScore;
                for iStep = 1:Obj.vGradNMaxSteps
                    cStepSize = Obj.vGradInitStepSize*2; % we multiply by 2 since we divide by 2 at every iteration (even the first)

                    % Transform cExpDistance into necessary term for the
                    % gradient, we use the same name... just because
                    cExpDistance = cExpDistance./(1 + cExpDistance);
                    cExpDistance(isnan(cExpDistance)) = 0; % inf/(inf + 1)

                    % Eq: 10
                    cChange = Obj.sparsnessLambda*ones(size(w)) - sum(bsxfun(@times,cExpDistance, zBar),1)';

                    for iStepSize = 1:Obj.vGradNMaxStepSizeChanges
                        cStepSize = cStepSize / 2; % Lower the step size, we get here if 1) first iteration (we multiplied the step size by 2) or 2) step size was too big so we need to shrink it.

                        vNew = v - cStepSize*cChange.*v; % Eq: 10

                        cExpDistance = vExpDistanceFunction(vNew,zBar);

                        vNewScore = vFitnessFunction(vNew,cExpDistance);

                        if vNewScore < vScore
                            vScore = vNewScore;
                            v = vNew;
                            % Note that cExpDistance will be reused but
                            % immediately over written with its alter ego
                            % this saves a little mem I guess
                            break
                        else
                            % We increased the fitness (bad) so the step
                            % size must be too big.
                            % Continue in the loop decrease the step size
                            % by 50%.
                        end
                    end

                    if abs(vScore-vScoreOld)/mean([vScore vScoreOld]) < Obj.vGradChangeThreshold
                        break
                    else
                        vScoreOld = vScore;
                    end
                end
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

                w = v.^2;

                cWChange = norm(abs(w-wOld));

                %%% Other ideas for exit criterion %%%
                % cWChange = norm(abs(w-wOld))/sqrt(length(w));
                % cWChange = mean(abs(w-wOld)./mean(cat(2,w,wOld),2)); % Average percent change
                % wNorm = w ./ max(w);
                % wOldNorm = wOld ./ max(wOld);
                % cWChange = mean(abs(wNorm-wOldNorm)./mean(cat(2,wNorm,wOldNorm),2)); % Normalized Average percent change
                % cWChange = mean(abs(wNorm-wOldNorm)); % Normalized Average change

                if Obj.verbosePlot
                    subplot(2,1,1)
                    stem(w./max(w));
                    title('Feature Importance','FontSize',14)
                    xlabel('Feature')
                    ylabel('Normalized Feature Weight');
                    xlim([1 length(w)]);

                    subplot(2,1,2)
                    wChanges(iter) = cWChange;
                    plot(1:iter,wChanges(1:iter),'b-',1:iter,wChanges(1:iter),'rx')
                    title('Optimization Exit Criterion','FontSize',14)
                    xlabel('Iteration')
                    ylabel('Weight Change From Last Iteration')
                    xlim([0.5 iter+0.5]);
                    set(gca,'XTick',1:iter);

                    drawnow;
                end

                if cWChange < Obj.weightChangeThreshold
                    break
                end
                wOld =w;

            end

            Obj.weights = w;
            Obj.selectedFeatures = find(w > Obj.normalizedWeightCutOff);
        end

        % Run %
        function DataSet = runAction(Obj,DataSet)
            DataSet = DataSet.retainFeatures(Obj.selectedFeatures);
        end
    end
end


classdef LIRCMOP6 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP with large infeasible regions

%------------------------------- Reference --------------------------------
% Z. Fan, W. Li, X. Cai, H. Huang, Y. Fang, Y. You, J. Mo, C. Wei, and E.
% Goodman, An improved epsilon constraint-handling method in MOEA/D for
% CMOPs with large infeasible regions, Soft Computing, 2019, 23:
% 12491-12510.
%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Wenji Li

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            [popsize,variable_length] = size(X);
            sum1 = zeros(popsize,1);
            sum2 = zeros(popsize,1);
            for j = 2 : variable_length
                if mod(j,2) == 1
                    sum1 = sum1+(X(:,j)-sin((0.5*j/variable_length*pi)*X(:,1))).^2;
                else
                    sum2 = sum2+(X(:,j)-cos((0.5*j/variable_length*pi)*X(:,1))).^2;
                end
            end
            gx          = 0.7057;
            PopObj(:,1) = X(:,1)+10*sum1+gx;
            PopObj(:,2) = 1-X(:,1).^2+10.*sum2+gx;
            Population  = SOLUTION(X,PopObj,Constraint(PopObj),varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^2;
            R      = R + 0.7057;
            R(any(Constraint(R)>0,2),:) = [];
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [x,y] = meshgrid(linspace(0.7057,5,400));
            fes   = all(Constraint([x(:),y(:)])<=0,2);
            z     = nan(size(x));
            z(reshape(fes,size(z)) & (x-0.7057).^2+y>=1.7057) = 0;
            R = {x,y,z};
        end
    end
end

function PopCon = Constraint(PopObj)
    p     = [1.8,2.8];
    q     = [1.8,2.8];
    a     = [2,2];
    b     = [8,8];
    r     = 0.1;
    theta = -0.25 * pi;
    for k = 1 : 2
        PopCon(:,k) = r - ((PopObj(:,1)-p(k))*cos(theta)-(PopObj(:,2)-q(k))*sin(theta)).^2/(a(k)^2) -...
                      ((PopObj(:,1)-p(k))*sin(theta)+(PopObj(:,2)-q(k))*cos(theta)).^2/(b(k)^2);
    end
end


function [data, clamped] = mk_mutilated_samples(bnet, ncases, max_clamp, usecell)
% GEN_MUTILATED_SAMPLES Do random interventions and then draw random samples
% [data, clamped] = gen_mutilated_samples(bnet, ncases, max_clamp, usecell)
%
% At each step, we pick a random subset of size 0 .. max_clamp, and
% clamp these nodes to random values.
%
% data(i,m) is the value of node i in case m.
% clamped(i,m) = 1 if node i in case m was set by intervention.

if nargin < 4, usecell = 1; end

ns = bnet.node_sizes;
n = length(bnet.dag);
if usecell
  data = cell(n, ncases);
else
  data = zeros(n, ncases);
end
clamped = zeros(n, ncases);

csubsets = subsets(1:n, max_clamp, 0); % includes the empty set
distrib_cset = normalise(ones(1, length(csubsets)));

for m=1:ncases
  cset = csubsets{sample_discrete(distrib_cset)};
  nvals = prod(ns(cset));
  distrib_cvals = normalise(ones(1, nvals));
  cvals = ind2subv(ns(cset), sample_discrete(distrib_cvals));
  mutilated_bnet = do_intervention(bnet, cset, cvals);
  ev = sample_bnet(mutilated_bnet);
  if usecell
    data(:,m) = ev;
  else
    data(:,m) = cell2num(ev);
  end
  clamped(cset,m) = 1;
end


report_this_filefun(mfilename('fullpath'));

l = isnan(tmap);
tmap(l) = 1;

%l = tmap <  0;
%tmap(l) = 0;

%l = Z < 0.12;
%Z(l) = 0.14;

[lat,lon] = meshgrat(tmap,tmapleg);
%[lat,lon] = meshgrat(vlat,vlon);

%[smap,smapleg] = country2mtx('peru',100);
%[lat0, lon0] = meshgrat(smap,smapleg);

%sw = interp2(lon0,lat0,smap,lon,lat);
%l =  isnan(sw) == 1 | sw == 2 ;

ren = interp2(X,Y,Z,lon,lat);

mi = min(min(ren));

ren(l) = mi-0.01;

% ren = interp2(X,Y,Z,lon,lat);

% mi = min(min(ren));
l =  isnan(ren);
ren(l) = 0;

figure_w_normalized_uicontrolunits('pos',[150 500 1000 700])

hold on; axis off
axesm('MapProjection','eqaconic','MapParallels',[],...
    'MapLatLimit',[s4_south s3_north],'MapLonLimit',[s2_west s1_east])

meshm(ren,tmapleg,size(tmap),tmap);

daspectm('m',10);
tightmap
view([0 90])
camlight; lighting phong
set(gca,'projection','perspective');

load worldlo
h = displaym(POline); set(h(1),'color',[0.9 0.9 0.9],'Linewidth',2)
% h2 = displaym(PPpoint);
%h = displaym(PPtext); trimcart(h);

%pl = plotm(lima(:,2),lima(:,1),'hw');
%set(pl,'LineWidth',1.5,'MarkerSize',12,...
% 'MarkerFaceColor','y','MarkerEdgeColor','k')

pl = plotm(dam(:,2),dam(:,1),'^w');
set(pl,'LineWidth',1.5,'MarkerSize',12,...
    'MarkerFaceColor','w','MarkerEdgeColor','k')
%pl = plotm(coastline(:,2), coastline(:,1),'w','Linewidth',2);

ci = worldlo('PPpoint');
cx = ci(1).long;
cy = ci(1).lat;
hold on
plotm(cy,cx,'sk','Markersize',12,'Markerfacecolor',[ 1 1 1])

ri = worldlo('DNline');
hold on

rx = [ri(1).long ; ri(2).long];
ry = [ri(1).lat ; ri(2).lat ];
hold on
% plotm(ry,rx,'b','Linewidth',2);

zdatam(handlem('allline'),10000) % keep line on surface
%zdatam(handlem('alltext'),10000) % keep line on surface

j = jet;
%j = j(64:-1:1,:);
j = [ [ 0.93 0.9 0.9] ; j];

caxis([0. 0.28])
colormap(j);

axis off; set(gcf,'color','w')

setm(gca,'ffacecolor','k')
setm(gca,'fedgecolor','k','flinewidth',3);

setm(gca,'mlabellocation',3)
setm(gca,'meridianlabel','on')
setm(gca,'plabellocation',3)
setm(gca,'parallellabel','on')
setm(gca,'Fontcolor','k','Fontweight','bold','FontSize',12,'Labelunits','dm')

h5 = colorbar;
set(h5,'position',[0.86 0.35 0.01 0.3],'TickDir','out','Ycolor','k','Xcolor','k',...
    'Fontweight','bold','FontSize',12);
%set(gcf,'Inverthardcopy','off');


function out = DSLInit(startPos, endPos, map, scalling)

%% generate shape pattern
    radius = 10/scalling;
    mat = zeros(radius*2,radius*2);
    dista = @(a,b) sqrt(a*a+b*b);

    shapePattern = [];

    for x = 1:1:2*radius
        for y =1:1:2*radius
            if dista(x-radius-0.5,y-radius-0.5) > radius -1 && dista(x-radius-0.5,y-radius-0.5) < radius
                mat(x,y) = 1;
                shapePattern = [shapePattern; floor(x-radius) floor(y-radius) 0 0];
            end
        end
    end

    neighbours = [
        0 1 0 0;
        -1 0 0 0;
        0 -1 0 0;
        1 1 0 0;
        -1 -1 0 0;
        1 0 0 0;
        1 -1 0 0;
        -1 1 0 0;
    ]';

%% prepare all data

		out.map = map;
    out.startPos = [startPos(2:-1:1); 0; 0];
    out.endPos = [endPos(2:-1:1); 0; 0];
    startPos = out.startPos;

    out.sacalling = scalling;
    out.pattern = shapePattern';
    out.ucc = neighbours;
    out.height = ceil(length(out.map(:,1)));
    out.width = ceil(length(out.map(1,:)));
    out.graph = zeros(out.height, out.width,5);
    out.graph(:,:,1:2) = inf;
    out.graph(:,:,3) = false;
    out.graph(:,:,5) = -1;
    out.kM = 0;
    SQRT2 = sqrt(2)-1;
    out.comparator = DStarcmp;
    out.stack = java.util.PriorityQueue(180247, out.comparator);

    setRhs(out.endPos, 0);
    setQ(out.endPos);
    out.endPos(3:4) = [heur(out.endPos); 0];
    add(out.stack, out.endPos);

    %-----------------------------------------------------------
    function setQ(s)
         out.graph(s(1), s(2), 3) = true;
    end
    function setg(s, val)
        out.graph(s(1), s(2),1) = val;
    end
    function setRhs(s, val)
        out.graph(s(1), s(2),2) = val;
    end
    function out = heur(s)
        k = abs(startPos - s);
        out = SQRT2*min(k(1:2)) + max(k(1:2));
    end

end


function [b,c,d,x,y,z,qfac] = vox2rasToQform(vox2ras)
% [a,b,c,x,y,z,qfac] = vox2rasToQform(vox2ras)
%
% Converts a vox2ras matrix to NIFTI qform parameters.
% Note: the vox2ras should be 6 DOF. This code mostly just
% follows CH's mriToNiftiQform() in mriio.c.
%
%  hdr.pixdim(1) = qfac;
%  hdr.quatern_b = b;
%  hdr.quatern_c = c;
%  hdr.quatern_d = d;
%  hdr.qoffset_x = x;
%  hdr.qoffset_y = y;
%  hdr.qoffset_z = z;
%  hdr.qform_code = NIFTI_XFORM_SCANNER_ANAT=1;
%

%
% vox2rasToQform.m
%
% Original Author: Doug Greve
% CVS Revision Info:
%    $Author: nicks $
%    $Date: 2011/03/02 00:04:13 $
%    $Revision: 1.3 $
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

a = [];
if(nargin ~= 1)
  fprintf('[a,b,c,x,y,z,qfac] = vox2rasToQform(vox2ras)\n');
  return;
end

x = vox2ras(1,4);
y = vox2ras(2,4);
z = vox2ras(3,4);

d = sqrt(sum(vox2ras(:,1:3).^2));
Mdc = vox2ras(1:3,1:3) ./ repmat(d,[3 1]);
if(det(Mdc) == 0.0)
  fprintf('ERROR: vox2ras determinant is 0\n');
  return;
end

r11 = Mdc(1,1);
r21 = Mdc(2,1);
r31 = Mdc(3,1);
r12 = Mdc(1,2);
r22 = Mdc(2,2);
r32 = Mdc(3,2);
r13 = Mdc(1,3);
r23 = Mdc(2,3);
r33 = Mdc(3,3);

if(det(Mdc) > 0.0) qfac = 1.0;
else
  r13 = -r13;
  r23 = -r23;
  r33 = -r33;
  qfac = -1.0;
end

%  /* following mat44_to_quatern() */

a = r11 + r22 + r33 + 1.0;
if(a > 0.5)
  a = 0.5 * sqrt(a);
  b = 0.25 * (r32-r23) / a;
  c = 0.25 * (r13-r31) / a;
  d = 0.25 * (r21-r12) / a;
else
  xd = 1.0 + r11 - (r22+r33);
  yd = 1.0 + r22 - (r11+r33);
  zd = 1.0 + r33 - (r11+r22);
  if(xd > 1.0)
    b = 0.5 * sqrt(xd);
    c = 0.25 * (r12+r21) / b;
    d = 0.25 * (r13+r31) / b;
    a = 0.25 * (r32-r23) / b;
  elseif( yd > 1.0 )
    c = 0.5 * sqrt(yd);
    b = 0.25 * (r12+r21) / c;
    d = 0.25 * (r23+r32) / c;
    a = 0.25 * (r13-r31) / c;
  else
    d = 0.5 * sqrt(zd);
    b = 0.25 * (r13+r31) / d;
    c = 0.25 * (r23+r32) / d;
    a = 0.25 * (r21-r12) / d;
  end
  if(a < 0.0)
    a = -a;
    b = -b;
    c = -c;
    d = -d;
  end
end

return


classdef BCEIBEA < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Bi-criterion evolution based IBEA
% kappa --- 0.05 --- Fitness scaling factor

%------------------------------- Reference --------------------------------
% M. Li, S. Yang, and X. Liu, Pareto or non-Pareto: Bi-criterion evolution
% in multiobjective optimization, IEEE Transactions on Evolutionary
% Computation, 2016, 20(5): 645-665.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            kappa = Algorithm.ParameterSet(0.05);

            %% Generate random population
            NPC      = Problem.Initialization();
            [PC,nND] = PCSelection(NPC,Problem.N);

            %% Optimization
            while Algorithm.NotTerminated(PC)
                % PC evolving
                NewPC = Exploration(Problem,PC,NPC,nND,Problem.N);
                % NPC selection
                NPC = EnvironmentalSelection([NPC,NewPC],Problem.N,kappa);
                % NPC evolving
                MatingPool = TournamentSelection(2,Problem.N,-CalFitness(NPC.objs,kappa));
                NewNPC     = OperatorGA(Problem,NPC(MatingPool));
                NPC        = EnvironmentalSelection([NPC,NewNPC],Problem.N,kappa);
                % PC selection
                [PC,nND] = PCSelection([PC,NewNPC,NewPC],Problem.N);
            end
        end
    end
end


%compare max, min a mean manipulability for vanilla
load('/Users/arturogilaparicio/Desktop/arte/robots/RETHINK/sgo_v0.5/experiment1/mat/experiment1A.mat')

[correct_manips]=eval_experiment(Gout, random_manips);

[y,i]=max(sum(correct_manips'));
max_manip = correct_manips(i,:);

[y,i]=min(sum(correct_manips'));
min_manip = correct_manips(i,:);

mean_manip = mean(correct_manips);

figure, plot(max_manip), hold
plot(min_manip)
plot(mean_manip)


classdef P1Function < FeFunction

    properties (Access = public)
    end

    properties (Access = private)
        interpolation
    end

    properties (Access = private)
        meshCoarse
        meshFine
    end

    methods (Access = public)

        function obj = P1Function(cParams)
            obj.init(cParams);
            obj.createInterpolation();
        end

        function fxV = evaluate(obj, xV)
            obj.interpolation.computeShapeDeriv(xV);
            shapes = obj.interpolation.shape;
            nNode  = size(shapes,1);
            nGaus  = size(shapes,2);
            nF     = size(obj.fValues,2);
            nElem  = size(obj.mesh.connec,1);
            fxV = zeros(nF,nGaus,nElem);
            for iGaus = 1:nGaus
                for iNode = 1:nNode
                    node = obj.mesh.connec(:,iNode);
                    Ni = shapes(iNode,iGaus);
                    fi = obj.fValues(node,:);
                    f(:,1,:) = Ni*fi';
                    fxV(:,iGaus,:) = fxV(:,iGaus,:) + f;
                end
            end
        end

        function N = computeShapeFunctions(obj, quad)
%             obj.mesh.computeInverseJacobian(quad,obj.interpolation);
            xV = quad.posgp;
            obj.interpolation.computeShapeDeriv(xV);
            N = obj.interpolation.shape;
        end

        function dNdx  = computeCartesianDerivatives(obj,quad)
            switch obj.mesh.type
                case 'LINE'
                    invJ  = obj.mesh.computeInverseJacobian(quad,obj.interpolation);
                    nElem = obj.mesh.nelem;
                    nNode = obj.interpolation.nnode;
                    nDime = obj.mesh.ndim;
                    nGaus = quad.ngaus;
                    deriv  = obj.mesh.interpolation.deriv(1,:,:,:);
                    dShapes = deriv;
                    dN = zeros(nDime,nNode,nElem,nGaus);
                    for iGaus = 1:nGaus
                        invJg = invJ(:,1);
                        for iDim = 1:nDime
                            for iNode = 1:nNode
                                dShapeI(:,1) = squeeze(dShapes(1,iNode,iGaus,:));
                                dN(iDim,iNode,:,iGaus) = invJg.*dShapeI;
                            end
                        end
                    end
                    dNdx = dN;

                otherwise
                    nElem = size(obj.mesh.connec,1);
                    nNode = obj.interpolation.nnode;
                    nDime = obj.interpolation.ndime;
                    nGaus = quad.ngaus;
                    invJ  = obj.mesh.computeInverseJacobian(quad,obj.interpolation);
                    dShapeDx  = zeros(nDime,nNode,nElem,nGaus);
                    for igaus = 1:nGaus
                        dShapes = obj.interpolation.deriv(:,:,igaus);
                        for jDime = 1:nDime
                            invJ_JI   = invJ(:,jDime,:,igaus);
                            dShape_KJ = dShapes(jDime,:);
                            dSDx_KI   = bsxfun(@times, invJ_JI,dShape_KJ);
                            dShapeDx(:,:,:,igaus) = dShapeDx(:,:,:,igaus) + dSDx_KI;
                        end
                    end
                    dNdx = dShapeDx;
            end
        end

        function gradFun = computeGradient(obj, quad)
            dNdx = obj.computeCartesianDerivatives(quad);
            nDimf = obj.ndimf;
            nDims = size(dNdx, 1); % derivX, derivY (mesh-related?)
            nNode = size(dNdx, 2);
            nElem = size(dNdx, 3);
            nGaus = size(dNdx, 4);

            grad = zeros(nDims,nDimf, nElem, nGaus);
            for iGaus = 1:nGaus
                dNdx_g = dNdx(:,:,:,iGaus);
                for iDims = 1:nDims
                    for iNode = 1:nNode
                        dNdx_i = squeeze(dNdx_g(iDims, iNode,:));
                        nodes  = obj.mesh.connec(:,iNode);
                        f      = obj.fValues(nodes,:);
                        dfdx(1,:,:) = (dNdx_i.*f)';
                        grad(iDims,:,:,iGaus) = grad(iDims,:,:,iGaus) + dfdx;
                    end
                end
            end
            fVR = reshape(grad, [nDims*nDimf,nElem, nGaus]);
%             s.fValues    = permute(fVR, [1 3 2]);
            s.fValues    = fVR;
            s.mesh       = obj.mesh;
            s.quadrature = quad;
            gradFun = FGaussDiscontinuousFunction(s);
        end

        function symGradFun = computeSymmetricGradient(obj,quad)
            grad = obj.computeGradient(quad);
            nDimf = obj.ndimf;
            nDims = size(grad.fValues, 1)/nDimf;
            nGaus = size(grad.fValues, 2);
            nElem = size(grad.fValues, 3);

            gradReshp = reshape(grad.fValues, [nDims,nDimf,nGaus,nElem]);
            gradT = permute(gradReshp, [2 1 3 4]);
            symGrad = 0.5*(gradReshp + gradT);

            rshp = reshape(symGrad, [nDims*nDimf,nGaus,nElem]);
            s.fValues    = permute(rshp, [1 3 2]);
            s.quadrature = quad;
            s.mesh       = obj.mesh;
            symGradFun = FGaussDiscontinuousFunction(s);
        end

        function divF = computeDivergence(obj,q)
            dNdx = obj.computeCartesianDerivatives(q);
            fV = obj.fValues;
            nodes = obj.mesh.connec;
            nNode = obj.mesh.nnodeElem;
            nDim  = obj.mesh.ndim;
            divV = zeros(q.ngaus,obj.mesh.nelem);
            for igaus = 1:q.ngaus
                for kNode = 1:nNode
                    nodeK = nodes(:,kNode);
                    for rDim = 1:nDim
                        dNkr = squeeze(dNdx(rDim,kNode,:,igaus));
                        fkr = fV(nodeK,rDim);
                        int(1,:) = dNkr.*fkr;
                        divV(igaus,:) = divV(igaus,:) + int;
                    end
                end
            end
            s.quadrature = q;
            s.mesh       = obj.mesh;
            s.fValues(1,:,:) = divV;
            divF = FGaussDiscontinuousFunction(s);
        end

        function fdivF = computeFieldTimesDivergence(obj,q)
            fG  = obj.evaluate(q.posgp);
            dfG = obj.computeDivergence(q);
            fdivFG = bsxfun(@times,dfG.fValues,fG);
            s.quadrature = q;
            s.mesh       = obj.mesh;
            s.fValues    = fdivFG;
            fdivF = FGaussDiscontinuousFunction(s);
        end

        function fFine = refine(obj,m,mFine)
            fNodes  = obj.fValues;
            fEdges  = obj.computeFunctionInEdges(m, fNodes);
            fAll    = [fNodes;fEdges];
            s.mesh    = mFine;
            s.fValues = fAll;
            fFine = P1Function(s);
        end

        function dofConnec = computeDofConnectivity(obj)
            conne  = obj.mesh.connec;
            nDimf  = obj.ndimf;
            nNode  = size(conne, 2);
            nDofsE = nNode*nDimf;
            dofsElem  = zeros(nDofsE,size(conne,1));
            for iNode = 1:nNode
                for iUnkn = 1:nDimf
                    idofElem   = nDimf*(iNode - 1) + iUnkn;
                    globalNode = conne(:,iNode);
                    idofGlobal = nDimf*(globalNode - 1) + iUnkn;
                    dofsElem(idofElem,:) = idofGlobal;
                end
            end
            dofConnec = dofsElem;
        end

        function plot(obj) % 2D domains only

            switch obj.mesh.type
                case {'TRIANGLE','QUAD'}
                    x = obj.mesh.coord(:,1);
                    y = obj.mesh.coord(:,2);
                    figure()
                    for idim = 1:obj.ndimf
                        subplot(1,obj.ndimf,idim);
                        z = obj.fValues(:,idim);
                        a = trisurf(obj.mesh.connec,x,y,z);
                        view(0,90)
                        %             colorbar
                        shading interp
                        a.EdgeColor = [0 0 0];
                        title(['dim = ', num2str(idim)]);
                    end
                case 'LINE'
                    x = obj.mesh.coord(:,1);
                    y = obj.fValues;
                    figure()
                    plot(x,y)
            end
        end

        function plotArrowVector(obj)
            figure()
            a = obj.fValues;
            x = obj.mesh.coord(:,1);
            y = obj.mesh.coord(:,2);
            ax = a(:,1);
            ay = a(:,2);
            q = quiver(x,y,ax,ay);
            q.ShowArrowHead = 'off';
        end

        function print(obj, s)
%             s.mesh
            s.mesh = obj.mesh;
            s.fun = {obj};
            p = FunctionPrinter(s);
            p.print();
        end

        function [res, pformat] = getDataToPrint(obj)
            nNods = size(obj.fValues, 1);
            s.nDimf   = obj.ndimf;
            s.nData   = nNods;
            s.nGroup  = nNods;
            s.fValues = obj.getFormattedFValues();
            fps = FunctionPrintingSettings(s);
            [res, pformat] = fps.getDataToPrint();
        end

    end

    methods (Access = public, Static)

        function p1 = create(mesh, ndimf)
            s.fValues = zeros(mesh.nnodes, ndimf);
            s.mesh    = mesh;
            p1 = P1Function(s);
        end

        function fS = times(f1,f2)
            fS = f1.fValues.*f2.fValues;
            s.fValues = fS;
            s.mesh    = f1.mesh;
            fS = P1Function(s);
        end

        function fS = sum(f1,f2)
            fS = f1.fValues+f2.fValues;
            s.fValues = fS;
            s.mesh    = f1.mesh;
            fS = P1Function(s);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh    = cParams.mesh;
            obj.fValues = cParams.fValues;
            obj.ndimf   = size(cParams.fValues,2);
            obj.order   = 'LINEAR';
            obj.meshCoarse = cParams.mesh;
        end

        function createInterpolation(obj)
            m.type = obj.mesh.type;
            obj.interpolation = Interpolation.create(m,'LINEAR');
        end

        % Printing
        function fM = getFormattedFValues(obj)
            fM = obj.fValues;
        end

        function f = computeFunctionInEdges(obj,m,fNodes)
            s.edgeMesh = m.computeEdgeMesh();
            s.fNodes   = fNodes;
            eF         = EdgeFunctionInterpolator(s);
            f = eF.compute();
        end

    end

end


function rgb=hsl2rgb(hsl_in)
%Converts Hue-Saturation-Luminance Color value to Red-Green-Blue Color value
%
%Usage
%       RGB = hsl2rgb(HSL)
%
%   converts HSL, a M [x N] x 3 color matrix with values between 0 and 1
%   into RGB, a M [x N] X 3 color matrix with values between 0 and 1
%
%See also rgb2hsl, rgb2hsv, hsv2rgb

% (C) Vladimir Bychkovsky, June 2008
% written using:
% - an implementation by Suresh E Joel, April 26,2003
% - Wikipedia: http://en.wikipedia.org/wiki/HSL_and_HSV

hsl=reshape(hsl_in, [], 3);

H=hsl(:,1);
S=hsl(:,2);
L=hsl(:,3);

lowLidx=L < (1/2);
q=(L .* (1+S) ).*lowLidx + (L+S-(L.*S)).*(~lowLidx);
p=2*L - q;
hk=H; % this is already divided by 360

t=zeros([length(H), 3]); % 1=R, 2=B, 3=G
t(:,1)=hk+1/3;
t(:,2)=hk;
t(:,3)=hk-1/3;

underidx=t < 0;
overidx=t > 1;
t=t+underidx - overidx;

range1=t < (1/6);
range2=(t >= (1/6) & t < (1/2));
range3=(t >= (1/2) & t < (2/3));
range4= t >= (2/3);

% replicate matricies (one per color) to make the final expression simpler
P=repmat(p, [1,3]);
Q=repmat(q, [1,3]);
rgb_c= (P + ((Q-P).*6.*t)).*range1 + ...
        Q.*range2 + ...
        (P + ((Q-P).*6.*(2/3 - t))).*range3 + ...
        P.*range4;

rgb_c=round(rgb_c.*10000)./10000;
rgb=reshape(rgb_c, size(hsl_in));


function [val,vec,info] = syevd(jobz,uplo,a)
%SYEV   Computing eigen information.
%   [VAL,VEC,INFO] = SYEVD(JOBZ,UPLO,A) computes eigenvalues and,
%	optionally, eigenvectors of a symmetric matrix A.
%
%	JOBZ allows the user to choose if eigenvectors are to be computed.
%	JOBZ
%          = 'N':  Compute eigenvalues only;
%          = 'V':  Compute eigenvalues and eigenvectors.
%
%	UPLO allows the user to choose which part of the matrix will be referenced.
%	UPLO
%          = 'U':  Upper triangle of A is stored;
%          = 'L':  Lower triangle of A is stored.
%
%   If only eigenvalues are desired, the QR algorithm is used.
%
%	No input parameter are optional. If you want an easy to use interface,
%	see SYEV_DRIVER.

    vec = [];
	val = [];
	info = [];

	if (nargin~=3),
		disp('Wrong number of input parameters.');
		return;
	end;

	    %Do all the boring checking
	if (~isnumeric(a)),
		disp('The matrix is not composed of numeric values.');
		return;
	elseif (isinteger(a)),
		a=double(a);
	end;

    if (size(a,1)~=size(a,2)),
        disp('The matrix must be square.');
        return;
	end;
	n=int32(size(a,1));
	lda=int32(n);

	if (isComplex(a)),
		if (isDouble(a))

			w=zeros(double(n),1);
			work=complex(zeros(1));
			rwork=zeros(1,1);
			iwork=int32(zeros(1,1));
			lrwork=int32(-1);
			liwork=int32(-1);
			lwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,lrwork, iwork, liwork, info]=lapack_zheevd(jobz, uplo, n, a, lda, w, work, lwork, rwork,lrwork, iwork, liwork, info);
			lwork=int32(work(1));
			liwork=int32(iwork(1));
			lrwork=int32(rwork(1));

			work=complex(zeros(double(lwork),1));
			iwork=int32(zeros(double(lwork),1));
			rwork=zeros(double(lwork),1);
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,lrwork, iwork, liwork, info]=lapack_zheevd(jobz, uplo, n, a, lda, w, work, lwork, rwork,lrwork, iwork, liwork, info);
		else
			w=single(zeros(double(n),1));
			work=single(complex(zeros(1)));
			rwork=single(zeros(1,1));
			iwork=int32(zeros(1,1));
			lrwork=int32(-1);
			liwork=int32(-1);
			lwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,lrwork, iwork, liwork, info]=lapack_cheevd(jobz, uplo, n, a, lda, w, work, lwork, rwork,lrwork, iwork, liwork, info);
			lwork=int32(work(1));
			liwork=int32(iwork(1));
			lrwork=int32(rwork(1));

			work=single(complex(zeros(double(lwork),1)));
			iwork=int32(zeros(double(liwork),1));
			rwork=single(zeros(double(lrwork),1));
			[jobz, uplo, n, vec, lda, val, work, lwork, rwork,lrwork, iwork, liwork, info]=lapack_cheevd(jobz, uplo, n, a, lda, w, work, lwork, rwork,lrwork, iwork, liwork, info);

		end;
	else
		if (isDouble(a)),
			w=zeros(double(n),1);
			work=zeros(1);
			iwork=int32(zeros(1));
			lwork=int32(-1);
			liwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, iwork,liwork, info]= lapack_dsyevd(jobz, uplo, n, a, lda, w, work, lwork, iwork,liwork, info);
			lwork=int32(work(1));
			liwork=int32(iwork(1));
			work=zeros(double(lwork),1);
			iwork=int32(zeros(double(liwork),1));
			[jobz, uplo, n, vec, lda, val, work, lwork, iwork,liwork, info]= lapack_dsyevd(jobz, uplo, n, a, lda, w, work, lwork, iwork,liwork, info);
		else
			w=single(zeros(double(n),1));
			work=single(zeros(1));
			iwork=int32(zeros(1));
			lwork=int32(-1);
			liwork=int32(-1);
			info=int32(-1);
			[jobz, uplo, n, vec, lda, val, work, lwork, iwork,liwork, info]= lapack_ssyevd(jobz, uplo, n, a, lda, w, work, lwork, iwork,liwork, info);
			lwork=int32(work(1));
			liwork=int32(iwork(1));
			work=single(zeros(double(lwork),1));
			iwork=int32(zeros(double(liwork),1));
			[jobz, uplo, n, vec, lda, val, work, lwork, iwork,liwork, info]= lapack_ssyevd(jobz, uplo, n, a, lda, w, work, lwork, iwork,liwork, info);
		end;
	end;


function check_FourierGrad

S = mtexdata('dubna');
pf = S.pf;
odf = calcODF(pf,'zeroRange');
fodf = FourierODF(odf);

ori = orientation.rand(1000,pf.CS);
g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)./norm(g1)) < 1e-2
  disp(' Fourier gradient test passed');
else
  disp(' Fourier gradient test failed');
end

return

%% test 1 - check f_theta
cs = crystalSymmetry('1');
odf = fibreODF(Miller(0,0,1,cs),vector3d.Z,'halfwidth',40*degree);
fodf = FourierODF(odf);

omega = linspace(-179,179) * degree;
ori = rotation.byAxisAngle(vector3d(0,1,0),omega);

g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-3, disp(' Test 1 passed'); end

%% test 2

ori = rotation.byAxisAngle(vector3d(1,2,0)s,omega);

g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-3, disp(' Test 2 passed'); end

%% test 3 - check last line in grad composition

ori = rotation.byAxisAngle(vector3d(1,2,3),omega);

g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-2
  disp(' Test 3 passed');
else
  disp(' Test 3 failed');
end

%% test 4 - final f_theta test

ori = orientation.rand(100,cs);
g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-2
  disp(' Test 4 passed');
else
  disp(' Test 4 failed');
end

%% test 5 - f_phi1 test

odf = fibreODF(Miller(0,1,0,cs),vector3d.Y,'halfwidth',40*degree);
fodf = FourierODF(odf);

ori = orientation.byEuler(omega,89.9999*degree,0*degree,cs,'ABG');

%odf.grad(rot)
g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-2
  disp(' f_phi1 test passed');
else
  disp(' f_phi1 test failed');
end

%% test 6 - f_phi1 test

odf = fibreODF(Miller(0,1,0,cs),vector3d.Y,'halfwidth',40*degree);
fodf = FourierODF(odf);

ori = orientation.byEuler(0,89.9999*degree,omega,cs,'ABG');

%odf.grad(rot)
g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-2
  disp(' f_phi2 test passed');
else
  disp(' f_phi2 test failed');
end

%% test 5 - f_phi1 + f_phi_2 test

odf = fibreODF(Miller(0,1,0,cs),vector3d.Y,'halfwidth',40*degree);
fodf = FourierODF(odf);

ori = orientation.byAxisAngle(vector3d(0,1,100),omega);

%odf.grad(rot)
g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-2
  disp(' Test 6 passed');
else
  disp(' Test 6 failed');
end

%% test 6 - f_phi1 - f_phi_2 test

odf = fibreODF(Miller(0,1,0,cs),vector3d.Y,'halfwidth',40*degree);
fodf = FourierODF(odf);

ori = orientation.byAxisAngle(vector3d(0,1,100),omega);

%odf.grad(rot)
g1 = fodf.grad(ori(:),'check','delta',0.1*degree);
g2 = fodf.grad(ori(:));

if max(norm(g1-g2)) < 1e-2
  disp(' Test 6 passed');
else
  disp(' Test 6 failed');
end

%%
function testing

cs = crystalSymmetry('1');
%ref = orientation.id(cs);
ref = orientation.rand(cs);
odf = unimodalODF(ref,'halfwidth',40*degree);

fodf = FourierODF(odf)

omega = linspace(0,40)*degree;
%omega = 5*degree;
rot = ref*rotation(vector3d(1,1,1),omega)

g2 = fodf.grad(rot,'check','delta',0.05*degree)
g3 = odf.grad(rot)
g1 = fodf.grad(rot)

fodf.eval(rot)
odf.eval(rot)

%plot(norm(odf.grad(rot))

end

%%
function test2

cs = crystalSymmetry('1');
odf = fibreODF(Miller(0,1,0,cs),vector3d.Y,'halfwidth',40*degree);
fodf = FourierODF(odf)

omega = 50*degree;
%omega = linspace(0,179)*degree;
%ori = orientation('axis',vector3d(0,1,100),'angle',omega,cs);
%ori = orientation.byEuler(0*degree,0.01*degree,omega,cs,'ABG');

% f_phi1 test with cos(Phi) == 0 --> ok
ori = orientation.byEuler(omega,89.9999*degree,30*degree,cs,'ABG')

%%
% f_phi2 test with cos(Phi) == 1

omega = 80*degree
%omega = linspace(0,89)*degree;

odf = fibreODF(Miller(0,1,0,cs),vector3d.Z,'halfwidth',40*degree);
fodf = FourierODF(odf)

%ori = orientation.byEuler(0*degree,omega,[ 90]*degree,cs,'ABG');

%ori = orientation.rand(100,cs);

ori = orientation.byEuler(0*degree,90*degree,45*degree,cs,'ABG');
%ori = orientation.byEuler(10*degree,80*degree,[ 90]*degree,cs,'ABG');

g2 = fodf.grad(ori(:))
g1 = odf.grad(ori(:))

d_Phi(odf,ori)
d_phi1(odf,ori)
d_phi2(odf,ori)

%%

fhat1 = zeros(3);% fhat1([4 6]) = [1 -1]*1i;
fhat1 = zeros(3); %fhat1(9) = 1;
fhat2 = zeros(5); %fhat2([6,8,10]) = 1;
fhat2 = zeros(5); fhat2(4) = 1;
fhat3 = zeros(7); %fhat3([22 24 26 28]) = [1 1 -1 -1]*1i;
odf2 = FourierODF([1;fhat1(:);fhat2(:);fhat3(:)],cs);

[g1,d1,d2,d3] = odf2.components{1}.grad(ori3);
g1
g2 = odf2.grad(ori3,'check')

[d_phi1(odf2,ori3),d_Phi(odf2,ori3),d_phi2(odf2,ori3);...
  d1,d2,d3]

%%

%plot([norm(g1-g2)])
plot([g1.z , g2.z]) % x anfangs ok dann zu schnell negativ
plot([g1.y,g2.y])   % y - sollte konstant 0 sein
plot([g2.z,g1.z])  % z - faktor 4 zu groß
fodf.eval(ori)
odf.eval(ori)

%plot(norm(odf.grad(rot))

end

function test1

cs = crystalSymmetry('1');
odf = fibreODF(Miller(0,0,1,cs),vector3d.Z,'halfwidth',40*degree);
fodf = FourierODF(odf)

omega = 55*degree
%omega = linspace(-179,179) * degree;
ori = rotation.byAxisAngle(vector3d(-1,1,3),omega)
%ori = orientation.rand(cs);

%odf.grad(rot)
g1 = fodf.grad(ori(:),'check','delta',0.1*degree)
g2 = fodf.grad(ori(:))

plot([g1.y,g2.y])

end

end


function [mmps2] = cmps22mmps2(cmps2)
% Convert acceleration from centimeters per square centimeter to
% millimeters per second-squared
% Chad A. Greene 2012
mmps2 = cmps2*1e+1;
end