function [p q m1 n1 tol] = spqr_singletons (A,tol)                          %#ok
%SPQR_SINGLETONS finds the singleton permutation of a sparse matrix A.
% [p q m1 n1 tol] = spqr_singletons (A, tol) finds permutation vectors p and q
% so that C=A(p,q) is a block 2-by-2 matrix in the form [C11 C12 ; 0 C22] where
% C11 is upper triangular (or "squeezed" upper trapezoidal), of dimension
% m1-by-n1.  The columns of C11 are the column singletons of A.  If C11 is
% square then all of its diagonal entries are larger in magnitude than tol.
%
% The input tol is optional; it defaults to 20*(m+n)*eps*maxcol2norm where
% [m n] = size(A) and maxcol2norm is the maximum 2-norm of the columns of A.
% The output tol is the tolerance used.
%
% If C11 is rectangular, then some of the column singletons have no
% corresponding row singleton.  A column j in C11 has a corresponding row i if
% if i = max(find(C(:,j))) > max(find(C(:,j-1))).  If present, abs(C(i,j)) will
% be larger than tol.
%
% Example:
%
%   A = [1 8 0 0
%        2 9 3 7
%        0 4 0 0
%        0 6 0 0 ]
%   [p q m1 n1 tol] = spqr_singletons (sparse (A))
%   C = A(p,q)
%
% In this example, C11 is 2-by-3.  One of the 3 column singletons (the 2nd on
% in C) has no corresponding row singleton.  This is an auxiliary routine that
% illustrates the singleton removal step used by SuiteSparseQR.  It is not need
% to solve a least-squares problem or find a sparse QR factorization; use
% SPQR_SOLVE or SPQR for those tasks, respectively.
%
% See also DMPERM, SPQR, SPQR_SOLVE.

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

error ('spqr_singletons mexFunction not found') ;


function lik = lik_inputdependentnoise(varargin)
%lik_inputdependentnoise    Create input-dependent noise likelihood structure
%
%  Description
%    LIK = LIK_INPUTDEPENDENTNOISE('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a Gaussian likelihood with input dependent noise structure
%    in which the named parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_INPUTDEPENDENTNOISE(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.
%
%    The likelihood is defined as follows:
%                    __ n
%      p(y|f1, f2) = || i=1 N(y_i | f1_i, sigma2*exp(f2_i))
%
%      where f1 is the first latent variable defining the mean of the
%      gaussian distribution, f2 is the second latent variable defining
%      the noise structure and sigma2 is coefficient for noise.
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%
% Copyright (c) 2007-2010 Jarno Vanhatalo & Jouni Hartikainen
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2011 Ville Tolvanen

% 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_INPUTDEPENDENTNOISE';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.nondiagW=true;
    lik.type = 'Inputdependentnoise';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Inputdependentnoise')
      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
  % 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_inputdependentnoise_pak;
    lik.fh.unpak = @lik_inputdependentnoise_unpak;
    lik.fh.lp = @lik_inputdependentnoise_lp;
    lik.fh.lpg = @lik_inputdependentnoise_lpg;
    lik.fh.ll = @lik_inputdependentnoise_ll;
    lik.fh.llg = @lik_inputdependentnoise_llg;
    lik.fh.llg2 = @lik_inputdependentnoise_llg2;
    lik.fh.llg3 = @lik_inputdependentnoise_llg3;
    lik.fh.predy = @lik_inputdependentnoise_predy;
    lik.fh.invlink = @lik_inputdependentnoise_invlink;
    lik.fh.predprcty = @lik_inputdependentnoise_predprcty;
    lik.fh.recappend = @lik_inputdependentnoise_recappend;
  end

  function [w,s,h] = lik_inputdependentnoise_pak(lik)
  %LIK_INPUTDEPENDENTNOISE_PAK  Combine likelihood parameters into one vector.
  %
  %  Description
  %    W = LIK_INPUTDEPENDENTNOISE_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)
  %
  %   See also
  %   LIK_INPUTDEPENDENTNOISE_UNPAK, GP_PAK

  w = []; s = {}; h=[];
  if isfield(lik.p, 'sigma2') && ~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_inputdependentnoise_unpak(lik, w)
  %LIK_INPUTDEPENDENTNOISE_UNPAK  Extract likelihood parameters from the vector.
  %
  %  Description
  %    [LIK, W] = LIK_INPUTDEPENDENTNOISE_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)
  %
  %   See also
  %   LIK_INPUTDEPENDENTNOISE_PAK, GP_UNPAK

  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 lp = lik_inputdependentnoise_lp(lik, varargin)
  %LIK_INPUTDEPENDENTNOISE_LP  log(prior) of the likelihood parameters
  %
  %  Description
  %    LP = LIK_INPUTDEPENDENTNOISE_LP(LIK) takes a likelihood structure
  %    LIK and returns log(p(th)), where th collects the parameters. This
  %    subfunction is needed when there are likelihood parameters.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_E

  % If prior for sigma2 parameter, add its contribution
  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_inputdependentnoise_lpg(lik)
  %LIK_INPUTDEPENDENTNOISE_LPG  d log(prior)/dth of the likelihood
  %                parameters th
  %
  %  Description
  %    E = LIK_INPUTDEPENDENTNOISE_LPG(LIK) takes a likelihood structure
  %    LIK and returns d log(p(th))/dth, where th collects the parameters.
  %    This subfunction is needed when there are likelihood parameters.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_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 ll = lik_inputdependentnoise_ll(lik, y, ff, z)
  %LIK_INPUTDEPENDENTNOISE_LL  Log likelihood
  %
  %  Description
  %    LL = LIK_INPUTDEPENDENTNOISE_LL(LIK, Y, F, Z) takes a likelihood
  %    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_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_E

    f=ff(:);
    n=size(y,1);
    f1=f(1:n);
    f2=f((n+1):2*n);
    expf2=exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;
    sigma2 = lik.sigma2;

    ll = sum(-0.5*log(2*pi.*sigma2.*expf2) - 1./(2.*sigma2.*expf2).*(y-f1).^2);

  end

  function llg = lik_inputdependentnoise_llg(lik, y, ff, param, z)
  %LIK_INPUTDEPENDENTNOISE_LLG  Gradient of the log likelihood
  %
  %  Description
  %    LLG = LIK_INPUTDEPENDENTNOISE_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_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_LLG2, LIK_INPUTDEPENDENTNOISE_LLG3, GPLA_E

    f=ff(:);
    n=size(y,1);
    f1=f(1:n);
    f2=f((n+1):2*n);
    expf2 = exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;
    sigma2 = lik.sigma2;

    switch param
      case 'param'

        llg=sum(-0.5./sigma2+(y-f1).^2./(2*expf2.*sigma2^2));
        % correction for the log transformation
        llg = llg.*lik.sigma2;

      case 'latent'

        llg1= (y-f1)./(sigma2*expf2);
        llg2= -0.5+(y-f1).^2./(2.*(expf2.*sigma2));

%         llg1=(-y+f1)/(sqrt(2*pi).*(sigma2.*expf2).^(3/2)).*exp(-1/(2.*sigma2*expf2).*(y-f1).^2);
%         llg2=-exp(f2-(y-f1).^2/(2.*expf2.*sigma2)).*sigma2/(2.*sqrt(2.*pi).*(expf2.*sigma2).^(3/2))+exp(-f2-(y-f1).^2/(2*expf2*sigma2)).*(y-f1).^2/(2.*sqrt(2.*pi.*expf2).*sigma2.^(3/2));

        llg=[llg1; llg2];
    end
  end

  function [llg2] = lik_inputdependentnoise_llg2(lik, y, ff, param, z)
  %function [pi_vec, pi_mat] = lik_inputdependentnoise_llg2(lik, y, ff, param, z)
  %LIK_INPUTDEPENDENTNOISE_LLG2  Second gradients of the log likelihood
  %
  %  Description
  %    LLG2 = LIK_INPUTDEPENDENTNOISE_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'. LLG2 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_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, GPLA_E

    f=ff(:);

    n=length(y);
    f1=f(1:n);
    f2=f((n+1):2*n);
    sigma2 = lik.sigma2;
    expf2=exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;

    switch param
      case 'param'

      case 'latent'

%         llg2 = [2./(sigma2.*expf2); 3/2.*(y-f1).^2./(sigma2.*expf2)];
%         llg2mat = [diag(1./sqrt(sigma2.*expf2)); diag(-(y-f1)./sqrt(sigma2.*expf2))];

        llg2_11=-1./(sigma2.*expf2);
        llg2_12=-(y-f1)./(sigma2*expf2);
        llg2_22=-(y-f1).^2./(2.*sigma2.*expf2);

        llg2 = [llg2_11 llg2_12; llg2_12 llg2_22];

      case 'latent+param'

        llg2_1=-(y-f1)./(expf2.*sigma2^2);
        llg2_2=-(y-f1).^2./(2*sigma2.^2.*expf2);

        llg2=[llg2_1; llg2_2];
        % correction for the log transformation
        llg2 = llg2.*lik.sigma2;

    end
  end

  function llg3 = lik_inputdependentnoise_llg3(lik, y, ff, param, z)
  %LIK_INPUTDEPENDENTNOISE_LLG3  Third gradients of the log likelihood
  %
  %  Description
  %    LLG3 = LIK_INPUTDEPENDENTNOISE_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'. LLG3 is a vector with third gradients. This
  %    subfunction is needed when using Laplace approximation for
  %    inference with non-Gaussian likelihoods.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_E, GPLA_G

    f=ff(:);

    n=size(y,1);
    f1=f(1:n);
    f2=f((n+1):2*n);
    expf2=exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;
    sigma2 = lik.sigma2;

    switch param
      case 'param'

      case 'latent'
        nl=2;
        llg3=zeros(nl,nl,nl,n);

        % y=0:
        % thrid derivative derivative wrt f1 (11)
        %       llg3(1,1,1,:) = 0
        % thrid derivative derivative wrt f2 (11)
        llg3(1,1,2,:) = 1./(expf2.*sigma2);

        % thrid derivative derivative wrt f1 (12/21)
        llg3(1,2,1,:) = 1./(expf2.*sigma2);
        llg3(2,1,1,:) = llg3(1,2,1,:);
        % thrid derivative derivative wrt f2 (12/21)
        llg3(1,2,2,:) = (y-f1)./(expf2.*sigma2);
        llg3(2,1,2,:) = llg3(1,2,2,:);

        % thrid derivative derivative wrt f1 (22)
        llg3(2,2,1,:) = llg3(1,2,2,:);
        % thrid derivative derivative wrt f1 (22)
        llg3(2,2,2,:) = (y-f1).^2./(2.*expf2.*sigma2);

      case 'latent2+param'

        llg3_11=1./(expf2.*sigma2.^2);
        llg3_12=(y-f1)./(expf2.*sigma2.^2);
        llg3_22=(y-f1).^2./(2.*expf2.*sigma2.^2);

        llg3 = [diag(llg3_11) diag(llg3_12); diag(llg3_12) diag(llg3_22)];
        % correction for the log transformation
        llg3 = llg3.*lik.sigma2;

    end
  end

  function [lpy, Ey, Vary] = lik_inputdependentnoise_predy(lik, Ef, Varf, yt, z)
  %LIK_INPUTDEPENDENTNOISE_PREDY  Returns the predictive mean, variance and density of y
  %
  %  Description
  %    [LPY] = LIK_INPUTDEPENDENTNOISE_PREDY(LIK, EF, VARF, YT) takes a
  %    likelihood structure LIK, posterior mean EF, posterior
  %    variance VARF of the latent variable and observations YT and
  %    returns the logarithm of the predictive density PY of YT, that is
  %        p(yt | th) = \int p(yt | f, th) p(f|y) df.
  %    This subfunction is needed when computing posterior predictive
  %    distributions for future observations.
  %
  %    [LPY, EY, VARY] = LIK_INPUTDEPENDENTNOISE_PREDY(LIK, EF, VARF YT)
  %    Returns also 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

%     ntest=size(yt,1);
    Ef = Ef(:);
    ntest = 0.5*size(Ef,1);
    Ef1=Ef(1:ntest); Ef2=Ef(ntest+1:end);
    if size(Varf,2) == size(Varf,1)
      Varf1=diag(Varf(1:ntest,1:ntest));Varf2=diag(Varf(ntest+1:end,ntest+1:end));
    else
      if size(Varf,2)==1
        Varf=reshape(Varf,ntest,2);
      end
      Varf1=Varf(:,1); Varf2=Varf(:,2);
    end
    sigma2=lik.sigma2;

    lpy = zeros(size(yt));

    Ey=Ef1;
    Vary=Varf1 + sigma2.*exp(Ef2+Varf2/2);

    if ~isempty(yt)
      for i2=1:ntest
        m1=Ef1(i2); m2=Ef2(i2);
        s1=sqrt(Varf1(i2)); s2=sqrt(Varf2(i2));
        pd=@(f1,f2) norm_pdf(yt(i2), f1, sqrt(sigma2.*exp(f2))).*norm_pdf(f1,Ef1(i2),sqrt(Varf1(i2))).*norm_pdf(f2,Ef2(i2),sqrt(Varf2(i2)));
        lpy(i2) = log(dblquad(pd, m1-6.*s1, m1+6.*s1, m2-6.*s2, m2+6.*s2));
      end
    else
      lpy=[];
    end
%     sigma2 = lik.sigma2;
%     Ef1=Ef(1:ntest);
%     Ef2=Ef((ntest+1):2*ntest);
%     Ey = Ef1;
%     Vary = Varf + sigma2.*exp(Ef2);

  end

  function prctys = lik_inputdependentnoise_predprcty(lik, Ef, Varf, zt, prcty)
  %LIK_BINOMIAL_PREDPRCTY  Returns the percentiles of predictive density of y
  %
  %  Description
  %    PRCTY = LIK_BINOMIAL_PREDPRCTY(LIK, EF, VARF YT, ZT)
  %    Returns percentiles of the predictive density PY of YT, that is
  %    This requires also the succes counts YT, numbers of trials ZT. This
  %    subfunction is needed when using function gp_predprcty.
  %
  %  See also
  %    GP_PREDPCTY

  n=size(Ef,1)./2;
  prcty = prcty./100;
  prcty = norminv(prcty, 0, 1);
  prctys = bsxfun(@plus, Ef(1:n), bsxfun(@times, sqrt(Varf(1:n) + lik.sigma2.*exp(Ef(n+1:end))), prcty));

  end

  function p = lik_inputdependentnoise_invlink(lik, f, z)
  %LIK_INPUTDEPENDENTNOISE_INVLINK  Returns values of inverse link function
  %
  %  Description
  %    P = LIK_INPUTDEPENDENTNOISE_INVLINK(LIK, F) takes a likelihood structure LIK and
  %    latent values F and returns the values of inverse link function P.
  %    This subfunction is needed when using function gp_predprcty.
  %
  %     See also
  %     LIK_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_PREDY

    p = exp(f);
  end

  function reclik = lik_inputdependentnoise_recappend(reclik, ri, lik)
  %RECAPPEND  Append the parameters to the record
  %
  %  Description
  %    RECLIK = GPCF_INPUTDEPENDENTNOISE_RECAPPEND(RECLIK, RI, LIK) takes a
  %    likelihood record structure RECLIK, record index RI and
  %    likelihood 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

  % Initialize record
    if nargin == 2
      reclik.type = 'Inputdependentnoise';
      reclik.nondiagW=true;

      % Initialize parameter

      % Set the function handles
      reclik.fh.pak = @lik_inputdependentnoise_pak;
      reclik.fh.unpak = @lik_inputdependentnoise_unpak;
      reclik.fh.lp = @lik_inputdependentnoise_lp;
      reclik.fh.lpg = @lik_inputdependentnoise_lpg;
      reclik.fh.ll = @lik_inputdependentnoise_ll;
      reclik.fh.llg = @lik_inputdependentnoise_llg;
      reclik.fh.llg2 = @lik_inputdependentnoise_llg2;
      reclik.fh.llg3 = @lik_inputdependentnoise_llg3;
      reclik.fh.predy = @lik_inputdependentnoise_predy;
      reclik.fh.predprcty = @lik_inputdependentnoise_predprcty;
      reclik.fh.invlink = @lik_inputdependentnoise_invlink;
      reclik.fh.recappend = @lik_inputdependentnoise_recappend;
      reclik.p=[];
      if ~isempty(ri.p.sigma2)
        reclik.p.sigma2 = ri.p.sigma2;
      end
      return
    end

    reclik.sigma2(ri,:)=lik.sigma2;
    if ~isempty(lik.p.sigma2)
      reclik.p.sigma2 = feval(lik.p.sigma2.fh.recappend, reclik.p.sigma2, ri, lik.p.sigma2);
    end
  end
end


function [area]=findano2() % autogenerated function wrapper
    % This script file is supposed to find an anomaly
    % and estimate the extent in time and space
    %
    % Stefan Wiemer  11/94
    % turned into function by Celso G Reyes 2017

    ZG=ZmapGlobal.Data; % used by get_zmap_globals

    report_this_filefun(mfilename('fullpath'));

    is3 = [];
    ni = 80;

    [i,j] = find(re3 > 5.5);
    X = reshape(loc(1,:),length(gy),length(gx));
    Y = reshape(loc(2,:),length(gy),length(gx));
    figure(map);
    hold on
    for k = 1:length(i)
        xa0 = X(i(k),j(k));
        ya0 = Y(i(k),j(k));
        plot(xa0,ya0,'xk')
        l=ZG.primeCatalog.epicentralDistanceTo(ya0,xa0);
        [s,is] = sort(l);
        is3 = [is3 ; is(1:ni)];
    end   % for k

    is3 = sort(is3);

    l = [];
    for k = 1:length(is3)-1
        if is3(k) ~= is3(k+1)
            l = [l ; is3(k)];
        end
    end

    l = sort(l);
    ZG.newt2= (a(l,:));
    figure(map);
    hold on
    plot(ZG.newt2.Longitude,ZG.newt2.Latitude,'bo');

    % estimate length of anomaly
    %
    i1 = find(ZG.newt2.Longitude == min(ZG.newt2.Longitude));i1 = max(i1);
    i2 = find(ZG.newt2.Longitude == max(ZG.newt2.Longitude));i2 = max(i2);
    di  = sqrt(((ZG.newt2.Longitude(i1)-ZG.newt2.Longitude(i2))*cosd(ya0)*111).^2 +   ((ZG.newt2.Latitude(i1)-ZG.newt2.Latitude(i2))*111).^2) ;
    li = [ZG.newt2.Longitude(i1) ZG.newt2.Latitude(i1) ; ZG.newt2.Longitude(i2) ZG.newt2.Latitude(i2)];
    plot(li(:,1),li(:,2))

    i1 = find(ZG.newt2.Latitude == min(ZG.newt2.Latitude));i1 = max(i1);
    i2 = find(ZG.newt2.Latitude == max(ZG.newt2.Latitude));i2 = max(i2)
    di2  = sqrt(((ZG.newt2.Longitude(i1)-ZG.newt2.Longitude(i2))*cosd(ya0)*111).^2 +   ((ZG.newt2.Latitude(i1)-ZG.newt2.Latitude(i2))*111).^2) ;

    li = [ZG.newt2(i1,1) ZG.newt2(i1,2) ; ZG.newt2(i2,1) ZG.newt2(i2,2)];
    plot(li(:,1),li(:,2))

    di = max([di; di2])
    area = (di/2)^2 *pi;
end


function outputImage = power(this, otherImage)
% Computes input image to the power of other image per pixel as new image
%
% NOTE: If voxel dimensions of 2nd image do not match - or a scalar is
% given as 2nd argument - data in the 2nd argument is automatically
% replicated to match this image geometry.
%
%
%   Y = MrImage()
%   outputImage = power(Y, otherImage, ...
%   functionHandle)
%
%   OR
%
%   outputImage = Y.^otherImage
%
% This is a method of class MrImage.
%
%
% IN
%   otherImage              image that will be subtracted from this one
%
% OUT
%   outputImage             new MrImage, difference of this and otherImage
%
% EXAMPLE
%
%   % Compute difference of 2 images
%		Y = MrImage();
%		Z = MrImage();
%		X = Y.power(Z);
%
%   % OR (cool overload!):
%       X = Y.^Z
%
%   e.g. take square of image
%       X = Y.^2
%
%   e.g. take square root of image
%       X = Y.^(1/2)
%
%   See also MrImage perform_binary_operation MrImage.plus

% Author:   Saskia Bollmann & Lars Kasper
% Created:  2014-11-13
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.
%

outputImage = this.perform_binary_operation(otherImage, @power);


function out = MF_FitSubsegments(y,model,order,subsetHow,samplep,randomSeed)
% MF_FitSubsegments Robustness of model parameters across different segments of a time series
%
% The spread of parameters obtained (including in-sample goodness of fit
% statistics) provide some indication of stationarity.
%
% Values of goodness of fit provide some indication of model suitability.
%
% This code inherits strongly from MF_CompareTestSets
%
%---INPUTS:
% y, the input time series.
%
% model, the model to fit in each segments of the time series:
%           'arsbc': fits an AR model using the ARfit package. Outputs
%                       statistics are on how the optimal order, p_{opt}, and
%                       the goodness of fit varies in different parts of the
%                       time series.
%           'ar': fits an AR model of a specified order using the code
%                   ar from Matlab's System Identification Toolbox. Outputs are
%                   on how Akaike's Final Prediction Error (FPE), and the fitted
%                   AR parameters vary across the different segments of time
%                   series.
%           'ss': fits a state space model of a given order using the code
%                   n4sid from Matlab's System Identification Toolbox. Outputs
%                   are on how the FPE varies.
%           'arma': fits an ARMA model using armax code from Matlab's System
%                   Identification Toolbox. Outputs are statistics on the FPE,
%                   and fitted AR and MA parameters.
%
% subsetHow, how to choose segments from the time series, either 'uniform'
%               (uniformly) or 'rand' (at random).
%
% samplep, a two-vector specifying how many segments to take and of what length.
%           Of the form [nsamples, length], where length can be a proportion of
%           the time-series length. e.g., [20,0.1] takes 20 segments of 10% the
%           time-series length.
%
% randomSeed, whether (and how) to reset the random seed, using BF_ResetSeed
%               (for when subsetHow is 'rand')
%
%---OUTPUTS: depend on the model, as described above.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Preliminaries
% ------------------------------------------------------------------------------
N = length(y); % length of time series

% ------------------------------------------------------------------------------
%% Check Inputs
% ------------------------------------------------------------------------------

% (1) y: column vector time series
if nargin < 1 || isempty(y)
    error('Give us a time series, ya mug');
end
% Convert y to time series object
y = iddata(y,[],1);

% (2) model, the type of model to fit
if nargin < 2 || isempty(model)
    model = 'ss'; % fit a state space model by default
end

% (3) order of model, order
if nargin < 3 || isempty(order)
    order = 2; % model of order 2 by default. Not very good defaults.
end

% (4) How to choose subsets from the time series, subsetHow
if nargin < 4 || isempty(subsetHow)
    subsetHow = 'rand'; % takes segments randomly from time series
end

% (5) Sampling parameters, samplep
if nargin < 5 || isempty(samplep)
    samplep = [20, 0.1]; % sample 20 times with 10%-length subsegments
end

% (6) randomSeed: how to treat the randomization
if nargin < 6
    randomSeed = [];
end

% ------------------------------------------------------------------------------
%% Set the ranges beforehand
% ------------------------------------------------------------------------------
% Number of samples to take, numPred
numPred = samplep(1);
r = zeros(numPred,2); % ranges

switch subsetHow
    case 'rand'
        if samplep(2) < 1 % specified a fraction of time series
            l = floor(N*samplep(2));
        else % specified an absolute interval
            l = samplep(2);
        end

        % Control the random seed (for reproducibility):
        BF_ResetSeed(randomSeed);

        % numPred random starting points:
        spts = randi(N-l+1,numPred,1);
        r(:,1) = spts;
        r(:,2) = spts+l-1;

    case 'uniform'
        if length(samplep) == 1 % size will depend on number of unique subsegments
            spts = round(linspace(0,N,numPred+1)); % numPred+1 boundaries = numPred portions
            r(:,1) = spts(1:numPred)+1;
            r(:,2) = spts(2:end);
        else
            if samplep(2) < 1 % specified a fraction of time series
                l = floor(N*samplep(2));
            else % specified an absolute interval
                l = samplep(2);
            end
            spts = round(linspace(1,N-l+1,numPred)); % numPred+1 boundaries = numPred portions
            r(:,1) = spts;
            r(:,2) = spts+l-1;
        end

    otherwise
        error('Unknown subset method ''%s''',subsetHow);
end

% ------------------------------------------------------------------------------
%% Fit the model to each training set
% ------------------------------------------------------------------------------
% model will be stored as a model object, m
% model is fitted using the entire dataset as the training set
% test sets will be smaller chunks of this.
% [could also fit multiple models using data not in multiple test sets, but
% this is messier]
switch model
    case 'arsbc'
        %% Fit AR models of 'best' order according to SBC, using arfit package
        % fit AR models of 'best' order, return statistics on how this best
        % order changes. The order input argument is not used for this
        % option.
        orders = zeros(numPred,1);
        sbcs = zeros(numPred,1);
        yy = y.y;
        for i = 1:numPred
            % Use arfit software to retrieve the optimum AR(p) order by
            % Schwartz's Bayesian Criterion, SBC (or BIC); in the range
            % p = 1-10
            % Enforce zero mean level. This could be relaxed.
            try
                [west, Aest, Cest, SBC] = ARFIT_arfit(yy(r(i,1):r(i,2)), 1, 10, 'sbc', 'zero');
            catch emsg
                if strcmp(emsg.message,'Time series too short.')
                   fprintf(1,'Time Series is too short for ARFIT\n');
                   out = NaN; return
                else
                    error('Problem fitting AR model');
                end
            end
            orders(i) = length(Aest);
            sbcs(i) = min(SBC);
        end

        % Return statistics
        out.orders_mode = mode(orders);
        out.orders_mean = mean(orders);
        out.orders_std = std(orders);
        out.orders_max = max(orders);
        out.orders_min = min(orders);
        out.orders_range = range(orders);

        out.sbcs_mean = mean(sbcs);
        out.sbcs_std = std(sbcs);
        out.sbcs_range = range(sbcs);
        out.sbcs_min = min(sbcs);
        out.sbcs_max = max(sbcs);

    case 'ar'
        %% Fit AR model of specified order
        % Return statistics on parameters and goodness of fit

        %% Check that a System Identification Toolbox license is available to run the 'ar' function:
        BF_CheckToolbox('identification_toolbox')

        fpes = zeros(numPred,1);
        as = zeros(numPred,order+1);
        for i = 1:numPred
            % fit the ar model
            m = ar(y(r(i,1):r(i,2)),order);
            % get parameters and goodness of fit
            fpes(i) = m.EstimationInfo.FPE;
            as(i,:) = m.a;
        end

        % statistics on FPE
        out.fpe_std = std(fpes);
        out.fpe_mean = mean(fpes);
        out.fpe_max = max(fpes);
        out.fpe_min = min(fpes);
        out.fpe_range = range(fpes);

        % Statistics on fitted AR parameters
        for i = 1:order % first column will be ones
            % Dynamic field referencing:
            out.(['a_',num2str(i),'_std']) = std(as(:,i+1));
            out.(['a_',num2str(i),'_mean']) = mean(as(:,i+1));
            out.(['a_',num2str(i),'_max']) = max(as(:,i+1));
            out.(['a_',num2str(i),'_min']) = min(as(:,i+1));
            % eval(sprintf('out.a_%u_std = std(as(:,%u+1));',i,i));
            % eval(sprintf('out.a_%u_mean = mean(as(:,%u+1));',i,i));
            % eval(sprintf('out.a_%u_max = max(as(:,%u+1));',i,i));
            % eval(sprintf('out.a_%u_min = min(as(:,%u+1));',i,i));
        end

    case 'ss'
        %% Fit state space models of specified order
        % Return statistics on goodness of fit
        % Could do parameters too, but I this would involve many outputs
        fpes = zeros(numPred,1);
        for i = 1:numPred
            try  m = n4sid(y(r(i,1):r(i,2)),order);
            catch
                % Some range of the time series is invalid for fitting the
                % model to.
                error('Couldn''t fit this state space model')
            end
            fpes(i) = m.EstimationInfo.FPE;
        end

        % statistics on FPE
        out.fpe_std = std(fpes);
        out.fpe_mean = mean(fpes);
        out.fpe_max = max(fpes);
        out.fpe_min = min(fpes);
        out.fpe_range = range(fpes);

    case 'arma'
        %% fit an ARMA model of specified order(s)
        % Note: order should be a two-component vector
        % Output parameters and goodness of fit
        fpes = zeros(numPred,1);
        ps = zeros(numPred,order(1)+1);
        qs = zeros(numPred,order(2)+1);

        for i = 1:numPred
            try
                m = armax(y(r(i,1):r(i,2)),order);
            catch emsg
                error('Couldn''t fit this ARMA model')
            end
            fpes(i) = m.EstimationInfo.FPE;
            ps(i,:) = m.a;
            qs(i,:) = m.c;
        end

        % statistics on FPE
        out.fpe_std = std(fpes);
        out.fpe_mean = mean(fpes);
        out.fpe_max = max(fpes);
        out.fpe_min = min(fpes);
        out.fpe_range = range(fpes);

        % Statistics on fitted AR parameters, p
        for i = 1:order % first column will be ones
            out.(['p_',num2str(i),'_std']) = std(ps(:,i+1));
            out.(['p_',num2str(i),'_mean']) = mean(ps(:,i+1));
            out.(['p_',num2str(i),'_max']) = max(ps(:,i+1));
            out.(['p_',num2str(i),'_min']) = min(ps(:,i+1));
        end

        % Statistics on fitted MA parameters, q
        for i = 1:order % first column will be ones
            out.(['q_',num2str(i),'_std']) = std(qs(:,i+1));
            out.(['q_',num2str(i),'_mean']) = mean(qs(:,i+1));
            out.(['q_',num2str(i),'_max']) = max(qs(:,i+1));
            out.(['q_',num2str(i),'_min']) = min(qs(:,i+1));
        end
    otherwise
        error('Unknown model ''%s''',model);
end

end


% clc;
clear;
warning off
addpath('support_methods/');
addpath(genpath('tools'));
vl_setupnn;

%% general parameters
% 4 supported datasets: ChallengeDB_release LIVE TID2013 CSIQ
testDatabase = 'ChallengeDB_release';
% 3 supported CNN architectures: ResNet AlexNet S_CNN
ModelType = 'S_CNN';
repetitions = 10;
trainingPropertion = 0.8;
%% parameters for PQR
quantizationMethod = 'uniform'; % LloydMax
beta = 64;
bins = 5;  % set bins=1 for scalar quality score regression

switch ModelType
    case 'AlexNet'
        patchSize = 227;
        patchStep = 16;
        epoch = 20;
    case 'ResNet'
        patchSize = 224;
        patchStep = 16;
        epoch = 10;
    case 'S_CNN'
        patchSize = 64;
        patchStep = 8;
        epoch = 40;
end

for patchNum = [50]
    if strcmp(ModelType,'S_CNN')
        patchNum = patchNum * 10;
    end

%% training
for seed = 1:repetitions
    TrainModel('Model',ModelType,'Database',testDatabase,'PatchNum', patchNum,...
               'seed',seed, 'trainingpropertion', trainingPropertion,...
               'quantizationMethod',quantizationMethod,...
               'bins',bins, 'beta', beta,'patchSize',patchSize,...
               'patchStep',patchStep,'epoch',epoch);
end

%% testing
for seed = 1:repetitions
    fprintf('seed = %d....\n', seed);
    for i = 1:epoch
        netStruct = load(fullfile('data',[ModelType '_' testDatabase '_TrainPropertion' num2str(trainingPropertion)...
                    '_PatchSize' num2str(patchSize) '_PatchNum' num2str(patchNum) '_Seed' num2str(seed) '_bins' num2str(bins)...
                    '_beta' num2str(beta)],['net-epoch-' num2str(i) '.mat']));
        net = dagnn.DagNN.loadobj(netStruct.net) ;
        move(net, 'gpu')
        net.mode = 'test';
        [SRCC(seed,i),PLCC(seed,i)] = testModel(ModelType, net, testDatabase,seed,trainingPropertion);
    end
end

%% save results
median_SRCC =[]; median_PLCC=[]; std_SRCC=[]; std_PLCC=[];
median_SRCC = median(SRCC);
median_PLCC = median(PLCC);
std_SRCC = std(SRCC);
std_PLCC = std(PLCC);
[max_SRCC,idx] = max(median_SRCC);
max_PLCC = median_PLCC(idx);
file = fopen(fullfile('result','results.txt'),'a');
fprintf(file,'Parameters: %s; %s; bins = %d; patches = %d;\n',testDatabase,ModelType,bins,patchNum);
fprintf(file,'SRCC:     ');fprintf(file,'%.4f; ',median_SRCC);fprintf(file,'\n');
fprintf(file,'std_SRCC: ');fprintf(file,'%.4f; ',std_SRCC);fprintf(file,'\n');
fprintf(file,'PLCC:     ');fprintf(file,'%.4f; ',median_PLCC);fprintf(file,'\n');
fprintf(file,'std_PLCC: ');fprintf(file,'%.4f; ',std_PLCC);fprintf(file,'\n');
fprintf(file,'best_SRCC: %.4f; best_PLCC: %.4f \n',max_SRCC,max_PLCC);
fclose(file);

end


function plotnsgtf(c,shift,sr,fmin,fmax,bins,cutout,dynrange)
%PLOTNSGTF  Plot nonstationary Gabor filterbank coefficients
%   Usage:  plotnsgtf(c,shift,sr,fmin,fmax,bins,cutout,dynrange)
%           plotnsgtf(c,shift,sr,fmin,fmax,bins,cutout)
%           plotnsgtf(c,shift,sr,fmin,fmax,bins)
%           plotnsgtf(c,shift,sr,cutout,dynrange)
%           plotnsgtf(c,shift,sr,cutout)
%           plotnsgtf(c,shift,sr)
%           plotnsgtf(c,shift)
%           plotnsgtf(c)
%
%   Input parameters:
%         c        : Array of coefficients.
%         shift    : Vector of frequency shifts
%         sr       : signal sample rate in Hz (default 1 Hz)
%         fmin     : Minimum frequency used in the transform
%         fmax     : Maximum frequency used in the transform
%         bins     : Bins per octave (in constant or vector form)
%         cutout   : Desired part of the spectrogram, e.g.
%                    choice of 2 shows frequencies up to Nyquist
%                    (X shows the number_of_bins/X lowest frequency
%                    bins)
%         dynrange : Colorscale dynamic range in dB (default 60 dB)
%
%   Given a coefficient array c and the frequency shift vector shift,
%   this function plots the dB-scaled nonstationary Gabor filterbank
%   spectrogram corresponding to c. The vector shift and sampling rate
%   sr are used to determine the correct time axis labels. The frequency
%   axis is by default labeled by bin number.
%
%   For constant-Q nonstationary Gabor filterbanks, labeling with the
%   actual center frequencies is supported, requiring the filterbank
%   parameters fmin, fmax and bins as additional input.
%
%   The shown frequency range can be controlled with the cutout parameter
%   (default: 2) and the dynamic range of the spectrogram can be adjusted
%   with dynrange.
%
%   See also:  nsgtf, plotnsgt, plotslicq
%
%   Url: http://nsg.sourceforge.net/doc/plotting/plotnsgtf.php

% Copyright (C) 2013 Nicki Holighaus.
% This file is part of NSGToolbox version 0.1.0
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 3.0 Unported
% License. To view a copy of this license, visit
% http://creativecommons.org/licenses/by-nc-sa/3.0/
% or send a letter to
% Creative Commons, 444 Castro Street, Suite 900,
% Mountain View, California, 94041, USA.

% Author:  Gino Velasco, Nicki Holighaus and Radu C. Frunza
% Original code by Florent Jaillet
% Date: 26.04.13

ticklabels = 0;

if nargin < 8
    % Default value for colorscale dynamic.
    dynrange = 60;
    if nargin < 7 % Default value for frequency cutout.
        cutout = 2;
        if nargin < 6
            ticklabels = 0;
            if nargin == 5
                cutout = fmin;
                dynrange = fmax;
            elseif nargin < 4
                cutout = 2;
                if nargin < 3
                    % Default sampling rate
                    sr = 1;
                    if nargin < 2
                        error('Not enough input arguments');
                    end
                end
            else
                cutout = fmin;
            end
        end
    end
end

N = length(shift)-2;
L=sum(shift);

% For the coefficient output of nsgtf_real, adjust the frequeny
% channels shown, if necessary

if N > size(c,1) && cutout < 2
    cutout = cutout/2;
end

cla %Clear previous axes

% Compute maximum of the representation for colorscale dynamic handling.
if iscell(c) == 1
    if size(c{1},2) > 1
        error(['Multichannel spectrograms are not supported. Please ',...
            'use ''cellfun(@(x) x(:,k),c,''UniformOutput'',0)'' to ',...
            'select the k-th channel.']);
    end
    temp=cell2mat(c);
else
    if size(c,3) > 1
        error(['Multichannel spectrograms are not supported. Please ',...
            'use ''c(:,:,k)'' to select the k-th channel.']);
    end
    temp=c;
end
ma=20*log10(max(abs(temp(:))));

% Plot the representation: as the sampling grid in the time frequency plane
% is irregular, the representation by done by plotting many images next to
% each other, with one image for each window
if iscell(c) == 1
    hold('on');
    for ii=1:floor((N-1)/cutout)+1
        temp = 20*log10(abs(c{ii})+eps);
        % +eps is here to avoid log of 0
        % Octave cannot plot images that are only one point wide, so we use
        % images that are to points wide
        imagesc([0,L-1]/sr,[ii,ii+1],[temp,temp].',...
            [ma-dynrange,ma]);
    end
    hold('off');
    axis('tight');
else
    imagesc([0, (L-1)/sr],[1 size(c,2)],...
        20*log10(abs(c)'+eps),[ma-dynrange, ma]);
    select=[0,(L-1)/sr,1,round(size(c,2)/cutout)+1];
    axis(select);
    set(gca,'YDir','normal');
end

% Compute YTickLabels for the correct frequencies
if ticklabels == 1

    vfq = [0, fmin*2.^(0:log2(fmax/fmin))];

    if length(bins) < length(vfq) - 1
        xbins = [bins, bins(end)*ones(1,length(vfq)-length(bins)-1)];
    else
        xbins = bins(1:length(vfq)-1);
    end;

    sbins = [0,2,1+cumsum(xbins)];

    yTick = [2;2+2*cumsum(xbins(1:end-1))';(1+N/2)];
    yTick = unique([yTick(yTick <= floor((length(shift)-1)/cutout)+1);...
        floor((N-1)/cutout)+1]);

    yTickLabel = zeros(length(yTick),1);

    for kk = 1:length(yTick)
        ind = find(yTick(kk) < sbins,1);
        yTickLabel(kk) = vfq(ind-1)*...
            2^(((yTick(kk)-1)-sbins(ind-1))/(sbins(ind)-sbins(ind-1)));
    end;

    yTickLabel = num2str(round(yTickLabel),5);
    set(gca,'YTick',yTick,'YTickLabel',yTickLabel);
end


function y = isnan(X)
    % True for Not-a-Number for symbolic arrays.
    %   ISNAN(X) returns an array that contains 1's where
    %   the elements of X are symbolic NaN's and 0's where they are not.
    %   For example, ISNAN(sym([pi NaN Inf -Inf])) is [0 1 0 0].
    %
    %   For any X, exactly one of ISFINITE(X), ISINF(X), or ISNAN(X)
    %   is 1 for each element.
    %
    %   See also SYM/ISFINITE, SYM/ISINF.

    % Convert inputs to SymExpression
    % X = SymExpression(X);

    % evaluate the operation in Mathematica and return the
    % expression string
    ret = eval_math(['Boole/@Map[SameQ, ' X.s ', NaN]'], 'math2matlab');

    y = logical(ret);
end


function img = gen_img_mesh(size, res)
    img = zeros(size);
    qs = 1 : res : size(1);
    for i = 1 : numel(qs)
%         qs(i)
        img(ceil(qs(i)), :) = 1;
    end
    qs = 1 : res : size(2);
    for i = 1 : numel(qs)
        img(:, ceil(qs(i))) = 1;
    end
    img(end, :) = 1;
    img(:, end) = 1;
end


function [distMat_cl,cluster_Groupi,ord,handles] = BF_ClusterDown(distMat,varargin)
% BF_ClusterDown    Reduce a pairwise distance matrix into smaller clusters.
%
% Yields a visualization of a pairwise distance matrix, including a set of
% clusters of objects showing highly correlated behavior.
%
%---INPUTS:
% distMat, a pairwise distance vector or matrix (e.g., generated from pdist)
% [EXTRA OPTIONS]:
% 'clusterThreshold', the threshold in the dendrogram for forming clusters
% 'objectLabels', labels for axes
% 'whatDistance', the type of distance metric used:
%        (i) 'corr' (default): provide 1-R, where R is correlation coefficient
%        (ii) 'abscorr': provide correlation distances, 1-R, but clustering is
%                        done on abscorr distances (sign of R is ignored), and
%                        correlations, R, are plotted
%        (iii) 'abscorr_ii': provide correlation distances, 1-R, but clustering
%                        is done on abscorr distances (sign of R is ignored), and
%                        absolute correlations, |R|, are plotted
%        (iv) 'general': provide any distance matrix, plots the distance matrix
% 'linkageMeth', the linkage method to use (for Matlab's linkage function)
%
%---OUTPUTS:
% distMat_cl, the distance matrix re-ordered by clustering
% cluster_Groupi, the assignment of nodes to clusters (cell of indices)
% ord, the ordering of objects in the dendrogram
% handles, structure containing handles to plotting elements, f (figure),
%               ax1 (dendrogram), ax2 (pairwise correlation matrix)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International License. To view a copy of
% this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or send
% a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View,
% California, 94041, USA.
% ------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
% Check Inputs
%-------------------------------------------------------------------------------
% Custom plotting options using an input parser
inputP = inputParser;

% Cluster threshold:
default_clusterThreshold = 0.2;
addParameter(inputP,'clusterThreshold',default_clusterThreshold,@isnumeric);

% Object labels for axes
default_objectLabels = {};
addParameter(inputP,'objectLabels',default_objectLabels,@iscell);

% Default input is a distance matrix based on (Spearman) correlation coefficients
default_whatDistance = 'corr';
addParameter(inputP,'whatDistance',default_whatDistance,@ischar);

% errth Set error threshold instead of just top number
% default_errTh = 30;
% addParameter(inputP,'errTh',default_errTh,@isnumeric);

% miOrCorr
% default_miOrCorr = 'corr'; % 'mi','corr'
% addParameter(inputP,'miOrCorr',default_miOrCorr,@ischar);

% clusterMeth, what method to use to do the clustering
% default_clusterMeth = 'linkage'; % 'linkage', 'kmedoids'
% addParameter(inputP,'clusterMeth',default_clusterMeth,@ischar);

% linkageMeth, what method to use to do the linkage clustering
default_linkageMeth = 'average';
addParameter(inputP,'linkageMeth',default_linkageMeth,@ischar);

% plotBig, whether to make a big plot
% default_plotBig = 0;
% check_plotBig = @(x) x==1 || x==0;
% addParameter(inputP,'plotBig',default_plotBig,check_plotBig);

%% Parse inputs:
parse(inputP,varargin{:});

% Make variables from results of input parser:
clusterThreshold = inputP.Results.clusterThreshold;
objectLabels = inputP.Results.objectLabels;
whatDistance = inputP.Results.whatDistance;
% errTh = inputP.Results.errTh;
% miOrCorr = inputP.Results.miOrCorr;
% clusterMeth = inputP.Results.clusterMeth;
linkageMeth = inputP.Results.linkageMeth;
% plotBig = inputP.Results.plotBig;
clear('inputP');

%-------------------------------------------------------------------------------
%% Process input distance data
%-------------------------------------------------------------------------------
[distVec,links,distVec0,numItems] = BF_DoLinkage(distMat,whatDistance,linkageMeth);

% ------------------------------------------------------------------------------
%% Compute dendrogram
% ------------------------------------------------------------------------------
f = figure('color','w');
f.Position(3:4) = [1200,800];

% Get the dendrogram reordering:
ax1 = subplot(1,6,6);
ord = BF_LinkageOrdering(distVec,links);
h_dend = dendrogram(links,0,'Orientation','right','Reorder',ord);
ax1.YDir = 'reverse'; % needs to be reversed to match the reversed y-axis of the imagesc plot

% Save the clustered distance matrix for output
distMat_cl = distMat(ord,ord);

% Make the dendrogram look aight
set(h_dend,'color','k','LineWidth',1)
ax1.YTickLabel = {};
xlabel('Distance');

%-------------------------------------------------------------------------------
%% Cluster into groups:
%-------------------------------------------------------------------------------
% Cluster the dendrogram:
T = cluster(links,'cutoff',clusterThreshold,'criterion','distance');
numClusters = max(T);

fprintf(1,'%s distance-based clustering with %u clusters.\n',whatDistance,numClusters);

% Reorder members of each cluster by distance to other members of the cluster:
cluster_Groupi = cell(numClusters,1);
for j = 1:numClusters
    cluster_Groupi{j} = find(T==j);
    if sum(T==j) > 1
        % Sort by increasing sum of distances to other members of the cluster
        [~,ix] = sort(sum(distMat(cluster_Groupi{j},cluster_Groupi{j})),'ascend');
        cluster_Groupi{j} = cluster_Groupi{j}(ix);
    end
end

% Reorder by decreasing cluster size
[~,ix] = sort(cellfun(@length,cluster_Groupi),'descend');
cluster_Groupi = cluster_Groupi(ix);

% Select the closest to cluster centre in each group
clusterCenters = cellfun(@(x) x(1),cluster_Groupi);

cluster_Groupi_cl = cellfun(@(x) arrayfun(@(y)find(ord==y),x),cluster_Groupi,...
                                                'UniformOutput',false);
clusterCenters_cl = arrayfun(@(y) find(ord==y),clusterCenters);

%-------------------------------------------------------------------------------
%% Plot it
%-------------------------------------------------------------------------------
% Make a square distance matrix, and plot as a pairwise similarity matrix
% (reordered by ord determined above)
ax2 = subplot(1,6,2:5);
switch whatDistance
case {'corr','abscorr_ii','corr_fast'}
    % Input is a correlation distance matrix, plot correlation coefficients:
    distMat0 = squareform(distVec0);
    BF_PlotCorrMat(1 - distMat0(ord,ord),'n1to1');
    rectangleColors = BF_GetColorMap('accent',5,true);
case 'abscorr'
    distMat0 = squareform(distVec0);
    % Input is correlation distance matrix, plot absolute correlation coefficients:
    BF_PlotCorrMat(abs(1 - distMat0(ord,ord)),'0to1');
    % (red colormap)
    rectangleColors = {[0,0,0],ones(1,3)*0.5};
case 'general'
    % Input is a general distance matrix (grayscale colormap):
    BF_PlotCorrMat(distMat_cl);
    rectangleColors = BF_GetColorMap('accent',5,true);
otherwise
    warning('No special plotting options for distance metric: ''%s''',whatDistance);
    BF_PlotCorrMat(distMat_cl);
    rectangleColors = BF_GetColorMap('accent',5,true);
end

%-------------------------------------------------------------------------------
%% Add rectangles to group highly correlated clusters:
%-------------------------------------------------------------------------------
for i = 1:numClusters
    % Label cluster borders:
    rectangle('Position',[min(cluster_Groupi_cl{i})-0.5,min(cluster_Groupi_cl{i})-0.5, ...
                            length(cluster_Groupi_cl{i}),length(cluster_Groupi_cl{i})], ...
                    'EdgeColor',rectangleColors{1},'LineWidth',3);

    % Label cluster centers:
    rectangle('Position',[clusterCenters_cl(i)-0.5,clusterCenters_cl(i)-0.5,1,1], ...
                                'EdgeColor',rectangleColors{2},'LineWidth',3);
end

%-------------------------------------------------------------------------------
%% Tweak plot details:
%-------------------------------------------------------------------------------

% Add object labels on y-axis if provided
if ~isempty(objectLabels)
    ax2.YTick = 1:numItems;
    ax2.YTickLabel = objectLabels(ord);
    ax2.TickLabelInterpreter = 'none';
else
    ax2.YTick = [];
end

% Remove X-ticks
ax2.XTick = [];

% Add a color bar:
cB = colorbar('southoutside');
switch whatDistance
case {'corr','abscorr_ii'}
    cB.Label.String = 'Spearman correlation, \rho';
case 'abscorr'
    cB.Label.String = 'Magnitude of Spearman correlation, |\rho|';
case 'general'
    cB.Label.String = 'distance';
end

% Link axes:
linkaxes([ax1,ax2],'y');

% Fix limits
ax2.YLim = [0.5,numItems+0.5];

% Align with dendrogram
ax1.Position(1) = ax2.Position(1) + ax2.Position(3);
ax1.Position(4) = ax2.Position(4);
ax1.Position(2) = ax2.Position(2);

% Handles
if nargout > 3
    handles = struct();
    handles.f = f;
    handles.ax1 = ax1;
    handles.ax2 = ax2;
end

end


function [inF] = prepare_dcm(A,B,C,D,kernelGroup)
% prepares matrices and indices vectors for DCM (without HRF)
% function [options] = prepare_fullDCM(A,B,C,D,TR,microDT,homogeneous)
% IN:
%   - A: binary matrix indicating where the connections are
%   - B: cell-array of binary matrices of modulatory effects
%   - C: binary matrix of input-state coupling
%   - D: cell-array of binay matrices for gating effects
% OUT:
%   - inF: the optional input structure to @f_dcm4fmri

%check for empty matrices

if nargin<5
    kernelGroup=1;
end

nu = max([numel(B) size(C,2)]);
nx1 = max([size(A,1) cellfun(@(cel) size(cel,1),B) size(C,1) cellfun(@(cel) size(cel,1),D) ]);
nx2 = max([size(A,2) cellfun(@(cel) size(cel,2),B) size(C,1) cellfun(@(cel) size(cel,2),D) ]);

if isempty(A)
    A = zeros(nx1,nx2);
end
for i=1:nu
    if isempty(B) || numel(B)<i || isempty(B{i})
        B{i} = zeros(nx1,nx2);
    end
end
if isempty(C)
    C = zeros(nx1,nu);
end
for i=1:nx2
    if isempty(D) || numel(D)<i || isempty(D{i})
        D{i} = zeros(nx1,nx2);
    end
end

%prepare indices
[indA,indB,indC,indD,indself] = find_dcm(A,B,C,D,kernelGroup);

%save
inF.indself = indself;
inF.A = A;
inF.indA = indA;
inF.B = B;
inF.indB = indB;
inF.C = C;
inF.indC = indC;
inF.D = D;
inF.indD = indD;

[inF.dA,inF.dB,inF.dC,inF.dD] = get_dMatdvec(inF);

function [dA,dB,dC,dD] = get_dMatdvec(inF)
if ~isempty(inF.indA)
    dA = dMatdvec(inF.A);
else
    dA = [];
end
dB = cell(length(inF.B),1);
for i=1:length(inF.B)
    if ~isempty(inF.indB{i})
        dB{i} = dMatdvec(inF.B{i});
    else
        dB{i} = [];
    end
end
if ~isempty(inF.indC)
    dC = dMatdvec(inF.C);
else
    dC = [];
end
dD = cell(length(inF.D),1);
for i=1:length(inF.D)
    if ~isempty(inF.indD{i})
        dD{i} = dMatdvec(inF.D{i});
    else
        dD{i} = [];
    end
end

function C = dMatdvec(A)
A = ~~A;
ind = find(A~=0);
n = numel(A);
ni = length(ind);
C = zeros(n,ni);
for i=1:length(ind)
    C(ind(i),i) = 1;
end

function [indA,indB,indC,indD,indself] = find_dcm(A,B,C,D,kernelGroup)
ia = find(A~=0);
if ~isempty(ia)
    indA = 1:length(ia);
else
    indA = [];
end
ib = [];
indB = cell(length(B),1);
for i=1:length(B)
    tmp = find(B{i}~=0);
    if ~isempty(tmp)
        indB{i} = length(ia)+length(ib)+1:...
            length(ia)+length(ib)+length(tmp);
    else
        indB{i} = [];
    end
    ib = [ib;tmp];
end
ic = find(C~=0);
if ~isempty(ic)
    indC = length(ia)+length(ib)+1:length(ia)+length(ib)+length(ic);
else
    indC = [];
end
id = [];
indD = cell(length(D),1);
for i=1:length(D)
    tmp = find(D{i}~=0);
    if ~isempty(tmp)
        indD{i} = length(ia)+length(ib)+length(ic)+length(id)+1:...
            length(ia)+length(ib)+length(ic)+length(id)+length(tmp);
    else
        indD{i} = [];
    end
    id = [id;tmp];
end
indself = (length(ia)+length(ib)+length(ic)+length(id))+kernelGroup;


function full_forward()
    global config mem;
    curr_layer_idx = config.misc.current_layer;
    mem.activations{curr_layer_idx} = bsxfun(@plus, config.weights{curr_layer_idx} * mem.layer_inputs{curr_layer_idx}, config.weights{config.layer_num+curr_layer_idx});
    config.misc.current_layer = curr_layer_idx + 1;
end


function analyse_dev_lin_fusion(key,scores_obj_array,scr_lin,prior)
% Analyses the dev scores for all input systems and their fusion
% so that their relative strengths can be compared.  It calculates
% the percentage of target trials calculated by each system and the
% actual dcf, min dcf and prbep values for each system.
% Inputs:
%   key: The Key for the dev trials.
%   scores_obj_array: An array of score objects (one object for
%     each system) that was fused.
%   scr_lin: An object of type Scores containing the result of the
%     linear fusion.
%   prior: The effective target prior.

assert(nargin==4)
assert(isa(key,'Key'))
assert(key.validate())

stacked_scores = stackScores(key,scores_obj_array);
trialmask = key.to_ndx().trialmask;
dev_lin_fusion = scr_lin.scoremat(trialmask(:))';
dev_scores = [stacked_scores;dev_lin_fusion];

true_prop = sum(key.tar(:))/(sum(key.tar(:))+sum(key.non(:)));
logprint(Logger.Info,'True Dev target proportion is %g%%. System estimates are:\n',100*true_prop);
logprint(Logger.Info,'%s',sprintfmatrix(100*estimate_target_proportions(dev_scores,true_prop)'));

numsystems = size(stacked_scores,1);
mf = size(dev_scores,1);
logprint(Logger.Info,'\n\nAnalysing Dev: %i subsystems and %i fusions:\n',numsystems,mf-numsystems);
E = evalAll(dev_scores,key,prior);
logprint(Logger.Info,'\nDev all: dcf*100, mindcf*100, prbep:\n%s',sprintfmatrix(E,1:mf));
end


filename='BridgeCool_Quadrilateral_Bilinear_Structured';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = 1;
constraint = {'volume'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'STANDARD';

nsteps = 15;
Vfrac_final = 0.2;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  battistn@tcnj.edu
% Current Institution: TCNJ
% Date Modified: April 2021
%
% Date Created: May 27th, 2015
% Institution when Created: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs or non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (battistn@tcnj.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the LEAF_LEAF-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Leaf_Leaf()

%-----------------------------------------------------
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%-----------------------------------------------------
Nx = 384         % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny = 64          % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 30.0        % Length of Eulerian Grid in x-Direction (cm)
Ly = 5.0         % Length of Eulerian Grid in y-Direction (cm)
ds= 0.5*Lx/Nx;   % Lagrangian spacing

%----------------------------------------------------------------
% AIR PROPERTIES FOR INPUT
%
%     Density:     [rho] = kg/m^3
%     Newton:      [N] = kg * m/s^2
%     Dyn. Visc.:  [N*s/m^2] = (kg*m/s^2)*(s/m^2) = kg / (m*s)
%
%----------------------------------------------------------------
% PROPERTIES IN MKS UNITS
%mu_Air = 18.03e-6; % N*s/m^2 dynamic viscosity of air at 18C (64.4F)
%rho_Air = 1.21;    % kg/m^3  density of air at 18C
mu_Air = 1.895e-5; % N*s/m^2 =  dynamic viscosity of air at 18C (64.4F)
rho_Air = 1.145;    % kg/m^3  density of air at 35C
%(from https://www.engineersedge.com/physics/viscosity_of_air_dynamic_and_kinematic_14483.htm)
%
% CONVERT TO CGS UNITS
mu_Air = mu_Air * (1000/1) * (1/100)     %(1000g/1kg) & (1m/100cm)
rho_Air = rho_Air * (1000/1) * (1/100)^3 %(1000g/1kg) & (1m/100cm)
%
% FINAL UNITS
% [mu_Air] = g/(cm*s)
% [rho_Air] = g/cm^3

%----------------------------------------------------
% LEAF GEOMETRY
%----------------------------------------------------
xLag = [];                     % Initialize Storage
yLag = [];                     % Initialize Storage
L_leaf = 1.0;                  % Length of tail off cylinder
struct_name = 'leaf'; % Name for .vertex, .spring, etc files
%
x0 = 3.0;                      % Starting x-Pt for LEAF
y0 = Ly/2;                     % Starting y_Pt for LEAF
%
xLag_L = x0:ds:x0+L_leaf;
yLag_L = y0*ones(1,length(xLag_L));

%----------------------------------------------------
% IF CHANNEL WALLS / CYLINDRICAL LEADING EDGE %
%----------------------------------------------------
r = 0.01;                      % Radii of Cylinder

%--------------------------------------------
% Call function to construct geometry
%       * No Channel Walls
%       * No Cylinder on Leading Edge
%--------------------------------------------
[xLag_Cy,yLag_Cy] = give_Me_Cylinder_Immersed_Boundary_Geometry(ds,r,x0,y0);
%
% % Plot Geometry to test
% plot(xLag_Ch(1:end/2),yLag_Ch(1:end/2),'r-'); hold on;
% plot(xLag_Ch(end/2+1:end),yLag_Ch(end/2+1:end),'r-'); hold on;
% plot(xLag_Cy,yLag_Cy,'r-'); hold on;
% plot(xLag_L,yLag_L,'m-'); hold on;
% axis([0 Lx 0 Ly]);
% %
% plot(xLag_Ch,yLag_Ch,'*'); hold on;
% plot(xLag_Cy,yLag_Cy,'g*'); hold on;
% xlabel('x'); ylabel('y');
% axis square;
%
% xLag = [xLag xLag_Ch xLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)
% yLag = [yLag yLag_Ch yLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)
%
% Nbefore = length(xLag);   % # of Lagrangian Pts Before Leaf to COUNT for...
%                           % ... Cylinder / Channel Points have TARGET PTS
%

xLag = [xLag xLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)
yLag = [yLag yLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)

Nbefore = length(xLag); % # of Lag Pts Before Leaf Starts

xLag = [xLag xLag_L];   % Update xLag Pts to Include Leaf
yLag = [yLag yLag_L];   % Update xLag Pts to Include Leaf

Ntot = length(xLag);    % Total # of Lag Pts

%------------------------
% PLOT GEOMETRY
%------------------------
plot(xLag,yLag,'.'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
k_Spring = 2.5e4; %1e7
print_Lagrangian_Springs(xLag,yLag,k_Spring,struct_name,Nbefore,Ntot);

% Prints .beam file!
k_Beam = 2e8; C = 0.0; %2e12 -> 4e12
print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name,Nbefore,Ntot);

% Prints .target file!
k_Target = 2.5e4;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,Nbefore);

% Prints .mass file!
k_Mass = 1e0;         % 'spring' stiffness parameter for tethering
Mass = 0.5e-3;        % "MASS" value for 'ghost' lag movement
print_Lagrangian_Mass_Pts(xLag,k_Mass,Mass,struct_name,Nbefore,Ntot);

% Call function for initial concentration
[Concentration]= give_Me_Initial_Concentration(Nx,Ny);

% Prints .concentration file!
print_Concentration_Info(Nx,Ny,Concentration,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called <struct_name>.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called <struct_name>.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,Nbefore)

    %N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', Nbefore+3 );

    %Loops over any Channel/Cylinder Pts AND First 3 Pts Along LEAF
    for s = 1:Nbefore+3
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAMs (Torsional Springs) to a file called <struct_name>.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name,Nbefore,Ntot)

    % k_Beam: beam stiffness
    % C: beam curvature

    if Nbefore ~= 0
        N = Ntot-Nbefore-1;
    else
        N = Ntot-Nbefore-2; % NOTE: Total number of beams
    end

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %------------------------------------------------
    % BEAM BETWEEN CYLINDER AND LEAF
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    if Nbefore ~= 0
        s1 = Nbefore;
        s2 = Nbefore+1;
        s3 = Nbefore+2;
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s1, s2, s3, k_Beam, C);
    end

    %------------------------------------------------
    % BEAM BETWEEN VERTICES ALONG "LEAF"
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    for s = Nbefore+2:Ntot-1
        s1 = s-1;
        s2 = s;
        s3 = s+1;
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s1, s2, s3, k_Beam, C);
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called <struct_name>.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,struct_name,Nbefore,Ntot)

    if Nbefore ~= 0
        N = Ntot-Nbefore;
    else
        N = Ntot-Nbefore-1;
    end

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %------------------------------------------------
    % SPRINGS BETWEEN CYLINDER AND LEAF
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    if Nbefore ~= 0

        % springs between adjacent pts on cylinder
        for i=1:Nbefore
            if i<Nbefore
                x1 = xLag(i); y1 = yLag(i);
                x2 = xLag(i+1); y2 = yLag(i+1);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', i, i+1, k_Spring, ds);
            else
                x1 = xLag(i); y1 = yLag(i);
                x2 = xLag(1); y2 = yLag(1);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', i, 1, k_Spring, ds);

            end
        end

        % springs across opposite sides of cylinder
        for i=1:floor(Nbefore/2)
            x1 = xLag(i); y1 = yLag(i);
            x2 = xLag( i+floor(Nbefore/2) ); y2 = yLag( i+floor(Nbefore/2) );
            ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
            fprintf(spring_fid, '%d %d %1.16e %1.16e\n', i, i+floor(Nbefore/2), k_Spring, ds);
        end

        % spring between cylinder and leaf
        x1 = xLag(Nbefore); y1 = yLag(Nbefore);
        x2 = xLag(Nbefore+1); y2 = yLag(Nbefore+1);
        ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', Nbefore, Nbefore+1, k_Spring, ds);

    end

    %------------------------------------------------
    % SPRINGS BETWEEN VERTICES ALONG LEAF
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    for s = Nbefore+1:Ntot-1

        x1 = xLag(s); y1 = yLag(s);
        x2 = xLag(s+1); y2 = yLag(s+1);

        ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );

        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);

    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called <struct_name>.mass
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Mass_Pts(xLag,kMass,Mass,struct_name,Nbefore,Ntot)

    %LOOP OVER LAG PTS FOR MASS PTS IN LAGRANGIAN INDEXING
    N = (Ntot-Nbefore);

    mass_fid = fopen([struct_name '.mass'], 'w');

    fprintf(mass_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = Nbefore+1:Ntot%Nbefore:Ntot
        fprintf(mass_fid, '%d %1.16e %1.16e\n', s, kMass, Mass );
    end

    fclose(mass_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Channel_Immersed_Boundary_Geometry(ds,L,w,Lx,Ly)

% The immsersed structure is a channel %
x = (Lx-L)/2:ds:(L+(Lx-L)/2);  %xPts
yBot = (Ly-w)/2;               %yVal for bottom of Channel
yTop = Ly - (Ly-w)/2;          %yVal for top of Channel

xLag = [x x];
yLag = [yBot*ones(1,length(x)) yTop*ones(1,length(x))];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for cylinder
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLagN,yLagN] = give_Me_Cylinder_Immersed_Boundary_Geometry(ds,r,x0,y0)

% The immsersed structure is a cylinder %

dtheta = ds/ (4*r);
theta = 0; i=1;
while theta < 2*pi
   xLag(i) = (x0-r-ds) + r*cos(theta);
   yLag(i) = y0 + r*sin(theta);
   theta = theta + dtheta;
   i=i+1;
end

for i=1:length(xLag)
   xLagN(i) = xLag(end-(i-1));
   yLagN(i) = yLag(end-(i-1));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints initial concentration to a file called
% <struct_name>.concentration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Concentration_Info(Nx,Ny,C,struct_name)

    con_fid = fopen([struct_name '.concentration'], 'w');

    for i=1:Ny
        for j=1:Nx
            fprintf(con_fid, '%1.16e ', C(i,j) );
        end
        fprintf(con_fid,'\n');
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives initial concentration gradient inside channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [C] = give_Me_Initial_Concentration(Nx,Ny)

C = zeros(Ny,Nx);


function pathcosts(ch,~,~,controls,~)
F  = controls.F;
ch.add( 1e-3*F^2 );


% This class is derived from:
%
% An implementation of direct collocation
% Joel Andersson, 2016
% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
%
% CasADi -- A symbolic framework for dynamic optimization.
% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
%                         K.U. Leuven. All rights reserved.
% Copyright (C) 2011-2014 Greg Horn
% Under GNU Lesser General Public License

classdef Collocation < handle

  properties

    daefun
    pathcostfun

    coefficients
    coeff_eval
    coeff_der
    coeff_int

    vars
    num_x
    num_z
    num_u
    num_p
    num_t

    num_i

    tau_root
    order
  end

  methods

    function self = Collocation(states, algvars, controls, parameters, statesOrder, daefun, pathcostfun, d)

      nx = length(states);
      nz = length(algvars);
      nu = length(controls);
      np = length(parameters);
      nt = d;

      v = ocl.types.Structure();
      v.addRepeated({'states', 'algvars'}, {states, algvars}, d);

      tau = [0 ocl.collocation.collocationPoints(d)];

      coeff = ocl.collocation.coefficients(tau);

      self.daefun = daefun;
      self.pathcostfun = pathcostfun;

      self.coefficients = coeff;
      self.coeff_eval = ocl.collocation.evalCoefficients(coeff, d, 1.0);
      self.coeff_der = ocl.collocation.evalCoefficientsDerivative(coeff, tau, d);
      self.coeff_int = ocl.collocation.evalCoefficientsIntegral(coeff, d, 1.0);

      self.vars = v;
      self.num_x = nx;
      self.num_z = nz;
      self.num_u = nu;
      self.num_p = np;
      self.num_t = nt;

      self.num_i = length(v);

      self.tau_root = tau;
      self.order = d;

    end

  end
end


function CPT = CPD_to_CPT(CPD)
% Compute the big CPT for an HHMM Q node (including F parents)
% by combining internal transprob and startprob
% function CPT = CPD_to_CPT(CPD)

Qsz = CPD.Qsz;

if ~isempty(CPD.Fbelow_ndx)
  if ~isempty(CPD.Fself_ndx) % general case
    error('not implemented')
  else % no F from self, hence no startprob (top level)
    nps = length(CPD.dom_sz)-1; % num parents
    CPT = 0*myones(CPD.dom_sz);
    % when Fself=1, the CPT(i,j) = delta(i,j) for all k
    for k=1:prod(CPD.Qpsizes)
      Qps_vals = ind2subv(CPD.Qpsizes, k);
      ndx = mk_multi_index(nps+1, [CPD.Fbelow_ndx CPD.Qps_ndx], [1 Qps_vals]);
      CPT(ndx{:}) = eye(Qsz); % CPT(:,2,k,:) or CPT(:,k,2,:) etc
    end
    ndx = mk_multi_index(nps+1, CPD.Fbelow_ndx, 2);
    CPT(ndx{:}) = CPD.transprob; % we assume transprob is in topo order
  end
else % no F signal from below
  if ~isempty(CPD.Fself_ndx) % bottom level
    nps = length(CPD.dom_sz)-1; % num parents
    CPT = 0*myones(CPD.dom_sz);
    ndx = mk_multi_index(nps+1, CPD.Fself_ndx, 1);
    CPT(ndx{:}) = CPD.transprob;
    ndx = mk_multi_index(nps+1, CPD.Fself_ndx, 2);
    CPT(ndx{:}) = CPD.startprob;
  else % no F from self
    error('An hhmmQ node without any F parents is just a tabular_CPD')
  end
end


function data=createdatamatpb(data,E,Fs,win)
%
% Helper function to create an event triggered matrix from a single
% channel of data.
% Usage: data=createdatamatpb(data,E,Fs,win)
% Inputs:
% data   (input time series as a single vector) - required
% E      (events to use as triggers) - required
% Fs     (sampling frequency of data) - required
% win    (window around triggers to use data matrix -[winl winr]) - required
%          e.g [1 1] uses a window starting 1 sec before E and
%              ending 1 sec after E if E is in secs
% Note that E, Fs, and win must have consistent units
%
% Outputs:
% data      (event triggered data)
%
if nargin < 4
   error('Need all input arguments');
end
NE=length(E);
nwinl=round(win(1)*Fs);
nwinr=round(win(2)*Fs);
nE=floor(E*Fs)+1;
datatmp=[];
for n=1:NE;
    indx=nE(n)-nwinl:nE(n)+nwinr-1;
    datatmp=[datatmp data(indx)];
end
data=datatmp;