function world = thickCloister(x,X,y,Y,h,n)

% THICKCLOISTER  Generates features in a 3D cloister shape.
%   THICKCLOISTER(XMIN,XMAX,YMIN,YMAX,H,N) generates a 3D cloister in the
%   limits indicated as parameters, with height H and with N points per
%   side.
%
%   See also CLOISTER.

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

switch nargin
    case {1, 2, 3, 4}
        n = 9;
        h = 1;
    case 5
        n = 9;
end

plane = cloister(x,X,y,Y,n);
nlm = length(plane);
low = zeros(1,nlm);
high  = low + h;
world = [plane plane;low high];

% nid = 2*nlm;

% ids = 1:nid;

% id = 0;
% ids = zeros(1,nid);
% for i = 1:nid
%     id = id+floor(10*rand+1);
%     ids(i) = id;
% end

% world = [ids;world];

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


classdef denoising_mppca < AbstractModel
% denoising_mppca :  4d image denoising and noise map estimation
%                      by exploiting data redundancy in the PCA domain using universal
%                      properties of the eigenspectrum or random covariance matrices,
%                      i.e. Marchenko Pastur distribution
%
% Assumptions:
%   Noise follows a rician distribution
%   image bounderies are not processed
%
% Inputs:
%   Data4D              4D data (any modality)
%  (Mask)               Binary mask with region-of-interest. (OPTIONAL)
%
% Outputs:
%   Data4D_denoised     denoised 4D data
%   sigma_g               standard deviation of the rician noise
%
% Options:
%	sampling
%   	'full'          sliding window
%       'fast'          block processing (warning: undersampled noise map will be returned)
%   kernel              window size, typically in order of [5 x 5 x 5]
%
% Example of command line usage:
%   Model = denoising_mppca;  % Create class from model
%   data.Data4D = load_nii_data('Data4D.nii.gz');  % Load data
%   FitResults = FitData(data,Model,1);  % Fit each voxel within mask
%   FitResultsSave_nii(FitResults,'Data4D.nii.gz');  % Save in local folder: FitResults/
%
% Author: Tanguy Duval, 2016
%
% References:
%   Please cite the following if you use this module:
%     Veraart, J.; Fieremans, E. & Novikov, D.S. Diffusion MRI noise mapping using random matrix theory Magn. Res. Med., 2016, early view, doi:10.1002/mrm.26059
%   In addition to citing the package:
%     Karakuzu A., Boudreau M., Duval T.,Boshkovski T., Leppert I.R., Cabana J.F.,
%     Gagnon I., Beliveau P., Pike G.B., Cohen-Adad J., Stikov N. (2020), qMRLab:
%     Quantitative MRI analysis, under one umbrella doi: 10.21105/joss.02343

    properties (Hidden=true)
        onlineData_url = 'https://osf.io/j5239/download?version=2';
    end

    properties
        MRIinputs = {'Data4D','Mask'};
        xnames = {};
        voxelwise = 0;

        % Protocol
        Prot  = struct(); % You can define a default protocol here.

        % Model options
        buttons = {'sampling',{'fast','full'},'kernel',[5 5 5]};
        options= struct(); % structure filled by the buttons. Leave empty in the code

    end

    methods

        function obj = denoising_mppca
            obj.options = button2opts(obj.buttons);
            obj = UpdateFields(obj);
        end

        function obj = UpdateFields(obj)
        end

        function FitResults = fit(obj,data)
            dims=size(data.Data4D);
            kernel = min(dims(1:3),obj.options.kernel);
            [V,Ind] = min(dims(1:3));
            if ~isfield(data,'Mask'), data.Mask = []; end
            if V<7 && kernel(Ind)>1, helpdlg(['your dataset has very few slices. To avoid loosing too many slices, kernel is set to 1 in the dimension #' num2str(Ind) '.']); kernel(Ind)=1; end
            [FitResults.Data4D_denoised, FitResults.sigma_g] = MPdenoising(data.Data4D,data.Mask,kernel, obj.options.sampling);
        end

    end

    methods(Access = protected)
        function obj = qMRpatch(obj,loadedStruct, version)
            obj = qMRpatch@AbstractModel(obj,loadedStruct, version);
            if checkanteriorver(version,[2 0 11])
                obj.buttons{2} = obj.buttons{2}([2 1]); % old: '|G| (T/m)', new Gnorm (T/m)
            end
        end
    end

end


function [U,G] = unzip_corners(A)
  % UNZIP_CORNERS Given a triangle mesh where corners of each triangle index
  % different matrices of attributes (e.g. read from an OBJ file), unzip the
  % corners into unique efficiently: attributes become properly vertex valued
  % (usually creating greater than #V but less than #F*3 vertices).
  %
  % Inputs:
  %   A  #F by 3 by #A, typically A = cat(3,F,FTC,FN)
  % Outputs:
  %   U  #U by #A list of indices into each attribute for each unique mesh
  %     vertex: U(v,a) is the attribute index of vertex v in attribute a.
  %   G  #F by 3 list of triangle indices into U
  % Example:
  %   [V,F,TC,FTC] = readOBJ('~/Downloads/kiwis/kiwi.obj');
  %   [U,G] = unzip_corners(cat(3,F,FTC));
  %   % display mesh
  %   tsurf(G,V(U(:,1),:));
  %   % display texture coordinates
  %   tsurf(G,TC(U(:,2),:));
  %

  % C  #F*3 by 2 list of vertex indices and uv indices
  C = reshape(A,[],size(A,3));
  [U,~,J] = unique(C,'rows');
  ss = size(A,2);
  G = J(reshape(1:size(A,1)*ss,[],ss));
end


function lik = lik_negbinztr(varargin)
%LIK_NEGBINZTR  Create a zero-truncated Negative-binomial likelihood structure
%
%  Description
%    LIK = LIK_NEGBINZTR('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates zero-truncated Negative-binomial likelihood structure
%    in which the named parameters have the specified values. Any
%    unspecified parameters are set to default values.
%
%    Zero-truncated Negative-binomial can be used as a part of Hurdle model.
%
%    LIK = LIK_NEGBINZTR(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood structure with the named parameters
%    altered with the specified values.
%
%    Parameters for zero-truncated Negative-binomial likelihood [default]
%      disper       - dispersion parameter r [10]
%      disper_prior - prior for disper [prior_logunif]
%
%    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|f, z) = || i=1 [ (r/(r+mu_i))^r * gamma(r+y_i)
%                           / ( gamma(r)*gamma(y_i+1) )
%                           * (mu/(r+mu_i))^y_i / (1-(r/(r+mu_i))^r)]
%
%    where mu_i = z_i*exp(f_i) and r is the dispersion parameter. z
%    is a vector of expected mean and f the latent value vector
%    whose components are transformed to relative risk exp(f_i).
%    The last term (1-(r/(r+mu_i))^r) normalizes the truncated
%    distribution.
%
%    When using the zero-truncated Negbin likelihood you can give the
%    vector z as an extra parameter to each function that requires
%    also y. For example, you can call gp_optim as follows:
%      gp_optim(gp, x, y, 'z', z)
%    If z is not given or it is empty, then z_i=1 is used.
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%
% Copyright (c) 2007-2010 Jarno Vanhatalo & Jouni Hartikainen
% Copyright (c) 2010-2011 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_NEGBINZTR';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('disper',10, @(x) isscalar(x) && x>0);
  ip.addParamValue('disper_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

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

  % Initialize parameters
  if init || ~ismember('disper',ip.UsingDefaults)
    lik.disper = ip.Results.disper;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('disper_prior',ip.UsingDefaults)
    lik.p.disper=ip.Results.disper_prior;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_negbinztr_pak;
    lik.fh.unpak = @lik_negbinztr_unpak;
    lik.fh.lp = @lik_negbinztr_lp;
    lik.fh.lpg = @lik_negbinztr_lpg;
    lik.fh.ll = @lik_negbinztr_ll;
    lik.fh.llg = @lik_negbinztr_llg;
    lik.fh.llg2 = @lik_negbinztr_llg2;
    lik.fh.llg3 = @lik_negbinztr_llg3;
    lik.fh.tiltedMoments = @lik_negbinztr_tiltedMoments;
    lik.fh.tiltedMoments2 = @lik_negbinztr_tiltedMoments2;
    lik.fh.siteDeriv = @lik_negbinztr_siteDeriv;
    lik.fh.siteDeriv2 = @lik_negbinztr_siteDeriv2;
    lik.fh.upfact = @lik_negbinztr_upfact;
    lik.fh.predy = @lik_negbinztr_predy;
    lik.fh.predprcty = @lik_negbinztr_predprcty;
    lik.fh.invlink = @lik_negbinztr_invlink;
    lik.fh.recappend = @lik_negbinztr_recappend;
  end

end

function [w,s,h] = lik_negbinztr_pak(lik)
%LIK_NEGBINZTR_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_NEGBINZTR_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.disper)
%
%   See also
%   LIK_NEGBINZTR_UNPAK, GP_PAK

  w=[];s={};h=[];
  if ~isempty(lik.p.disper)
    w = log(lik.disper);
    s = [s; 'log(negbinztr.disper)'];
    h = [h 0];
    [wh,sh,hh] = lik.p.disper.fh.pak(lik.p.disper);
    w = [w wh];
    s = [s; sh];
    h = [h hh];
  end
end

function [lik, w] = lik_negbinztr_unpak(lik, w)
%LIK_NEGBINZTR_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    [LIK, W] = LIK_NEGBINZTR_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.disper)
%
%   See also
%   LIK_NEGBINZTR_PAK, GP_UNPAK

  if ~isempty(lik.p.disper)
    lik.disper = exp(w(1));
    w = w(2:end);
    [p, w] = lik.p.disper.fh.unpak(lik.p.disper, w);
    lik.p.disper = p;
  end
end

function lp = lik_negbinztr_lp(lik, varargin)
%LIK_NEGBINZTR_LP  log(prior) of the likelihood parameters
%
%  Description
%    LP = LIK_NEGBINZTR_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_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, LIK_NEGBINZTR_LLG2, GPLA_E

% If prior for dispersion parameter, add its contribution
  lp=0;
  if ~isempty(lik.p.disper)
    lp = lik.p.disper.fh.lp(lik.disper, lik.p.disper) +log(lik.disper);
  end

end

function lpg = lik_negbinztr_lpg(lik)
%LIK_NEGBINZTR_LPG  d log(prior)/dth of the likelihood
%                parameters th
%
%  Description
%    E = LIK_NEGBINZTR_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_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, LIK_NEGBINZTR_LLG2, GPLA_G

  lpg=[];
  if ~isempty(lik.p.disper)
    % Evaluate the gprior with respect to disper
    ggs = lik.p.disper.fh.lpg(lik.disper, lik.p.disper);
    lpg = ggs(1).*lik.disper + 1;
    if length(ggs) > 1
      lpg = [lpg ggs(2:end)];
    end
  end
end

function ll = lik_negbinztr_ll(lik, y, f, z)
%LIK_NEGBINZTR_LL  Log likelihood
%
%  Description
%    LL = LIK_NEGBINZTR_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_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, LIK_NEGBINZTR_LLG2, GPLA_E

  if numel(z)==0
    z=1;
  end

  r = lik.disper;
  mu = exp(f).*z;
  lp0=r.*(log(r) - log(r+mu));
  ll = sum(r.*(log(r) - log(r+mu)) + gammaln(r+y) - gammaln(r) - gammaln(y+1) + y.*(log(mu) - log(r+mu)) -log(1-exp(lp0)));
end

function llg = lik_negbinztr_llg(lik, y, f, param, z)
%LIK_NEGBINZTR_LLG  Gradient of the log likelihood
%
%  Description
%    LLG = LIK_NEGBINZTR_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_NEGBINZTR_LL, LIK_NEGBINZTR_LLG2, LIK_NEGBINZTR_LLG3, GPLA_E

  if numel(z)==0
    z=1;
  end

  mu = exp(f).*z;
  r = lik.disper;
  switch param
    case 'param'
      % Derivative using the psi function
      llg = sum(1 + log(r./(r+mu)) - (r+y)./(r+mu) + psi(r + y) - psi(r));
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg=llg-sum(1./(1 - exp(-lp0)).*(log(r./(mu + r)) - r./(mu + r) + 1));
      % correction for the log transformation
      llg = llg.*lik.disper;

% $$$             % Derivative using sum formulation
% $$$             llg = 0;
% $$$             for i1 = 1:length(y)
% $$$                 llg = llg + log(r/(r+mu(i1))) + 1 - (r+y(i1))/(r+mu(i1));
% $$$                 for i2 = 0:y(i1)-1
% $$$                     llg = llg + 1 / (i2 + r);
% $$$                 end
% $$$             end
% $$$             % correction for the log transformation
% $$$             llg = llg.*lik.disper;
    case 'latent'
      llg = y - (r+y).*mu./(r+mu);
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg = llg -(1./(1-exp(-lp0)).*-r./(mu + r).*mu);
  end
end

function llg2 = lik_negbinztr_llg2(lik, y, f, param, z)
%LIK_NEGBINZTR_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_NEGBINZTR_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_NEGBINZTR_LL, LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, GPLA_E

  if numel(z)==0
    z=1;
  end

  mu = exp(f).*z;
  r = lik.disper;
  switch param
    case 'param'

    case 'latent'
      llg2 = - mu.*(r.^2 + y.*r)./(r+mu).^2;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg2=llg2+...
           (r.^2 + r.^2.*exp(-lp0).*(mu-1))./((mu + r).^2.*(exp(-lp0)-1).^2).*mu;
    case 'latent+param'
      llg2 = (y.*mu - mu.^2)./(r+mu).^2;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg2=llg2+(exp(lp0)./(exp(lp0) - 1).^2 .* (log(r) - log(mu + r) - r.*(1./(mu + r) - 1./r)) .* (-r./(mu + r)) -1./(1 - exp(-lp0)).*-mu./(mu + r).^2).*mu;

      % correction due to the log transformation
      llg2 = llg2.*lik.disper;

  end
end

function llg3 = lik_negbinztr_llg3(lik, y, f, param, z)
%LIK_NEGBINZTR_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_NEGBINZTR_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_NEGBINZTR_LL, LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG2, GPLA_E, GPLA_G

  if numel(z)==0
    z=1;
  end

  mu = exp(f).*z;
  r = lik.disper;
  switch param
    case 'param'

    case 'latent'
      llg3 = - mu.*(r.^2 + y.*r)./(r + mu).^2 + 2.*mu.^2.*(r.^2 + y.*r)./(r + mu).^3;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg3=llg3+ ...
           (exp(lp0).*(r.^2.*(r + r.*exp(2.*lp0)) + mu.^2.*r.^3 - mu.*r.^2.*(3.*r + exp(2.*lp0) + 1)) + exp(2.*lp0).*(mu.^2.*r.^3 - 2.*r.^3 + mu.*r.^2.*(3.*r + 2)))./((exp(lp0) - 1).^3.*(mu + r).^3).*mu;
    case 'latent2+param'
      llg3 = mu.*(y.*r - 2.*r.*mu - mu.*y)./(r+mu).^3;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      ip0=exp(-lp0);
      llg3=llg3+ ...
           (mu.*(2.*r + 2.*r.*ip0.*(mu - 1) + r.^2.*ip0.*(mu - 1).*(log(mu + r) - log(r) + r./(mu + r) - 1)))./((mu + r).^2.*(ip0 - 1).^2) - (2.*mu.*(r.^2 + r.^2.*ip0.*(mu - 1)))./((mu + r).^3.*(ip0 - 1).^2) - (2.*mu.*ip0.*(r.^2 + r.^2.*ip0.*(mu - 1)).*(log(mu + r) - log(r) + r./(mu + r) - 1))./((mu + r).^2.*(ip0 - 1).^3);
      % correction due to the log transformation
      llg3 = llg3.*lik.disper;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_negbinztr_tiltedMoments(lik, y, i1, sigma2_i, myy_i, z)
%LIK_NEGBINZTR_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_NEGBINZTR_TILTEDMOMENTS(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 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

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    if isscalar(sigma2_i)
      sigma2ii = sigma2_i;
    else
      sigma2ii = sigma2_i(i);
    end

    % get a function handle of an unnormalized tilted distribution
    % (likelihood * cavity = Negative-binomial * Gaussian)
    % and useful integration limits
    [tf,minf,maxf]=init_negbinztr_norm(yy(i),myy_i(i),sigma2ii,avgE(i),r,1);

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    if isnan(m_0)
      logM_0=NaN;
      return
    end
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigma2ii
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      if sigm2hati1(i) >= sigma2ii
        warning('lik_negbinztr_tilted_moments: sigm2hati1 >= sigm2_i');
        %sigm2hati1=sigm2_i-1e-9;
      end
    end
    logM_0(i) = log(m_0);
  end
end

function [logM_0, m_1, sigm2hati1] = lik_negbinztr_tiltedMoments2(lik, y, i1, sigm2_i, myy_i, z, eta)
%LIK_NEGBINZTR_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_NEGBINZTR_TILTEDMOMENTS(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 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

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    % get a function handle of an unnormalized tilted distribution
    % (likelihood * cavity = Negative-binomial * Gaussian)
    % and useful integration limits
    [tf,minf,maxf]=init_negbinztr_norm(yy(i),myy_i(i),sigm2_i(i),avgE(i),r,eta);

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigm2_i(i)
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      if sigm2hati1(i) >= sigm2_i(i)
        warning('lik_negbinztr_tilted_moments: sigm2hati1 >= sigm2_i');
        %sigm2hati1=sigm2_i-1e-9;
      end
    end
    logM_0(i) = log(m_0);
  end
end

function [g_i] = lik_negbinztr_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_NEGBINZTR_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_NEGBINZTR_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

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;

  % get a function handle of an unnormalized tilted distribution
  % (likelihood * cavity = Negative-binomial * Gaussian)
  % and useful integration limits
  [tf,minf,maxf]=init_negbinztr_norm(yy,myy_i,sigm2_i,avgE,r, 1);
  % additionally get function handle for the derivative
  td = @deriv;

  % Integrate with quadgk
  [m_0, fhncnt] = quadgk(tf, minf, maxf);
  [g_i, fhncnt] = quadgk(@(f) td(f).*tf(f)./m_0, minf, maxf);
  g_i = g_i.*r;

  function g = deriv(f)
    mu = avgE.*exp(f);
    % Derivative using the psi function
    g = 1 + log(r./(r+mu)) - (r+yy)./(r+mu) + psi(r + yy) - psi(r);
    lp0=r.*(log(r) - log(r+mu));
    g = g -(1./(1 - exp(-lp0)).*(log(r./(mu + r)) - r./(mu + r) + 1));
  end
end

function [g_i] = lik_negbinztr_siteDeriv2(lik, y, i1, sigm2_i, myy_i, z, eta, lnZhat)
%LIK_NEGBINZTR_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_NEGBINZTR_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

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;

  % get a function handle of an unnormalized tilted distribution
  % (likelihood * cavity = Negative-binomial * Gaussian)
  % and useful integration limits
  [tf,minf,maxf]=init_negbinztr_norm(yy,myy_i,sigm2_i,avgE,r, eta);
  % additionally get function handle for the derivative
  td = @deriv;

  % Integrate with quadgk
  [m_0, fhncnt] = quadgk(tf, minf, maxf);
  [g_i, fhncnt] = quadgk(@(f) td(f).*tf(f)./m_0, minf, maxf);
  g_i = g_i.*r;

  function g = deriv(f)
    mu = avgE.*exp(f);
    % Derivative using the psi function
    g = 1 + log(r./(r+mu)) - (r+yy)./(r+mu) + psi(r + yy) - psi(r);
    lp0=r.*(log(r) - log(r+mu));
    g = g -(1./(1 - exp(-lp0)).*(log(r./(mu + r)) - r./(mu + r) + 1));
%     g = eta.*g;
  end
end

function upfact = lik_negbinztr_upfact(gp, y, mu, ll, z)
  r = gp.lik.disper;
  sll = sqrt(ll);

  fh_e = @(f) negbinztr_pdf(y, exp(f).*z', r).*norm_pdf(f, mu, sll);
  EE = quadgk(fh_e, max(mu-6*sll,-30), min(mu+6*sll,30));

  fm = @(f) f.*negbinztr_pdf(y, exp(f).*z', r).*norm_pdf(f, mu, sll)./EE;
  mm  = quadgk(fm, max(mu-6*sll,-30), min(mu+6*sll,30));

  fV = @(f) (f - mm).^2.*negbinztr_pdf(y, exp(f).*z', r).*norm_pdf(f, mu, sll)./EE;
  Varp = quadgk(fV, max(mu-6*sll,-30), min(mu+6*sll,30));

  upfact = -(Varp - ll)./ll^2;
end

function [lpy, Ey, Vary] = lik_negbinztr_predy(lik, Ef, Varf, yt, zt)
%LIK_NEGBINZTR_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_NEGBINZTR_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | 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_NEGBINZTR_PREDY(LIK, EF, VARF) 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

  if numel(zt)==0
    zt=ones(size(Ef));
  end

  avgE = zt;
  r = lik.disper;

  lpy = zeros(size(Ef));
  Ey = zeros(size(Ef));
  EVary = zeros(size(Ef));
  VarEy = zeros(size(Ef));

  if nargout > 1
      % Evaluate Ey and Vary
      for i1=1:length(Ef)
        %%% With quadrature
        myy_i = Ef(i1);
        sigm_i = sqrt(Varf(i1));
        minf=myy_i-6*sigm_i;
        maxf=myy_i+6*sigm_i;

        F = @(f) exp(log(avgE(i1))+f+norm_lpdf(f,myy_i,sigm_i));
        Ey(i1) = quadgk(F,minf,maxf);

        F2 = @(f) exp(log(avgE(i1).*exp(f)+((avgE(i1).*exp(f)).^2/r))+norm_lpdf(f,myy_i,sigm_i));
        EVary(i1) = quadgk(F2,minf,maxf);

        F3 = @(f) exp(2*log(avgE(i1))+2*f+norm_lpdf(f,myy_i,sigm_i));
        VarEy(i1) = quadgk(F3,minf,maxf) - Ey(i1).^2;
      end
      Vary = EVary + VarEy;
  end

  % Evaluate the posterior predictive densities of the given observations
  lpy = zeros(length(yt),1);
  if (size(Ef,2) > 1) && (size(Ef,2) > 1) && size(yt,2) == 1
    % Approximate integral with sum of grid points when using corrected
    % marginal posterior pf
    for i1=1:length(yt)
      py = arrayfun(@(f) exp(lik.fh.ll(lik, yt(i1), f, zt(i1))), Ef(i1,:));
      pf = Varf(i1,:)./sum(Varf(i1,:));
      lpy(i1) = log(sum(py.*pf));
    end
  else
    for i1=1:length(yt)
      % get a function handle of the likelihood times posterior
      % (likelihood * posterior = Negative-binomial * Gaussian)
      % and useful integration limits
      [pdf,minf,maxf]=init_negbinztr_norm(...
        yt(i1),Ef(i1),Varf(i1),avgE(i1),r);
      % integrate over the f to get posterior predictive distribution
      lpy(i1) = log(quadgk(pdf, minf, maxf));
    end
  end
end

function [df,minf,maxf] = init_negbinztr_norm(yy,myy_i,sigm2_i,avgE,r, eta)
%INIT_NEGBINZTR_NORM
%
%  Description
%    Return function handle to a function evaluating
%    Negative-Binomial * Gaussian which is used for evaluating
%    (likelihood * cavity) or (likelihood * posterior) Return
%    also useful limits for integration. This is private function
%    for lik_negbinztr. This subfunction is needed by subfunctions
%    tiltedMoments, siteDeriv and predy.
%
%  See also
%    LIK_NEGBINZTR_TILTEDMOMENTS, LIK_NEGBINZTR_SITEDERIV,
%    LIK_NEGBINZTR_PREDY

% avoid repetitive evaluation of constant part
  ldconst = eta*(-gammaln(r)-gammaln(yy+1)+gammaln(r+yy))...
            - log(sigm2_i)/2 - log(2*pi)/2;
  % Create function handle for the function to be integrated
  df = @negbinztr_norm;
  % use log to avoid underflow, and derivates for faster search
  ld = @log_negbinztr_norm;
  ldg = @log_negbinztr_norm_g;
  ldg2 = @log_negbinztr_norm_g2;

  % Set the limits for integration
  % Negative-binomial likelihood is log-concave so the negbinztr_norm
  % function is unimodal, which makes things easier
  if yy==0
    % with yy==0, the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation
    % of the Negative-Binomial likelihood and Gaussian
    mu=log(yy/avgE);
    s2=(yy+r)./(yy.*r);
    modef = (myy_i/sigm2_i + mu/s2)/(1/sigm2_i + 1/s2);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=4;       % number of Newton iterations
  mindelta=1e-6; % tolerance in stopping Newton iterations
  for ni=1:niter
    g=ldg(modef);
    h=ldg2(modef);
    delta=-g/h;
    modef=modef+delta;
    if abs(delta)<mindelta
      break
    end
  end
  % integrand limits based on Gaussian approximation at mode
  modes=sqrt(-1/h);
  minf=modef-8*modes;
  maxf=modef+8*modes;
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=20; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld<(modeld-lddiff) && minf<modef;
    % sometimes minf is too small
    minf=minf+step*modes;
    minld=ld(minf);
  end
  while minld>(modeld-lddiff)
    minf=minf-step*modes;
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbinztr -> init_negbinztr_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  iter=0;
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*modes;
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbinztr -> init_negbinztr_norm: ' ...
             'integration interval maximum not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = negbinztr_norm(f)
  % Negative-binomial * Gaussian
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      if yy==1
        integrand = exp(eta*(log(avgE)+f)...
                        -0.5*(f-myy_i).^2./sigm2_i -log(sigm2_i)/2 -log(2*pi)/2);
      else
        integrand = 0;
      end
    else
      integrand = exp(ldconst ...
                      +eta*(yy.*(log(mu)-log(r+mu))+r.*(log(r)-log(r+mu))) ...
                      -0.5*(f-myy_i).^2./sigm2_i ...
                      -eta*log(1-exp(lp0)));
    end
  end

  function log_int = log_negbinztr_norm(f)
  % log(Negative-binomial * Gaussian)
  % log_negbinztr_norm is used to avoid underflow when searching
  % integration interval
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      if yy==1
        log_int = eta*(log(avgE)+f) ...
                  -0.5*(f-myy_i).^2./sigm2_i - log(sigm2_i)/2 - log(2*pi)/2;
      else
        log_int=-Inf;
      end
    else
      log_int = ldconst...
                +eta*(yy.*(log(mu)-log(r+mu))+r.*(log(r)-log(r+mu)) -gammaln(r)-gammaln(yy+1)+gammaln(r+yy)) ...
                -0.5*(f-myy_i).^2./sigm2_i ...
                -eta*log(1-exp(lp0));
    end
  end

  function g = log_negbinztr_norm_g(f)
  % d/df log(Negative-binomial * Gaussian)
  % derivative of log_negbinztr_norm
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      g = eta+(myy_i - f)./sigm2_i;
    else
      g = -eta*(r.*(mu - yy))./(mu.*(mu + r)).*mu ...
          + (myy_i - f)./sigm2_i ...
          -eta*(1/(1 - exp(-lp0))*-r/(mu + r)*mu);
    end
  end

  function g2 = log_negbinztr_norm_g2(f)
  % d^2/df^2 log(Negative-binomial * Gaussian)
  % second derivate of log_negbinztr_norm
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      g2 = -1/sigm2_i;
    else
      g2 = -eta*(r*(r + yy))/(mu + r)^2.*mu ...
           -1/sigm2_i ...
           + eta*(r^2 + r^2*exp(-lp0)*(mu - 1))/((mu + r)^2*(exp(-lp0) - 1)^2)*mu;
    end
  end

end

function prctys = lik_negbinztr_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_BINOMIAL_PREDPRCTY  Returns the percentiled 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

  if numel(zt)==0
    zt=ones(size(Ef));
  end

  opt=optimset('TolX',1e-7,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  r = lik.disper;
  mu = zt.*exp(Ef);
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      minf = floor(fminbnd(@(b) (quadgk(@(y) llvec(lik,y,Ef(i1)-1.96*ci,zt(i1)), 0, b)-prcty(i2)).^2,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))-5,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))+5,opt));
      if minf<0
        minf = 0;
      end
      maxf = floor(fminbnd(@(b) (quadgk(@(y) llvec(lik,y,Ef(i1)+1.96*ci,zt(i1)), 0, b)-prcty(i2)).^2,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))-5,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))+5,opt));
      if maxf<0
        maxf = 0;
      end
%       j=0;
%       figure;
%       for a=-2:0.1:50
%         j=j+1;
%         testi(j) = (quadgk(@(f) quadgk(@(y) llvec(lik,y,Ef(i1),zt(i1)), 0, a).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2;
%       end
%       plot(-2:0.1:50,testi)
%       if minf<maxf
%         set(gca,'XTick',[minf maxf])
%       else
%         set(gca,'XTick',[minf minf+1])
%       end
%       hold on;
%       a=floor(fminbnd(@(a) (quadgk(@(f) quadgk(@(y) llvec(lik,y,Ef(i1),zt(i1)), 0, a) ...
%              .*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2, minf, maxf,opt));
      a=floor(fminbnd(@(a) (quadgk(@(f) sum(llvec(lik,0:1:a,Ef(i1),zt(i1))) ...
             .*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2, minf, maxf,opt));
      if quadgk(@(f) sum(llvec(lik,0:1:a,Ef(i1),zt(i1))).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4) < prcty(i2)
        a=a+1;
      end
      prctys(i1,i2)=a;
    end
  end

  function expll = llvec(lik,yt,f,z)
    % Compute vector of likelihoods of single predictions
    n = length(yt);
    if n>0
      for i=1:n
        expll(i) = exp(lik.fh.ll(lik, yt(i), f, z));
      end
    else
      expll = 0;
    end
  end

end

function mu = lik_negbinztr_invlink(lik, f, z)
%LIK_NEGBINZTR_INVLINK  Returns values of inverse link function
%
%  Description
%    MU = LIK_NEGBINZTR_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values MU of inverse link function.
%    This subfunction is needed when using function gp_predprctmu.
%
%     See also
%     LIK_NEGBINZTR_LL, LIK_NEGBINZTR_PREDY

  if numel(z)==0
    z=1;
  end
  mu = bsxfun(@times,z,exp(f));
end

function reclik = lik_negbinztr_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = GPCF_NEGBINZTR_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

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

    % Initialize parameter
    reclik.disper = [];

    % Set the function handles
    reclik.fh.pak = @lik_negbinztr_pak;
    reclik.fh.unpak = @lik_negbinztr_unpak;
    reclik.fh.lp = @lik_negbinztr_lp;
    reclik.fh.lpg = @lik_negbinztr_lpg;
    reclik.fh.ll = @lik_negbinztr_ll;
    reclik.fh.llg = @lik_negbinztr_llg;
    reclik.fh.llg2 = @lik_negbinztr_llg2;
    reclik.fh.llg3 = @lik_negbinztr_llg3;
    reclik.fh.tiltedMoments = @lik_negbinztr_tiltedMoments;
    reclik.fh.tiltedMoments2 = @lik_negbinztr_tiltedMoments2;
    reclik.fh.siteDeriv = @lik_negbinztr_siteDeriv;
    reclik.fh.siteDeriv2 = @lik_negbinztr_siteDeriv2;
    reclik.fh.predy = @lik_negbinztr_predy;
    reclik.fh.predprcty = @lik_negbinztr_predprcty;
    reclik.fh.invlink = @lik_negbinztr_invlink;
    reclik.fh.recappend = @lik_negbinztr_recappend;
    reclik.p=[];
    reclik.p.disper=[];
    if ~isempty(ri.p.disper)
      reclik.p.disper = ri.p.disper;
    end
  else

    % Append to the record
    reclik.disper(ri,:)=lik.disper;
    if ~isempty(lik.p.disper)
      reclik.p.disper = lik.p.disper.fh.recappend(reclik.p.disper, ri, lik.p.disper);
    end
  end
end


function [init_sz, final_sz] = ideal_size(net, max_sz)

final_sz = forward(net, max_sz);
init_sz = backward(net, final_sz);
while ~all(init_sz <= max_sz)
    final_sz = final_sz - 1;
    init_sz = backward(net, final_sz);
end

end

function n = forward(net, n)
    for i = 1:numel(net.layers)
        l = net.layers{i};
        switch l.type
        case 'conv'
            m = [size(l.weights{1}, 1), size(l.weights{1}, 2)];
            n = filter(n, l.pad, m, l.stride);
        case 'pool'
            n = filter(n, l.pad, l.pool, l.stride);
        end
    end
end

function n = backward(net, n)
    for i = numel(net.layers):-1:1
        l = net.layers{i};
        switch l.type
        case 'conv'
            m = [size(l.weights{1}, 1), size(l.weights{1}, 2)];
            n = unfilter(n, l.pad, m, l.stride);
        case 'pool'
            n = unfilter(n, l.pad, l.pool, l.stride);
        end
    end
end

function n = filter(n, pad, m, k)
assert(numel(pad) == 1);
n = floor((n + 2*pad - m) / k) + 1;
end

function n = unfilter(n, pad, m, k)
assert(numel(pad) == 1);
n = k*(n - 1) + m - 2*pad;
end


function [atlasA,atlasE,atlasMask]=atlasAjustHoles(atlasA,atlasE,atlasMask)
% [atlasA,atlasE,atlasMask]=atlasAjustHoles(atlasA,atlasE,atlasMask)
% Author: Schira
% Purpose:
%    Clears little holes in an complete "Atlas"
%    basicly this is just a cosmetic operation, assuming any -1 point
%    completly surrounded by "Atlas" points is a "hole"
%    The tool fills in the mean for eccentricity and angle and the median
%    for the mask.

map=zeros(size(atlasE));
ind=find(atlasE~=-1);
map(ind)=1;
map=imdilate(map,[1 1])-map;
ind=find(map==1);
for ii=1:length(ind)
    i=ind(ii);
    b=floor(i/size(atlasE,1));
    a=i-size(atlasE,1)*b;
    b=b+1;
    vekt=cat(2,atlasA(a-1,b),atlasA(a+1,b),atlasA(a,b-1),atlasA(a,b-1));
    if sum(vekt==-1)==0
        atlasA(i)=mean([atlasA(a-1,b),atlasA(a+1,b),atlasA(a,b-1),atlasA(a,b-1)]);
        atlasE(i)=mean([atlasE(a-1,b),atlasE(a+1,b),atlasE(a,b-1),atlasE(a,b-1)]);
        atlasMask(i)=median([atlasMask(a-1,b),atlasMask(a+1,b),atlasMask(a,b-1),atlasMask(a,b-1)]);
    end
end


classdef RollAngleConstraint < AbstractConstraint
    %RollAngleConstraint Summary of this class goes here
    %   Detailed explanation goes here

    properties
        normFact = 1;
        event LaunchVehicleEvent
        eventNode(1,1) ConstraintStateComparisonNodeEnum = ConstraintStateComparisonNodeEnum.FinalState;

        lb(1,1) double = 0;
        ub(1,1) double = 0;

        evalType(1,1) ConstraintEvalTypeEnum = ConstraintEvalTypeEnum.FixedBounds;
        stateCompType(1,1) ConstraintStateComparisonTypeEnum = ConstraintStateComparisonTypeEnum.Equals;
        stateCompEvent LaunchVehicleEvent
        stateCompNode(1,1) ConstraintStateComparisonNodeEnum = ConstraintStateComparisonNodeEnum.FinalState;
    end

    methods
        function obj = RollAngleConstraint(event, lb, ub)
            obj.event = event;
            obj.lb = lb;
            obj.ub = ub;

             obj.id = rand();
        end

        function [lb, ub] = getBounds(obj)
            lb = obj.lb;
            ub = obj.ub;
        end

        function [c, ceq, value, lwrBnd, uprBnd, type, eventNum, valueStateComp] = evalConstraint(obj, stateLog, celBodyData)
            type = obj.getConstraintType();

            switch obj.eventNode
                case ConstraintStateComparisonNodeEnum.FinalState
                    stateLogEntry = stateLog.getLastStateLogForEvent(obj.event);

                case ConstraintStateComparisonNodeEnum.InitialState
                    stateLogEntry = stateLog.getFirstStateLogForEvent(obj.event);

                otherwise
                    error('Unknown event node.');
            end

            ut = stateLogEntry.time;
            rVect = stateLogEntry.position;
            vVect = stateLogEntry.velocity;
            bodyInfo = stateLogEntry.centralBody;

            [rollAngle, ~, ~] = stateLogEntry.attitude.getEulerAngles(ut, rVect, vVect, bodyInfo);
            value = rad2deg(rollAngle);

            if(obj.evalType == ConstraintEvalTypeEnum.StateComparison)
                switch obj.stateCompNode
                    case ConstraintStateComparisonNodeEnum.FinalState
                        stateLogEntryStateComp = stateLog.getLastStateLogForEvent(obj.stateCompEvent).deepCopy();

                    case ConstraintStateComparisonNodeEnum.InitialState
                        stateLogEntryStateComp = stateLog.getFirstStateLogForEvent(obj.stateCompEvent).deepCopy();

                    otherwise
                        error('Unknown event node.');
                end

                cartElem = stateLogEntryStateComp.getCartesianElementSetRepresentation();
                cartElem = cartElem.convertToFrame(stateLogEntry.centralBody.getBodyCenteredInertialFrame());
                stateLogEntryStateComp.setCartesianElementSet(cartElem);

                ut = stateLogEntryStateComp.time;
                rVect = stateLogEntryStateComp.position;
                vVect = stateLogEntryStateComp.velocity;
                bodyInfo = stateLogEntryStateComp.centralBody;

                [rollAngle, ~, ~] = stateLogEntryStateComp.attitude.getEulerAngles(ut, rVect, vVect, bodyInfo);
                valueStateComp = rad2deg(rollAngle);
            else
                valueStateComp = NaN;
            end

            [c, ceq] = obj.computeCAndCeqValues(value, valueStateComp);

            lwrBnd = obj.lb;
            uprBnd = obj.ub;

            eventNum = obj.event.getEventNum();
        end

        function sF = getScaleFactor(obj)
            sF = obj.normFact;
        end

        function setScaleFactor(obj, sF)
            obj.normFact = sF;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end

        function tf = usesExtremum(obj, extremum)
            tf = false;
        end

        function tf = canUseSparseOutput(obj)
            tf = true;
        end

        function tf = usesEvent(obj, event)
            tf = obj.event == event;
            if(obj.evalType == ConstraintEvalTypeEnum.StateComparison)
                tf = tf || obj.stateCompEvent == event;
            end
        end

        function event = getConstraintEvent(obj)
            event = obj.event;
        end

        function type = getConstraintType(obj)
            type = 'Roll Angle';
        end

%         function name = getName(obj)
%             name = sprintf('%s - Event %i', obj.getConstraintType(), obj.event.getEventNum());
%         end

        function [unit, lbLim, ubLim, usesLbUb, usesCelBody, usesRefSc] = getConstraintStaticDetails(obj)
            unit = 'deg';
            lbLim = -360;
            ubLim = 360;
            usesLbUb = true;
            usesCelBody = false;
            usesRefSc = false;
        end

        function addConstraintTf = openEditConstraintUI(obj, lvdData)
%             addConstraintTf = lvd_EditGenericMAConstraintGUI(obj, lvdData);

            output = AppDesignerGUIOutput({false});
            lvd_EditGenericMAConstraintGUI_App(obj, lvdData, output);
            addConstraintTf = output.output{1};
        end
    end

    methods(Static)
        function constraint = getDefaultConstraint(~, ~)
            constraint = RollAngleConstraint(LaunchVehicleEvent.empty(1,0),0,0);
        end
    end
end


function updateTSDF(camRtC2W)

global ViewFrustumC;
ViewFrustumW = transformRT(ViewFrustumC, camRtC2W);

range2test = [min(ViewFrustumW, [], 2) max(ViewFrustumW, [], 2)];

global XYZcam;
global voxel;
global K;
global tsdf_value;
global tsdf_weight;

% choose a bounding box to contain viewing frustum
rangeGrid = (range2test - voxel.range(:,[1 1])) / voxel.unit + 1;
rangeGrid(:,1) = max(1,floor(rangeGrid(:,1)));
rangeGrid(:,2) = min(ceil (rangeGrid(:,2)),voxel.size_grid);
rangeGrid = int32(rangeGrid);

% get the grid there
disp('meshgrid');
tic;
[Y,X,Z]=meshgrid(rangeGrid(1,1):rangeGrid(1,2),rangeGrid(2,1):rangeGrid(2,2),rangeGrid(3,1):rangeGrid(3,2)); % strange matlab syntax
toc;
disp('grid to world');
tic;
X = X(:)'; Y = Y(:)'; Z = Z(:)';
gridIndex = sub2ind(voxel.size_grid',X,Y,Z);
gridCoordinateW = [single(X)*voxel.unit + voxel.range(1,1); single(Y)*voxel.unit + voxel.range(2,1); single(Z)*voxel.unit + voxel.range(3,1)];
clear X Y Z;
toc;

% transform the grid
disp('transform');
tic;
gridCoordinateC = transformRT(gridCoordinateW, camRtC2W, true);
toc;

% select: in front of camera
disp('select: in front of camera');
tic;
isValid = find(gridCoordinateC(3,:)>0);
gridCoordinateC = gridCoordinateC(:,isValid);
gridIndex = gridIndex(isValid);
toc;

% select: project
disp('select: project');
tic;
px = round(K(1,1)*(gridCoordinateC(1,:)./gridCoordinateC(3,:)) + K(1,3));
py = round(K(2,2)*(gridCoordinateC(2,:)./gridCoordinateC(3,:)) + K(2,3));
isValid = (1<=px & px <= 640 & 1<=py & py<= 480);
gridCoordinateC = gridCoordinateC(:,isValid);
gridIndex = gridIndex(isValid);
py = py(isValid);
px = px(isValid);
toc;

% select: valid depth
disp('select: valid depth');
tic;
ind = sub2ind([480 640],py,px);
isValid = XYZcam(ind+640*480*3)~=0;
gridCoordinateC = gridCoordinateC(:,isValid);
gridIndex = gridIndex(isValid);
ind = ind(isValid);
toc;

% compare distance between measurement and the grid
disp('compare distance between measurement and the grid');
%{

        const float diff = (depth[px] - cameraX.z) * sqrt(1+sq(pos.x/pos.z) + sq(pos.y/pos.z));
        if(diff > -mu){
            const float sdf = fminf(1.f, diff/mu);
            float2 data = vol[pix];
            data.x = clamp((data.y*data.x + sdf)/(data.y + 1), -1.f, 1.f);
            data.y = fminf(data.y+1, maxweight);
            vol.set(pix, data);
        }

%}
tic;
eta = (XYZcam(ind+640*480*2)- gridCoordinateC(3,:)) .* ((1+ (gridCoordinateC(1,:)./gridCoordinateC(3,:)).^2 + (gridCoordinateC(2,:)./gridCoordinateC(3,:)).^2 ).^0.5);
toc;

%tic;
%XYZmeasure = [XYZcam(ind);XYZcam(ind+640*480);XYZcam(ind+640*480*2)];
%Dmeasure = sqrt(sum(XYZmeasure.^2,1));
%Dtran    = sqrt(sum(gridCoordinateC.^2,1));
%eta = Dmeasure-Dtran;
%toc;

% select: > - mu
disp('select: > - mu');
tic;
isValid = eta>-voxel.mu;
eta = eta(isValid);
gridIndex = gridIndex(isValid);
ind = ind(isValid);
new_value = min(1,eta/voxel.mu);
toc;

disp('read write tsdf');
tic;
old_weight = tsdf_weight(gridIndex);
new_weight = old_weight + 1;
tsdf_weight (gridIndex)= new_weight;
tsdf_value (gridIndex) = (tsdf_value(gridIndex).*old_weight +new_value)./new_weight;
toc;

disp('coloring');
tic;
global tsdf_color;
global IMGcam;
if ~isempty(tsdf_color)
    isValid = eta<=voxel.mu;
    eta = eta(isValid);
    gridIndex = gridIndex(isValid);
    ind = ind(isValid);

    IMGmeasure = [IMGcam(ind);IMGcam(ind+640*480);IMGcam(ind+640*480*2)];

    colorArray = reshape(typecast(tsdf_color(gridIndex),'uint8'),4,[]);

    old_weight = colorArray(4,:);
    old_weight(double(old_weight)>=255) = 254;
    new_weight = old_weight + 1;

    newArray = uint8(round((double(colorArray(1:3,:)) .* repmat(double(old_weight),3,1) + double(IMGmeasure)) ./ repmat(double(new_weight),3,1)));
    newArray(4,:) = new_weight;

    tsdf_color(gridIndex) = typecast(newArray(:),'uint32');
end
toc;

%{
__global__ void integrate( Volume vol, const Image<float> depth, const Matrix4 invTrack, const Matrix4 K, const float mu, const float maxweight){
    uint3 pix = make_uint3(thr2pos2());
    float3 pos = invTrack * vol.pos(pix);
    float3 cameraX = K * pos;
    const float3 delta = rotate(invTrack, make_float3(0,0, vol.dim.z / vol.size.z));
    const float3 cameraDelta = rotate(K, delta);

    for(pix.z = 0; pix.z < vol.size.z; ++pix.z, pos += delta, cameraX += cameraDelta){
       if(pos.z < 0.0001f) // some near plane constraint
            continue;
        const float2 pixel = make_float2(cameraX.x/cameraX.z + 0.5f, cameraX.y/cameraX.z + 0.5f);
        if(pixel.x < 0 || pixel.x > depth.size.x-1 || pixel.y < 0 || pixel.y > depth.size.y-1)
            continue;
        const uint2 px = make_uint2(pixel.x, pixel.y);
        if(depth[px] == 0)
            continue;
        const float diff = (depth[px] - cameraX.z) * sqrt(1+sq(pos.x/pos.z) + sq(pos.y/pos.z));
        if(diff > -mu){
            const float sdf = fminf(1.f, diff/mu);
            float2 data = vol[pix];
            data.x = clamp((data.y*data.x + sdf)/(data.y + 1), -1.f, 1.f);
            data.y = fminf(data.y+1, maxweight);
            vol.set(pix, data);
        }
    }
}
%}


function display_multifractal(S,t0)
	clrs = {'r','g','b','c','k','m'};

	S1 = [S{2}.signal{:}].';
	S2 = [S{3}.signal{:}].';

	subplot(121);
	plot(S1(:,t0));
	title('m=1');
	xlabel('j1');

	j1s = unique(S{3}.meta.scale(:,1)).';
	subplot(122);
	hold on
	for j1 = j1s
		plot(S2(S{3}.meta.scale(:,1)==j1,t0), ...
			clrs{mod(j1,length(clrs))+1});
	end
	hold off
	title('m=2');
	xlabel('j2-j1');
end


function [Xmat,pobjround,info] = local_search_quasar(Zmat,C,rrPar,opt,roundonly)
%% Round and refine for QUASAR SDP
%% Input:
%% Zmat: SDP primal iterate
%% C: cost matrix of the SDP
%% rrPar: a structure where you can pass any data necessary for local search
%% opt: indices of eigenvectors for rounding
%% roundonly: if true, then only round a solution without NLP
%% Output:
%% Xmat: a rank-one SDP iterate
%% pobjround: primal SDP cost attained by Xmat

if nargin < 5
    roundonly = false;
end
if nargin < 4
    % default round the first two eigenvectors
    opt = [1,2];
end

if roundonly
    [q,theta] = quasar_round(Zmat,[1]);
    xtld      = kron([1;theta],q);
    pobjround = xtld' * C{1} * xtld;
    Xmat      = {xtld * xtld'};
else
    [q,theta] = quasar_round(Zmat,opt);
    Zmatround = {};
    pobjround = zeros(length(opt),1);
    for i = 1:length(opt)
        [Ztmp,~,~,pobjtmp] = quasar_manopt(C,q(:,i),theta(:,i));
        Zmatround{end+1} = Ztmp;
        pobjround(i) = pobjtmp;
    end
    pobjs            = pobjround;
    [pobjround,idx]  = min(pobjround);
    if pobjround == inf
        fprintf('        NLP fails to find a good solution, return a rounded solution only.\n');
        qopt         = q(:,1); thetaopt = theta(:,1);
        xopt         = kron([1;thetaopt],qopt);
        Xmat         = {xopt * xopt'};
        nlpsuccess   = false;
    else
        Xmat         = Zmatround{idx};
        nlpsuccess   = true;
    end
end

if nargout > 2
    info.minidx     = idx;
    info.nlpsuccess = nlpsuccess;
    info.pobjs      = pobjs;
    info.diffpobj   = pobjs(1) - pobjround;
end
end

function [q,theta] = quasar_round(X,opt)
if nargin < 2
    opt = 1;
end
[V,D] = eig(X{1});
[~,I] = sort(diag(D),'descend');
V     = V(:,I);
%% take the opt-th eigenvector
x     = V(:,opt);
q     = [];
theta = [];
for k = 1:size(x,2)
    xk    = x(:,k);
    %% round that eigenvector
    n     = length(xk);
    N     = n/4 - 1;
    qk    = xk(blkIndices(1,4));
    qk    = qk/norm(qk);
    thetak= zeros(N,1);
    for i = 1:N
        inprod = qk' * xk(blkIndices(i+1,4));
        if inprod > 0
            thetak(i) = 1;
        else
            thetak(i) = -1;
        end
    end
    q     = [q,qk];
    theta = [theta,thetak];
end
end

function [Xmat,qnew,thetanew,fopt] = quasar_manopt(C,q,theta)
N           = length(theta);
s           = spherefactory(4,1);
o           = obliquefactory(1,N);
elements.A  = s;
elements.B  = o;
manifold    = productmanifold(elements);

problem.M   = manifold;

warning('off', 'manopt:getHessian:approx')
% Define the problem cost function and its Euclidean gradient.
problem.cost  = @(x) quasar_cost(x,C);
problem.egrad = @(x) quasar_egrad(x,C);

% Numerically check gradient consistency (optional).
% checkgradient(problem);
% Solve.
x0.A                 = q;
x0.B                 = theta';
options.verbosity    = 0;
options.tolgradnorm  = 1e-6;
[xopt, fopt, output, options] = trustregions(problem,x0,options);
qopt = xopt.A;
thetaopt = xopt.B';
constraintviolationq          = abs(norm(qopt) - 1);
constraintviolationtheta      = max(abs(thetaopt.^2 - 1));
firstorderopt                 = output(end).gradnorm;
fprintf('        MANOPT: itr: %3d, constraint violation: %3.2e, %3.2e, gradnorm: %3.2e, cost: %3.8e.\n',...
    length(output),constraintviolationq,constraintviolationtheta,firstorderopt,fopt);
if max(constraintviolationq,constraintviolationtheta) < 1e-8 ...
        && firstorderopt < 1e-6
    % Do nothing
else
    fopt    = inf;
end
qnew = qopt/norm(qopt);
thetanew = sign(thetaopt);
vnew     = kron([1;thetanew],qnew);
Xmat     = {vnew * vnew'};
end

function f = quasar_cost(x,C)
q       = x.A;
theta   = x.B;
v       = kron([1;theta(:)],q);
f       = v'*C{1}*v;
end

function g = quasar_egrad(x,C)
q       = x.A;
theta   = x.B;
N       = length(theta);
v       = kron([1;theta(:)],q);

gv      = 2 * v' * C{1}; % 1 x n

vdq     = kron([1;theta(:)],speye(4)); % n x 4
gq      = (gv * vdq)'; % 4 x 1

vdtheta = [sparse(4,N);kron(speye(N),q)]; % n x N
gtheta  = (gv * vdtheta); % 1 x N

g.A     = gq;
g.B     = gtheta;
end


function addScaleBar(in,barLength,barWidth, barPosition,barColor)
% Adds scale bar into an image.
% out=addScaleBar(in,barLength,barWidth,barPosition,barColor)
% in: image to which the scale bar should be added.
% barLength: length of the scale bar in pixels (defaults barLength=10);
% barWidth: thickness of the scale bar in pixesl (default: barWidth = 3);
% barPosition: position of the scale bar in the image (default: barPosition = size(in)*9/10;
% barColor: color of the scale bar (default: 'white')

if and(exist('in','var'),~isempty(in));
    dipshow(in);
else
    % added to a current picture
    c=get(gca);
    in = zeros(floor([c.XLim(2),c.YLim(2)])); % just to get size right for barPosition
end;
if ~exist('barLength','var'); barLength = 10; end;
if ~exist('barWidth','var'); barWidth = 3; end;
if ~exist('barPosition','var'); barPosition = size(in)*9/10; end;
if ~exist('barColor','var'); barColor = [1 1 .99]; end;

line([barPosition(1) barPosition(1)-barLength], [barPosition(2) barPosition(2)],'color',barColor,'linewidth',barWidth)


function q_out=qmult(q1,q2)
% QMULT(Q1,Q2) calculates the product of two quaternions Q1 and Q2.
%    Inputs can be vectors of quaternions, but they must either have the
%    same number of component quaternions, or one input must be a single
%    quaternion.  QMULT will determine whether the component quaternions of
%    the inputs are row or column vectors according to ISQ.
%
%    The output will have the same shape as Q1.  If the component
%    quaternions of either Q1 or Q2 (but not both) are of indeterminate
%    shape (see ISQ), then the shapes will be assumed to be the same for
%    both inputs.  If both Q1 and Q2 are of indeterminate shape, then both
%    are assumed to be composed of row vector quaternions.
%
% See also ISQ.

% Release: $Name: quaternions-1_2_2 $
% $Revision: 1.12 $
% $Date: 2001/05/01 20:20:31 $

% Copyright (C) 2001, Jay A. St. Pierre.  All rights reserved.

if nargin~=2
  error('qmult() requires two input arguments');
else
  q1type = isq(q1);
  if ( q1type == 0 )
    error(['Invalid input: q1 must be a quaternion or a vector of' ...
          ' quaternions'])
  end
  q2type = isq(q2);
  if ( q2type == 0 )
    error(['Invalid input: q2 must be a quaternion or a vector of' ...
          ' quaternions'])
  end
end

% Make sure q1 is a column of quaternions (components are rows)
if ( q1type==1 | (q1type==3 & q2type==1) )
  q1=q1.';
end

% Make sure q2 is a column of quaternions (components are rows)
if ( q2type==1 | (q2type==3 & q1type==1) )
  q2=q2.';
end

num_q1=size(q1,1);
num_q2=size(q2,1);

if (  num_q1~=num_q2 & num_q1~=1 & num_q2~=1 )
  error(['Inputs do not have the same number of elements:', 10, ...
         '   number of quaternions in q1 = ', num2str(num_q1), 10,...
         '   number of quaternions in q2 = ', num2str(num_q2), 10,...
         'Inputs must have the same number of elements, or', 10, ...
         'one of the inputs must be a single quaternion (not a', 10, ...
         'vector of quaternions).'])
end

% Build up full quaternion vector if one input is a single quaternion
if ( num_q1 ~= num_q2 )
  ones_length = ones(max(num_q1,num_q2),1);
  if ( num_q1 == 1 )
    q1 = [q1(1)*ones_length ...
          q1(2)*ones_length ...
          q1(3)*ones_length ...
          q1(4)*ones_length ];
  else % num_q2 == 1
    q2 = [q2(1)*ones_length ...
          q2(2)*ones_length ...
          q2(3)*ones_length ...
          q2(4)*ones_length ];
  end
end

% Products

% If q1 and q2 are not vectors of quaternions, then:
%
%   q1*q2 = q1*[ q2(4) -q2(3)  q2(2) -q2(1)
%                q2(3)  q2(4) -q2(1) -q2(2)
%               -q2(2)  q2(1)  q2(4) -q2(3)
%                q2(1)  q2(2)  q2(3)  q2(4) ]
%
% But to deal with vectorized quaternions, we have to use the ugly
% commands below.

prod1 = ...
    [ q1(:,1).*q2(:,4) -q1(:,1).*q2(:,3)  q1(:,1).*q2(:,2) -q1(:,1).*q2(:,1)];
prod2 = ...
    [ q1(:,2).*q2(:,3)  q1(:,2).*q2(:,4) -q1(:,2).*q2(:,1) -q1(:,2).*q2(:,2)];
prod3 = ...
    [-q1(:,3).*q2(:,2)  q1(:,3).*q2(:,1)  q1(:,3).*q2(:,4) -q1(:,3).*q2(:,3)];
prod4 = ...
    [ q1(:,4).*q2(:,1)  q1(:,4).*q2(:,2)  q1(:,4).*q2(:,3)  q1(:,4).*q2(:,4)];

q_out = prod1 + prod2 + prod3 + prod4;

% Make sure output is same format as q1
if ( q1type==1 | (q1type==3 & q2type==1) )
  q_out=q_out.';
end

% NOTE that the following algorithm proved to be slower than the one used
% above:
%
% q_out = zeros(size(q1));
%
% q_out(:,1:3) = ...
%     [q1(:,4) q1(:,4) q1(:,4)].*q2(:,1:3) + ...
%     [q2(:,4) q2(:,4) q2(:,4)].*q1(:,1:3) + ...
%     cross(q1(:,1:3), q2(:,1:3));
%
% q_out(:,4) = q1(:,4).*q2(:,4) - dot(q1(:,1:3), q2(:,1:3), 2);


%%
% Copyright (c) 2019-present, Mahmoud Afifi
%
% This source code is licensed under the license found in the
% LICENSE file in the root directory of this source tree.
%
% Please, cite the following paper if you use this code:
%
% Mahmoud Afifi and Michael S. Brown. What else can fool deep learning?
% Addressing color constancy errors on deep neural network performance.
% ICCV, 2019
%
% Email: mafifi@eecs.yorku.ca | m.3afifi@gmail.com
%%

classdef synthWBmodel
% WB emulator model
    properties
        features % training features
        mappingFuncs9 % training mapping functions 9x3 poly
        K % K value for KNN
        encoder % PCA object
        wb_photo_finishing % WB & photo finishing (PF) styles
    end
    methods

        function feature = encode(obj,hist)
		% Generates a compacted feature of a given histogram tensor.
            feature =  obj.encoder.encode(hist);
        end

        function hist = RGB_UVhist(obj,I)
		% Generates an RGB-histogram tensor of a given image I.
            I = im2double(I);
            if size(I,1)*size(I,2) > 202500 % if input image > (450*450),
                factor = sqrt(202500/(size(I,1)*size(I,2))); % scale factor
                newH = floor(size(I,1)*factor); % new dimensions
                newW = floor(size(I,2)*factor);
                I = imresize(I,[newH,newW],'nearest'); % scale it down
            end

            h= sqrt(max(size(obj.encoder.weights,1),...
                size(obj.encoder.weights,2))/3); % histogram dimension
            eps= 6.4/h; % threshold
            I=(reshape(I,[],3)); % reshape input image
            A=[-3.2:eps:3.19]; % dummy vector
            hist=zeros(size(A,2),size(A,2),3); % histogram tensor will be stored here
            i_ind=I(:,1)~=0 & I(:,2)~=0 & I(:,3)~=0; % remove zero pixels
            I=I(i_ind,:);
            Iy=sqrt(I(:,1).^2+I(:,2).^2+I(:,3).^2); % intensity vector
            for i = 1 : 3 % for each layer in the histogram
                r = setdiff([1,2,3],i); % extract the current color channel
                Iu=log((I(:,i))./(I(:,r(1)))); % Iu vector
                Iv=log((I(:,i))./(I(:,r(2)))); % Iv vector
                diff_u=abs(Iu-A); % differences in u space
                diff_v=abs(Iv-A); % differences in v space
                diff_u=(reshape((reshape(diff_u,[],1)<=eps/2),...
                    [],size(A,2))); % set 1's for all pixels below the threshold
                diff_v=(reshape((reshape(diff_v,[],1)<=eps/2),...
                    [],size(A,2))); % the same in the v space
                hist(:,:,i)= ... % hist = Iy .* diff_u' * diff_v (.* element-wise mult)
                    (Iy.*double(diff_u))'*double(diff_v);
                hist(:,:,i)= ... %final hist is sqrt(hist/sum(hist))
                    sqrt(hist(:,:,i)/sum(sum(hist(:,:,i))));
            end
            hist = imresize(hist,[h h],'bilinear');
        end

        function [synthWBimages,wb_pf] = generate_wb_srgb (obj,I, ...
                outNum, feature, sigma)
		% Generates outNum new images from a given image I, where outNum should be <=10
            I = im2double(I); % convert to double
            if nargin == 2
                outNum = length(obj.wb_photo_finishing); % use all WB & PF styles (default)
                feature = obj.encode(obj.RGB_UVhist(I)); % encode histogram of I
                sigma = 0.25; % fall-off factor
            elseif nargin == 3
                feature = obj.encode(obj.RGB_UVhist(I));
                sigma = 0.25;
            elseif nargin == 4
                sigma = 0.25;
            end

            if outNum > length(obj.wb_photo_finishing) % if selected styles > the available WB & PF styles
                error('Error: number of new images should be <= %d',...
                    length(obj.wb_photo_finishing));
            end

            if outNum ~= length(obj.wb_photo_finishing) % if selected styles < the available WB & PF styles
                inds = randperm(length(obj.wb_photo_finishing)); % randomize from the available WB & PF styles
                wb_pf = obj.wb_photo_finishing(...
                    inds(1:outNum));
            else
                wb_pf = obj.wb_photo_finishing; % otherwise, use all available WB & PF styles
                inds = [1:length(wb_pf)];
            end

            synthWBimages = zeros(size(I,1),size(I,2),size(I,3),...
                length(wb_pf)); % new images will be stored here

            [dH,idH] = pdist2(obj.features,feature,...
                'euclidean','Smallest',obj.K); % K nearest neighbor
            weightsH = exp(-((dH).^2)/(2*sigma^2)); % compute blending weights
            weightsH = weightsH/sum(weightsH); % normalize weights
            count = 1;
            for i = inds(1:outNum) % for each WB & PF style, do
                mf = sum(weightsH .* ... % compute mapping funciton
                    obj.mappingFuncs9((idH-1)*10 + i,:),1);
                mf = reshape(mf,[9,3]); % reshape it to be 9*3
                synthWBimages(:,:,:,count) = obj.change_wb(I,mf); % apply mf
                count = count + 1;
            end

        end

        function out = change_wb(obj,input, m)
		% Applies a given mapping function m to input image input.
            sz=size(input);
            input=reshape(input,[],3); % reshape image to be n*3 (n total number of pixels)
            %higher degree (N-D)
            input=obj.kernelP9(input); % raise it to a higher-dim space
            out=input*m; % apply m
            out = obj.out_of_gamut_clipping(out); % clip out-of-gamut pixels
            out=reshape(out,[sz(1),sz(2),sz(3)]); % reshape the image back to its original shape
        end

        function O=kernelP9(obj,I)
		% Kernel function: \phi(r,g,b) -> (r,g,b,r2,g2,b2,rg,rb,gb)
            O=[I,... %r,g,b
                I.*I,... %r2,g2,b2
                I(:,1).*I(:,2),I(:,1).*I(:,3),I(:,2).*I(:,3),... %rg,rb,gb
                ];
        end

        function [I] = out_of_gamut_clipping(obj,I)
		% Clips out-of-gamut pixels.
            I(I>1)=1;
            I(I<0)=0;
        end

    end
end


function cmap= cmap_posneg(m)
%CMAP_POSNEG - Colormap going from cyan/blue over white to red/yellow
%
%Synposis:
% MAP= cmap_rainbow(M)
%
%Input:
% M: Size of the colormap (number of entries). Default value: Same size
%    as current colormap
%
%Output:
% MAP: A colormap matrix of size [M 3]
%
%Example:
% clf;
% colormap(cmap_posneg(65));
% imagesc(toeplitz(1:65)); colorbar;
%
%See also COLORMAP, HSV2RGB, CMAP_HSVFADE

% 01-2005 Benjamin Blankertz

if nargin<1 | isempty(m),
  m= size(get(gcf,'colormap'),1);
end

if mod(m,2)==0,
  warning('m should be odd -> using m+1');
  m= m+1;
end

mb6= floor((m-1)/6);
mq= m-1 - 6*mb6;
m1= mb6;
m2= mb6 + (mq>=2);
m3= mb6 + (mq>=4);
map1= cmap_hsvFade(m1+1, 0, 1, [0.5 1]);
map2= cmap_hsvFade(m2+1, [0 1/6], 1, 1);
map3= cmap_hsvFade(m3+1, 1/6, [1 0], 1);
map4= cmap_hsvFade(m3+1, 3/6, [0 1], 1);
map5= cmap_hsvFade(m2+1, [3/6 4/6], 1, 1);
map6= cmap_hsvFade(m1+1, 4/6, 1, [1 0.5]);
cmap= [map1; map2(2:end,:); map3(2:end,:); ...
       map4(2:end,:); map5(2:end,:); map6(2:end,:)];
cmap= flipud(cmap);


function C = slcovlarge(X, w, vmean, cachesize)
%SLCOVLARGE Computes large covariance matrix using memory-efficient way
%
% $ Syntax $
%   - C = slcovlarge(X, w, vmean, cachesize)
%
% $ Arguments $
%   - X:            the sample matrix
%   - w:            the weights of samples (default = [])
%                   if it is specifed, it should be a 1 x n vector
%   - vmean:        the pre-computed mean (default = [])
%   - cachesize:    the size of working cache (in the unit of Mbytes)
%
% $ Description $
%   - C = slcovlarge(X, w, vmean, cachesize) computes the large covariance
%     matrix within a memory-limited context. Compared to slcov, which is
%     faster at the expense of using more memory, it is slower, however
%     the memory used is under control. Thus it can effectively prevent
%     from the situation of out of memory.
%
% $ Remarks $
%   - The memory increased in the function will not exceed
%     the size of the covariance matrix plus the cachesize.
%
%   - For vmean
%       - if it is empty, then a mean vector will be calculated
%       - if it is zero, then the data is assumed to be centralized
%       - if it is specified, then its size should be d x 1.
%
%   - The memory estimated to be used for each section is
%     if has no weight
%           2 x sd x sn + 2 x (sd + sn).
%     else
%           3 x sd x sn + 2 x (sd + sn).
%     end
%     Here sd and sn are the sub-dimension
%     and sub-number of that section.
%
% $ History $
%   - Created by Dahua Lin, on Aug 17, 2006
%

%% parse and verify input arguments

if nargin < 4
    raise_lackinput('slcovlarge', 4);
end

if ndims(X) ~= 2 || ~isnumeric(X)
    error('sltoolbox:invalidarg', ...
        'X should be a 2D numeric matrix');
end

[d, n] = size(X);

if ~isempty(w)
    if ~isequal(size(w), [1, n])
        error('sltoolbox:sizmismatch', ...
            'The w should be an 1 x n vector');
    end
end

% convert cache size
cacheelems = floor(cachesize * 1e6 / 8);

if isempty(vmean)
    if cacheelems < d
        error('sltoolbox:notenoughmem', ...
            'The cache size is not enough to hold even a mean vector');
    end
    cacheelems = cacheelems - d;
    vmean = slmean(X, w);
else
    if ~isequal(vmean, 0)
        if ~isequal(size(vmean), [d, 1])
            error('sltoolbox:sizmismatch', ...
                'The vmean should be a d x 1 vector');
        end
    end
end

%% decide partition struct

if cacheelems > 2*d*n + 2*(d+n)   % can compute without partitioning
    ps = [];
else
    if isempty(w)
        cblks = 2;
    else
        cblks = 3;
    end

    sd = floor( (cacheelems - 2 * n) / (cblks * n + 2) );
    divide_row = false;
    if sd <= 0
        sd = 1;
        if cacheelems < cblks + 4
            error('sltoolbox:notenoughmem', ...
                'The cache is not large enough');
        end
        sn = floor((cacheelems - 2) / (cblks + 2));
        divide_row = true;
    end

    ps = slpartition(d, 'maxblksize', sd);
    if divide_row
        rps = slpartition(d, 'maxblksize', sn);
    end
end

%% compute

if isempty(ps)

    if ~isempty(w);
        X = weight_x(X, w);
    end
    X = shift_x(X, vmean);
    Xt = X';
    C = X * Xt;

else

    if ~divide_row      % each row is taken as integral

        C = zeros(d, d);
        nsecs = length(ps.sinds);

        for i = 1 : nsecs
            for j = 1 : nsecs
                si = ps.sinds(i);
                ei = ps.einds(i);
                sj = ps.sinds(j);
                ej = ps.einds(j);

                if isempty(w)
                    curXj = X(sj:ej, :);
                    curXj = shift_x(curXj, vmean, sj, ej);
                    curXjt = curXj';
                    clear curXj;

                    curXi = X(si:ei, :);
                    curXi = shift_x(curXi, vmean, si, ei);
                else
                    curXj = X(sj:ej, :);
                    curXj = shift_x(curXj, vmean, sj, ej);
                    curXj = weight_x(curXj, w);
                    clear curwj;
                    curXjt = curXj';
                    clear curXj;

                    curXi = X(si:ei, :);
                    curXi = shift_x(curXi, vmean, si, ei);
                    curXi = weight_x(curXi, w);
                    clear curwi;
                end

                C(si:ei, sj:ej) = curXi * curXjt;

            end
        end

    else                % even each row need to be divided

        slignorevars(rps);

        error('sltoolbox:rterror', ...
            'In current implementation, row-division is not implemeted yet');
    end

end

%% scale down

if isempty(w)
    C = C / n;
else
    tw = sum(w);
    C = C / tw;
end

%% Core computing function

function sx = weight_x(sx, w)

sn = size(sx, 2);
for i = 1 : sn
    sx(:, i) = sx(:, i) * sqrt(w(i));
end

function sy = shift_x(sx, vmean, sidx, eidx)

[sd, sn] = size(sx);
if ~isequal(vmean, 0)

    if nargin == 2
        curmean = vmean;
    else
        curmean = vmean(sidx:eidx);
    end

    sy = zeros(sd, sn);
    for i = 1 : sn
        sy(:,i) = sx(:,i) - curmean;
    end
else
    sy = sx;
end


%STEREOCALIBRATE  Calibrates the stereo camera
%
%     S = cv.stereoCalibrate(objectPoints, imagePoints1, imagePoints2, imageSize)
%     [...] = cv.stereoCalibrate(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __objectPoints__ A cell array of cells of calibration pattern points in
%   the calibration pattern coordinate space  `{{[x,y,z], ..}, ...}`.
% * __imagePoints1__ A cell array of cells of the projections of calibration
%   pattern points `{{[x,y], ..}, ...}`, observed by the first camera.
% * __imagePoints2__ A cell array of cells of the projections of calibration
%   pattern points `{{[x,y], ..}, ...}`, observed by the second camera.
% * __imageSize__ Size of the image used only to initialize the intrinsic
%   camera matrix `[w,h]`.
%
% ## Output
% * __S__ scalar struct having the following fields:
%   * __cameraMatrix1__ output first camera matrix
%     `A = [fx1 0 cx1; 0 fy1 cy1; 0 0 1]`.
%   * __distCoeffs1__ output vector of distortion coefficients
%     `[k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,taux,tauy]` of 4, 5, 8, 12
%     or 14 elements. The output vector length depends on the options.
%   * __cameraMatrix2__ output second camera matrix
%     `A = [fx2 0 cx2; 0 fy2 cy2; 0 0 1]`. The parameter is similar to
%     `cameraMatrix1`.
%   * __distCoeffs2__ output lens distortion coefficients for the second
%     camera. The parameter is similar to `distCoeffs1`.
%   * __R__ output 3x3 rotation matrix between the 1st and the 2nd camera
%     coordinate systems.
%   * __T__ output 3x1 translation vector between the coordinate systems of
%     the cameras.
%   * __E__ output 3x3 essential matrix.
%   * __F__ output 3x3 fundamental matrix.
%   * __reprojErr__ output final re-projection error (scalar).
% * __perViewErrors__ Output matrix of the RMS re-projection error estimated
%   for each pattern view.
%
% ## Options
% * __CameraMatrix1__, __CameraMatrix2__ Initial camera matrices. If any of
%   `UseIntrinsicGuess`, `FixAspectRatio`, `FixIntrinsic` (default), or
%   `FixFocalLength` are specified, some or all of the matrix components must
%   be initialized. See the flags description for details.
% * __DistCoeffs1__, __DistCoeffs2__ Initial lens distortion coefficients.
% * __R__, __T__ Initial extrinsic parameters `R` and `T`.
%   If `UseExtrinsicGuess` is set, these must be initialized. Not set by
%   default.
% * __FixIntrinsic__ Fix `cameraMatrix1`,`cameraMatrix2` and `distCoeffs1`,
%   `distCoeffs2` so that only `R`, `T`, `E`, and `F` matrices are estimated.
%   default true.
% * __UseIntrinsicGuess__ Optimize some or all of the intrinsic parameters
%   according to the specified flags. Initial values are provided by the user.
%   default false.
% * __UseExtrinsicGuess__ `R`, `T` contain valid initial values that are
%   optimized further. Otherwise `R`, `T` are initialized to the median value
%   of the pattern views (each dimension separately). default false
% * __FixPrincipalPoint__ Fix the principal points during the optimization.
%   default false.
% * __FixFocalLength__ Fix `fx1`,`fx2` and `fy1`,`fy2`. default false.
% * __FixAspectRatio__ Optimize `fy1`,`fy2` and fix the ratio `fx1/fy1`,
%   `fx2/fy2`. default false.
% * __SameFocalLength__ Enforce same `fx1=fx2` and `fy1=fy2`. default false.
% * __ZeroTangentDist__ Tangential distortion coefficients for each camera are
%   set to zeros and stay fixed. default false.
% * __FixTangentDist__ The tangential distortion coefficients are not
%   changed during the optimization. If `UseIntrinsicGuess` is set, the
%   coefficient from the supplied `DistCoeffs` matrix is used. Otherwise, it
%   is set to 0. default false.
% * __FixK1__, ..., __FixK6__ The corresponding radial distortion coefficient
%   is not changed during the optimization. If `UseIntrinsicGuess` is set, the
%   coefficient from the supplied `DistCoeffs` matrix is used. Otherwise, it
%   is set to 0. default false.
% * __RationalModel__ Coefficients `k4`, `k5`, and `k6` are enabled. To
%   provide the backward compatibility, this extra flag should be explicitly
%   specified to make the calibration function use the rational model and
%   return 8 coefficients. If the flag is not set, the function computes and
%   returns only 5 distortion coefficients. default false.
%   (`RationalModel` as false implies `FixK4`,`FixK5`,`FixK6` as true).
% * __ThinPrismModel__ Coefficients `s1`, `s2`, `s3` and `s4` are enabled. To
%   provide the backward compatibility, this extra flag should be explicitly
%   specified to make the calibration function use the thin prism model and
%   return 12 coefficients. If the flag is not set, the function computes and
%   returns only 5 distortion coefficients. default false.
%   (`ThinPrismModel` as false implies `FixS1S2S3S4` as true).
% * __FixS1S2S3S4__ The thin prism distortion coefficients are not changed
%   during the optimization. If `UseIntrinsicGuess` is set, the coefficient
%   from the supplied `DistCoeffs` matrix is used. Otherwise, it is set to 0.
%   default false.
% * __TiltedModel__ Coefficients `tauX` and `tauY` are enabled. To provide the
%   backward compatibility, this extra flag should be explicitly specified to
%   make the calibration function use the tilted sensor model and return 14
%   coefficients. If the flag is not set, the function computes and returns
%   only 5 distortion coefficients. default false.
%   (`TiltedModel` as false implies `FixTauXTauY` as true).
% * __FixTauXTauY__ The coefficients of the tilted sensor model are not
%   changed during the optimization. If `UseIntrinsicGuess` is set, the
%   coefficient from the supplied `DistCoeffs` matrix is used. Otherwise, it
%   is set to 0. default false.
% * __UseLU__ Use LU instead of SVD decomposition for solving. Much faster but
%   potentially less precise. default false.
% * __UseQR__ Use QR instead of SVD decomposition for solving. Faster but
%   potentially less precise. default false.
% * __Criteria__ Termination criteria for the iterative optimization algorithm.
%   default `struct('type','Count+EPS', 'maxCount',30, 'epsilon',1e-6)`
%
% The function estimates transformation between two cameras making a stereo
% pair. If you have a stereo camera where the relative position and
% orientation of two cameras is fixed, and if you computed poses of an object
% relative to the first camera and to the second camera, `(R1,T1)` and
% `(R2,T2)`, respectively (this can be done with cv.solvePnP), then those
% poses definitely relate to each other. This means that, given `(R1,T1)`, it
% should be possible to compute `(R2,T2)`. You only need to know the position
% and orientation of the second camera relative to the first camera. This is
% what the described function does. It computes `(R,T)` so that:
%
%     R2 = R * R1
%     T2 = R * T1 + T
%
% Optionally, it computes the essential matrix `E`:
%
%     E = [ 0 -T2  T1;
%          T2   0 -T0;
%         -T1  T0   0] * R
%
% where `Ti` are components of the translation vector `T`: `T = [T0,T1,T2]'`.
% And the function can also compute the fundamental matrix `F`:
%
%     F = inv(cameraMatrix2)' * E * inv(cameraMatrix1)
%
% Besides the stereo-related information, the function can also perform a full
% calibration of each of two cameras. However, due to the high dimensionality
% of the parameter space and noise in the input data, the function can diverge
% from the correct solution. If the intrinsic parameters can be estimated with
% high accuracy for each of the cameras individually (for example, using
% cv.calibrateCamera), you are recommended to do so and then pass
% `FixIntrinsic` flag to the function along with the computed intrinsic
% parameters. Otherwise, if all the parameters are estimated at once, it makes
% sense to restrict some parameters, for example, pass `SameFocalLength` and
% `ZeroTangentDist` flags, which is usually a reasonable assumption.
%
% Similarly to cv.calibrateCamera, the function minimizes the total
% re-projection error for all the points in all the available views from both
% cameras. The function returns the final value of the re-projection error.
%
% See also: cv.calibrateCamera, cv.solvePnP, cv.stereoRectify,
%  estimateCameraParameters, stereoParameters, extrinsics
%


function res = ge(a,b)
%GE           Implements  a >= b  for Taylor, compares only a.x and b.x
%

% written  05/21/09     S.M. Rump
%

  if ~isa(a,'taylor')
    res = ( a>=reshape(b.t(1,:),b.size) );
  elseif ~isa(b,'taylor')
    res = ( reshape(a.t(1,:),a.size)>=b );
  else
    res = ( reshape(a.t(1,:),a.size)>=reshape(b.t(1,:),b.size) );
  end


function a = triu(a,k)
%TRIU         Implements  triu(a,k)  for gradients
%
%   c = triu(a,k)
%
% functionality as Matlab function triu for matrices
%

% written  10/16/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    improved performance
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if nargin==1
    k = 0;
  end

  a.x = triu(a.x,k);
  index = ( triu( ones(size(a.x)) , k ) == 0 );
  a.dx(index,:) = 0;


function test_subplot_2D_3D
%TEST_SUBPLOT_2D_3D Test mixed subplot 2D/3D with DRAGZOOM

figure;
hax1 = subplot(2,1,1);
x = -pi*2:0.1:pi*2;
y = sin(x);
plot(x,y);

hax2 = subplot(2,1,2);
k = 5;
n = 2^k-1;
[x,y,z] = sphere(n);
surf(x,y,z);

dragzoom();


function Hv = lbfgsHvFunc2(v,Hdiag,N,M)
Hv = v/Hdiag - N*(M\(N'*v));


filename='Bridge_quad_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT'; incrementFactor = 1;
filterType = 'P1';

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

Vfrac_initial = 1;
optimality_initial = 1e-2;
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;


function [bodyX, bodyY, bodyZ, R_body_2_inertial] = computeInertialBodyAxesFromFrameAeroAngles(ut, rVect, vVect, bodyInfo, bankAng, angOfAttack, angOfSideslip, baseFrame)
    arguments
        ut(1,1) double
        rVect(3,1) double
        vVect(3,1) double
        bodyInfo(1,1) KSPTOT_BodyInfo
        bankAng(1,1) double
        angOfAttack(1,1) double
        angOfSideslip(1,1) double
        baseFrame(1,1) AbstractReferenceFrame
    end

    frame = bodyInfo.getBodyCenteredInertialFrame();
    ce = CartesianElementSet(ut, rVect, vVect, frame);
    ce = ce.convertToFrame(baseFrame, true);
    rVectFrame = ce.rVect;
    vVectFrame = ce.vVect;

    [R_wind_2_frame, ~, ~, ~] = computeWindFrame(rVectFrame, vVectFrame);
    R_body_2_wind = eul2rotmARH([angOfSideslip,angOfAttack,bankAng],'zyx');
    [~,~,~, R_frame_2_inertial] = baseFrame.getOffsetsWrtInertialOrigin(ut, ce);
    R_body_2_inertial = real(R_frame_2_inertial * R_wind_2_frame * R_body_2_wind);

    bodyX = R_body_2_inertial(:,1);
    bodyY = R_body_2_inertial(:,2);
    bodyZ = R_body_2_inertial(:,3);
end


function fixed = p11_fixed_points ( m, fixed_num )

%*****************************************************************************80
%
%% P11_FIXED_POINTS returns the fixed points in problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer FIXED_NUM, the number of fixed points.
%
%    Output, real FIXED(M,FIXED_NUM), the fixed points.
%
  fixed = [    ...
    0.0,  0.0; ...
    1.0,  0.0; ...
    1.0,  0.5; ...
    0.5,  0.5; ...
    0.5,  1.0; ...
    0.0,  1.0 ]';

  return
end


function test_suite = test_distanceLines3d
%TESTDISTANCELINES3D  One-line description here, please.
%
%   output = testDistanceLines3d(input)
%
%   Example
%   testDistanceLines3d
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-01-21,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function testOrthogonal(testCase) %#ok<*DEFNU>

line1 = [0 0 0   0 0 1];
line2 = [10 0 0  0 1 0];

d = distanceLines3d(line1, line2);
testCase.assertEqual(10, d, 'AbsTol', .01);

line1 = [0 0 0   0 0 1];
line2 = [0 10 0  1 0 0];

d = distanceLines3d(line1, line2);
testCase.assertEqual(10, d, 'AbsTol', .01);

line1 = [0 0 0   0 1 0];
line2 = [0 0 10  1 0 0];

d = distanceLines3d(line1, line2);
testCase.assertEqual(10, d, 'AbsTol', .01);

function testNotOrthogonal(testCase) %#ok<*DEFNU>

line1 = [0 0 0   0 2 3];
line2 = [10 0 0  0 -1 4];

d = distanceLines3d(line1, line2);
testCase.assertEqual(10, d, 'AbsTol', .01);

function testArrays(testCase)

line1 = [0 0 0   0 2 3];
line2 = [10 0 0  0 -1 4];

dist = distanceLines3d(line1, repmat(line2, 5, 1));
exp = repmat(10, 1, 5);
testCase.assertEqual(exp, dist, 'AbsTol', .01);

dist = distanceLines3d(repmat(line1, 5, 1), line2);
exp = repmat(10, 5, 1);
testCase.assertEqual(exp, dist, 'AbsTol', .01);


function K = lfmjXrbfKernCompute(lfmKern, rbfKern, t1, t2)

% LFMJXRBFKERNCOMPUTE Compute cross kernel between the LFMJ and RBF kernels.
% FORMAT
% DESC computes cross kernel terms between LFMJ and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFMJ
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between LFMJ and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFMJ
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

if nargin < 4
  t2 = t1;
end
if size(t1, 2) > 1 || size(t2, 2) > 1
  error('Input can only have one column');
end

if lfmKern.inverseWidth ~= rbfKern.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Get length scale out.
sigma2 = 2/lfmKern.inverseWidth;
sigma = sqrt(sigma2);

% Parameters of the kernel
alpha = lfmKern.damper./(2*lfmKern.mass);
omega = sqrt(lfmKern.spring./lfmKern.mass - alpha.*alpha);

gamma1 = alpha + j*omega;
gamma2 = alpha - j*omega;

sK = lfmjpComputeUpsilonMatrix(gamma2,sigma2,t1, t2, 0) - ...
    lfmjpComputeUpsilonMatrix(gamma1,sigma2,t1, t2, 0);

if lfmKern.isNormalised
   K0 = lfmKern.sensitivity/(j*4*sqrt(2)*lfmKern.mass*omega);
else
   K0 = sqrt(pi)*sigma*lfmKern.sensitivity/(j*4*lfmKern.mass*omega);
end

K = K0*sK;


function out = param_map(GA)

% rundesignsimISI2
% Tor Wager, 11/24/01
% if your input var name is already a variable in the wkspace, uses that...
% otherwise creates one with zeros.

warning off
index = 1;

% -------------------------------------------------
% load GA field names
% -------------------------------------------------
fN = fieldnames(GA);
for i = 1:size(fN,1)
    eval([fN{i} ' = GA.' fN{i}])
end

% -------------------------------------------------
% common user variables
% -------------------------------------------------
tickRes = [.1 .1];
ISIrange = [.1 16]
FCrange = [0 0];
GA.sizegenerations = 100;

myISI = ISIrange(1):tickRes(1):ISIrange(2);
myFC = FCrange(1):tickRes(2):FCrange(2);

ISIsize = length(myISI);
FCsize = length(myFC);

ISItick = 1:max(round(ISIsize./25),1):ISIsize;
ISIticklab = myISI(ISItick);
FCtick = 1:2:FCsize;
FCticklab = myFC(FCtick);

disp(['Starting ' num2str(prod([ISIsize FCsize])) ' surface points.'])

out.ga = GA;
out.myISI = myISI;
out.myFC = myFC;

% ----------------------------------------------------------------
% * HRF GAMMA FUNCTION - spm 99
% ----------------------------------------------------------------

   HRF = spm_hrf(.1);						% SPM HRF sampled at .1 s
   HRF = HRF/ max(HRF);

% ----------------------------------------------------------------
% * make the map
% ----------------------------------------------------------------

for RR = 1:size(myISI,2)

    % set ISI
    GA.ISI = myISI(RR);
    numsamps = ceil(numStim*GA.ISI/TR);

    out.numsamps(find(myISI == GA.ISI)) = numsamps;

    % ----------------------------------------------------------------
    % * get smoothing matrix and autocorrelation matrix
    % ----------------------------------------------------------------
    if ~isfield(GA,'LPsmooth'),GA.LPsmooth = 1;,end
    [S,Vi,svi] = getSmoothing(HPlength,GA.LPsmooth,TR,numsamps,xc);
    clear Vi

	for CC = 1:size(myFC,2)
		disp(['Starting model ' num2str(index)])

		% set rest frequency
		GA.freqConditions(end+1) = myFC(CC);
		GA.freqConditions(1:end-1) = (1 - myFC(CC)) / (size(GA.freqConditions,2)-1);

		disp(['FREQC and ISI are:	' num2str(GA.freqConditions) ' and ' num2str(GA.ISI)])

        % ----------------------------------------------------------------
        % * generate an unsorted list of conditions
        % ----------------------------------------------------------------
        numStim = ceil(scanLength / (GA.ISI));
        if ~isempty(restevery) & ~isempty(restlength),
            numRestStim = (ceil(numStim/(mean(restevery)+restlength)) - 1) * restlength;
        else
	        numRestStim = 0;
        end
        numStimEachCond = ceil((numStim-numRestStim) * freqConditions);			            % row vector of stim in each cond
        numBlanks = numStim - numRestStim - sum(numStimEachCond);

        unsortedList = [];
        for i = 1:size(conditions,2)
            unsortedList = [unsortedList; conditions(i) * ones(numStimEachCond(i),1)];
        end
        unsortedList = [unsortedList; zeros(numBlanks,1)];

        % ----------------------------------------------------------------
        % * randomize order and test each list
        % ----------------------------------------------------------------

	    clear listMatrix
        for z = 1:GA.sizeGenerations

  	        stimList = getRandom(unsortedList);

            % -------------------------------------------------------------------------------------------------
		    % * counterbalancing
		    % -------------------------------------------------------------------------------------------------

            cbal(z) = getCounterBal(stimList, maxOrder,conditions,freqConditions);

            % -------------------------------------------------------------------------------------------------
		    % * efficiency
		    % -------------------------------------------------------------------------------------------------

			model = designvector2model(stimList,ISI,HRF,TR,numsamps,nonlinthreshold,S);
        	xtxitx = pinv(model);                                       		% inv(X'S'SX)*(SX)'; pseudoinv of (S*X)
			eff(z) = calcEfficiency(contrastweights,contrasts,xtxitx,svi);

            % -------------------------------------------------------------------------------------------------
		    % * HRF shape estimation efficiency
		    % -------------------------------------------------------------------------------------------------

            delta = [];
            for i = 1:max(stimList(:,1))
                delta(:,i) = (stimList == i);
            end

			[model] = tor_make_deconv_mtx2(delta,5,TR / ISI);
            if ~isempty(S), model = S * model;,end

        	xtxitx = pinv(model);                                       		% inv(X'S'SX)*(SX)'; pseudoinv of (S*X)
			hrf_eff(z) = calcEfficiency([],[],xtxitx,svi);

            % -------------------------------------------------------------------------------------------------
		    % calculate average time between stimulus repetitions in each trial type
		    % -------------------------------------------------------------------------------------------------

            for i = 1:max(stimList),mytimeBtwn(i) = mean(diff(find(stimList==i))) .* GA.ISI;, end
            timeBtwn(z) = mean(mytimeBtwn);

            % old method:
            %[fitness,convar,xtxi,models] = testlist(listMatrix,GA,HRF,'svi',svi,'S',S);
            %convar = 1./convar;
	        %eval([name '(RR,CC) = mean(convar,2);']);

        end % testing at this parameter combination

        out.cbal(RR,CC) = mean(cbal);
        out.eff(RR,CC) = mean(eff);
        out.hrf_eff(RR,CC) = mean(hrf_eff);

        out.cbal_se(RR,CC) = std(cbal) / sqrt(sizeGenerations);
        out.eff_se(RR,CC) = std(eff) / sqrt(sizeGenerations);
        out.hrf_eff_se(RR,CC) = std(hrf_eff) / sqrt(sizeGenerations);
        out.timeBtwn(RR,CC) = mean(timeBtwn);
        out.timeBtwn_se(RR,CC) = std(timeBtwn) / sqrt(sizeGenerations);

		%eval(['save ' name num2str(index) ' ' name ' SIM'])
		index = index + 1;
	end
end


function c8_normal_01_test ( )

%*****************************************************************************80
%
%% C8_NORMAL_01_TEST tests C8_NORMAL_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2006
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8_NORMAL_01_TEST\n' );
  fprintf ( 1, '  C8_NORMAL_01 computes pseudorandom complex values\n' );
  fprintf ( 1, '  normally distributed in the unit circle.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The initial seed is %d\n', seed );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    [ r, seed ] = c8_normal_01 ( seed );
    fprintf ( 1, '  %6d  (%14f,  %14f)\n', i, real ( r ), imag ( r ) );
  end

  return
end


function [d,fsign] = checkrestrictions2(restriction,y)

% Check the restrictions
d     = 0;
count = 0;
fsign = 1;

for ii = 1 : size(restriction,2)
    tmp = eval(restriction{ii});
    count = count + min(tmp);
end
if isempty(count)==1
    error('There is a nan in the narrative restrictions.');
end
% check if -1*IRF are verified
y        = -y;
count1   = 0;
for ii = 1 : size(restriction,2)
    tmp = eval(restriction{ii});
    count1 = count1 + min(tmp);
end
if count == size(restriction,2) || count1 == size(restriction,2) % if all signs are verified stop
    d=1;
end
if count1 == size(restriction,2)
    fsign = -1;
end