%% FUNCTION Least_Dirty
%   Dirty Multi-Task Learning with Least Squares Loss.
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: group sparsity regularization parameter
%   rho2: elementwise sparsity regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   C: constant parameters
%   P: group sparsity structure (joint feature selection)
%   Q: elementwise sparsity component
%   funcVal: function (objective) value vector.
%   lossVal: loss value for every iteration.
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Jalali, A. and Ravikumar, P. and Sanghavi, S. and Ruan, C. A dirty
%       model for multi-task learning, NIPS 2010.
%

function [W, C, P, Q, funcVal, lossVal] = Logistic_Dirty(X, Y, rho1, rho2, opts)

if nargin <4
    error('\n Inputs: X, Y, rho1, should be specified!\n');
end
X = multi_transpose(X);

if nargin <5
    opts = [];
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];
lossVal = [];

%initialize a starting point
C0_prep = zeros(1, task_num);
for t_idx = 1: task_num
    m1 = nnz(Y{t_idx} == 1);
    m2 = nnz(Y{t_idx} == -1);
    if ( m1==0 || m2==0 )
        C0_prep(t_idx) = 0;
    else
        C0_prep(t_idx) = log(m1/m2);
    end
end

if opts.init==2
    P0 = zeros(dimension, task_num);
    Q0 = zeros(dimension, task_num);
    C0 = zeros(1, task_num);
elseif opts.init== 0
    P0 = randn(dimension, task_num);
    Q0 = randn(dimension, task_num);
    C0 = C0_prep;
elseif opts.init== 1
    if isfield(opts,'P0')
        P0=opts.P0;
        if (nnz(size(P0)-[dimension, task_num]))
            error('\n Check the input .P0');
        end
    else
        error('\n check opt.init');
    end

     if isfield(opts,'Q0')
        Q0=opts.Q0;
        if (nnz(size(Q0)-[dimension, task_num]))
            error('\n Check the input .Q0');
        end
    else
        error('\n check opt.init');
    end

    if isfield(opts,'C0')
        C0=opts.C0;
    else
        error('\n check opt.init');
    end
end

Pz= P0;
Qz= Q0;
Cz= C0;
Pz_old = P0;
Qz_old = Q0;
Cz_old = C0;

t = 1;
t_old = 0;
iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ps = (1 + alpha) * Pz - alpha * Pz_old;
    Qs = (1 + alpha) * Qz - alpha * Qz_old;
    Cs = (1 + alpha) * Cz - alpha * Cz_old;

    % compute function value and gradients of the search point
    [gWs, gCs, Fs ]  = gradVal_eval(Ps+Qs, Cs);

    % the Armijo Goldstein line search scheme
    while true
        Pzp = proximalL1infnorm(Ps - gWs/gamma, rho1 / gamma);
        Qzp = proximalL11norm(Qs - gWs/gamma, rho2 / gamma);

        Czp = Cs - gCs/gamma;
        Fzp = funVal_eval(Pzp+Qzp, Czp);

        delta_Pzp = Pzp - Ps;
        delta_Qzp = Qzp - Qs;
        delta_Czp = Czp - Cs;
        nm_delta_Pzp=norm(delta_Pzp, 'fro')^2;
        nm_delta_Qzp=norm(delta_Qzp, 'fro')^2;
        nm_delta_Czp=norm(delta_Czp, 'fro')^2;

        r_sum = (nm_delta_Pzp+nm_delta_Qzp+nm_delta_Czp)/2;

%         Fzp_gamma = Fs + sum(sum((delta_Pzp+delta_Qzp).* gWs))...
%             + sum(sum(delta_Czp .* gCs))...
%             + gamma/2 * norm(delta_Pzp + delta_Qzp, 'fro')^2 ...
%             + gamma/2 * nm_delta_Czp;

        Fzp_gamma = Fs + sum(sum((delta_Pzp+delta_Qzp).* gWs))...
            + sum(sum(delta_Czp .* gCs))...
            + gamma/2 * (nm_delta_Pzp +nm_delta_Qzp) ...
            + gamma/2 * nm_delta_Czp;

        if (r_sum <=1e-20)
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Pz_old = Pz;
    Qz_old = Qz;
    Cz_old = Cz;
    Pz = Pzp;
    Qz = Qzp;
    Cz = Czp;

    funcVal = cat(1, funcVal, Fzp + rho1*L1infnorm(Pzp) + rho2*L11norm(Qzp));
    lossVal = cat(1, lossVal, Fzp);

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

P=Pzp;
Q=Qzp;
W = Pzp+Qzp;
C = Czp;

% private functions

    function [grad_W, grad_C, funcVal] = gradVal_eval(W, C)
        grad_W = zeros(dimension, task_num);
        grad_C = zeros(1, task_num);
        lossValVect = zeros (1 , task_num);
        for i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
        end
        % here when computing function value we do not include
        % l1 norm.
        funcVal = sum(lossValVect);
    end

    function [funcVal] = funVal_eval (W, C)
        funcVal = 0;
        for i = 1: task_num
            funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
        end
        % here when computing function value we do not include
        % l1 norm.
    end

end

function [ grad_w, grad_c, funcVal ] = unit_grad_eval( w, c, x, y)
    %gradient and logistic evaluation for each task
    m = length(y);
    weight = ones(m, 1)/m;
    weighty = weight.* y;
    aa = -y.*(x'*w + c);
    bb = max( aa, 0);
    funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
    pp = 1./ (1+exp(aa));
    b = -weighty.*(1-pp);
    grad_c = sum(b);
    grad_w = x * b;
end

function [ funcVal ] = unit_funcVal_eval( w, c, x, y)
    %function value evaluation for each task
    m = length(y);
    weight = ones(m, 1)/m;
    aa = -y.*(x'*w + c);
    bb = max( aa, 0);
    funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
end


function [res]=tt_mm(mat1,mat2)
%Matrix-by-matrix product in TT1.0 format
%   [RES]=TT_MM(MAT1,MAT2) Matrix MAT1 by matrix MAT2 product in the
%   TT-format. Please avoid its usage: it will be removed in
%   future releases. Use * operator from the object-oriented version.
%
%
% TT-Toolbox 2.2, 2009-2011
%
%This is TT Toolbox, written by Ivan Oseledets, Olga Lebedeva
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
d=size(mat1,1);
res=cell(d,1);
k=1;
for s=1:2
n1=size(mat1{k},1);
m1=size(mat1{k},2);
rm=size(mat1{k},3);
n2=size(mat2{k},1);
m2=size(mat2{k},2);
rv=size(mat2{k},3);
%mat1{1} is (n1 x m1 x rm , mat2{1} is n2xm2xrm, result is n1xm2xrv1*rv)
%res{1}=reshape(permute(mat1{1},[1,3,2],[n1*rm1]),m1)*reshape(mat2{1},[n2,m2*rv2]);
res{k}=reshape(permute(mat1{k},[1,3,2]),[n1*rm,m1])*reshape(mat2{k},[n2,m2*rv]);
%res{1} is n1 x rm x m2 x rv
res{k}=reshape(res{k},[n1,rm,m2,rv]);
res{k}=permute(res{k},[1,3,2,4]);
res{k}=reshape(res{k},[n1,m2,rm*rv]);
if ( k == 1 )
 k=d;
end
end
for i=2:d-1
  n1=size(mat1{i},1);
  m1=size(mat1{i},2);
  rm1=size(mat1{i},3);
  rm2=size(mat1{i},4);
  n2=size(mat2{i},1);
  m2=size(mat2{i},2);
  rv1=size(mat2{i},3);
  rv2=size(mat2{i},4);
  %mat1{i} is n1 x m1 x rm1 x rm2
  %mat2{i} is n2 x m2 x rv1 x rv2
  %n1 x rm1 x rm2 x m1 * n2 x m2 x rv1 x rv2
  %n1 x rm1 x rm2 x m2 x rv1 x rv2
  %Need: n1 x m2 x rm1 x rv1 x rm2 x rv2
  %Permut: 1 4 2 5 3 6
  res{i}=reshape(permute(mat1{i},[1,3,4,2]),[n1*rm1*rm2,m1])*reshape(mat2{i},[n2,m2*rv1*rv2]);
  res{i}=reshape(res{i},[n1,rm1,rm2,m2,rv1,rv2]);
  res{i}=permute(res{i},[1,4,2,5,3,6]);
  res{i}=reshape(res{i},[n1,m2,rm1*rv1,rm2*rv2]);
end
return
end


function [post nlZ dnlZ] = infFITC_EP(hyp, mean, cov, lik, x, y)

% FITC-EP approximation to the posterior Gaussian process. The function is
% equivalent to infEP with the covariance function:
%   Kt = Q + G; G = diag(g); g = diag(K-Q);  Q = Ku'*inv(Kuu + snu2*eye(nu))*Ku;
% where Ku and Kuu are covariances w.r.t. to inducing inputs xu and
% snu2 = sn2/1e6 is the noise of the inducing inputs. We fixed the standard
% deviation of the inducing inputs snu to be a one per mil of the measurement
% noise's standard deviation sn. In case of a likelihood without noise
% parameter sn2, we simply use snu2 = 1e-6.
% For details, see The Generalized FITC Approximation, Andrew Naish-Guzman and
%                  Sean Holden, NIPS, 2007.
%
% The implementation exploits the Woodbury matrix identity
%   inv(Kt) = inv(G) - inv(G)*Ku'*inv(Kuu+Ku*inv(G)*Ku')*Ku*inv(G)
% in order to be applicable to large datasets. The computational complexity
% is O(n nu^2) where n is the number of data points x and nu the number of
% inducing inputs in xu.
% The posterior N(f|h,Sigma) is given by h = m+mu with mu = nn + P'*gg and
% Sigma = inv(inv(K)+diag(W)) = diag(d) + P'*R0'*R'*R*R0*P. Here, we use the
% site parameters: b,w=$b,\pi$=tnu,ttau, P=$P'$, nn=$\nu$, gg=$\gamma$
%
% The function takes a specified covariance function (see covFunctions.m) and
% likelihood function (see likFunctions.m), and is designed to be used with
% gp.m and in conjunction with covFITC.
%
% The inducing points can be specified through 1) the 2nd covFITC parameter or
% by 2) providing a hyp.xu hyperparameters. Note that 2) has priority over 1).
% In case 2) is provided and derivatives dnlZ are requested, there will also be
% a dnlZ.xu field allowing to optimise w.r.t. to the inducing points xu. However
% the derivatives dnlZ.xu can only be computed for one of the following eight
% covariance functions: cov{Matern|PP|RQ|SE}{iso|ard}.
%
% Copyright (c) by Hannes Nickisch, 2013-10-29.
%
% See also INFMETHODS.M, COVFITC.M.

persistent last_ttau last_tnu              % keep tilde parameters between calls
tol = 1e-4; max_sweep = 20; min_sweep = 2;     % tolerance to stop EP iterations
inf = 'infEP';
cov1 = cov{1}; if isa(cov1, 'function_handle'), cov1 = func2str(cov1); end
if ~strcmp(cov1,'covFITC'); error('Only covFITC supported.'), end    % check cov
if isfield(hyp,'xu'), cov{3} = hyp.xu; end  % hyp.xu is provided, replace cov{3}

[diagK,Kuu,Ku] = feval(cov{:}, hyp.cov, x);         % evaluate covariance matrix
if ~isempty(hyp.lik)                          % hard coded inducing inputs noise
  sn2 = exp(2*hyp.lik(end)); snu2 = 1e-6*sn2;               % similar to infFITC
else
  snu2 = 1e-6;
end
[n, D] = size(x); nu = size(Kuu,1);
m = feval(mean{:}, hyp.mean, x);                      % evaluate the mean vector

rot180   = @(A)   rot90(rot90(A));                     % little helper functions
chol_inv = @(A) rot180(chol(rot180(A))')\eye(nu);                 % chol(inv(A))
R0 = chol_inv(Kuu+snu2*eye(nu));           % initial R, used for refresh O(nu^3)
V = R0*Ku; d0 = diagK-sum(V.*V,1)';    % initial d, needed for refresh O(n*nu^2)

% A note on naming: variables are given short but descriptive names in
% accordance with Rasmussen & Williams "GPs for Machine Learning" (2006): mu
% and s2 are mean and variance, nu and tau are natural parameters. A leading t
% means tilde, a subscript _ni means "not i" (for cavity parameters), or _n
% for a vector of cavity parameters.

% marginal likelihood for ttau = tnu = zeros(n,1); equals n*log(2) for likCum*
nlZ0 = -sum(feval(lik{:}, hyp.lik, y, m, diagK, inf));
if any(size(last_ttau) ~= [n 1])      % find starting point for tilde parameters
  ttau = zeros(n,1);             % initialize to zero if we have no better guess
  tnu  = zeros(n,1);
  [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu); % compute initial repres.
  nlZ = nlZ0;
else
  ttau = last_ttau;                    % try the tilde values from previous call
  tnu  = last_tnu;
  [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu); % compute initial repres.
  nlZ = epfitcZ(d,P,R,nn,gg,ttau,tnu,d0,R0,Ku,y,lik,hyp,m,inf);
  if nlZ > nlZ0                                           % if zero is better ..
    ttau = zeros(n,1);                    % .. then initialize with zero instead
    tnu  = zeros(n,1);
    [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu);       % initial repres.
    nlZ = nlZ0;
  end
end

nlZ_old = Inf; sweep = 0;               % converged, max. sweeps or min. sweeps?
while (abs(nlZ-nlZ_old) > tol && sweep < max_sweep) || sweep<min_sweep
  nlZ_old = nlZ; sweep = sweep+1;
  for i = randperm(n)       % iterate EP updates (in random order) over examples
    pi = P(:,i); t = R*(R0*pi);                            % temporary variables
    sigmai = d(i) + t'*t; mui = nn(i) + pi'*gg;           % post moments O(nu^2)

    tau_ni = 1/sigmai-ttau(i);          %  first find the cavity distribution ..
    nu_ni = mui/sigmai+m(i)*tau_ni-tnu(i);          % .. params tau_ni and nu_ni

    % compute the desired derivatives of the indivdual log partition function
    [lZ, dlZ, d2lZ] = feval(lik{:}, hyp.lik, y(i), nu_ni/tau_ni, 1/tau_ni, inf);
    ttaui =                            -d2lZ  /(1+d2lZ/tau_ni);
    ttaui = max(ttaui,0);     % enforce positivity i.e. lower bound ttau by zero
    tnui  = ( dlZ + (m(i)-nu_ni/tau_ni)*d2lZ )/(1+d2lZ/tau_ni);
    [d,P(:,i),R,nn,gg,ttau,tnu] = ...                    % update representation
              epfitcUpdate(d,P(:,i),R,nn,gg, ttau,tnu,i,ttaui,tnui, m,d0,Ku,R0);
  end
  % recompute since repeated rank-one updates can destroy numerical precision
  [d,P,R,nn,gg] = epfitcRefresh(d0,Ku,R0,V, ttau,tnu);
  [nlZ,nu_n,tau_n] = epfitcZ(d,P,R,nn,gg,ttau,tnu,d0,R0,Ku,y,lik,hyp,m,inf);
end

if sweep == max_sweep
  warning('maximum number of sweeps reached in function infEP')
end
last_ttau = ttau; last_tnu = tnu;                       % remember for next call

post.sW = sqrt(ttau);                  % unused for FITC_EP prediction with gp.m
dd = 1./(d0+1./ttau);
alpha = tnu./(1+d0.*ttau);
RV = R*V; R0tV = R0'*V;
alpha = alpha - (RV'*(RV*alpha)).*dd;     % long alpha vector for ordinary infEP
post.alpha = R0tV*alpha;       % alpha = R0'*V*inv(Kt+diag(1./ttau))*(tnu./ttau)
B = R0tV.*repmat(dd',nu,1); L = B*R0tV'; B = B*RV';
post.L = B*B' - L;                      % L = -R0'*V*inv(Kt+diag(1./ttau))*V'*R0

if nargout>2                                           % do we want derivatives?
  dnlZ = hyp;                                   % allocate space for derivatives
  RVdd = RV.*repmat(dd',nu,1);
  for i=1:length(hyp.cov)
    [ddiagK,dKuu,dKu] = feval(cov{:}, hyp.cov, x, [], i); % eval cov derivatives
    dA = 2*dKu'-R0tV'*dKuu;                                       % dQ = dA*R0tV
    w = sum(dA.*R0tV',2); v = ddiagK-w;   % w = diag(dQ); v = diag(dK)-diag(dQ);
    z = dd'*(v+w) - sum(RVdd.*RVdd,1)*v - sum(sum( (RVdd*dA)'.*(R0tV*RVdd') ));
    dnlZ.cov(i) = (z - alpha'*(alpha.*v) - (alpha'*dA)*(R0tV*alpha))/2;
  end
  for i = 1:numel(hyp.lik)                                   % likelihood hypers
    dlik = feval(lik{:}, hyp.lik, y, nu_n./tau_n, 1./tau_n, inf, i);
    dnlZ.lik(i) = -sum(dlik);
    if i==numel(hyp.lik)
      % since snu2 is a fixed fraction of sn2, there is a covariance-like term
      % in the derivative as well
      v = sum(R0tV.*R0tV,1)';
      z = sum(sum( (RVdd*R0tV').^2 )) - sum(RVdd.*RVdd,1)*v;
      z = z + post.alpha'*post.alpha - alpha'*(v.*alpha);
      dnlZ.lik(i) = dnlZ.lik(i) + snu2*z;
    end
  end
  [junk,dlZ] = feval(lik{:}, hyp.lik, y, nu_n./tau_n, 1./tau_n, inf);% mean hyps
  for i = 1:numel(hyp.mean)
    dm = feval(mean{:}, hyp.mean, x, i);
    dnlZ.mean(i) = -dlZ'*dm;
  end
  if isfield(hyp,'xu')                   % derivatives w.r.t. inducing points xu
    xu = cov{3};
    cov = cov{2};             % get the non FITC part of the covariance function
    Kpu  = cov_deriv_sq_dist(cov,hyp.cov,xu,x);             % d K(xu,x ) / d D^2
    Kpuu = cov_deriv_sq_dist(cov,hyp.cov,xu);               % d K(xu,xu) / d D^2
    if iscell(cov), covstr = cov{1}; else covstr = cov; end
    if ~ischar(covstr), covstr = func2str(covstr); end
    if numel(strfind(covstr,'iso'))>0              % characteristic length scale
      e = 2*exp(-2*hyp.cov(1));
    else
      e = 2*exp(-2*hyp.cov(1:D));
    end
    B = (R0'*R0)*Ku;

    W = ttau;
    t = W./(1+W.*d0);
    diag_dK = alpha.*alpha + sum(RVdd.*RVdd,1)' - t;
    v = diag_dK+t;                 % BdK = B * ( dnlZ/dK - diag(diag(dnlZ/dK)) )
    BdK = (B*alpha)*alpha' - B.*repmat(v',nu,1);
    BdK = BdK + (B*RVdd')*RVdd;
    A = Kpu.*BdK; C = Kpuu.*(BdK*B'); C = diag(sum(C,2)-sum(A,2)) - C;
    dnlZ.xu = A*x*diag(e) + C*xu*diag(e);    % bring in data and inducing points
  end
end

% refresh the representation of the posterior from initial and site parameters
% to prevent possible loss of numerical precision after many epfitcUpdates
% effort is O(n*nu^2) provided that nu<n
function [d,P,R,nn,gg] = epfitcRefresh(d0,P0,R0,R0P0, w,b)
  nu = size(R0,1);                                   % number of inducing points
  rot180   = @(A)   rot90(rot90(A));                   % little helper functions
  chol_inv = @(A) rot180(chol(rot180(A))')\eye(nu);               % chol(inv(A))
  t  = 1./(1+d0.*w);                                   % temporary variable O(n)
  d  = d0.*t;                                                             % O(n)
  P  = repmat(t',nu,1).*P0;                                            % O(n*nu)
  T  = repmat((w.*t)',nu,1).*R0P0;                % temporary variable O(n*nu^2)
  R  = chol_inv(eye(nu)+R0P0*T');                                    % O(n*nu^3)
  nn = d.*b;                                                              % O(n)
  gg = R0'*(R'*(R*(R0P0*(t.*b))));                                     % O(n*nu)

% compute the marginal likelihood approximation
% effort is O(n*nu^2) provided that nu<n
function [nlZ,nu_n,tau_n] = ...
                        epfitcZ(d,P,R,nn,gg,ttau,tnu, d0,R0,P0, y,lik,hyp,m,inf)
  T = (R*R0)*P;                                             % temporary variable
  diag_sigma = d + sum(T.*T,1)'; mu = nn + P'*gg;       % post moments O(n*nu^2)
  tau_n = 1./diag_sigma-ttau;              % compute the log marginal likelihood
  nu_n  = mu./diag_sigma-tnu+m.*tau_n;            % vectors of cavity parameters
  lZ = feval(lik{:}, hyp.lik, y, nu_n./tau_n, 1./tau_n, inf);
  nu = size(gg,1);
  U = (R0*P0)'.*repmat(1./sqrt(d0+1./ttau),1,nu);
  L = chol(eye(nu)+U'*U);
  ld = 2*sum(log(diag(L))) + sum(log(1+d0.*ttau));
  t = T*tnu; tnu_Sigma_tnu = tnu'*(d.*tnu) + t'*t;
  nlZ = ld/2 -sum(lZ) -tnu_Sigma_tnu/2  ...
    -(nu_n-m.*tau_n)'*((ttau./tau_n.*(nu_n-m.*tau_n)-2*tnu)./(ttau+tau_n))/2 ...
      +sum(tnu.^2./(tau_n+ttau))/2-sum(log(1+ttau./tau_n))/2;

% update the representation of the posterior to reflect modification of the site
% parameters by w(i) <- wi and b(i) <- bi
% effort is O(nu^2)
% Pi = P(:,i) is passed instead of P to prevent allocation of a new array
function [d,Pi,R,nn,gg,w,b] = ...
                           epfitcUpdate(d,Pi,R,nn,gg, w,b, i,wi,bi, m,d0,P0,R0)
  dwi = wi-w(i); dbi = bi-b(i);
  hi = nn(i) + m(i) + Pi'*gg;                   % posterior mean of site i O(nu)
  t = 1+dwi*d(i);
  d(i) = d(i)/t;                                                          % O(1)
  nn(i) = d(i)*bi;                                                        % O(1)
  r = 1+d0(i)*w(i);
  r = r*r/dwi + r*d0(i);
  v = R*(R0*P0(:,i));
  r = 1/(r+v'*v);
  if r>0
    R = cholupdate(R,sqrt( r)*R'*v,'-');
  else
    R = cholupdate(R,sqrt(-r)*R'*v,'+');
  end
  gg = gg + ((dbi-dwi*(hi-m(i)))/t)*(R0'*(R'*(R*(R0*Pi))));            % O(nu^2)
  w(i) = wi; b(i) = bi;                            % update site parameters O(1)
  Pi = Pi/t;                                                             % O(nu)


function lik = lik_liks(varargin)
%  LIKS creates a likelihood structure which is composed by many
%       different likelihoods.
%
%  Description:
%    LIK_LIK = LIK_LIKS(LIKELIHOODS, VALUE1, ...) creates a likelihood
%    structure that allows alternative likelihoods for latent variables.
%    The full likelihood is the product of independent likelihoods
%    (independent observations given the latent process),
%
%               __k     __
%      p(y|f) = ||      ||   L_j(y_i|f_i, th_j)
%                 j=1   i \in I_j
%
%      f = (f1' f2' ... fk')'
%      y = (y1' y2' ... yk')'
%
%    Here j is the index for different likelihoods and I_j is the index
%    vector telling for which observations likelihood j is used.
%
%    When using the Liks likelihood you must give the vector/matrix z as
%    an extra parameter to each function that requires also y. The matrix z
%    must include the covariates for independent likelihoods (see e.g.
%    LIK_POISSON) and a column telling the index of likelihood attached to
%    respective observation. For example:
%      lik = lik_liks('likelihoods', {lik1 lik2}, 'classVariables', 2);
%      gp = gp_set('lik', lik, 'cf', k, 'latent_method', 'EP');
%      z = [1 1 1 2 2 2]';
%      gp_optim(gp, x, y, 'z', z)
%    Here z is a vector telling that the first 3 observations are related
%    to likelihood 1 and the last 3 to likelihood 2.
%
%    Parameters for Liks likelihood are [default]
%      likelihoods      - array of likelyhood structures [ {} ]
%      classVariables   - a scalar telling which column of matrix z defines
%                         the likelihood indices
%
%    For the demonstration of the use of lik_liks see DEMO_MULTIVARIATEGP.
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%
% Copyright (c) 2011 Jaakko Riihimäki
% Copyright (c) 2011 Aki Vehtari
% Copyright (c) 2012 Ville Tolvanen
% Copyright (c) 2015-2017 Jarno Vanhatalo
% ------------- 2015-2017 Marcelo Hartmann
% Copyright (c) 2017 Ville Tolvanen

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

  ip = inputParser;
  ip.FunctionName = 'LIK_LIKS';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('likelihoods', {}, @(x) ~isempty(x) && iscell(x));
  ip.addParamValue('classVariables', [], @(x) ~isempty(x) && isscalar(x));
  ip.parse(varargin{:});
  lik = ip.Results.lik;

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

  % Initialize likelihoods
  if init || ~ismember('likelihoods', ip.UsingDefaults)
      lik.liks = ip.Results.likelihoods;
  end

  % number of likelihood functions
  lik.nliks = length(lik.liks);
  if lik.nliks < 2
      error('use one likelihood structure')

  else
      % column of z where the classVariables can be found
      lik.classVariables = ip.Results.classVariables;
  end

  % Initialize prior structure
  % Even if there is no prior at all
  % see line 2065 in the file gpla_e.m, and you may see the light !!!
  if init
      lik.p = [];
  end

  if init
      % Set the function handles to the subfunctions
      lik.fh.pak = @lik_liks_pak;
      lik.fh.unpak = @lik_liks_unpak;
      lik.fh.lp = @lik_liks_lp;
      lik.fh.lpg = @lik_liks_lpg;
      lik.fh.ll = @lik_liks_ll;
      lik.fh.llg = @lik_liks_llg;
      lik.fh.llg2 = @lik_liks_llg2;
      lik.fh.llg3 = @lik_liks_llg3;
      lik.fh.tiltedMoments = @lik_liks_tiltedMoments;
      lik.fh.siteDeriv = @lik_liks_siteDeriv;
      lik.fh.predy = @lik_liks_predy;
      lik.fh.invlink = @lik_liks_invlink;
      lik.fh.recappend = @lik_liks_recappend;
  end

end

function [w, s, h] = lik_liks_pak(lik)
% LIK_LIKS_PAK  Combines each likelihood parameters into one vector.
%
%  Description:
%    W = LIK_LIKS_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.
%
%   See also
%   LIK_LIKS_UNPAK, GP_PAK

 w = []; s = {}; h = [];

 for j = 1:lik.nliks
     lik_j = lik.liks{j};
     [wj, sj, hj] = lik_j.fh.pak(lik_j);
     w = [w wj];
     s = [s; sj];
     h = [h hj];

 end

end

function [lik, w] = lik_liks_unpak(lik, w)
% LIK_LIKS_UNPAK  Extracts each likelihood parameters from the vector.
%
%  Description:
%    [LIK, W] = LIK_LIKS_UNPAK(W, LIK) takes each likelihood
%    structure inside LIK and extracts the parameters from the vector W
%    to the whole LIK structure. This is a mandatory subfunction used
%    for example in energy and gradient computations.
%
%   See also:
%   LIK_LIKS_PAK, GP_UNPAK

% Assignment is inverse parameter transformation

 for j = 1:lik.nliks
     lik_j = lik.liks{j};
     [lik_j, w] = lik_j.fh.unpak(lik_j, w);
     lik.liks{j} = lik_j;

 end

end

function lp = lik_liks_lp(lik, varargin)
% LIK_LIKS_LP  log(prior) of each the likelihood parameters
%
%  Description:
%    LP = LIK_LIKS_LP(LIK) takes the 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_LIKS_LLG, LIK_LIKS_LLG2, LIK_LIKS_LLG3, GPLA_E

 % If there is prior for the likelihood parameters
 lp = 0;

 for j = 1:lik.nliks
     if ~isempty(lik.liks{j}.fh.pak(lik.liks{j}))
         lp = lp + lik.liks{j}.fh.lp(lik.liks{j});
     end
 end

end

function lpg = lik_liks_lpg(lik)
% LIK_LIKS_LPG  dlog(prior)/dth of each the likelihood parameters th
%
%  Description:
%    E = LIK_LIKS_LPG(LIK) takes a likelihood structure LIK and
%    returns d log(p(th))/dth for each likelihood function,
%    where th collects the parameters.
%    This subfunction is needed when there are likelihood parameters.
%
%  See also:
%    LIK_LIKS_LLG, LIK_LIKS_LG3, LIK_LIKS_LLG2, GPLA_G

 lpg = [];

 for j = 1:lik.nliks
     if ~isempty(lik.liks{j}.fh.pak(lik.liks{j}))
         lpg = [lpg lik.liks{j}.fh.lpg(lik.liks{j})];
     end
 end

end

function ll = lik_liks_ll(lik, y, ff, z)
% LIK_LIKS_LL log-likelihood
%
%  Description:
%    LL = LIK_LIKS_LL(LIK, Y, F) takes a likelihood structure LIK.
%    Returns the log-likelihood, sum_{i=1}^{k} (log p_i(y|f)).
%    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_LLG, LIK_LLG3, LIK_LLG2, GPLA_E

 n  = size(y, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj  = unique(zi);
 nind = numel(indj);

 if n ~= numel(zi)
     error('row-length of y and z are different')
 end

 f = ff(:);
 ll = 0;
 for j = 1:nind
     ind = zi==indj(j);
     likj = lik.liks{indj(j)};

     yj = y(ind);
     fj = f(ind);
     if isempty(z)
         zj = z;
     else
         zj = z(ind);
     end
     ll = ll + likj.fh.ll(likj, yj, fj, zj);

 end

end

function llg = lik_liks_llg(lik, y, ff, param, z)
% LIK_LIKS_LLG  Gradient of the log-likelihood
%
%  Description:
%    LLG = LIK_LIKS_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, Returns the gradient of the log likelihood
%    with respect to PARAM for each likelihood function. 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_LIKS_LL, LIK_LIKS_LLG2, LIK_LIKS_LLG3, GPLA_E

 n  = size(y, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj  = unique(zi);
 nind = numel(indj);

 if n ~= numel(zi)
     error('row-length of y and z are different')
 end

 f = ff(:);

 switch param
     case 'param'
         llg = [];
     case 'latent'
         llg=zeros(size(f));
 end
 for j = 1:nind
     ind = zi==indj(j);
     likj = lik.liks{indj(j)};

     yj = y(ind);
     fj = f(ind);
     if isempty(z)
         zj = z;
     else
         zj = z(ind);
     end

     switch param
         case 'param'
             if ~isempty(lik.liks{j}.fh.pak(likj))
                 llg = [llg likj.fh.llg(likj, yj, fj, param, zj)];
             end
         case 'latent'
             llg(ind) = likj.fh.llg(likj, yj, fj, param, zj);
     end
 end

end

function llg2 = lik_liks_llg2(lik, y, ff, param, z)
% LIK_LIKS_LLG2  Second gradients of the log-likelihood
%
%  Description
%    LLG2 = LIK_LIKS_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, 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_LIKS_LL, LIK_LIKS_LLG, LIK_LIKS_LLG3, GPLA_E

 n  = size(y, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj = unique(zi);
 nind = numel(indj);

 if n ~= numel(zi)
     error('row-length of y and z are different')
 end

 f = ff(:);

 nlikpar = length(lik.fh.pak(lik));
 z0 = zeros(n, nlikpar);
 aux(1) = 0;

 switch param
     case 'latent'
         llg2=zeros(size(f));
     case 'latent+param'
         llg2 = [];
 end
 for j = 1:nind
     ind = zi==indj(j);
     likj = lik.liks{indj(j)};

     yj = y(ind);
     fj = f(ind);
     if isempty(z)
         zj = z;
     else
         zj = z(ind);
     end

     switch param
         case 'param'

         case 'latent'
             llg2(ind) = likj.fh.llg2(likj, yj, fj, param, zj);

         case 'latent+param'
             if ~isempty(likj.fh.pak(likj))
                 % take the column vectors
                 llg2_tmp = likj.fh.llg2(likj, yj, fj, param, zj);

                 % auxiliar indexes
                 aux(end + 1) = aux(end) + size(likj.fh.pak(likj), 2);

                 % auxiliar matrices for derivatives w.r.t. parameters in
                 % the specific likelihood j
                 z0(ind, (aux(end - 1) + 1) : aux(end)) = llg2_tmp;
                 llg2 = [llg2 z0(:, aux(end - 1) + 1 : aux(end))];
                 z0(ind, (aux(end - 1) + 1) : aux(end)) = 0;

             end
     end

 end

end

function llg3 = lik_liks_llg3(lik, y, ff, param, z)
% LIK_LIKS_LLG3  Third gradients of the log likelihood
%
%  Description:
%    LLG3 = LIK_LIKS_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, 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_LIKS_LL, LIK_LIKS_LLG, LIK_LIKS_LLG2, GPLA_E, GPLA_G

 n  = size(y, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj  = unique(zi);
 nind = numel(indj);

 if n ~= numel(zi)
     error('row-length of y and z are different')
 end

 f = ff(:);

 switch param
     case 'latent'
         llg3=zeros(size(f));
     case 'latent2+param'
         llg3 = [];
 end
 % auxiliar matrix for derivatives of parameters w.r.t. many likelihoods
 nlikpar = length(lik.fh.pak(lik));
 z0 = zeros(n, nlikpar);
 aux(1) = 0;

 for j = 1:nind
     switch param
         case 'param'

         case 'latent'
             ind = zi==indj(j);
             likj = lik.liks{indj(j)};

             yj = y(ind);
             fj = f(ind);
             if isempty(z)
                 zj = z;
             else
                 zj = z(ind);
             end

             llg3(ind) = likj.fh.llg3(likj, yj, fj, param, zj);

         case 'latent2+param'
             if ~isempty(lik.liks{indj(j)}.fh.pak(lik.liks{indj(j)}))
                 % take indexes and respective observations for specific likelihood
                 ind = zi==indj(j);
                 likj = lik.liks{indj(j)};

                 yj = y(ind);
                 fj = f(ind);
                 if isempty(z)
                     zj = z;
                 else
                     zj = z(ind);
                 end

                 % take the column vectors
                 llg3_tmp = likj.fh.llg3(likj, yj, fj, param, zj);

                 % auxiliar indexes
                 aux(end + 1) = aux(end) + size(likj.fh.pak(likj), 2);

                 % auxiliar matrices for derivatives w.r.t parameters in
                 % the specific likelihood j
                 z0(ind, (aux(end - 1) + 1) : aux(end)) = llg3_tmp;
                 llg3 = [llg3 z0(:, aux(end - 1) + 1 : aux(end))];
                 z0(ind, (aux(end - 1) + 1) : aux(end)) = 0;

             end

     end

 end

end

function [logM_0, m_1, sigm2hati1] = lik_liks_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
% LIK_LIKS_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_LIKS_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY) takes a likelihood structure LIKS, the observation Y, index
%    I, 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

 n  = size(y, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj = unique(zi);
 nind = numel(indj);

 if n ~= numel(zi)
     error('row-length of y and z are different')
 end

 logM_0 = zeros(n, 1);
 m_1 = zeros(n, 1);
 sigm2hati1 = zeros(n, 1);

 for j = 1:nind
     likj = lik.liks{indj(j)};
     ind = zi==indj(j);
     if numel(sigm2_i)>1
         yj = y(ind);
         if isempty(z)
             zj = z;
         else
             zj = z(ind);
         end
         sigm2_ij = sigm2_i(ind);
         myy_ij = myy_i(ind);
         [logM_0(ind), m_1(ind), sigm2hati1(ind)] = ...
             likj.fh.tiltedMoments(likj, yj, 1:length(yj), sigm2_ij, myy_ij, zj);
     else
         if any(find(ind)==i1)
             [logM_0, m_1, sigm2hati1] = ...
                 likj.fh.tiltedMoments(likj, y, i1, sigm2_i, myy_i, z);
         end
     end

 end

end

function [g_i] = lik_liks_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
% LIK_LIKS_SITEDERIV  Evaluate the expectation of the gradient
%                 of the log likelihood term with respect
%                 to the multiple-likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_LIKS_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIKS, 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 is done for all likelihoods.
%    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

 n  = size(y, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj = unique(zi);
 nind = numel(indj);

 if n ~= numel(zi)
     error('row-length of y and z are different')
 end

 nlikpar = length(lik.fh.pak(lik));
 g_i = zeros(1, nlikpar);
 aux = 0;

 for j = 1:nind
     if ~isempty(lik.liks{indj(j)}.fh.pak(lik.liks{indj(j)}))
         % auxiliar indexes for paramters
         aux = aux + 1;

         % indexes for that specific likelihood
         ind = find(zi==indj(j));
         likj = lik.liks{indj(j)};

         if any(ind == i1)
             % !!!! if some specific likelihood has more than one parameter this will not work
             g_i(1, aux) = likj.fh.siteDeriv(likj, y, i1, sigm2_i, myy_i, z);
         end
     end

 end

end

function [lpy, Ey, Vary] = lik_liks_predy(lik, Ef, Varf, yt, zt)
% LIK_LIKS_PREDY  Returns the predictive mean, variance and density of y
%
%  Description:
%    LPY = LIK_LIKS_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 the observations YT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_LIKS_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_PRED1

% number of values to predict;
n = size(Ef, 1);
indClass = 1:size(zt,2)==lik.classVariables;
zi = zt(:, indClass);
zt  = zt(:, ~indClass);

% check some conditions
if ~issorted(zi)
    error('you need to give the class variable increasing downwards');
end

if max(zi) > lik.nliks
    error('more classes than the number of classes you are trying to model');
end

% getting the information of the classes in the data
indj = unique(zi);
nind = numel(indj);

% log-density
lpy = zeros(n, 1);
if nargout > 1
    Ey = zeros(n, 1);
    Vary = zeros(n, 1);

    for j = 1:nind
        ind = zi==indj(j);
        likj = lik.liks{indj(j)};

        if numel(yt) ~= 0
           [lpy(ind), Ey(ind), Vary(ind)] = ...
           likj.fh.predy(likj, Ef(ind), Varf(ind), yt(ind), zt(ind));
        else
           [~, Ey(ind), Vary(ind)] = likj.fh.predy(likj, Ef(ind), Varf(ind), yt, zt(ind));
        end
    end
else
    for j = 1:nind
        ind = zi==indj(j);
        likj = lik.liks{indj(j)};

        if isempty(zt)
            lpy(ind) = likj.fh.predy(likj, Ef(ind), Varf(ind), yt(ind), []);
        else
            lpy(ind) = likj.fh.predy(likj, Ef(ind), Varf(ind), yt(ind), zt(ind));
        end
    end
end

end

function mu = lik_liks_invlink(lik, f, z)
%LIK_LIKS_INVLINK  Returns values of inverse link function
%
%  Description
%    P = LIK_LIKS_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 gp_predprctmu.
%
%     See also
%     LIK_POISSON_LL, LIK_POISSON_PREDY

 n  = size(f, 1);
 indClass = 1:size(z,2)==lik.classVariables;
 zi = z(:, indClass);
 z  = z(:, ~indClass);

 indj = unique(zi);
 nind = numel(indj);

 if n ~= size(zi,1)
     error('row-length of f and z are different')
 end

 for j = 1:nind
     likj = lik.liks{indj(j)};
     ind = zi==indj(j);
     fj = f(ind,:);
     if isempty(z)
         zj = z;
     else
         zj = z(ind);
     end

     mu(ind,:) = likj.fh.invlink(likj, fj, zj);
 end

end

function reclik = lik_liks_recappend(reclik, ri, lik)
% RECAPPEND Append the parameters to the record
%
%  Description:
%    RECLIK = LIK_LIKS_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 = 'Liks';

     % Initialize parameters
     nliks = length(ri.liks);
     for i = 1:nliks
         lik_i = ri.liks{i};
         reclik.liks{i} = lik_i.fh.recappend([], ri.liks{i});
     end

     % Set the function handles
     reclik.fh.pak = @lik_liks_pak;
     reclik.fh.unpak = @lik_liks_unpak;
     reclik.fh.lp = @lik_liks_lp;
     reclik.fh.lpg = @lik_liks_lpg;
     reclik.fh.ll = @lik_liks_ll;
     reclik.fh.llg = @lik_liks_llg;
     reclik.fh.llg2 = @lik_liks_llg2;
     reclik.fh.llg3 = @lik_liks_llg3;
     reclik.fh.tiltedMoments = @lik_liks_tiltedMoments;
     reclik.fh.siteDeriv = @lik_liks_siteDeriv;
     reclik.fh.predy = @lik_liks_predy;
     reclik.fh.recappend = @lik_liks_recappend;

     if isfield(ri, 'classVariables')
         reclik.classVariables = ri.classVariables;
         reclik.nliks = ri.nliks;
     end

 else
     % Append to the record
     % Loop over all of the likelihood functions
     nliks = length(lik.liks);
     for i = 1:nliks;
         lik_i = lik.liks{i};
         reclik.liks{i} = lik_i.fh.recappend(reclik.liks{i}, ri, lik_i);
     end

 end
end


function [powerterms_MR,powerterms_RZF,powerterms_MMMSE] = functionComputeULPowerLevels_impairments(H,Hhat,C,nbrOfRealizations,M,K,L,p,f,kappatUE,kapparBS)
%Compute and categorize the UL signal power for different receive combining
%schemes, under hardware impairments.
%
%INPUT:
%H                 = M x nbrOfRealizations x K x L x L matrix with the
%                    exact channel realizations
%Hhat              = M x nbrOfRealizations x K x L x L matrix with the MMSE
%                    channel estimates
%C                 = M x M x K x L x L matrix with estimation error
%                    correlation matrices when using MMSE estimation.
%nbrOfRealizations = Number of channel realizations
%M                 = Number of antennas per BS
%K                 = Number of UEs per cell
%L                 = Number of BSs and cells
%p                 = Uplink transmit power per UE (same for everyone)
%f                 = Pilot reuse factor
%kappatUE          = Hardware quality of the UEs' transmitters
%kapparBS          = Hardware quality of the BSs' receivers
%
%OUTPUT:
%powerterms_MR    = 6 x K x L matrix with signal and interference powers
%                   for each of the UEs with MR combining.
%                   powerterms_MR(:,k,l) is the vector for UE k in cell l,
%                   where the first element is the average desired signal
%                   power, the second element is the interference from UEs
%                   having the same pilot, the third term is the
%                   interference from UEs having different pilots, the
%                   fourth term is the transmitter distortion from other
%                   UEs, the fifth term is the receiver distortion from all
%                   UEs, and the last term is the self-distortion/interference
%powerterms_RZF   = Same as powerterms_MR but with RZF combining
%powerterms_MMMSE = Same as powerterms_MR but with M-MMSE combining
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Store identity matrices of different sizes
eyeK = eye(K);
eyeM = eye(M);

%Generate pilot pattern
if f == 1

    pilotPattern = ones(L,1);

elseif f == 2 %Only works in the running example with its 16 BSs

    pilotPattern = kron(ones(2,1),[1; 2; 1; 2; 2; 1; 2; 1]);

elseif f == 4 %Only works in the running example with its 16 BSs

    pilotPattern = kron(ones(2,1),[1; 2; 1; 2; 3; 4; 3; 4]);

elseif f == 16 %Only works in the running example with its 16 BSs

    pilotPattern = (1:L)';

end

%Compute sum of all estimation error correlation matrices at every BS
C_totM = reshape(p*sum(sum(C,3),4),[M M L]);

%Prepare to store simulation results for the signal gains
signal_MR = zeros(K,L);
signal_RZF = zeros(K,L);
signal_MMMSE = zeros(K,L);

%Prepare to store simulation results for the norms of combining vectors
vectornorm_MR = zeros(K,L);
vectornorm_RZF = zeros(K,L);
vectornorm_MMMSE = zeros(K,L);

%Prepare to store simulation results for the sum of interference powers
interference_MR = zeros(K,L);
interference_RZF = zeros(K,L);
interference_MMMSE = zeros(K,L);

%Prepare to store simulation results for the sum of interference powers
%from UEs having the same pilot signals, including the user itself
reuseinterf_MR = zeros(K,L);
reuseinterf_RZF = zeros(K,L);
reuseinterf_MMMSE = zeros(K,L);

%Prepare to store simulation results for the self-interference power
selfinterf_MR = zeros(K,L);
selfinterf_RZF = zeros(K,L);
selfinterf_MMMSE = zeros(K,L);

%Prepare to store simulation results for the sum of receiver distortion
receiverdist_MR = zeros(K,L);
receiverdist_RZF = zeros(K,L);
receiverdist_MMMSE = zeros(K,L);

%% Go through all channel realizations
for n = 1:nbrOfRealizations

    %Go through all cells
    for j = 1:L

        %Extract channel realizations from all UEs to BS j
        Hallj = reshape(H(:,n,:,:,j),[M K*L]);

        %Extract channel estimate realizations from all UEs to BS j
        Hhatallj = reshape(Hhat(:,n,:,:,j),[M K*L]);

        %Extract cells that use same pilots as cell j
        groupMembers = find(pilotPattern==pilotPattern(j))';

        %Compute three different combining schemes
        V_MR = Hhatallj(:,K*(j-1)+1:K*j);
        V_RZF = p*V_MR/(p*(V_MR'*V_MR)+eyeK);
        V_MMMSE = p*(p*(Hhatallj*Hhatallj')+C_totM(:,:,j)+eyeM)\V_MR;

        %Go through all UEs in cell j
        for k = 1:K

            %Extract channel realizations from UEs that cause pilot
            %contamination to UE k in cell j
            HjkPC = reshape(H(:,n,k,groupMembers,j),[M length(groupMembers)]);

            %Extract channel from BS j to its k:th UE
            Hjk = H(:,n,k,j,j);

            %%MR combining
            w = V_MR(:,k)/norm(V_MR(:,k))^2; %Extract combining vector
            wrep = repmat(w,[1 K*L]);

            %Use Monte Carlo simulations to compute the expectations that
            %appear in the six different components in Section 6.3.3
            signal_MR(k,j) = signal_MR(k,j) + (w'*Hjk)/nbrOfRealizations;
            vectornorm_MR(k,j) = vectornorm_MR(k,j) + norm(w).^2/nbrOfRealizations;
            interference_MR(k,j) = interference_MR(k,j) + p*sum(abs(w'*Hallj).^2)/nbrOfRealizations;
            receiverdist_MR(k,j) = receiverdist_MR(k,j) + p*sum(sum(abs(wrep.*Hallj).^2,1))/nbrOfRealizations;
            reuseinterf_MR(k,j) = reuseinterf_MR(k,j) + p*sum(abs(w'*HjkPC).^2)/nbrOfRealizations;
            selfinterf_MR(k,j) = selfinterf_MR(k,j) + p*sum(abs(w'*Hjk).^2)/nbrOfRealizations;

            %%RZF combining
            w = V_RZF(:,k); %Extract combining vector
            wrep = repmat(w,[1 K*L]);

            %Use Monte Carlo simulations to compute the expectations that
            %appear in the six different components in Section 6.3.3
            signal_RZF(k,j) = signal_RZF(k,j) + (w'*Hjk)/nbrOfRealizations;
            vectornorm_RZF(k,j) = vectornorm_RZF(k,j) + norm(w).^2/nbrOfRealizations;
            interference_RZF(k,j) = interference_RZF(k,j) + p*sum(abs(w'*Hallj).^2)/nbrOfRealizations;
            receiverdist_RZF(k,j) = receiverdist_RZF(k,j) + p*sum(sum(abs(wrep.*Hallj).^2,1))/nbrOfRealizations;
            reuseinterf_RZF(k,j) = reuseinterf_RZF(k,j) + p*sum(abs(w'*HjkPC).^2)/nbrOfRealizations;
            selfinterf_RZF(k,j) = selfinterf_RZF(k,j) + p*sum(abs(w'*Hjk).^2)/nbrOfRealizations;

            %%MMMSE combining
            w = V_MMMSE(:,k); %Extract combining vector
            wrep = repmat(w,[1 K*L]);

            %Use Monte Carlo simulations to compute the expectations that
            %appear in the six different components in Section 6.3.3
            signal_MMMSE(k,j) = signal_MMMSE(k,j) + (w'*Hjk)/nbrOfRealizations;
            vectornorm_MMMSE(k,j) = vectornorm_MMMSE(k,j) + norm(w).^2/nbrOfRealizations;
            interference_MMMSE(k,j) = interference_MMMSE(k,j) + p*sum(abs(w'*Hallj).^2)/nbrOfRealizations;
            receiverdist_MMMSE(k,j) = receiverdist_MMMSE(k,j) + p*sum(sum(abs(wrep.*Hallj).^2,1))/nbrOfRealizations;
            reuseinterf_MMMSE(k,j) = reuseinterf_MMMSE(k,j) + p*sum(abs(w'*HjkPC).^2)/nbrOfRealizations;
            selfinterf_MMMSE(k,j) = selfinterf_MMMSE(k,j) + p*sum(abs(w'*Hjk).^2)/nbrOfRealizations;

        end

    end

end

%Set the normalized noise variance
sigma2 = 1;

%Compute the desired signal term, as described in Section 6.3.3
signalpower_MR = kappatUE*kapparBS*mean(mean(p*abs(signal_MR).^2./vectornorm_MR))/sigma2;
signalpower_RZF = kappatUE*kapparBS*mean(mean(p*abs(signal_RZF).^2./vectornorm_RZF))/sigma2;
signalpower_MMMSE = kappatUE*kapparBS*mean(mean(p*abs(signal_MMMSE).^2./vectornorm_MMMSE))/sigma2;

%Compute the interference from UEs having the same pilot, as described in
%Section 6.3.3
reuseinterference_MR = kappatUE*kapparBS*mean(mean((reuseinterf_MR-selfinterf_MR)./vectornorm_MR))/sigma2;
reuseinterference_RZF = kappatUE*kapparBS*mean(mean((reuseinterf_RZF-selfinterf_RZF)./vectornorm_RZF))/sigma2;
reuseinterference_MMMSE = kappatUE*kapparBS*mean(mean((reuseinterf_MMMSE-selfinterf_MMMSE)./vectornorm_MMMSE))/sigma2;

%Compute the interference from UEs having different pilots, as described in
%Section 6.3.3
noreuseinterference_MR = kappatUE*kapparBS*mean(mean((interference_MR-reuseinterf_MR)./vectornorm_MR))/sigma2;
noreuseinterference_RZF = kappatUE*kapparBS*mean(mean((interference_RZF-reuseinterf_RZF)./vectornorm_RZF))/sigma2;
noreuseinterference_MMMSE = kappatUE*kapparBS*mean(mean((interference_MMMSE-reuseinterf_MMMSE)./vectornorm_MMMSE))/sigma2;

%Compute the transmitter distortion from other UEs, as described in
%Section 6.3.3
transmitDistortion_MR = (1-kappatUE)*kapparBS*mean(mean((interference_MR-selfinterf_MR)./vectornorm_MR))/sigma2;
transmitDistortion_RZF = (1-kappatUE)*kapparBS*mean(mean((interference_RZF-selfinterf_RZF)./vectornorm_RZF))/sigma2;
transmitDistortion_MMMSE = (1-kappatUE)*kapparBS*mean(mean((interference_MMMSE-selfinterf_MMMSE)./vectornorm_MMMSE))/sigma2;

%Compute the receiver distortion from all UEs, as described in
%Section 6.3.3
receiverDistortion_MR = (1-kapparBS)*mean(mean(receiverdist_MR./vectornorm_MR))/sigma2;
receiverDistortion_RZF = (1-kapparBS)*mean(mean(receiverdist_RZF./vectornorm_RZF))/sigma2;
receiverDistortion_MMMSE = (1-kapparBS)*mean(mean(receiverdist_MMMSE./vectornorm_MMMSE))/sigma2;

%Compute the self-distortion and self-interference, as described in
%Section 6.3.3
selfinterference_MR = kapparBS*mean(mean((selfinterf_MR-kappatUE*p*abs(signal_MR).^2)./vectornorm_MR))/sigma2;
selfinterference_RZF = kapparBS*mean(mean((selfinterf_RZF-kappatUE*p*abs(signal_RZF).^2)./vectornorm_RZF))/sigma2;
selfinterference_MMMSE = kapparBS*mean(mean((selfinterf_MMMSE-kappatUE*p*abs(signal_MMMSE).^2)./vectornorm_MMMSE))/sigma2;

%Prepare to output the power values
powerterms_MR = [signalpower_MR reuseinterference_MR noreuseinterference_MR transmitDistortion_MR receiverDistortion_MR selfinterference_MR];
powerterms_RZF = [signalpower_RZF reuseinterference_RZF noreuseinterference_RZF transmitDistortion_RZF receiverDistortion_RZF selfinterference_RZF];
powerterms_MMMSE = [signalpower_MMMSE reuseinterference_MMMSE noreuseinterference_MMMSE transmitDistortion_MMMSE receiverDistortion_MMMSE selfinterference_MMMSE];


%% Face and Eyes Detection
%
% In this demo, we will learn the basics of face detection using Haar
% Feature-based Cascade Classifiers, and how the same extends for eye
% detection, etc.
%
% This program demonstrates the use of |cv.CascadeClassifier| class to detect
% objects (face + eyes). You can use Haar or LBP features. This classifier can
% detect many kinds of rigid objects, once the appropriate classifier is
% trained. It's most known use is for faces.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/facedetect.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/tapi/ufacedetect.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/gpu/cascadeclassifier.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/facedetect.py>
% * <https://docs.opencv.org/3.2.0/db/d28/tutorial_cascade_classifier.html>
% * <https://docs.opencv.org/3.2.0/d7/d8b/tutorial_py_face_detection.html>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/objectDetection/objectDetection.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/objectDetection/objectDetection2.cpp>
%

%% Theory
%
% Object Detection using Haar feature-based cascade classifiers is an
% effective object detection method proposed by Paul Viola and Michael Jones
% in their paper, "Rapid Object Detection using a Boosted Cascade of Simple
% Features" in 2001. It is a machine learning based approach where a cascade
% function is trained from a lot of positive and negative images. It is then
% used to detect objects in other images.
%
% Here we will work with face detection. Initially, the algorithm needs a lot
% of positive images (images of faces) and negative images (images without
% faces) to train the classifier. Then we need to extract features from it.
% For this, haar features shown in the below image are used. They are just like
% our convolutional kernel. Each feature is a single value obtained by
% subtracting sum of pixels under the white rectangle from sum of pixels under
% the black rectangle.
%
% <<https://docs.opencv.org/3.4.0/haar_features.jpg>>
%
% Now all possible sizes and locations of each kernel are used to calculate
% lots of features. (Just imagine how much computation it needs? Even a
% 24x24 window results over 160000 features). For each feature calculation, we
% need to find the sum of the pixels under white and black rectangles. To
% solve this, they introduced the integral image. However large your image, it
% reduces the calculations for a given pixels, to an operation involving just
% four pixels. Nice, isn't it? It makes things super-fast.
%
% But among all these features we calculated, most of them are irrelevant. For
% example, consider the image below. The top row shows two good features. The
% first feature selected seems to focus on the property that the region of the
% eyes is often darker than the region of the nose and cheeks. The second
% feature selected relies on the property that the eyes are darker than the
% bridge of the nose. But the same windows applied to cheeks or any other
% place is irrelevant. So how do we select the best features out of 160000+
% features? It is achieved by *Adaboost*.
%
% <<https://docs.opencv.org/3.4.0/haar.png>>
%
% For this, we apply each and every feature on all the training images. For
% each feature, it finds the best threshold which will classify the faces to
% positive and negative. Obviously, there will be errors or misclassifications.
% We select the features with minimum error rate, which means they are the
% features that most accurately classify the face and non-face images. (The
% process is not as simple as this. Each image is given an equal weight in the
% beginning. After each classification, weights of misclassified images are
% increased. Then the same process is done. New error rates are calculated.
% Also new weights. The process is continued until the required accuracy or
% error rate is achieved or the required number of features are found).
%
% The final classifier is a weighted sum of these weak classifiers. It is
% called weak because it alone can't classify the image, but together with
% others forms a strong classifier. The paper says even 200 features provide
% detection with 95% accuracy. Their final setup had around 6000 features.
% (Imagine a reduction from 160000+ features to 6000 features. That is a big
% gain).
%
% So now you take an image. Take each 24x24 window. Apply 6000 features to it.
% Check if it is face or not. Wow.. Isn't it a little inefficient and time
% consuming? Yes, it is. The authors have a good solution for that.
%
% In an image, most of the image is non-face region. So it is a better
% idea to have a simple method to check if a window is not a face region. If
% it is not, discard it in a single shot, and don't process it again. Instead,
% focus on regions where there can be a face. This way, we spend more time
% checking a possible face region.
%
% For this they introduced the concept of *Cascade of Classifiers*. Instead of
% applying all 6000 features on a window, the features are grouped into
% different stages of classifiers and applied one-by-one. (Normally the first
% few stages will contain very many fewer features). If a window fails the
% first stage, discard it. We don't consider the remaining features on it. If
% it passes, apply the second stage of features and continue the process. The
% window which passes all stages is a face region. How is that plan!
%
% The authors' detector had 6000+ features with 38 stages with 1, 10, 25, 25
% and 50 features in the first five stages. (The two features in the above
% image are actually obtained as the best two features from Adaboost).
% According to the authors, on average, 10 features out of 6000+ are evaluated
% per sub-window.
%
% So this is a simple intuitive explanation of how Viola-Jones face detection
% works. Read the paper for more details or check out the following references:
%
% * Video Lecture on
%   <http://www.youtube.com/watch?v=WfdYYNamHZ8 Face Detection and Tracking>
% * An interesting interview regarding Face Detection by
%   <http://www.makematics.com/research/viola-jones/ Adam Harvey>
%
% OpenCV comes with a trainer as well as detector. If you want to train your
% own classifier for any object like car, planes etc. you can use OpenCV to
% create one. See the OpenCV docs for full details on Cascade Classifier
% Training.
%
% Here we will deal with detection. OpenCV already contains many pre-trained
% classifiers for face, eyes, smiles, etc. Those XML files are stored in
% the |opencv/data/haarcascades/| folder.
%

%% Code
%
% In this example, we will create a face and eyes detector with OpenCV:
%
% * First we need to load the required XML classifiers.
% * Then load our input image (or video) in grayscale mode.
% * Now we find the faces in the image. If faces are found, it returns the
%   positions of each detected faces as a rectangle |[x,y,w,h]|. Once we get
%   these locations, we can create a ROI for the face and apply eye detection
%   on this ROI (since eyes are always on the face!).
%

%% Options

% this is the primary trained classifier such as frontal face
cascadeName = fullfile(mexopencv.root(),'test','haarcascade_frontalface_alt.xml');
% this an optional secondary classifier such as eyes
nestedCascadeName = fullfile(mexopencv.root(),'test','haarcascade_eye_tree_eyeglasses.xml');
% image scale greater or equal to 1, try 1.3 for example
scale = 1.0;
% attempts detection of flipped image as well
tryflip = false;

%% Initialization

% download XML files if missing
download_classifier_xml(cascadeName);
download_classifier_xml(nestedCascadeName);

% load cacade classifiers
cascade = cv.CascadeClassifier(cascadeName);
assert(~cascade.empty(), 'Could not load classifier cascade');
nestedCascade = cv.CascadeClassifier();
if ~nestedCascade.load(nestedCascadeName)
    disp('Could not load classifier cascade for nested objects');
end
scale = max(scale, 1.0);

%% Main loop
% (either video feed or a still image)
if false
    % read an image
    frame = cv.imread(fullfile(mexopencv.root(),'test','lena.jpg'), 'Color',true);
    % detect faces/eyes and draw detections
    frame = detectAndDraw(frame, cascade, nestedCascade, scale, tryflip);
    imshow(frame);
else
    % prepare video input
    cap = cv.VideoCapture();
    pause(1);
    assert(cap.isOpened());

    % prepare figure
    frame = cap.read();
    assert(~isempty(frame));
    hImg = imshow(frame);

    % video feed
    while ishghandle(hImg)
        % read frame
        frame = cap.read();
        if isempty(frame), break; end

        % detect faces/eyes and draw detections
        frame = detectAndDraw(frame, cascade, nestedCascade, scale, tryflip);

        % update
        set(hImg, 'CData',frame);
        drawnow;
    end
    cap.release();
end

%% Processing function

function img = detectAndDraw(img, cascadeF, cascadeE, scale, tryflip)
    % downscale image and preprocess it
    fx = 1/scale;
    gray = cv.cvtColor(img, 'RGB2GRAY');
    gray = cv.resize(gray, fx, fx);
    gray = cv.equalizeHist(gray);
    [h,w] = size(gray);

    % detection options
    detectOpts = {
        'ScaleFactor',1.1, ...
        'MinNeighbors',2, ...
        ... 'FindBiggestObject',true, ...
        ... 'DoRoughSearch',true, ...
        'ScaleImage',true, ...
        'MinSize',[30 30]
    };

    % detect faces
    tic
    faces = cascadeF.detect(gray, detectOpts{:});
    if tryflip
        faces2 = cascadeF.detect(cv.flip(gray, 1), detectOpts{:});
        faces2 = cellfun(@(r) [w-r(1)-r(3) r(2:4)], faces2, 'UniformOutput',false);
        faces = [faces(:); faces2(:)];
    end
    toc

    % draw
    clrs = uint8(255 * lines(7));
    for i=1:numel(faces)
        r = faces{i};
        ii = mod(i-1, size(clrs,1)) + 1;
        drawOpts = {'Color',clrs(ii,:), 'Thickness',3};

        % draw faces
        aspect_ratio = r(3)/r(4);
        if 0.75 < aspect_ratio && aspect_ratio < 1.3
            center = round((r(1:2) + r(3:4)*0.5) * scale);
            radius = round((r(3) + r(4)) * 0.25*scale);
            img = cv.circle(img, center, radius, drawOpts{:});
        else
            pt1 = round(r(1:2) * scale);
            pt2 = round((r(1:2) + r(3:4) - 1) * scale);
            img = cv.rectangle(img, pt1, pt2, drawOpts{:});
        end

        if ~cascadeE.empty()
            % detect nested objects (eyes)
            if false && mexopencv.require('images')
                grayROI = imcrop(gray, [r(1:2)+1 r(3:4)]);
            else
                grayROI = cv.Rect.crop(gray, r);
            end
            nestedObjs = cascadeE.detect(grayROI, detectOpts{:});

            % draw eyes
            for j=1:numel(nestedObjs)
                nr = nestedObjs{j};
                center = round((r(1:2) + nr(1:2) + nr(3:4)*0.5) * scale);
                radius = round((nr(3) + nr(4)) * 0.25*scale);
                img = cv.circle(img, center, radius, drawOpts{:});
            end
        end
    end
end

%% Helper function

function download_classifier_xml(fname)
    if exist(fname, 'file') ~= 2
        % attempt to download trained Haar/LBP/HOG classifier from Github
        url = 'https://cdn.rawgit.com/opencv/opencv/3.4.0/data/';
        [~, f, ext] = fileparts(fname);
        if strncmpi(f, 'haarcascade_', length('haarcascade_'))
            url = [url, 'haarcascades/'];
        elseif strncmpi(f, 'lbpcascade_', length('lbpcascade_'))
            url = [url, 'lbpcascades/'];
        elseif strncmpi(f, 'hogcascade_', length('hogcascade_'))
            url = [url, 'hogcascades/'];
        else
            error('File not found');
        end
        fprintf('Downloading cascade classifier "%s"...\n', [f ext]);
        url = [url f ext];
        urlwrite(url, fname);
    end
end


function out = feval(N, varargin)
%FEVAL   Evaluate the operator of the CHEBOP at a CHEBFUN or CHEBMATRIX.
%   OUT = FEVAL(N, U) for a CHEBFUN or CHEBMATRIX U applies the CHEBOP N to U,
%   i.e., it returns N(U). Here, N.OP should be of the form @(u) diff(u,2) + ...
%   If N.op is of the form @(x, u) diff(u,2) + ... then an x variable is
%   instantiated internally and included automatically, however, this is not
%   the preferred syntax and may not be supported in future releases.
%
%   OUT = FEVAL(N, X, U) for the CHEBFUN X and CHEBFUN or CHEBMATRIX U applies
%   the CHEBOP N to X and  U, i.e., it returns N(X, U) where N.OP has the form
%   @(x, u) diff(u,2) + .... Here, X should be the dependent variable on
%   N.DOMAIN.
%
%   OUT = FEVAL(N, X, U1, U2, ..., UM) for a CHEBFUN X and CHEBFUN or CHEMBATRIX
%   objects U1, ..., UM applies the CHEBOP N to the functions Uk; i.e., it
%   returns N(X, U1, U2, ..., UM) where N.OP has the form @(x, u1, u2, ..., um).
%   Note that for systems of equations, X _must_ be included in N.OP.
%
%   OUT = FEVAL(N, X, U) where U is a CHEBMATRIX of M entries and N.OP has the
%   form @(X, U1, U2, ..., UM) is equivalent FEVAL(N, X, U{1}, ..., U{M}).
%   Again, OUT = FEVAL(N, U) will also work in this situation, but this is not
%   the preferred syntax.
%
%   OUT = FEVAL(N, DIM) returns an DIM-point discretization of the linear
%   operator N. If N is not linear an error is thrown. OUT = FEVAL(N, DIM,
%   'oldschool') uses boundary bordering to deal wiuth the boundary conditions,
%   rather than the rectangular projection approach of hale and Driscoll. Note
%   that this syntax exists only the support ATAP and doesn't necessarily give a
%   clear picture of the discretizations now being used in the Chebfun release.
%   CHEBOP/MATRIX, which is the preferred syntax for this functionality,
%   provides further details
%
% See also CHEBOP/SUBSREF, LINOP/MTIMES, CHEBOP/MATRIX.

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

% Support for calling a linear CHEBOP with a numerical input to get its
% discretization. This is deprecated
if ( (nargin == 2) && isnumeric(varargin{1}) )
    warning('CHEBFUN:CHEBOP:feval:deprecated', ...
        ['FEVAL(N, DIM) or N(DIM) exists only to provide backwards\n', ...
        'compatibility with ATAP. The preferred method for visualizing a\n', ...
        'discretization of a linear CHEBOP is MATRIX(N, DIM). Note, however,\n', ...
        'that these may not give the same result due to changes in how\n', ...
        'CHEBOP discretizes differential operators.'])
    warning('off', 'CHEBFUN:CHEBOP:feval:deprecated');

    % We cannot support boundary conditions in this way.
    throwBCwarning = false;
    if ( ~isempty(N.lbc) )
        N.lbc = [];
        throwBCwarning = true;
    end
    if ( ~isempty(N.rbc) )
        N.rbc = [];
        throwBCwarning = true;
    end
    if ( ~isempty(N.bc) )
        N.bc = [];
        throwBCwarning = true;
    end
    if ( throwBCwarning )
        warning('CHEBFUN:CHEBOP:feval:BCs', ...
            'Boundary conditions are not supported in FEVAL(N, DIM).')
    end

    % Because the native V5 matrix is rectangular, we have to do some resizing
    % to recapture V4 behavior. Note: this may break for systems, piecewise
    % cases.
    n = varargin{1};
    L = linop(N);
    pref = cheboppref();
    disc = pref.discretization;
    if ( isa(disc, 'char') && strcmpi(disc, 'values') )
        pref.discretization = @chebcolloc2;
    elseif ( isa(disc, 'function_handle') && ~isequal(disc, @chebcolloc2) )
        % We only support CHEBCOLLOC2 discretizations!
        error('CHEBFUN:CHEBOP:feval:notColloc2', ...
            ['FEVAL(N, DIM) only supports CHEBCOLLOC2 discretizations.\n', ...
             'Use MATRIX(N, DIM) or change the discretization in CHEBOPPREF.']);
    end
    A = matrix(L, n, pref); % has n rows but maybe more columns

    % We want an n by n result. We have that A is (n-d) x n where d =
    % L.diffOrder. Also, the output of A is at 1st kind points. We need to do:
    %  (map from n 1st kind to n 2nd kind) * A * (map from n 2nd kind to
    %  n+d 2nd kind).

    % Note that we don't have to translate/scale to the domain for barymat
    % matrices that go between grids.
    d = L.diffOrder;
    [x1,~,w1] = chebtech1.chebpts( n );
    x2 = chebtech2.chebpts( n );
    x3 = chebtech2.chebpts( n + d );

    out = barymat(x2, x1, w1) * A * barymat(x3, x2);
    return

elseif ((nargin == 3) && ischar(varargin{2}) ...
        && strcmpi(varargin{2}, 'oldschool'))

    % TODO: Is this still correct?
    out = matrix(N, varargin{:});
    return
end

% We must expand CHEBMATRIX entries out to a cell for {:} to work below.
isChebMat = cellfun(@(u) isa(u, 'chebmatrix'), varargin);
if ( any(isChebMat) )
    args = {};
    for k = 1:numel(varargin)
        % Append variables from the kth input:
        if ( isChebMat(k) )
            args = [args , varargin{k}.blocks.']; %#ok<AGROW>
        else
            args = [args , varargin(k)];          %#ok<AGROW>
        end
    end
else
    args = varargin;
end

% How many input arguments are there to N.op?
numberOfInputs = nargin(N);

% If no arguments, return empty:
if ( numberOfInputs == 0 )
    out = [];
    return
end

if ( numberOfInputs == 1)
    % If N has one input arguments, either we have a scalar problem, or the
    % problem is specified on chebmatrix format, e.g.,
    %   N.op = @(u) [ diff(u{1},2) + u{2}; u{1} + diff(u{2}];
    % Here, importantly, x does not appear in the argument list for N.op.
    u = varargin{1};

    % If we have a scalar problem, but U is still a CHEBMATRIX, we need to
    % extract the BLOCK of U in order to be evaluate N. If on the other hand, U
    % has more than one block, but NUMBEROFINPUTS is still equal to 1 (which got
    % us here in the first place), we must be dealing with a CHEBOP N, whose OP
    % is specified on CHEBMATRIX format, e.g.
    %   N.op = @(u) [diff(u{1}) + u{2}; u{1} + diff(u{2})];
    if ( isa(u, 'chebmatrix') && max(size(u)) == 1 )
        u = u.blocks{1};
    end

    out = N.op(u);

elseif ( numberOfInputs == 2 )
    % If N has two input arguments, either we have a scalar problem, or the
    % problem is specified on chebmatrix format, e.g.,
    %   N.op = @(x, u) [ diff(u{1},2) + u{2}; u{1} + diff(u{2}];
    % Here, importantly, x must appear in the argument list for N.op.

    % Did we not get the x variable passed in as argument?
    if ( numel(varargin) == 1 )
        u = varargin{1};

        % Construct the independent variable X.
        x = chebfun(@(x) x, N.domain);
        % If the CHEBFUN U passed in is a quasimatrix, we need to tile the
        % independent variable X to have the same dimensions as U:
        x = repmat(x, 1, size(u,2));

    elseif ( numel(varargin) == numberOfInputs )
        % Got passed both X and U.
        x = varargin{1};
        u = varargin{2};

    else
        error('CHEBFUN:CHEBOP:feval:numInputs', ...
            'Unexpected number of input arguments.')
    end

    % If we have a scalar problem, but U is still a CHEBMATRIX, we need to
    % extract the BLOCK of U in order to be evaluate N. If on the other
    % hand, U has more than one block, but NUMBEROFINPUTS is still less than
    % or equal to 2 (which got us here in the first place), we must be
    % dealing with a CHEBOP N, whose OP is specified on CHEBMATRIX format,
    % e.g.,
    %   N.op = @(x,u) [diff(u{1}) + u{2}; u{1} + diff(u{2})];
    if ( isa(u, 'chebmatrix') && max(size(u)) == 1 )
        u = u.blocks{1};
    end

    % Evaluate the operator!
    out = N.op(x, u);

else
    % The operator is specified on the form
    %   N.op = @(x, u, v) = [diff(u,2) + v; u + diff(v)]

    % Count the number of RHSs:
    numCols = max(max(cellfun(@(v) size(v, 2), varargin)));

    % We must expand CHEBMATRIX entries out to a cell for {:} to work below.
    isChebMat = cellfun(@(u) isa(u, 'chebmatrix'), varargin);
    if ( any(isChebMat) )
        args = {};
        for k = 1:numel(varargin)
            % Append variables from the kth input:
            if ( isChebMat(k) )
                args = [args , varargin{k}.blocks.']; %#ok<AGROW>
            else
                args = [args , varargin(k)];          %#ok<AGROW>
            end
        end
        % ARGS need to be vertically concatenated for operator to be evaluated
        % correctly.
        args = args.';
    else
        % ARGS need to be vertically concatenated for operator to be evaluated
        % correctly.
        args = varargin.';
    end

    if ( numCols > 1 )
        % Deal with multiple RHS
        out = cell(1, numCols);
        for k = 1:numCols
            out{k} = doEval(N, args(:,k));
        end
        out = horzcat(out{:});
    else
        out = doEval(N, args);
    end

end

% If the operator is written in a function file or nested function, then it
% is natural for it to be returned in cell form. Convert it to a chebmatrix:
if ( iscell(out) )
    out = vertcat(out{:}); % TODO: is this correct for multiple RHSs?
end

end

function out = doEval(N, args)

    numberOfInputs = nargin(N);
    if ( numel(args) == numberOfInputs - 1 )
        % Check if we need to include an x (independent variable):
        x = chebfun(@(x) x, N.domain);
        args = [ {x} ; args ];
    end
    % Evaluate the operator:
    out = N.op(args{:});

end


function chopDoseOutsideStruct(doseNum,structureNum)
%function chopDoseOutsideStruct(doseNum,structureNum)
%
%This function creates a new dose which is same as doseNum, but which lies
%within structureNum.
%
%Usage: planC = chopDoseOutsideStruct(1,6)
%
%APA, 04/01/2010
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

global stateS planC
indexS = planC{end};

%Get associated scan number
scanNum = getStructureAssociatedScan(structureNum);
assocScanUID = planC{indexS.dose}(doseNum).assocScanUID;
scanNumDose = getAssociatedScan(assocScanUID);
tmDose = getTransM('dose',doseNum,planC);
if isempty(tmDose)
    tmDose = eye(4);
end
tmScan = getTransM('scan',scanNum,planC);
if isempty(tmScan)
    tmScan = eye(4);
end
if isempty(scanNumDose) && ~isequal(tmDose,tmScan)
    error('This function currently supports dose and structure with same transformation matrix')
end

%Get dose grid
[xDoseVals, yDoseVals, zDoseVals] = getDoseXYZVals(planC{indexS.dose}(doseNum));

%Get Uniformized structure and grid
[xUnifVals, yUnifVals, zUnifVals] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));
structureMask3M = getUniformStr(structureNum);

[xDoseValsM, yDoseValsM, zDoseValsM] = meshgrid(xDoseVals, yDoseVals, zDoseVals);
xDoseValsV = xDoseValsM(:);
yDoseValsV = yDoseValsM(:);
zDoseValsV = zDoseValsM(:);

%Interpolate structure on to dose grid
structureOnDoseV = interp3(xUnifVals, yUnifVals, zUnifVals, single(structureMask3M), xDoseValsV, yDoseValsV, zDoseValsV, 'nearest',0);

structureOnDoseM = reshape(structureOnDoseV,[length(yDoseVals), length(xDoseVals), length(zDoseVals)]);

planC{indexS.dose}(end+1) = planC{indexS.dose}(doseNum);
planC{indexS.dose}(end).doseArray = planC{indexS.dose}(end).doseArray .* structureOnDoseM;
planC{indexS.dose}(end).doseUID = createUID('dose');

stateS.doseSet = length(planC{indexS.dose});

sliceCallBack('refresh');


% IMAGE_SCAT_LAYER engine function of IMAGE_SCAT
%
% Usage
%     big_img = image_scat_layer(Scatt, renorm, dsp_legend)
%
% Input
%	Scatt (struct): a layer of  scattering (either U or S)
%	renorm (boolean): if 1 renormalize each path by its max. Default
%	value is set to 1.
%	dsp_legend (boolean): if set to 1, display legend. Default value is 1
%
% Output
%	big_img (numerical): a large image where all paths are concatenated

function big_img = image_scat_layer(Scatt, renorm, dsp_legend)
	if (numel(size(Scatt.signal{1})) == 3)
		p2 = 1;
		for p = 1:numel(Scatt.signal)
			for theta = 1:size(Scatt.signal{1},3)
				ScattBis.signal{p2} = Scatt.signal{p}(:,:,theta);
				ScattBis.meta.j(:,p2) = Scatt.meta.j(:,p);
				ScattBis.meta.theta2(:,p2) = Scatt.meta.theta2(:,p);
				ScattBis.meta.theta(:,p2) = theta;
				ScattBis.meta.k2(:,p2) = Scatt.meta.k2(:,p);
				p2 = p2 + 1;
			end
		end

		if ~exist('renorm','var');
			renorm  = 1;
		end
		if ~exist('dsp_legend','var');
			dsp_legend  = 1;
		end
		big_img = image_scat_layer(ScattBis, renorm, dsp_legend);
		return
	end
	if ~exist('renorm','var');
		renorm  = 1;
	end
	if ~exist('dsp_legend','var');
		dsp_legend  = 1;
	end

	numel_per_width= zeros(10000,1);
	numel_per_height = zeros(10000,1);
	if (~isfield(Scatt,'signal'))
		Scatt2 = Scatt;
		clear Scatt; % avoid warning
		Scatt.signal = Scatt2;
	end
	if (size(Scatt.signal{1},3)==3)
		for rgb = 1:3
			for p=1:numel(Scatt.signal)
				Scattrgb{rgb}.signal{p}=squeeze(Scatt.signal{p}(:,:,rgb));
			end
			big_img(:,:,rgb) = display_scatt_2d_all_layer(Scattrgb{rgb});
		end
	else
		for p = 1:numel(Scatt.signal)
			curr_sz = size(Scatt.signal{p});
			numel_per_width(curr_sz(2)) = numel_per_width(curr_sz(2)) + 1;
			numel_per_height(curr_sz(1)) = numel_per_height(curr_sz(1)) + 1;
		end

		% block style
		widths = find(numel_per_width);

		total_width = ceil(sqrt(numel_per_width(max(widths))))* max(widths);
		curr_y = 0;

		% first pass to compute size and allocate

		for j = numel(widths):-1:1
			w= widths(j);
			curr_x = 0;

			for p = 1:numel(Scatt.signal)
				curr_sig = Scatt.signal{p};
				if (size(curr_sig,2) == w)
					h = size(curr_sig,1);
					if (curr_x >= total_width)
						curr_x = 0;
						curr_y = curr_y + size(curr_sig,1);
					end
					curr_x = curr_x + w;
				end
			end
			curr_y = curr_y + h;

		end
		big_img = zeros(curr_y,total_width);
		% second pass to put in the matrix what has to be put in
		curr_y = 0;
		for j = numel(widths):-1:1
			w= widths(j);
			curr_x = 0;

			for p = 1:numel(Scatt.signal)
				curr_sig = Scatt.signal{p};
				if (size(curr_sig,2) == w)
					h = size(curr_sig,1);
					if (curr_x >= total_width)
						curr_x = 0;
						curr_y = curr_y + size(curr_sig,1);
					end
					K = max(max(abs(Scatt.signal{p})));
					if (renorm==0)
						ds = size(Scatt.signal{1},1)/size(Scatt.signal{p},1);
						K = ds;
					end
					if (renorm==3)
						K=1 ;% preserve_l2_norm is off;
					end
					if (renorm == 2) % input is in log2 : substract the constant !
						ds = size(Scatt.signal{1},1)/size(Scatt.signal{p},1);
						K = log2(ds);
						big_img( (1:size(Scatt.signal{p},1)) + curr_y , (1:size(Scatt.signal{p},2)) + curr_x ) = Scatt.signal{p} -K;
					else
						big_img( (1:size(Scatt.signal{p},1)) + curr_y , (1:size(Scatt.signal{p},2)) + curr_x ) = Scatt.signal{p}/K;
					end
					curr_x = curr_x + w;
				end
			end
			curr_y = curr_y + h;

		end

		% compute min and max for future renormalization of legend
		img_min = min(big_img(:));
		img_max = max(big_img(:));
		%big_img =  (big_img-m)/(M-m);

		% last pass for legend
		curr_y = 0;
		for j = numel(widths):-1:1
			w= widths(j);
			curr_x = 0;

			for p = 1:numel(Scatt.signal)
				curr_sig = Scatt.signal{p};
				if (size(curr_sig,2) == w)
					h = size(curr_sig,1);
					if (curr_x >= total_width)
						curr_x = 0;
						curr_y = curr_y + size(curr_sig,1);
					end

					if (isstruct(Scatt) && isfield(Scatt,'meta'))
						str_legend = meta2str(Scatt.meta,p);
						if (numel(str_legend)>0)
							str_legend_split = textscan(str_legend,'%s','delimiter',' ');
						else
							str_legend_split{1} ={};
						end
					else % no meta to display
						str_legend_split{1}={};
					end
					if (dsp_legend ==0)
						str_legend_split{1}={};
					end
					str_legend_split = str_legend_split{1};
					for jl = 1:numel(str_legend_split)

						str = str_legend_split{jl};

						imstr = str2imfast(str);
						% renormalize imstr
						imstr = img_min + (img_max-img_min)*imstr;
						[n,m] = size(imstr);

						y_min =   n*(jl-1) + 1 +  curr_y;
						y_max =   n*(jl-1) + n + curr_y;
						x_min =   curr_x + 1;
						x_max =   curr_x + m;

						y_min = min(y_min,size(big_img,1));
						x_min = min(x_min,size(big_img,2));
						y_max = min(y_max,size(big_img,1));
						x_max = min(x_max,size(big_img,2));
						ny = y_max - y_min +1;
						nx = x_max - x_min +1;
						big_img(y_min:y_max,x_min:x_max) = max(imstr(1:ny,1:nx),0.5* big_img(y_min:y_max,x_min:x_max));
					end

					curr_x = curr_x + w;
				end
			end
			curr_y = curr_y + h;

		end

	end
	imagesc(big_img);
end


function mpc = case6ww
%CASE6WW  Power flow data for 6 bus, 3 gen case from Wood & Wollenberg.
%   Please see CASEFORMAT for details on the case file format.
%
%   This is the 6 bus example from pp. 104, 112, 119, 123-124, 549 of
%   "Power Generation, Operation, and Control, 2nd Edition",
%   by Allen. J. Wood and Bruce F. Wollenberg, John Wiley & Sons, NY, Jan 1996.

%   MATPOWER

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1.05	0	230	1	1.05	1.05;
	2	2	0	0	0	0	1	1.05	0	230	1	1.05	1.05;
	3	2	0	0	0	0	1	1.07	0	230	1	1.07	1.07;
	4	1	70	70	0	0	1	1	0	230	1	1.05	0.95;
	5	1	70	70	0	0	1	1	0	230	1	1.05	0.95;
	6	1	70	70	0	0	1	1	0	230	1	1.05	0.95;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	0	0	100	-100	1.05	100	1	200	50	0	0	0	0	0	0	0	0	0	0	0;
	2	50	0	100	-100	1.05	100	1	150	37.5	0	0	0	0	0	0	0	0	0	0	0;
	3	60	0	100	-100	1.07	100	1	180	45	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	2	0.1	0.2	0.04	40	40	40	0	0	1	-360	360;
	1	4	0.05	0.2	0.04	60	60	60	0	0	1	-360	360;
	1	5	0.08	0.3	0.06	40	40	40	0	0	1	-360	360;
	2	3	0.05	0.25	0.06	40	40	40	0	0	1	-360	360;
	2	4	0.05	0.1	0.02	60	60	60	0	0	1	-360	360;
	2	5	0.1	0.3	0.04	30	30	30	0	0	1	-360	360;
	2	6	0.07	0.2	0.05	90	90	90	0	0	1	-360	360;
	3	5	0.12	0.26	0.05	70	70	70	0	0	1	-360	360;
	3	6	0.02	0.1	0.02	80	80	80	0	0	1	-360	360;
	4	5	0.2	0.4	0.08	20	20	20	0	0	1	-360	360;
	5	6	0.1	0.3	0.06	40	40	40	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	3	0.00533	11.669	213.1;
	2	0	0	3	0.00889	10.333	200;
	2	0	0	3	0.00741	10.833	240;
];


function value = i1mach ( i )

%*****************************************************************************80
%
%% I1MACH returns integer machine constants.
%
%  Discussion:
%
%    Input/output unit numbers.
%
%      I1MACH(1) = the standard input unit.
%      I1MACH(2) = the standard output unit.
%      I1MACH(3) = the standard punch unit.
%      I1MACH(4) = the standard error message unit.
%
%    Words.
%
%      I1MACH(5) = the number of bits per integer storage unit.
%      I1MACH(6) = the number of characters per integer storage unit.
%
%    Integers.
%
%    Assume integers are represented in the S digit base A form:
%
%      Sign * (X(S-1)*A^(S-1) + ... + X(1)*A + X(0))
%
%    where 0 <= X(1:S-1) < A.
%
%      I1MACH(7) = A, the base.
%      I1MACH(8) = S, the number of base A digits.
%      I1MACH(9) = A^S-1, the largest integer.
%
%    Floating point numbers
%
%    Assume floating point numbers are represented in the T digit
%    base B form:
%
%      Sign * (B^E) * ((X(1)/B) + ... + (X(T)/B^T) )
%
%    where 0 <= X(I) < B for I=1 to T, 0 < X(1) and EMIN <= E <= EMAX.
%
%      I1MACH(10) = B, the base.
%
%    Single precision
%
%      I1MACH(11) = T, the number of base B digits.
%      I1MACH(12) = EMIN, the smallest exponent E.
%      I1MACH(13) = EMAX, the largest exponent E.
%
%    Double precision
%
%      I1MACH(14) = T, the number of base B digits.
%      I1MACH(15) = EMIN, the smallest exponent E.
%      I1MACH(16) = EMAX, the largest exponent E.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2007
%
%  Author:
%
%    Original FORTRAN77 version by Phyllis Fox, Andrew Hall, Norman Schryer
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Phyllis Fox, Andrew Hall, Norman Schryer,
%    Algorithm 528,
%    Framework for a Portable Library,
%    ACM Transactions on Mathematical Software,
%    Volume 4, Number 2, June 1978, page 176-188.
%
%  Parameters:
%
%    Input, integer I, chooses the parameter to be returned.
%    1 <= I <= 16.
%
%    Output, integer VALUE, the value of the chosen parameter.
%
  if ( i < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I1MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I1MACH - Fatal error!' );
  elseif ( i == 1 )
    value = 5;
  elseif ( i == 2 )
    value = 6;
  elseif ( i == 3 )
    value = 7;
  elseif ( i == 4 )
    value = 6;
  elseif ( i == 5 )
    value = 32;
  elseif ( i == 6 )
    value = 4;
  elseif ( i == 7 )
    value = 2;
  elseif ( i == 8 )
    value = 31;
  elseif ( i == 9 )
    value = 2147483647;
  elseif ( i == 10 )
    value = 2;
  elseif ( i == 11 )
    value = 24;
  elseif ( i == 12 )
    value = -125;
  elseif ( i == 13 )
    value = 128;
  elseif ( i == 14 )
    value = 53;
  elseif ( i == 15 )
    value = -1021;
  elseif ( i == 16 )
    value = 1024;
  elseif ( 16 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I1MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I1MACH - Fatal error!' );
  end

  return
end


function obj = Jacob_pnp_func_new(in1,in2,in3,in4,in5)
coef_f0_q_sym1 = in1(:,1);
coef_f0_q_sym2 = in1(:,2);
coef_f1_q_sym1 = in2(:,1);
coef_f0_q_sym3 = in1(:,3);
coef_f1_q_sym2 = in2(:,2);
coef_f2_q_sym1 = in3(:,1);
coef_f0_q_sym4 = in1(:,4);
coef_f1_q_sym3 = in2(:,3);
coef_f2_q_sym2 = in3(:,2);
coef_f3_q_sym1 = in4(:,1);
coef_f0_q_sym5 = in1(:,5);
coef_f1_q_sym4 = in2(:,4);
coef_f2_q_sym3 = in3(:,3);
coef_f3_q_sym2 = in4(:,2);
coef_f0_q_sym6 = in1(:,6);
coef_f1_q_sym5 = in2(:,5);
coef_f2_q_sym4 = in3(:,4);
coef_f3_q_sym3 = in4(:,3);
coef_f0_q_sym7 = in1(:,7);
coef_f1_q_sym6 = in2(:,6);
coef_f2_q_sym5 = in3(:,5);
coef_f3_q_sym4 = in4(:,4);
coef_f0_q_sym8 = in1(:,8);
coef_f1_q_sym7 = in2(:,7);
coef_f2_q_sym6 = in3(:,6);
coef_f3_q_sym5 = in4(:,5);
coef_f0_q_sym9 = in1(:,9);
coef_f1_q_sym8 = in2(:,8);
coef_f2_q_sym7 = in3(:,7);
coef_f3_q_sym6 = in4(:,6);
coef_f1_q_sym9 = in2(:,9);
coef_f2_q_sym8 = in3(:,8);
coef_f3_q_sym7 = in4(:,7);
coef_f2_q_sym9 = in3(:,9);
coef_f3_q_sym8 = in4(:,8);
coef_f3_q_sym9 = in4(:,9);
coef_f0_q_sym10 = in1(:,10);
coef_f0_q_sym11 = in1(:,11);
coef_f0_q_sym20 = in1(:,20);
coef_f1_q_sym10 = in2(:,10);
coef_f0_q_sym12 = in1(:,12);
coef_f0_q_sym21 = in1(:,21);
coef_f1_q_sym11 = in2(:,11);
coef_f1_q_sym20 = in2(:,20);
coef_f2_q_sym10 = in3(:,10);
coef_f0_q_sym13 = in1(:,13);
coef_f0_q_sym22 = in1(:,22);
coef_f1_q_sym12 = in2(:,12);
coef_f1_q_sym21 = in2(:,21);
coef_f2_q_sym11 = in3(:,11);
coef_f2_q_sym20 = in3(:,20);
coef_f3_q_sym10 = in4(:,10);
coef_f0_q_sym14 = in1(:,14);
coef_f0_q_sym23 = in1(:,23);
coef_f1_q_sym13 = in2(:,13);
coef_f1_q_sym22 = in2(:,22);
coef_f2_q_sym12 = in3(:,12);
coef_f2_q_sym21 = in3(:,21);
coef_f3_q_sym11 = in4(:,11);
coef_f3_q_sym20 = in4(:,20);
coef_f0_q_sym15 = in1(:,15);
coef_f0_q_sym24 = in1(:,24);
coef_f1_q_sym14 = in2(:,14);
coef_f1_q_sym23 = in2(:,23);
coef_f2_q_sym13 = in3(:,13);
coef_f2_q_sym22 = in3(:,22);
coef_f3_q_sym12 = in4(:,12);
coef_f3_q_sym21 = in4(:,21);
coef_f0_q_sym16 = in1(:,16);
coef_f1_q_sym15 = in2(:,15);
coef_f1_q_sym24 = in2(:,24);
coef_f2_q_sym14 = in3(:,14);
coef_f2_q_sym23 = in3(:,23);
coef_f3_q_sym13 = in4(:,13);
coef_f3_q_sym22 = in4(:,22);
coef_f0_q_sym17 = in1(:,17);
coef_f1_q_sym16 = in2(:,16);
coef_f2_q_sym15 = in3(:,15);
coef_f2_q_sym24 = in3(:,24);
coef_f3_q_sym14 = in4(:,14);
coef_f3_q_sym23 = in4(:,23);
coef_f0_q_sym18 = in1(:,18);
coef_f1_q_sym17 = in2(:,17);
coef_f2_q_sym16 = in3(:,16);
coef_f3_q_sym15 = in4(:,15);
coef_f3_q_sym24 = in4(:,24);
coef_f0_q_sym19 = in1(:,19);
coef_f1_q_sym18 = in2(:,18);
coef_f2_q_sym17 = in3(:,17);
coef_f3_q_sym16 = in4(:,16);
coef_f1_q_sym19 = in2(:,19);
coef_f2_q_sym18 = in3(:,18);
coef_f3_q_sym17 = in4(:,17);
coef_f2_q_sym19 = in3(:,19);
coef_f3_q_sym18 = in4(:,18);
coef_f3_q_sym19 = in4(:,19);
q0 = in5(1,:);
q1 = in5(2,:);
q2 = in5(3,:);
q3 = in5(4,:);
t114 = q0.^2;
t115 = q1.^2;
t116 = q2.^2;
t117 = q3.^2;
t118 = coef_f0_q_sym11.*q0;
t119 = coef_f0_q_sym24.*q3;
t120 = coef_f0_q_sym1.*q0.*t114;
t121 = coef_f0_q_sym23.*q3.*t117;
t122 = coef_f0_q_sym4.*q3.*t114;
t123 = coef_f0_q_sym10.*q0.*t117;
t124 = coef_f0_q_sym6.*q0.*q1.*q2.*2.0;
t125 = coef_f0_q_sym16.*q1.*q2.*q3.*2.0;
t126 = coef_f0_q_sym18.*q1;
t127 = coef_f0_q_sym22.*q2;
t128 = coef_f0_q_sym12.*q1.*t115;
t129 = coef_f0_q_sym19.*q2.*t116;
t130 = coef_f0_q_sym2.*q1.*t114;
t131 = coef_f0_q_sym5.*q0.*t115;
t132 = coef_f0_q_sym3.*q2.*t114;
t133 = coef_f0_q_sym8.*q0.*t116;
t134 = coef_f0_q_sym7.*q0.*q1.*q3.*2.0;
t135 = coef_f0_q_sym9.*q0.*q2.*q3.*2.0;
obj = reshape([coef_f1_q_sym11.*q0.*-2.0+coef_f0_q_sym11.*q1-coef_f1_q_sym18.*q1-coef_f1_q_sym22.*q2-coef_f1_q_sym24.*q3-coef_f1_q_sym1.*q0.*t114.*4.0+coef_f0_q_sym2.*q0.*t115.*2.0-coef_f1_q_sym5.*q0.*t115.*2.0-coef_f1_q_sym8.*q0.*t116.*2.0-coef_f1_q_sym10.*q0.*t117.*2.0+coef_f0_q_sym1.*q1.*t114.*3.0-coef_f1_q_sym2.*q1.*t114.*3.0+coef_f0_q_sym5.*q1.*t115+coef_f0_q_sym8.*q1.*t116+coef_f0_q_sym10.*q1.*t117-coef_f1_q_sym12.*q1.*t115-coef_f1_q_sym15.*q1.*t116-coef_f1_q_sym17.*q1.*t117-coef_f1_q_sym3.*q2.*t114.*3.0+coef_f0_q_sym6.*q2.*t115-coef_f1_q_sym13.*q2.*t115-coef_f1_q_sym21.*q2.*t117-coef_f1_q_sym19.*q2.*t116-coef_f1_q_sym4.*q3.*t114.*3.0+coef_f0_q_sym7.*q3.*t115-coef_f1_q_sym20.*q3.*t116-coef_f1_q_sym14.*q3.*t115-coef_f1_q_sym23.*q3.*t117+coef_f0_q_sym3.*q0.*q1.*q2.*2.0-coef_f1_q_sym6.*q0.*q1.*q2.*2.0+coef_f0_q_sym4.*q0.*q1.*q3.*2.0-coef_f1_q_sym7.*q0.*q1.*q3.*2.0-coef_f1_q_sym9.*q0.*q2.*q3.*2.0+coef_f0_q_sym9.*q1.*q2.*q3-coef_f1_q_sym16.*q1.*q2.*q3,coef_f2_q_sym11.*q0.*-2.0-coef_f2_q_sym18.*q1+coef_f0_q_sym11.*q2-coef_f2_q_sym22.*q2-coef_f2_q_sym24.*q3-coef_f2_q_sym1.*q0.*t114.*4.0+coef_f0_q_sym3.*q0.*t116.*2.0-coef_f2_q_sym5.*q0.*t115.*2.0-coef_f2_q_sym8.*q0.*t116.*2.0-coef_f2_q_sym10.*q0.*t117.*2.0-coef_f2_q_sym2.*q1.*t114.*3.0+coef_f0_q_sym6.*q1.*t116-coef_f2_q_sym12.*q1.*t115-coef_f2_q_sym15.*q1.*t116-coef_f2_q_sym17.*q1.*t117+coef_f0_q_sym1.*q2.*t114.*3.0-coef_f2_q_sym3.*q2.*t114.*3.0+coef_f0_q_sym5.*q2.*t115+coef_f0_q_sym8.*q2.*t116+coef_f0_q_sym10.*q2.*t117-coef_f2_q_sym13.*q2.*t115-coef_f2_q_sym21.*q2.*t117-coef_f2_q_sym19.*q2.*t116-coef_f2_q_sym4.*q3.*t114.*3.0+coef_f0_q_sym9.*q3.*t116-coef_f2_q_sym20.*q3.*t116-coef_f2_q_sym14.*q3.*t115-coef_f2_q_sym23.*q3.*t117+coef_f0_q_sym2.*q0.*q1.*q2.*2.0-coef_f2_q_sym6.*q0.*q1.*q2.*2.0-coef_f2_q_sym7.*q0.*q1.*q3.*2.0+coef_f0_q_sym4.*q0.*q2.*q3.*2.0-coef_f2_q_sym9.*q0.*q2.*q3.*2.0+coef_f0_q_sym7.*q1.*q2.*q3-coef_f2_q_sym16.*q1.*q2.*q3,coef_f3_q_sym11.*q0.*-2.0-coef_f3_q_sym18.*q1-coef_f3_q_sym22.*q2+coef_f0_q_sym11.*q3-coef_f3_q_sym24.*q3-coef_f3_q_sym1.*q0.*t114.*4.0+coef_f0_q_sym4.*q0.*t117.*2.0-coef_f3_q_sym5.*q0.*t115.*2.0-coef_f3_q_sym8.*q0.*t116.*2.0-coef_f3_q_sym10.*q0.*t117.*2.0-coef_f3_q_sym2.*q1.*t114.*3.0+coef_f0_q_sym7.*q1.*t117-coef_f3_q_sym12.*q1.*t115-coef_f3_q_sym15.*q1.*t116-coef_f3_q_sym17.*q1.*t117-coef_f3_q_sym3.*q2.*t114.*3.0+coef_f0_q_sym9.*q2.*t117-coef_f3_q_sym13.*q2.*t115-coef_f3_q_sym21.*q2.*t117-coef_f3_q_sym19.*q2.*t116+coef_f0_q_sym1.*q3.*t114.*3.0+coef_f0_q_sym5.*q3.*t115-coef_f3_q_sym4.*q3.*t114.*3.0+coef_f0_q_sym8.*q3.*t116+coef_f0_q_sym10.*q3.*t117-coef_f3_q_sym20.*q3.*t116-coef_f3_q_sym14.*q3.*t115-coef_f3_q_sym23.*q3.*t117-coef_f3_q_sym6.*q0.*q1.*q2.*2.0+coef_f0_q_sym2.*q0.*q1.*q3.*2.0-coef_f3_q_sym7.*q0.*q1.*q3.*2.0+coef_f0_q_sym3.*q0.*q2.*q3.*2.0-coef_f3_q_sym9.*q0.*q2.*q3.*2.0+coef_f0_q_sym6.*q1.*q2.*q3-coef_f3_q_sym16.*q1.*q2.*q3,q0.*2.0,t118+t119+t120+t121+t122+t123+t124+t125+t127+t129+t132+t133+t134-coef_f1_q_sym18.*q0+coef_f0_q_sym18.*q1.*2.0-coef_f1_q_sym2.*q0.*t114+coef_f0_q_sym5.*q0.*t115.*3.0-coef_f1_q_sym12.*q0.*t115.*3.0-coef_f1_q_sym15.*q0.*t116-coef_f1_q_sym17.*q0.*t117+coef_f0_q_sym2.*q1.*t114.*2.0-coef_f1_q_sym5.*q1.*t114.*2.0+coef_f0_q_sym12.*q1.*t115.*4.0+coef_f0_q_sym15.*q1.*t116.*2.0+coef_f0_q_sym17.*q1.*t117.*2.0-coef_f1_q_sym6.*q2.*t114+coef_f0_q_sym13.*q2.*t115.*3.0+coef_f0_q_sym21.*q2.*t117-coef_f1_q_sym7.*q3.*t114+coef_f0_q_sym20.*q3.*t116+coef_f0_q_sym14.*q3.*t115.*3.0-coef_f1_q_sym13.*q0.*q1.*q2.*2.0-coef_f1_q_sym14.*q0.*q1.*q3.*2.0+coef_f0_q_sym9.*q0.*q2.*q3-coef_f1_q_sym16.*q0.*q2.*q3,-coef_f2_q_sym18.*q0+coef_f0_q_sym18.*q2-coef_f2_q_sym2.*q0.*t114+coef_f0_q_sym6.*q0.*t116-coef_f2_q_sym12.*q0.*t115.*3.0-coef_f2_q_sym15.*q0.*t116-coef_f2_q_sym17.*q0.*t117-coef_f2_q_sym5.*q1.*t114.*2.0+coef_f0_q_sym13.*q1.*t116.*2.0+coef_f0_q_sym2.*q2.*t114-coef_f2_q_sym6.*q2.*t114+coef_f0_q_sym12.*q2.*t115.*3.0+coef_f0_q_sym15.*q2.*t116+coef_f0_q_sym17.*q2.*t117-coef_f2_q_sym7.*q3.*t114+coef_f0_q_sym16.*q3.*t116+coef_f0_q_sym5.*q0.*q1.*q2.*2.0-coef_f2_q_sym13.*q0.*q1.*q2.*2.0-coef_f2_q_sym14.*q0.*q1.*q3.*2.0+coef_f0_q_sym7.*q0.*q2.*q3-coef_f2_q_sym16.*q0.*q2.*q3+coef_f0_q_sym14.*q1.*q2.*q3.*2.0,-coef_f3_q_sym18.*q0+coef_f0_q_sym18.*q3-coef_f3_q_sym2.*q0.*t114+coef_f0_q_sym7.*q0.*t117-coef_f3_q_sym12.*q0.*t115.*3.0-coef_f3_q_sym15.*q0.*t116-coef_f3_q_sym17.*q0.*t117-coef_f3_q_sym5.*q1.*t114.*2.0+coef_f0_q_sym14.*q1.*t117.*2.0-coef_f3_q_sym6.*q2.*t114+coef_f0_q_sym16.*q2.*t117+coef_f0_q_sym2.*q3.*t114-coef_f3_q_sym7.*q3.*t114+coef_f0_q_sym12.*q3.*t115.*3.0+coef_f0_q_sym15.*q3.*t116+coef_f0_q_sym17.*q3.*t117-coef_f3_q_sym13.*q0.*q1.*q2.*2.0+coef_f0_q_sym5.*q0.*q1.*q3.*2.0-coef_f3_q_sym14.*q0.*q1.*q3.*2.0+coef_f0_q_sym6.*q0.*q2.*q3-coef_f3_q_sym16.*q0.*q2.*q3+coef_f0_q_sym13.*q1.*q2.*q3.*2.0,q1.*2.0,-coef_f1_q_sym22.*q0+coef_f0_q_sym22.*q1-coef_f1_q_sym3.*q0.*t114+coef_f0_q_sym6.*q0.*t115-coef_f1_q_sym13.*q0.*t115-coef_f1_q_sym21.*q0.*t117-coef_f1_q_sym19.*q0.*t116.*3.0+coef_f0_q_sym3.*q1.*t114-coef_f1_q_sym6.*q1.*t114+coef_f0_q_sym13.*q1.*t115+coef_f0_q_sym21.*q1.*t117+coef_f0_q_sym19.*q1.*t116.*3.0-coef_f1_q_sym8.*q2.*t114.*2.0+coef_f0_q_sym15.*q2.*t115.*2.0-coef_f1_q_sym9.*q3.*t114+coef_f0_q_sym16.*q3.*t115+coef_f0_q_sym8.*q0.*q1.*q2.*2.0-coef_f1_q_sym15.*q0.*q1.*q2.*2.0+coef_f0_q_sym9.*q0.*q1.*q3-coef_f1_q_sym16.*q0.*q1.*q3-coef_f1_q_sym20.*q0.*q2.*q3.*2.0+coef_f0_q_sym20.*q1.*q2.*q3.*2.0,t118+t119+t120+t121+t122+t123+t124+t125+t126+t128+t130+t131+t135-coef_f2_q_sym22.*q0+coef_f0_q_sym22.*q2.*2.0-coef_f2_q_sym3.*q0.*t114+coef_f0_q_sym8.*q0.*t116.*3.0-coef_f2_q_sym13.*q0.*t115-coef_f2_q_sym21.*q0.*t117-coef_f2_q_sym19.*q0.*t116.*3.0-coef_f2_q_sym6.*q1.*t114+coef_f0_q_sym15.*q1.*t116.*3.0+coef_f0_q_sym17.*q1.*t117+coef_f0_q_sym3.*q2.*t114.*2.0-coef_f2_q_sym8.*q2.*t114.*2.0+coef_f0_q_sym13.*q2.*t115.*2.0+coef_f0_q_sym21.*q2.*t117.*2.0+coef_f0_q_sym19.*q2.*t116.*4.0-coef_f2_q_sym9.*q3.*t114+coef_f0_q_sym20.*q3.*t116.*3.0+coef_f0_q_sym14.*q3.*t115-coef_f2_q_sym15.*q0.*q1.*q2.*2.0+coef_f0_q_sym7.*q0.*q1.*q3-coef_f2_q_sym16.*q0.*q1.*q3-coef_f2_q_sym20.*q0.*q2.*q3.*2.0,-coef_f3_q_sym22.*q0+coef_f0_q_sym22.*q3-coef_f3_q_sym3.*q0.*t114+coef_f0_q_sym9.*q0.*t117-coef_f3_q_sym13.*q0.*t115-coef_f3_q_sym21.*q0.*t117-coef_f3_q_sym19.*q0.*t116.*3.0-coef_f3_q_sym6.*q1.*t114+coef_f0_q_sym16.*q1.*t117-coef_f3_q_sym8.*q2.*t114.*2.0+coef_f0_q_sym20.*q2.*t117.*2.0+coef_f0_q_sym3.*q3.*t114-coef_f3_q_sym9.*q3.*t114+coef_f0_q_sym13.*q3.*t115+coef_f0_q_sym21.*q3.*t117+coef_f0_q_sym19.*q3.*t116.*3.0-coef_f3_q_sym15.*q0.*q1.*q2.*2.0+coef_f0_q_sym6.*q0.*q1.*q3-coef_f3_q_sym16.*q0.*q1.*q3+coef_f0_q_sym8.*q0.*q2.*q3.*2.0-coef_f3_q_sym20.*q0.*q2.*q3.*2.0+coef_f0_q_sym15.*q1.*q2.*q3.*2.0,q2.*2.0,-coef_f1_q_sym24.*q0+coef_f0_q_sym24.*q1-coef_f1_q_sym4.*q0.*t114+coef_f0_q_sym7.*q0.*t115-coef_f1_q_sym20.*q0.*t116-coef_f1_q_sym14.*q0.*t115-coef_f1_q_sym23.*q0.*t117.*3.0+coef_f0_q_sym4.*q1.*t114-coef_f1_q_sym7.*q1.*t114+coef_f0_q_sym20.*q1.*t116+coef_f0_q_sym14.*q1.*t115+coef_f0_q_sym23.*q1.*t117.*3.0-coef_f1_q_sym9.*q2.*t114+coef_f0_q_sym16.*q2.*t115-coef_f1_q_sym10.*q3.*t114.*2.0+coef_f0_q_sym17.*q3.*t115.*2.0+coef_f0_q_sym9.*q0.*q1.*q2-coef_f1_q_sym16.*q0.*q1.*q2+coef_f0_q_sym10.*q0.*q1.*q3.*2.0-coef_f1_q_sym17.*q0.*q1.*q3.*2.0-coef_f1_q_sym21.*q0.*q2.*q3.*2.0+coef_f0_q_sym21.*q1.*q2.*q3.*2.0,-coef_f2_q_sym24.*q0+coef_f0_q_sym24.*q2-coef_f2_q_sym4.*q0.*t114+coef_f0_q_sym9.*q0.*t116-coef_f2_q_sym20.*q0.*t116-coef_f2_q_sym14.*q0.*t115-coef_f2_q_sym23.*q0.*t117.*3.0-coef_f2_q_sym7.*q1.*t114+coef_f0_q_sym16.*q1.*t116+coef_f0_q_sym4.*q2.*t114-coef_f2_q_sym9.*q2.*t114+coef_f0_q_sym20.*q2.*t116+coef_f0_q_sym14.*q2.*t115+coef_f0_q_sym23.*q2.*t117.*3.0-coef_f2_q_sym10.*q3.*t114.*2.0+coef_f0_q_sym21.*q3.*t116.*2.0+coef_f0_q_sym7.*q0.*q1.*q2-coef_f2_q_sym16.*q0.*q1.*q2-coef_f2_q_sym17.*q0.*q1.*q3.*2.0+coef_f0_q_sym10.*q0.*q2.*q3.*2.0-coef_f2_q_sym21.*q0.*q2.*q3.*2.0+coef_f0_q_sym17.*q1.*q2.*q3.*2.0,t118+t120+t125+t126+t127+t128+t129+t130+t131+t132+t133+t134+t135-coef_f3_q_sym24.*q0+coef_f0_q_sym24.*q3.*2.0-coef_f3_q_sym4.*q0.*t114+coef_f0_q_sym10.*q0.*t117.*3.0-coef_f3_q_sym20.*q0.*t116-coef_f3_q_sym14.*q0.*t115-coef_f3_q_sym23.*q0.*t117.*3.0-coef_f3_q_sym7.*q1.*t114+coef_f0_q_sym15.*q1.*t116+coef_f0_q_sym17.*q1.*t117.*3.0-coef_f3_q_sym9.*q2.*t114+coef_f0_q_sym13.*q2.*t115+coef_f0_q_sym21.*q2.*t117.*3.0+coef_f0_q_sym4.*q3.*t114.*2.0+coef_f0_q_sym20.*q3.*t116.*2.0-coef_f3_q_sym10.*q3.*t114.*2.0+coef_f0_q_sym14.*q3.*t115.*2.0+coef_f0_q_sym23.*q3.*t117.*4.0+coef_f0_q_sym6.*q0.*q1.*q2-coef_f3_q_sym16.*q0.*q1.*q2-coef_f3_q_sym17.*q0.*q1.*q3.*2.0-coef_f3_q_sym21.*q0.*q2.*q3.*2.0,q3.*2.0],[4, 4]);