function [OffDec,OffVel] = LCSA_CoefficientSMPSOOperator(Particle,Pbest,Gbest,xlower,xupper)
% -----------------------------------------------------------------------
%  Copyright (C) 2020 Heiner Zille
%
%  This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0
%  International License. (CC BY-NC-SA 4.0). To view a copy of this license,
%  visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or see the
%  pdf-file "License-CC-BY-NC-SA-4.0.pdf" that came with this code.
%
%  You are free to:
%  * Share ? copy and redistribute the material in any medium or format
%  * Adapt ? remix, transform, and build upon the material
%  Under the following terms:
%  * Attribution ? You must give appropriate credit, provide a link to the
%     license, and indicate if changes were made. You may do so in any reasonable
%     manner, but not in any way that suggests the licensor endorses you or your use.
%  * NonCommercial ? You may not use the material for commercial purposes.
%  * ShareAlike ? If you remix, transform, or build upon the material, you must
%    distribute your contributions under the same license as the original.
%  * No additional restrictions ? You may not apply legal terms or technological
%    measures that legally restrict others from doing anything the license permits.
%
%  Author of this Code:
%   Heiner Zille <heiner.zille@ovgu.de> or <heiner.zille@gmail.com>
%
%  This code is based on the following publications:
%
%  1) Heiner Zille
%     "Large-scale Multi-objective Optimisation: New Approaches and a Classification of the State-of-the-Art"
%     PhD Thesis, Otto von Guericke University Magdeburg, 2019
%     http://dx.doi.org/10.25673/32063
%
%  2) Heiner Zille and Sanaz Mostaghim
%     "Linear Search Mechanism for Multi- and Many-Objective Optimisation"
%     10th International Conference on Evolutionary Multi-Criterion Optimization (EMO 2019),
%        Lecture Notes in Computer Science, vol 11411.
%        Deb K. et al. (eds), Springer, Cham, East Lansing, Michigan, USA, March 2019
%     https://doi.org/10.1007/978-3-030-12598-1_32.
%
%  This file is intended to work with the PlatEMO framework version 2.5.
%  Date of publication of this code: 06.04.2020
%  Last Update of this code: 06.04.2020
%  A newer version of this algorithm may be available. Please contact the author
%  or see http://www.ci.ovgu.de/Research/Codes.html.
%
% The files may have been modified in Feb 2021 by the authors of the Platemo framework to work with the Platemo 3.0 release.
% -----------------------------------------------------------------------
% This file is derived from its original version containied in the PlatEMO
% framework.
% -----------------------------------------------------------------------

    %% Parameter setting
    [ParticleDec,ParticleVel] = unpackDecAndVel(Particle.adds);
    [PbestDec,~] = unpackDecAndVel(Pbest.adds);
    [GbestDec,~] = unpackDecAndVel(Gbest.adds);
    [N,D]        = size(ParticleDec);

    %% Particle swarm optimization
    W  = repmat(unifrnd(0.1,0.5,N,1),1,D);
    r1 = repmat(rand(N,1),1,D);
    r2 = repmat(rand(N,1),1,D);
    C1 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    C2 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    OffVel = W.*ParticleVel + C1.*r1.*(PbestDec-ParticleDec) + C2.*r2.*(GbestDec-ParticleDec);
    phi    = max(4,C1+C2);
    OffVel = OffVel.*2./abs(2-phi-sqrt(phi.^2-4*phi));
    delta  = repmat((xupper-xlower)/2,N,1);
    OffVel = max(min(OffVel,delta),-delta);
    OffDec = ParticleDec + OffVel;

    %% Deterministic back
    Lower  = repmat(xlower,N,1);
    Upper  = repmat(xupper,N,1);
    repair = OffDec < Lower | OffDec > Upper;
    OffVel(repair) = 0.001*OffVel(repair);
    OffDec = max(min(OffDec,Upper),Lower);

    %% Polynomial mutation
    disM  = 20;
    Site1 = repmat(rand(N,1)<0.15,1,D);
    Site2 = rand(N,D) < 1/D;
    mu    = rand(N,D);
    temp  = Site1 & Site2 & mu<=0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp  = Site1 & Site2 & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
end

function [positions, velocities] = unpackDecAndVel(pack)
   noOfSolutions = size(pack,1);
   P = arrayfun(@(K) pack(K).xDecs, 1:noOfSolutions, 'UniformOutput',0);
   positions = cell2mat(transpose(P));
   V = arrayfun(@(K) pack(K).xDecs, 1:noOfSolutions, 'UniformOutput',0);
   velocities = cell2mat(transpose(V));
end


function  h11=derotated_dtcwt(n,Yh)
%FUNCTION to perform a derotated DT_CWT

% n  -> No. of levels of wavelet decomposition
% Yh -> A cell array containing the 6 complex highpass subimages for
%  each level.
% h11 ->detotated Yh
h11{n}=Yh{n};
for k=n:-1:2
    for m=1:6
        xp=imresize(Yh{k}(:,:,m),2);%
        argxp=angle(xp);
        argx=angle(Yh{k-1}(:,:,m));
        argx=argx-2.*argxp;
        absx=abs(Yh{k-1}(:,:,m));
        xa=absx.*cos(argx);
        xb=absx.*sin(argx);
        h11{k-1}(:,:,m)=complex(xa,xb);
    end
end
%figure;
%cimage5(h11{1}(:,:,4));
%figure;
%cimage5(h1{1}(:,:,4));
%figure;
%cimage5(h11{2}(:,:,4));
%figure;
%cimage5(h1{2}(:,:,4));


function [loss, fchg, tchg, dloss_dV, dchg_dVm] = get_losses(baseMVA, bus, branch)
%GET_LOSSES   Returns series losses (and reactive injections) per branch.
%
%   LOSS = GET_LOSSES(RESULTS)
%   LOSS = GET_LOSSES(BASEMVA, BUS, BRANCH)
%
%   [LOSS, CHG] = GET_LOSSES(RESULTS)
%   [LOSS, FCHG, TCHG] = GET_LOSSES(RESULTS)
%   [LOSS, FCHG, TCHG, DLOSS_DV] = GET_LOSSES(RESULTS)
%   [LOSS, FCHG, TCHG, DLOSS_DV, DCHG_DVM] = GET_LOSSES(RESULTS)
%
%   Computes branch series losses, and optionally reactive injections from
%   line charging, as functions of bus voltages and branch parameters, using the
%   following formulae:
%
%       loss = abs( Vf / tau - Vt ) ^ 2 / (Rs - j Xs)
%       fchg = abs( Vf / tau ) ^ 2 * Bc / 2
%       tchg = abs( Vt ) ^ 2 * Bc / 2
%
%   Optionally, computes the partial derivatives of the line losses with
%   respect to voltage angles and magnitudes.
%
%   Input:
%       RESULTS - a MATPOWER case struct with bus voltages corresponding to
%                 a valid power flow solution.
%                 (Can optionally be specified as individual fields BASEMVA,
%                  BUS, and BRANCH.)
%
%   Output(s):
%       LOSS - complex NL x 1 vector of losses (in MW), where NL is the number
%              of branches in the system, representing only the losses in the
%              series impedance element of the PI model for each branch.
%       CHG -  NL x 1 vector of total reactive injection for each line
%              (in MVAr), representing the line charging injections of both
%              of the shunt elements of PI model for each branch.
%       FCHG - Same as CHG, but for the element at the "from" end of the
%              branch only.
%       TCHG - Same as CHG, but for the element at the "to" end of the branch.
%       DLOSS_DV - Struct with partial derivatives of LOSS with respect to bus
%              voltages, with fields:
%           .a  - Partial with respect to bus voltage angles.
%           .m  - Partial with respect to bus voltage magnitudes.
%       DCHG_DVM - Struct with partial derivatives of FCHG and TCHG with
%              respect to bus voltage magnitudes, with fields:
%           .f  - Partial of FCHG with respect to bus voltage magnitudes.
%           .t  - Partial of TCHG with respect to bus voltage magnitudes.
%
%   Example:
%       results = runpf(mycase);
%       [loss, chg] = get_losses(results);
%       total_system_real_losses = sum(real(loss));
%       total_system_reac_losses = sum(imag(loss)) - sum(chg);
%
%       [loss, fchg, tchg, dloss_dV] = get_losses(results);

%   MATPOWER
%   Copyright (c) 1996-2017, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% default arguments
if isstruct(baseMVA)
    mpc = baseMVA;
    [baseMVA, bus, branch] = deal(mpc.baseMVA, mpc.bus, mpc.branch);
end

%% define named indices into bus, gen, branch matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

%% create map of external bus numbers to bus indices
i2e = bus(:, BUS_I);
e2i = sparse(max(i2e), 1);
e2i(i2e) = (1:size(bus, 1))';
out = find(branch(:, BR_STATUS) == 0);          %% out-of-service branches

%% sizes of things
nb = size(bus, 1);      %% number of buses
nl = size(branch, 1);   %% number of branches

%% construct complex bus voltage vector
V = bus(:, VM) .* exp(1j * pi/180 * bus(:, VA));

%% parameters
Cf = sparse(1:nl, e2i(branch(:, F_BUS)), branch(:, BR_STATUS), nl, nb);
Ct = sparse(1:nl, e2i(branch(:, T_BUS)), branch(:, BR_STATUS), nl, nb);
tap = ones(nl, 1);                              %% default tap ratio = 1 for lines
xfmr = find(branch(:, TAP));                    %% indices of transformers
tap(xfmr) = branch(xfmr, TAP);                  %% include transformer tap ratios
tap = tap .* exp(1j*pi/180 * branch(:, SHIFT)); %% add phase shifters
A = spdiags(1 ./ tap, 0, nl, nl) * Cf - Ct;
Ysc = 1 ./ (branch(:, BR_R) - 1j * branch(:, BR_X));
Vdrop = A * V;      %% vector of voltage drop across series impedance element
loss = baseMVA * Ysc .* Vdrop .* conj(Vdrop);
% loss = baseMVA * abs(V(e2i(branch(:, F_BUS))) ./ tap - V(e2i(branch(:, T_BUS)))) .^ 2 ./ ...
%             (branch(:, BR_R) - 1j * branch(:, BR_X));
% loss(out) = 0;

if nargout > 1
    Vf = Cf * V;
    Vt = Ct * V;
    fchg = real(baseMVA / 2 * branch(:, BR_B) .* Vf .* conj(Vf) ./ (tap .* conj(tap)));
    tchg = real(baseMVA / 2 * branch(:, BR_B) .* Vt .* conj(Vt));
%     fchg = abs(V(e2i(branch(:, F_BUS))) ./ tap) .^ 2 .* branch(:, BR_B) * baseMVA / 2;
%     tchg = abs(V(e2i(branch(:, T_BUS)))       ) .^ 2 .* branch(:, BR_B) * baseMVA / 2;
    fchg(out) = 0;
    tchg(out) = 0;

    if nargout == 2
        fchg = fchg + tchg;
    end
end

if nargout > 3
    B = spdiags(A * V, 0, nl, nl) * conj(A) * spdiags(conj(V), 0, nb, nb);
    dYsc = spdiags(Ysc, 0, nl, nl);
    dloss_dV = struct(...
        'a', -1j * baseMVA * dYsc * (B - conj(B)), ...
        'm',       baseMVA * dYsc * (B + conj(B)) * spdiags(1 ./ abs(V), 0, nb, nb) ...
    );
    if nargout > 4
        Bc = spdiags(branch(:, BR_B), 0, nl, nl);
        tt = spdiags(1 ./ (tap .* conj(tap)), 0, nl, nl);
        dchg_dVm = struct(...
            'f', baseMVA * Bc * tt * spdiags(Cf * bus(:, VM), 0, nl, nl) * Cf, ...
            't', baseMVA * Bc      * spdiags(Ct * bus(:, VM), 0, nl, nl) * Ct ...
        );
    end
end


function g = findBestGammaAsCRF(img0, img1, exposure0, exposure1)
%
%       g = findBestGammaAsCRF(img0, img1, exposure0, exposure1)
%
%       This function computes the best gamma paramters for RGB that approximate
%       the camera response function.
%
%        Input:
%           -img0: an SDR image
%           -img1: an SDR image
%           -exposure0: the shutter speed of img0
%           -exposure1: the shutter speed of img1
%
%        Output:
%           -lin_fun: the inverse CRF
%           -max_lin_fun: maximum value of the inverse CRF
%
%     Copyright (C) 2014-15  Francesco Banterle
%
%     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/>.
%

delta_exposure = exposure1 / exposure0;
mask = ones(size(img0));
mask(img0>0.95) = 0.0;
mask(img1>0.95) = 0.0;
mask(img0<0.05) = 0.0;
mask(img1<0.05) = 0.0;

    function err = residualFunction(p)
        err = 0.0;
        for i=1:3
            gt = p(i);
            img0_lin(:,:,i) = (img0(:,:,i).^gt);
            img0_re(:,:,i) = img0_lin(:,:,i) * delta_exposure;
            img0_re(:,:,i) = img0_re(:,:,i).^(1.0/gt);

            delta = abs(img0_re(:,:,i) - img1(:,:,i)) .* mask(:,:,i);
            err = err + mean(delta(:));
        end
    end

    opts = optimset('Display', 'iter', 'TolFun', 1e-12, 'TolX', 1e-12, 'MaxIter', 1000);
    gi = 2.2 * ones(1,3);
    g = fminsearch(@residualFunction, gi, opts);
    for i=1:3
        img0_lin(:,:,i) = (img0(:,:,i).^g(i));
        img0_re(:,:,i) = img0_lin(:,:,i) * delta_exposure;
        img0_re(:,:,i) = img0_re(:,:,i).^(1.0/g(i));
    end

    imshow([img0, img0_re, img1]);

end


%% FUNCTION Least_SRMTL
%   Sparse Structure-Regularized Learning with Least Squares Loss.
%
%% OBJECTIVE
%   argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + rho1 * norm(W*R, 'fro')^2 + rho2 * \|W\|_1}
%
%% R encodes structure relationship
%   1)Structure order is given by using [1 -1 0 ...; 0 1 -1 ...; ...]
%    e.g.: R=zeros(t,t-1);R(1:(t+1):end)=1;R(2:(t+1):end)=-1;
%   2)Ridge penalty term by setting: R = eye(t)
%   3)All related regularized: R = eye (t) - ones (t) / t
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   R: regularization structure
%   rho1: structure regularization parameter
%   rho2: sparsity controlling parameter
%
%% OUTPUT
%   W: model: d * t
%   funcVal: function value vector.
%
%% 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 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] Evgeniou, T. and Pontil, M. Regularized multi-task learning, KDD 2004
% [2] Zhou, J. Technical Report. http://www.public.asu.edu/~jzhou29/Software/SRMTL/CrisisEventProjectReport.pdf
%
%% RELATED FUNCTIONS
%  Logistic_SRMTL, init_opts

%% Code starts here
function [W, funcVal] = Least_SRMTL(X, Y, R, rho1, rho2, opts)

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

if nargin <6
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

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

% precomputation.
RRt = R * R';
XY = cell(task_num, 1);
W0_prep = [];
for t_idx = 1: task_num
    XY{t_idx} = X{t_idx}*Y{t_idx};
    W0_prep = cat(2, W0_prep, XY{t_idx});
end

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = W0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0=W0_prep;
    end
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

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

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

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws, rho1);
    Fs   = funVal_eval  (Ws, rho1);

    while true
        [Wzp l1c_wzp] = l1_projection(Ws - gWs/gamma, 2 * rho2 / gamma);
        Fzp = funVal_eval  (Wzp, rho1);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + trace(delta_Wzp' * gWs) + gamma/2 * norm(delta_Wzp, 'fro')^2;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

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

    Wz_old = Wz;
    Wz = Wzp;

    funcVal = cat(1, funcVal, Fzp + rho2 * l1c_wzp);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % 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

W = Wzp;

% private functions

    function [z l1_comp_val] = l1_projection (v, beta)
        % this projection calculates
        % argmin_z = \|z-v\|_2^2 + beta \|z\|_1
        % z: solution
        % l1_comp_val: value of l1 component (\|z\|_1)

        z = zeros(size(v));
        vp = v - beta/2;
        z (v> beta/2)  = vp(v> beta/2);
        vn = v + beta/2;
        z (v< -beta/2) = vn(v< -beta/2);

        l1_comp_val = sum(sum(abs(z)));
    end

    function [grad_W] = gradVal_eval(W, rho1)

        if opts.pFlag
            grad_W = zeros(size(W));
            parfor t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W(:, t_ii) = XTXWi - XY{t_ii};
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        else
            grad_W = [];
            for t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W = cat(2, grad_W, XTXWi - XY{t_ii});
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        end
        grad_W = grad_W + rho1 * 2 *  W * RRt...
            + rho_L2 * 2 * W;
    end

    function [funcVal] = funVal_eval (W, rho1)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
        funcVal = funcVal + rho1 * norm(W*R, 'fro')^2 ...
            + rho_L2 * norm(W, 'fro')^2;
    end

end


function varargout=bss_decomp_filt(varargin)

% decompose an estimated source into target/interference/noise/artefacts components, assuming the admissible distortion is a pure time-invariant filter.
% components, assuming the admissible distortion is a pure time-invariant
% filter.
%
% Usage:
%
% [s_target,e_interf[,e_noise],e_artif]=bss_decomp_filt(se,index,S[,N],L)
%
% Input:
%   - se: row vector of length T containing the estimated source,
%   - index: points which component of S se has to be compared to,
%   - S: n x T matrix containing the original sources,
%   - N: m x T matrix containing the noise on the observations (if any).
%   - L: the number of lags of the allowed filter
%
% Output:
%   - s_target: row vector of length T containing the target source(s)
%   contribution,
%   - e_interf: row vector of length T containing the interferences
%   contribution,
%   - e_noise: row vector of length T containing the noise contribution (if
%   any),
%   - e_artif: row vector of length T containing the artifacts
%   contribution.
%
% Developers:  - Cedric Fevotte (fevotte@tsi.enst.fr) - Emmanuel Vincent
% (emmanuel.vincent@irisa.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

se=varargin{1}; index=varargin{2}; S=varargin{3};

switch nargin
    case 4
    N=[];
    L=varargin{4};
    case 5
    N=varargin{4};
    L=varargin{5};
    otherwise
    disp('Wrong number of arguments.')
end

[ne,Te]=size(se);
[n,T]=size(S);

%%%%%%%%%% WARNINGS %%%%%%%%%%%%%
switch isempty(N)
    case 1
        if n>T | ne>Te, disp('Watch out: signals must be in rows.'), return; end
        if ne~=1, disp('Watch out: se must contain only one row.'), return; end
        if T~=Te, disp('Watch out: se and S have different lengths.'), return; end
    case 0
        [m,Tm]=size(N);
        if n>T | ne>Te | m>Tm, disp('Watch out: signals must be in rows.'), return; end
        if ne~=1, disp('Watch out: se must contain only one row.'), return; end
        if T~=Te, disp('Watch out: S and Se have different lengths.'), return; end
        if T~=Tm, disp('Watch out: N, S and Se have different lengths.'), return; end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Create the space of target source(s)
target_space=bss_make_lags(S(index,:),L);
% Create the space of sources
sources_space=bss_make_lags(S,L);
% Create the noise space
noise_space=bss_make_lags(N,L);

s_target=zeros(1,T);
e_interf=zeros(1,T);
e_artif=zeros(1,T);
if isempty(noise_space)==0, e_noise=zeros(1,T); end

%%% Target source(s) contribution %%%
s_target = bss_proj(se,target_space);

%%% Interferences contribution %%%
P_S_se = bss_proj(se,sources_space);
e_interf = P_S_se - s_target;

switch isempty(noise_space)
    case 1 % No noise
        %%% Artifacts contribution %%%
        e_artif= se - P_S_se;

        %%% Output %%%
        varargout{1}=s_target;
        varargout{2}=e_interf;
        varargout{3}=e_artif;

    case 0 % Noise
        %%% Noise contribution %%%
        P_SN_se= bss_proj(se,[sources_space;noise_space]);
        e_noise=P_SN_se-P_S_se;

        %%% Artifacts contribution %%%
        e_artif=se-P_SN_se;

        %%% Output %%%
        varargout{1}=s_target;
        varargout{2}=e_interf;
        varargout{3}=e_noise;
        varargout{4}=e_artif;
end


function ActiveImportanceSampling = activeimportancesampling_vbmc(vp,gp,acqfun,acqinfo,options)
%ACTIVEIMPORTANCESAMPLING_VBMC Setup importance sampling acquisition functions

% This function samples from the base importance sampling (IS) density in
% three steps:
% 1) Use importance sampling-resampling (ISR) to sample from the
%    base IS density based on a proposal distribuion which is a mixture of
%    a smoothed variational posterior and on box-uniform mixture centered
%    around the current training points
% 2) Optionally use MCMC initialized with the ISR samples to sample from
%    the base IS density
% 3) Compute IS statistics and book-keeping

% Does the importance sampling step use the variational posterior?
isamplevp_flag = isfield(acqinfo,'importance_sampling_vp') ...
    && acqinfo.importance_sampling_vp;

% Do we simply sample from the variational posterior?
onlyvp_flag = isfield(acqinfo,'variational_importance_sampling') ...
    && acqinfo.variational_importance_sampling;

D = size(gp.X,2);

Ns_gp = numel(gp.post); % # GP hyperparameter samples

% Input space bounds and typical scales (for MCMC only)
widths = std(gp.X,[],1);
MaxBnd = 0.5;
diam = max(gp.X) - min(gp.X);
LB = min(gp.X) - MaxBnd*diam;
UB = max(gp.X) + MaxBnd*diam;

ActiveImportanceSampling.lnw = [];
ActiveImportanceSampling.Xa = [];
ActiveImportanceSampling.fs2a = [];

if onlyvp_flag
    %% Step 0: Simply sample from the variational posterior

    if ischar(options.ActiveImportanceSamplingMCMCSamples)
        K = vp.K;
        nvars = D;
        Na = ceil(eval(options.ActiveImportanceSamplingMCMCSamples));
    elseif isscalar(options.ActiveImportanceSamplingMCMCSamples)
        Na = ceil(options.ActiveImportanceSamplingMCMCSamples);
    else
        Na = 0;
    end

    if ~isfinite(Na) || ~isscalar(Na) || Na <= 0
        error('OPTIONS.ActiveImportanceSamplingMCMCSamples should be (or evaluate to) a positive integer.');
    end

    Xa = vbmc_rnd(vp,Na,0);
    [~,~,fmu,fs2] = gplite_pred(gp,Xa,[],[],1,0);

    if isfield(acqinfo,'mcmc_importance_sampling') && acqinfo.mcmc_importance_sampling

        % Compute fractional effective sample size (ESS)
        fESS = fess_vbmc(vp,fmu,Xa);

        % If fESS is less than thresh ==> major mismatch, do MCMC
        if fESS < options.ActiveImportanceSamplingfESSThresh
            Xa_old = Xa;

            if isamplevp_flag
                logpfun = @(x) log_isbasefun(x,acqfun,gp,vp);
            else
                logpfun = @(x) log_isbasefun(x,acqfun,gp,[]);
            end

            % Get MCMC options
            Nmcmc_samples = Na*options.ActiveImportanceSamplingMCMCThin;
            thin = 1;
            burnin = 0;
            sampleopts = get_mcmcopts([],thin,burnin);
            logPfuns = logpfun;
            W = Na;    % # walkers

            % Perform a single MCMC step for all samples
            Xa = eissample_lite(logPfuns,Xa,Nmcmc_samples,W,widths,LB,UB,sampleopts);
            Xa = Xa(end-Na+1:end,:);
            [~,~,fmu,fs2] = gplite_pred(gp,Xa,[],[],1,0);

            if 0
                hold off;
                scatter(Xa_old(:,1),Xa_old(:,2),'b'); hold on;
                scatter(Xa(:,1),Xa(:,2),'k'); hold on;
                drawnow;
            end
        end
    end

    if isamplevp_flag
        vlnpdf = max(vbmc_pdf(vp,Xa,0,1),log(realmin));
        lny = acqfun('islogf1',vlnpdf,[],[],fmu,fs2);
    else
        lny = acqfun('islogf1',[],[],[],fmu,fs2);
    end

    ActiveImportanceSampling.fs2a = fs2;
    ActiveImportanceSampling.lnw = lny';
    ActiveImportanceSampling.Xa = Xa;

else
    %% Step 1: Importance sampling-resampling

    Nvp_samples = options.ActiveImportanceSamplingVPSamples;
    Nbox_samples = options.ActiveImportanceSamplingBoxSamples;
    w_vp = Nvp_samples/(Nvp_samples + Nbox_samples);

    rect_delta = 2*std(gp.X);

    % Smoothed posterior for importance sampling-resampling
    if Nvp_samples > 0
        scale_vec = [0.05,0.2,1];
        %scale = sqrt(mean(var(gp.X,[],1)));

        vp_is = vp;
        for ii = 1:numel(scale_vec)
            vp_is.K = vp_is.K + vp.K;
            vp_is.w = [vp_is.w, vp.w];
            vp_is.mu = [vp_is.mu, vp.mu];
            vp_is.sigma = [vp_is.sigma, sqrt(vp.sigma.^2 + scale_vec(ii)^2)];
        end
        vp_is.w = vp_is.w/sum(vp_is.w);

        % Sample from smoothed posterior
        Xa_vp = vbmc_rnd(vp_is,Nvp_samples,0,0);
        [lnw,fs2a_vp] = activesample_proposalpdf(Xa_vp,gp,vp_is,w_vp,rect_delta,acqfun,vp,isamplevp_flag);
        ActiveImportanceSampling.lnw = [ActiveImportanceSampling.lnw, lnw'];
        ActiveImportanceSampling.Xa = [ActiveImportanceSampling.Xa; Xa_vp];
        ActiveImportanceSampling.fs2a = [ActiveImportanceSampling.fs2a; fs2a_vp];
    else
        vp_is = [];
    end

    % Box-uniform sampling around training inputs
    if Nbox_samples > 0
        jj = randi(size(gp.X,1),[1,Nbox_samples]);
        Xa_box = gp.X(jj,:) + bsxfun(@times,2*rand(numel(jj),D)-1,rect_delta);
        [lnw,fs2a_box] = activesample_proposalpdf(Xa_box,gp,vp_is,w_vp,rect_delta,acqfun,vp,isamplevp_flag);
        ActiveImportanceSampling.lnw = [ActiveImportanceSampling.lnw, lnw'];
        ActiveImportanceSampling.Xa = [ActiveImportanceSampling.Xa; Xa_box];
        ActiveImportanceSampling.fs2a = [ActiveImportanceSampling.fs2a; fs2a_box];
    end

    ActiveImportanceSampling.lnw(~isfinite(ActiveImportanceSampling.lnw)) = -Inf;
    % optimState.w = exp(optimState.lnw - max(optimState.lnw))';
    % optimState.w = optimState.w / sum(optimState.w);
    % 1./sum(optimState.w.^2)

    %% Step 2 (optional): MCMC sample

    Nmcmc_samples = options.ActiveImportanceSamplingMCMCSamples;

    if Nmcmc_samples > 0

        ActiveImportanceSampling_old = ActiveImportanceSampling;

        ActiveImportanceSampling.lnw = zeros(Ns_gp,Nmcmc_samples);
        ActiveImportanceSampling.Xa = zeros(Nmcmc_samples,D,Ns_gp);
        ActiveImportanceSampling.fs2a = zeros(Nmcmc_samples,Ns_gp);

        gp1 = gp;   % Consider only one GP sample at a time

        for s = 1:Ns_gp

            gp1.post = [];  % Assign current GP sample
            gp1.post = gp.post(s);

            if D == 2 && 0
                % We could use a quasi-random grid for D <= 2, but not implemented
                Xa = rand(Na,D).*(UB - LB) + LB;
                [~,~,fmu,optimState.fs2a] = gplite_pred(gp1,Xa,[],[],1,0);
                optimState.lnw = mean(fmu,2)';
                optimState.Xa = Xa;
            else
                if isamplevp_flag
                    logpfun = @(x) log_isbasefun(x,acqfun,gp1,vp);
                else
                    logpfun = @(x) log_isbasefun(x,acqfun,gp1,[]);
                end

                % Get MCMC options
                thin = options.ActiveImportanceSamplingMCMCThin;
                burnin = ceil(thin*Nmcmc_samples/2);
                sampleopts = get_mcmcopts(Nmcmc_samples,thin,burnin);

                logPfuns = logpfun;
                % sampleopts.TransitionOperators = {'transSliceSampleRD'};
                W = 2*(D+1);    % # walkers

                if 0
                    % Take starting points from high posterior density region
                    hpd_frac = 0.5;
                    N = numel(gp1.y);
                    N_hpd = min(N,max(W,round(hpd_frac*N)));
                    [~,ord] = sort(gp1.y,'descend');
                    X_hpd = gp1.X(ord(1:N_hpd),:);
                    x0 = X_hpd(randperm(N_hpd,min(W,N_hpd)),:);
                    x0 = bsxfun(@min,bsxfun(@max,x0,LB),UB);
                else
                    % Use importance sampling-resampling
                    [~,~,fmu,fs2] = gplite_pred(gp1,ActiveImportanceSampling_old.Xa,[],[],1,0);
                    lnw = ActiveImportanceSampling_old.lnw(s,:) + acqfun('islogf2',[],[],[],fmu,fs2)';
                    w = exp(bsxfun(@minus,lnw,max(lnw,[],2)));
                    x0 = zeros(W,D);
                    for ii = 1:W
                        idx = catrnd(w,1);
                        w(idx) = 0;
                        x0(ii,:) = ActiveImportanceSampling_old.Xa(idx,:);
                    end
                end

                [Xa,logp] = eissample_lite(logPfuns,x0,Nmcmc_samples,W,widths,LB,UB,sampleopts);
                [~,~,fmu,fs2] = gplite_pred(gp1,Xa,[],[],1,0);

                % Fixed log weight for importance sampling (log fixed integrand)
                if isamplevp_flag
                    vlnpdf = max(vbmc_pdf(vp,Xa,0,1),log(realmin));
                    lny = acqfun('islogf1',vlnpdf,[],[],fmu,fs2);
                else
                    lny = acqfun('islogf1',[],[],[],fmu,fs2);
                end
                % lny = lny - warpvars_vbmc(Xa,'logp',vp.trinfo);

                ActiveImportanceSampling.fs2a(:,s) = fs2;
                ActiveImportanceSampling.lnw(s,:) = bsxfun(@minus,lny',logp');
                ActiveImportanceSampling.Xa(:,:,s) = Xa;
            end

        end
    end
end

if 0
    hold off;
    scatter(ActiveImportanceSampling.Xa(:,1),ActiveImportanceSampling.Xa(:,2)); hold on;
    % scatter(x0(:,1),x0(:,2),'ro','MarkerFaceColor','r')
    xlim([-2,2]);
    ylim([-2,2]);
    drawnow;
end

%% Step 3: Pre-compute quantities for importance sampling calculations

% Precompute cross-kernel matrix on importance points
Kax_mat = zeros(size(ActiveImportanceSampling.Xa,1),size(gp.X,1),Ns_gp);
Ctmp_mat = zeros(size(gp.X,1),size(ActiveImportanceSampling.Xa,1),Ns_gp);
for s = 1:Ns_gp
    if size(ActiveImportanceSampling.Xa,3) == 1
        Xa = ActiveImportanceSampling.Xa;
    else
        Xa(:,:) = ActiveImportanceSampling.Xa(:,:,s);
    end
    hyp = gp.post(s).hyp;
    L = gp.post(s).L;
    Lchol = gp.post(s).Lchol;
    sn2_eff = 1/gp.post(s).sW(1)^2;
    if gp.covfun(1) == 1    % Hard-coded SE-ard for speed
        ell = exp(hyp(1:D))';
        sf2 = exp(2*hyp(D+1));
        Kax_tmp = sq_dist(Xa*diag(1./ell),gp.X*diag(1./ell));
        Kax_mat(:,:,s) = sf2 * exp(-Kax_tmp/2);
    else
        error('Other covariance functions not supported yet.');
    end

    if Lchol
        Ctmp_mat(:,:,s) = (L\(L'\Kax_mat(:,:,s)'))/sn2_eff;
    else
        Ctmp_mat(:,:,s) = (L*Kax_mat(:,:,s)');
    end
end
ActiveImportanceSampling.Kax_mat = Kax_mat;
ActiveImportanceSampling.Ctmp_mat = Ctmp_mat;

% Precompute integrated mean basis function on importance points
if isfield(gp,'intmeanfun') && gp.intmeanfun > 0
    plus_idx = gp.intmeanfun_var > 0;
    if size(ActiveImportanceSampling.Xa,3) == 1
        Ha = gplite_intmeanfun(ActiveImportanceSampling.Xa,gp.intmeanfun);
        ActiveImportanceSampling.Ha = Ha(plus_idx,:);
    else
        for s = 1:Ns_gp
            Ha = gplite_intmeanfun(ActiveImportanceSampling.Xa(:,:,s),gp.intmeanfun);
            ActiveImportanceSampling.Ha(:,:,s) = Ha(plus_idx,:);
        end
    end
end

end

%--------------------------------------------------------------------------
function [lnw,fs2] = activesample_proposalpdf(Xa,gp,vp_is,w_vp,rect_delta,acqfun,vp,isamplevp_flag)
%ACTIVESAMPLE_PROPOSALPDF Compute importance weights for proposal pdf

[N,D] = size(gp.X);
Na = size(Xa,1);

[~,~,fmu,fs2] = gplite_pred(gp,Xa,[],[],1,0);

Ntot = 1 + N; % Total number of mixture elements

if w_vp < 1; templpdf = zeros(Na,Ntot); end

% Mixture of variational posteriors
if w_vp > 0
    logflag = true;
    templpdf(:,1) = vbmc_pdf(vp_is,Xa,0,logflag) + log(w_vp);
else
    templpdf(:,1) = -Inf;
end

% Fixed log weight for importance sampling (log fixed integrand)
if isamplevp_flag
    vlnpdf = max(vbmc_pdf(vp,Xa,0,1),log(realmin));
    lny = acqfun('islogf1',vlnpdf,[],[],fmu,fs2);
else
    lny = acqfun('islogf1',[],[],[],fmu,fs2);
end
% lny = lny - warpvars_vbmc(Xa,'logp',vp.trinfo);

% Mixture of box-uniforms
if w_vp < 1
    VV = prod(2*rect_delta);

    for ii = 1:N
        templpdf(:,ii+1) = log(all(abs(bsxfun(@minus,Xa,gp.X(ii,:))) < rect_delta,2) / VV / N * (1 - w_vp));
    end

    mmax = max(templpdf,[],2);
    lpdf = log(sum(exp(templpdf - mmax),2));
    lnw = bsxfun(@minus,lny,lpdf + mmax);
else
    lnw = bsxfun(@minus,lny,templpdf);
end

end

%--------------------------------------------------------------------------
function y = log_isbasefun(x,acqfun,gp,vp)
%LOG_ISBASEFUN Base importance sampling proposal log pdf

[fmu,fs2] = gplite_pred(gp,x);
if isempty(vp)
    y = acqfun('islogf',[],[],[],fmu,fs2);
else
    vlnpdf = max(vbmc_pdf(vp,x,0,1),log(realmin));
    y = acqfun('islogf',vlnpdf,[],[],fmu,fs2);
end

end

%--------------------------------------------------------------------------
%SQ_DIST Compute matrix of all pairwise squared distances between two sets
% of vectors, stored in the columns of the two matrices, a (of size n-by-D)
% and b (of size m-by-D).
function C = sq_dist(a,b)

n = size(a,1);
m = size(b,1);
mu = (m/(n+m))*mean(b,1) + (n/(n+m))*mean(a,1);
a = bsxfun(@minus,a,mu); b = bsxfun(@minus,b,mu);
C = bsxfun(@plus,sum(a.*a,2),bsxfun(@minus,sum(b.*b,2)',2*a*b'));
C = max(C,0);

end

%--------------------------------------------------------------------------
function sampleopts = get_mcmcopts(Ns,thin,burnin)
%GET_MCMCOPTS Get standard MCMC options.

if nargin < 2 || isempty(thin); thin = 1; end
if nargin < 3; burnin = []; end

sampleopts.Thin = thin;
if isempty(burnin)
    sampleopts.Burnin = ceil(sampleopts.Thin*Ns/2);
else
    sampleopts.Burnin = burnin;
end
sampleopts.Display = 'off';
sampleopts.Diagnostics = false;
sampleopts.VarTransform = false;
sampleopts.InversionSample = false;
sampleopts.FitGMM = false;

end

%--------------------------------------------------------------------------
function x = catrnd(p,n)
%CATRND Sample from categorical distribution.

maxel = 1e6;
Nel = n*numel(p);
stride = ceil(maxel/numel(p));

cdf(1,:) = cumsum(p);
u = rand(n,1)*cdf(end);

% Split for memory reasons
if Nel <= maxel
    x = sum(bsxfun(@lt, cdf, u),2) + 1;
else
    x = zeros(n,1);
    idx_min = 1;
    while idx_min <= n
        idx_max = min(idx_min+stride-1,n);
        idx = idx_min:idx_max;
        x(idx) = sum(bsxfun(@lt, cdf, u(idx)),2) + 1;
        idx_min = idx_max+1;
    end
end

end


function [V, converged, i] = newtonpf_I_cart(Ybus, Sbus, V0, ref, pv, pq, mpopt)
%NEWTONPF_I_CART  Solves power flow using full Newton's method (current/cartesian)
%   [V, CONVERGED, I] = NEWTONPF_I_CART(YBUS, SBUS, V0, REF, PV, PQ, MPOPT)
%
%   Solves for bus voltages using a full Newton-Raphson method, using nodal
%   current balance equations and cartesian coordinate representation of
%   voltages, given the following inputs:
%       YBUS  - full system admittance matrix (for all buses)
%       SBUS  - handle to function that returns the complex bus power
%               injection vector (for all buses), given the bus voltage
%               magnitude vector (for all buses)
%       V0    - initial vector of complex bus voltages
%       REF   - bus index of reference bus (voltage ang reference & gen slack)
%       PV    - vector of bus indices for PV buses
%       PQ    - vector of bus indices for PQ buses
%       MPOPT - (optional) MATPOWER option struct, used to set the
%               termination tolerance, maximum number of iterations, and
%               output options (see MPOPTION for details).
%
%   The bus voltage vector contains the set point for generator
%   (including ref bus) buses, and the reference angle of the swing
%   bus, as well as an initial guess for remaining magnitudes and
%   angles.
%
%   Returns the final complex voltages, a flag which indicates whether it
%   converged or not, and the number of iterations performed.
%
%   See also RUNPF, NEWTONPF, NEWTONPF_S_CART, NEWTONPF_I_POLAR.

%   MATPOWER
%   Copyright (c) 1996-2019, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Baljinnyam Sereeter, Delft University of Technology
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% default arguments
if nargin < 7
    mpopt = mpoption;
end

%% options
tol         = mpopt.pf.tol;
max_it      = mpopt.pf.nr.max_it;
lin_solver  = mpopt.pf.nr.lin_solver;

%% initialize
converged = 0;
i = 0;
V = V0;
Vm = abs(V);
Vmpv = Vm(pv);
n = length(V0);

%% set up indexing for updating V
npv = length(pv);
npq = length(pq);
j1 = 1;         j2 = npv;           %% j1:j2 - Q of pv buses
j3 = j2 + 1;    j4 = j2 + npq;      %% j3:j4 - Vr of pq buses
j5 = j4 + 1;    j6 = j4 + npv;      %% j5:j6 - Vr of pv buses
j7 = j6 + 1;    j8 = j6 + npq;      %% j7:j8 - Vi of pq buses
j9 = j8 + 1;    j10= j8 + npv;      %% j9:j10- Vi of pv buses

%% evaluate F(x0)
Sb = Sbus(Vm);
Sb(pv) = real(Sb(pv)) + 1j * imag(V(pv) .* conj(Ybus(pv, :) * V));
mis = Ybus * V - conj(Sb ./ V);
F = [   real(mis([pv; pq]));
        imag(mis([pv; pq]));
        V(pv) .* conj(V(pv)) - Vmpv.^2  ];

%% check tolerance
normF = norm(F, inf);
if mpopt.verbose > 1
    fprintf('\n it   max Ir & Ii mismatch (p.u.)');
    fprintf('\n----  ---------------------------');
    fprintf('\n%3d        %10.3e', i, normF);
end
if normF < tol
    converged = 1;
    if mpopt.verbose > 1
        fprintf('\nConverged!\n');
    end
end

%% attempt to pick fastest linear solver, if not specified
if isempty(lin_solver)
    nx = length(F);
    if nx <= 10 || have_feature('octave')
        lin_solver = '\';       %% default \ operator
    else    %% MATLAB and nx > 10 or Octave and nx > 2000
        lin_solver = 'LU3';     %% LU decomp with 3 output args, AMD ordering
    end
end

%% do Newton iterations
while (~converged && i < max_it)
    %% update iteration counter
    i = i + 1;

    %% evaluate Jacobian
    dImis_dQ = sparse(pv, pv, 1j./conj(V(pv)), n, n);
    dV2_dVr = sparse(1:npv, npq+(1:npv), 2*real(V(pv)), npv, npv+npq);
    dV2_dVi = sparse(1:npv, npq+(1:npv), 2*imag(V(pv)), npv, npv+npq);
    [dImis_dVr, dImis_dVi] = dImis_dV(Sb, Ybus, V, 1);

    %% handling of derivatives for voltage dependent loads
    %% (not yet implemented) goes here

    j11 = real(dImis_dQ([pv; pq], pv));
    j12 = real(dImis_dVr([pv; pq], [pq; pv]));
    j13 = real(dImis_dVi([pv; pq], [pq; pv]));
    j21 = imag(dImis_dQ([pv; pq], pv));
    j22 = imag(dImis_dVr([pv; pq], [pq; pv]));
    j23 = imag(dImis_dVi([pv; pq], [pq; pv]));
    j31 = sparse(npv, npv);
    j32 = dV2_dVr;
    j33 = dV2_dVi;

    J = [   j11 j12 j13;
            j21 j22 j23;
            j31 j32 j33;    ];

    %% compute update step
    dx = mplinsolve(J, -F, lin_solver);

    %% update voltage
    if npv
        V(pv) = V(pv) + dx(j5:j6) + 1j * dx(j9:j10);
        Sb(pv) = real(Sb(pv)) + 1j * (imag(Sb(pv)) + dx(j1:j2));
    end
    if npq
        V(pq) = V(pq) + dx(j3:j4) + 1j * dx(j7:j8);
    end

    %% evalute F(x)
    mis = Ybus * V - conj(Sb ./ V);
    F = [   real(mis([pv; pq]));
            imag(mis([pv; pq]));
            V(pv) .* conj(V(pv)) - Vmpv.^2  ];

    %% check for convergence
    normF = norm(F, inf);
    if mpopt.verbose > 1
        fprintf('\n%3d        %10.3e', i, normF);
    end
    if normF < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nNewton''s method power flow (current balance, cartesian) converged in %d iterations.\n', i);
        end
    end
end

if mpopt.verbose
    if ~converged
        fprintf('\nNewton''s method power flow (current balance, cartesian) did not converge in %d iterations.\n', i);
    end
end


% Zhou, Zongwei, et al. "Fine-tuning convolutional neural networks for biomedical image analysis: actively and incrementally."
% IEEE conference on computer vision and pattern recognition, Hawaii. 2017.

% Author: [Zongwei Zhou](http://www.zongweiz.com)
% Email: zongweiz@asu.edu
% Last modified: Jan.26.2017

%% Table 1. examples of seven different patterns
% Relationships among seven prediction patterns and six AIFT methods in active candidate selection. We assume that
% a candidate has 11 patches, and their probabilities predicted by the current CNN are listed in Column 2. AIFT Entropyα,
% Diversityα, and (Entropy+Diversity)α operate on the top or bottom α percent of the candidate’s patches based on the majority
% prediction as described in Sec. 3.3. In this illustration, we choose α to be 1/4, meaning that the selection criterion (Eq. 3) is
% computed based on 3 patches within each candidate. The first choice of each method is highlighted in yellow and the second
% choice is in light yellow.

function Alg_Analysis()

close all; clear; clc;

% A, B, C, D, E, F represents outputs from CNN after Softmax Layer belong to one candidate.
A = [0.4 0.4 0.4 0.5 0.5 0.5 0.5 0.5 0.5 0.6 0.6];
B = [0.0 0.1 0.2 0.3 0.4 0.4 0.6 0.7 0.8 1.0 1.0];
C = [0.0 0.0 0.0 0.1 0.1 0.9 0.9 1.0 1.0 1.0 1.0];
D = [1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.9 0.9 0.9 0.9];
E = [0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.1 0.1 0.1 0.1];
F  = [0.0 0.0 0.1 0.1 0.1 0.1 0.2 0.2 0.3 0.9 1.0];
G  = [0.0 0.1 0.7 0.8 0.8 0.9 0.9 0.9 0.9 1.0 1.0];

%% Produce values in Table 1 by row pattern A
disp('Table 1. pattern A');
disp(['Entropy_A = ', num2str(roundn(compute_matrix(A, 1, 0, 1), -2))]);
disp(['Entropy^1/4_A = ', num2str(roundn(compute_matrix(A, 1, 0, 0.25), -2))]);
disp(['Diversity_A = ', num2str(roundn(compute_matrix(A, 0, 1, 1), -2))]);
disp(['Diversity^1/4_A = ', num2str(roundn(compute_matrix(A, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_A = ', num2str(roundn(compute_matrix(A, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_A = ', num2str(roundn(compute_matrix(A, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern B
disp('Table 1. pattern B');
disp(['Entropy_B = ', num2str(roundn(compute_matrix(B, 1, 0, 1), -2))]);
disp(['Entropy^1/4_B = ', num2str(roundn(compute_matrix(B, 1, 0, 0.25), -2))]);
disp(['Diversity_B = ', num2str(roundn(compute_matrix(B, 0, 1, 1), -2))]);
disp(['Diversity^1/4_B = ', num2str(roundn(compute_matrix(B, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_B = ', num2str(roundn(compute_matrix(B, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_B = ', num2str(roundn(compute_matrix(B, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern C
disp('Table 1. pattern C');
disp(['Entropy_C = ', num2str(roundn(compute_matrix(C, 1, 0, 1), -2))]);
disp(['Entropy^1/4_C = ', num2str(roundn(compute_matrix(C, 1, 0, 0.25), -2))]);
disp(['Diversity_C = ', num2str(roundn(compute_matrix(C, 0, 1, 1), -2))]);
disp(['Diversity^1/4_C = ', num2str(roundn(compute_matrix(C, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_C = ', num2str(roundn(compute_matrix(C, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_C = ', num2str(roundn(compute_matrix(C, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern D
disp('Table 1. pattern D');
disp(['Entropy_D = ', num2str(roundn(compute_matrix(D, 1, 0, 1), -2))]);
disp(['Entropy^1/4_D = ', num2str(roundn(compute_matrix(D, 1, 0, 0.25), -2))]);
disp(['Diversity_D = ', num2str(roundn(compute_matrix(D, 0, 1, 1), -2))]);
disp(['Diversity^1/4_D = ', num2str(roundn(compute_matrix(D, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_D = ', num2str(roundn(compute_matrix(D, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_D = ', num2str(roundn(compute_matrix(D, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern E
disp('Table 1. pattern E');
disp(['Entropy_E = ', num2str(roundn(compute_matrix(E, 1, 0, 1), -2))]);
disp(['Entropy^1/4_E = ', num2str(roundn(compute_matrix(E, 1, 0, 0.25), -2))]);
disp(['Diversity_E = ', num2str(roundn(compute_matrix(E, 0, 1, 1), -2))]);
disp(['Diversity^1/4_E = ', num2str(roundn(compute_matrix(E, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_E = ', num2str(roundn(compute_matrix(E, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_E = ', num2str(roundn(compute_matrix(E, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern F
disp('Table 1. pattern F');
disp(['Entropy_F = ', num2str(roundn(compute_matrix(F, 1, 0, 1), -2))]);
disp(['Entropy^1/4_F = ', num2str(roundn(compute_matrix(F, 1, 0, 0.25), -2))]);
disp(['Diversity_F = ', num2str(roundn(compute_matrix(F, 0, 1, 1), -2))]);
disp(['Diversity^1/4_F = ', num2str(roundn(compute_matrix(F, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_F = ', num2str(roundn(compute_matrix(F, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_F = ', num2str(roundn(compute_matrix(F, 1, 1, 0.25), -2))]);
disp(' ');

%% Produce values in Table 1 by row pattern G
disp('Table 1. pattern G');
disp(['Entropy_G = ', num2str(roundn(compute_matrix(G, 1, 0, 1), -2))]);
disp(['Entropy^1/4_G = ', num2str(roundn(compute_matrix(G, 1, 0, 0.25), -2))]);
disp(['Diversity_G = ', num2str(roundn(compute_matrix(G, 0, 1, 1), -2))]);
disp(['Diversity^1/4_G = ', num2str(roundn(compute_matrix(G, 0, 1, 0.25), -2))]);
disp(['(Entropy+Diversity)_G = ', num2str(roundn(compute_matrix(G, 1, 1, 1), -2))]);
disp(['(Entropy+Diversity)^1/4_G = ', num2str(roundn(compute_matrix(G, 1, 1, 0.25), -2))]);
disp(' ');

% %% Visualize the diversity equation
% x = 0:0.01:1;
% y = 0:0.01:1;
% [x,y] = meshgrid(x,y);
% div = (x-y).*log(x./(y+eps))+(y-x).*log((1-x)./(1-y+eps));
% mesh(x,y,div)

end

%% Compute matrix R (Eq. 3 in the paper)
function [S] = compute_matrix(p, lamda_1, lamda_2, alpha)

% lamda_1 and lamda_2 are trade-offs between entropy and diversity
% lamda_1: turn on/off entropy (1 or 0)
% lamda_2: turn on/off diversity (1 or 0)

% Handling noisy labels via majority selection (see Sec 3.3)
top = round(length(p) * alpha);
p = Majority(p,top);

% Initialize the R matrix
R = zeros(length(p));
for i = 1:length(p)
    for j = 1:i
        if i == j % entropy computed for diagonal of R
            R(i,i) = - lamda_1 * (p(i)*log(p(i)+eps)+(1-p(i))*log(1-p(i)+eps));
        else % diversity computed for rest of R
            R(i,j) = lamda_2 * ((p(i)-p(j)) * log((p(i)+eps)/(p(j)+eps)) + ...
                     (1-p(i)-1+p(j)) * log((1-p(i)+eps)/(1-p(j)+eps)));
            R(j,i) = R(i,j);
        end
    end
end

% Compute active selecting value from matrix R for this candidate
% by numerical sum of R (see Alg 1 line 11)
S = sum(R(:));

end

%% Sec 3.3. Handling noisy labels via majority selection
% eq. 4, calculate the mean and sort based on dominate predictions.
function [p] = Majority(p,top)

m = mean(p);
if m > 0.5
    p = sort(p,'descend');
else
    p = sort(p,'ascend');
end
p = p(1:top);

end


load('table_T.mat')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Params for UAV
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% UAV == Zagi
%     p_drone.mass = 1.56;
%     p_drone.Jx   = 0.1147;
%     p_drone.Jy   = 0.0576;
%     p_drone.Jz   = 0.1712;
%     p_drone.Jxz  = 0.0015;

%%% UAV == Aerosonde
p_drone.mass = 13.5;
p_drone.Jx   = 0.8244;
p_drone.Jy   = 1.135;
p_drone.Jz   = 1.759;
p_drone.Jxz  = .1204;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute gammas
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p_drone.gamma  = p_drone.Jx*p_drone.Jz - p_drone.Jxz^2;
p_drone.gamma1 = p_drone.Jxz*(p_drone.Jx-p_drone.Jy+p_drone.Jz)/p_drone.gamma;
p_drone.gamma2 = (p_drone.Jz*(p_drone.Jz-p_drone.Jy)+p_drone.Jxz^2)/p_drone.gamma;
p_drone.gamma3 = p_drone.Jz/p_drone.gamma;
p_drone.gamma4 = p_drone.Jxz/p_drone.gamma;
p_drone.gamma5 = (p_drone.Jz-p_drone.Jx)/p_drone.Jy;
p_drone.gamma6 = p_drone.Jxz/p_drone.Jy;
p_drone.gamma7 = ((p_drone.Jx-p_drone.Jy)*p_drone.Jx+p_drone.Jxz^2)/p_drone.gamma;
p_drone.gamma8 = p_drone.Jx/p_drone.gamma;

% Aerodynamic coefficients
p_drone.S_wing          = 0.55;     % Surface of the plane
p_drone.b              = 2.8956;
p_drone.c              = 0.18994;
p_drone.S_prop         = 0.2027;    % Surface of the propeller
p_drone.k_motor        = 80;        % k motor
p_drone.k_TP           = 0;
p_drone.k_omega        = 0;
p_drone.e              = 0.9;

p_drone.kb = @(Va) 0.5*p_drone.b/Va;
p_drone.kc = @(Va) 0.5*p_drone.c/Va;

% Longitudinal dynamics:
%   lift and drag (F_L, F_D)
p_drone.CL0            = 0.28;     % C_L_0
p_drone.CL_alpha       = 3.45;     % C_L_alpha
p_drone.CL_q           = 0.0;      % C_L_q
p_drone.CL_de          = -0.36;    % C_L_delta_e
p_drone.CD0            = 0.03;     % C_D_0
p_drone.CD_alpha       = 0.30;     % C_D_alpha
p_drone.CD_p           = 0.0437;   % C_D_p
p_drone.CD_q           = 0.0;      % C_D_q
p_drone.CD_de          = 0.0;      % C_D_delta_e

%   moment around y (m)
p_drone.Cm0            = -0.02338; % C_m_0
p_drone.Cm_alpha       = -0.38;    % C_m_alpha
p_drone.Cm_q           = -3.6;     % C_m_q
p_drone.Cm_de          = -0.5;     % C_m_delta_e

% Lateral dynamics
%   F_y
p_drone.CY0            = 0.0;
p_drone.CY_beta        = -0.98;
p_drone.CY_p           = 0.0;
p_drone.CY_r           = 0.0;      % C_Y_r
p_drone.CY_da          = 0.0;      % C_Y_delta_a
p_drone.CY_dr          = -0.17;    % C_Y_delta_r

%   moment around x (l) and z (n)
p_drone.Cl0            = 0.0;      % C_l_0
p_drone.Cl_beta        = -0.12;    % Cl_beta
p_drone.Cl_p           = -0.26;    % C_l_p
p_drone.Cl_r           = 0.14;     % C_l_r
p_drone.Cl_da          = 0.08;     % C_l_delta_aileron
p_drone.Cl_dr          = 0.105;    % C_l_delta_rudder
p_drone.Cn0            = 0.0;      % C_n_0
p_drone.Cn_beta        = 0.25;     % C_n_beta
p_drone.Cn_p           = 0.022;    % C_n_p
p_drone.Cn_r           = -0.35;    % C_n_r
p_drone.Cn_da          = 0.06;     % C_n_delta_a
p_drone.Cn_dr          = -0.032;   % C_n_delta_r

p_drone.C_prop         = 1.0;      % propeller proportional coeff fx=Sprop*Cprop*dp
p_drone.M              = 50;
p_drone.epsilon        = 0.1592;
p_drone.alpha0         = 0.4712;

p_drone.Cp0     = p_drone.gamma3 * p_drone.Cl0     + p_drone.gamma4 * p_drone.Cn0;
p_drone.Cp_beta = p_drone.gamma3 * p_drone.Cl_beta + p_drone.gamma4 * p_drone.Cn_beta;
p_drone.Cp_p    = p_drone.gamma3 * p_drone.Cl_p    + p_drone.gamma4 * p_drone.Cn_p;
p_drone.Cp_r    = p_drone.gamma3 * p_drone.Cl_r    + p_drone.gamma4 * p_drone.Cn_r;
p_drone.Cp_da   = p_drone.gamma3 * p_drone.Cl_da   + p_drone.gamma4 * p_drone.Cn_da;
p_drone.Cp_dr   = p_drone.gamma3 * p_drone.Cl_dr   + p_drone.gamma4 * p_drone.Cn_dr;
p_drone.Cr0     = p_drone.gamma4 * p_drone.Cl0     + p_drone.gamma8 * p_drone.Cn0;
p_drone.Cr_b    = p_drone.gamma4 * p_drone.Cl_beta + p_drone.gamma8 * p_drone.Cn_beta;
p_drone.Cr_p    = p_drone.gamma4 * p_drone.Cl_p    + p_drone.gamma8 * p_drone.Cn_p;
p_drone.Cr_r    = p_drone.gamma4 * p_drone.Cl_r    + p_drone.gamma8 * p_drone.Cn_r;
p_drone.Cr_da   = p_drone.gamma4 * p_drone.Cl_da   + p_drone.gamma8 * p_drone.Cn_da;
p_drone.Cr_dr   = p_drone.gamma4 * p_drone.Cl_dr   + p_drone.gamma8 * p_drone.Cn_dr;

% first cut at initial conditions
% p_drone.pn_T    = xyu_T(1);  % initial North position
% p_drone.pe_T    = xyu_T(2);  % initial East position
% p_drone.pd_T    = xyu_T(3);  % initial Down position (negative altitude)
% p_drone.vx_T    = xyu_T(4); % initial velocity along body x-axis
% p_drone.vy_T    = xyu_T(5);  % initial velocity along body y-axis
% p_drone.vz_T    = xyu_T(6);  % initial velocity along body z-axis
% p_drone.phi_T   = xyu_T(7);  % initial roll angle
% p_drone.theta_T = xyu_T(8);  % initial pitch angle
% p_drone.psi_T   = xyu_T(9);  % initial yaw angle
% p_drone.p_T     = xyu_T(10);  % initial body frame roll rate
% p_drone.q_T     = xyu_T(11);  % initial body frame pitch rate
% p_drone.r_T     = xyu_T(12);  % initial body frame yaw rate

% p_drone.Va_T = xyu_T(13);
% p_drone.alpha_T = xyu_T(14);
% p_drone.beta_T = xyu_T(15);

% p_drone.de_T = xyu_T(16);
% p_drone.da_T = xyu_T(17);
% p_drone.dr_T = xyu_T(18);
% p_drone.dt_T = xyu_T(19);

% Values for the Aerosonde UAV
p_drone.Va0 = 35;        % m/s (~85 mph)

% Trim conditions used for static PID tuning
Va_Td = 35;
gamma_Td = deg2rad(0);
R_Td = Inf;

Td = [Va_Td, gamma_Td, R_Td];

% Compute PID control
% p_drone.C = compute_pid_controller(Td, P)
[Va_idx, gamma_idx, R_idx]  = Td2idx(Td);
xyu_T = table_T(:,Va_idx, gamma_idx, R_idx);
p_drone.phi_T   = 0;  % initial roll angle
p_drone.theta_T = 0;  % initial pitch angle
p_drone.psi_T   = 0;  % initial yaw angle
p_drone.Va_T = 35;
[a,TF] = compute_tf_model(xyu_T, p_drone, p_physics);
% PVa_T = xyu_T(13);  % Extract trimmed state

% Altitude state machine parameters
p_drone.altitude_take_off_zone = 40;
p_drone.h_theta_hold_zone      = 10;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Tuning PID loops

% Lateral dynamics
% Roll_aileron-hold
ephi_max = deg2rad(15);
p_drone.kp_phi = p_drone.da_max / ephi_max;
ksi_phi = KSI_OPT;
wn_phi = sqrt(p_drone.kp_phi * a.phi2);
p_drone.kd_phi = (2*ksi_phi*wn_phi - a.phi1) / a.phi2;

p_drone.tau_phi = 0.05; % [seconds] <=> 1 autopilot loop

% Course_roll-hold
W_chi_phi = 10; % Bandwith separation factor
% Between 5 and 10. Safety corresponds to 10.
wn_chi = wn_phi / W_chi_phi; %[rad/sec]
ksi_chi = KSI_OPT;
p_drone.kp_chi = 2*ksi_chi*wn_chi*p_drone.Va_T/p_physics.gravity;
p_drone.ki_chi = wn_chi^2*p_drone.Va_T/p_physics.gravity;

% Sideslip_rudder-hold
%TODO

% Longitudinal dynamics
% Pitch_elevator-hold
etheta_max  = deg2rad(10);
p_drone.kp_theta  = p_drone.de_max / etheta_max * sign(a.theta3);
wn_theta = sqrt(a.theta2 + p_drone.kp_theta*a.theta3);
ksi_theta = KSI_OPT;
if isfinite(a.theta3)
    p_drone.kd_theta  = (2*ksi_theta*wn_theta - a.theta1) / a.theta3;
else
    p_drone.kd_theta  = 0;
    disp("kd_theta is null!");
end
p_drone.tau_theta = 0.1; % [seconds] <=> 10 autopilot loops

% Altitude_pitch-hold
% Scale factor between theta_c and theta
K_theta_DC = p_drone.kp_theta*a.theta3/(a.theta2+p_drone.kp_theta*a.theta3);
W_h_theta = 10; % Between 5 and 15. Safety corresponds to 15.
wn_h = wn_theta / W_h_theta; % [rad/sec]
ksi_h = 1;
p_drone.kp_h  = 2*ksi_h*wn_h/(K_theta_DC*p_drone.Va_T);
p_drone.ki_h  = wn_h^2 / (K_theta_DC*p_drone.Va_T);

% Airspeed_thrust-hold
wn_va_dt = 10; % [rad/sec] Need to be tuned.
ksi_va_dt = 1;
p_drone.kp_va_dt = (2*ksi_va_dt*wn_va_dt - a.va1)/a.va2;
p_drone.ki_va_dt = wn_va_dt^2/a.va2;

% Airspeed_pitch-hold
W_va_theta = 7; % [rad/sec] 10 by default, can be lower.
wn_va_theta = wn_theta / W_va_theta;
ksi_va_theta = KSI_OPT;
p_drone.kp_va_theta = (a.va1 - 2*ksi_va_theta*wn_va_theta)/(K_theta_DC*p_physics.gravity);
p_drone.ki_va_theta = -wn_va_theta^2/(K_theta_DC*p_physics.gravity);

% first cut at initial conditions
p_drone.pn_T    = xyu_T(1);  % initial North position
p_drone.pe_T    = xyu_T(2);  % initial East position
p_drone.pd_T    = xyu_T(3)-200;  % initial Down position (negative altitude)
p_drone.vx_T    = xyu_T(4); % initial velocity along body x-axis
p_drone.vy_T    = xyu_T(5);  % initial velocity along body y-axis
p_drone.vz_T    = xyu_T(6);  % initial velocity along body z-axis
p_drone.phi_T   = xyu_T(7);  % initial roll angle
p_drone.theta_T = xyu_T(8);  % initial pitch angle
p_drone.psi_T   = xyu_T(9);  % initial yaw angle
p_drone.p_T     = xyu_T(10);  % initial body frame roll rate
p_drone.q_T     = xyu_T(11);  % initial body frame pitch rate
p_drone.r_T     = xyu_T(12);  % initial body frame yaw rate

p_drone.Va_T    = xyu_T(13);
p_drone.alpha_T = xyu_T(14);
p_drone.beta_T  = xyu_T(15);

p_drone.de_T    = xyu_T(16);
p_drone.da_T    = xyu_T(17);
p_drone.dr_T    = xyu_T(18);
p_drone.dt_T    = xyu_T(19);

run('param_sensors')


function rhs = globNedRHSVIFE3D(fun, mesh, fem, femI, dind, vind)

%% USAGE: generate global load vector on a tetrahedral mesh
%
% INPUTS:
% fun --- the load function from PDE (pde.f)
% mesh --- a struct data contains mesh information.
% fem --- global DoF for test function space
% dind --- derivative info for test function
%            d = [0,0,0]: function value
%            d = [1,0,0]: Dx value
%            d = [0,1,0]: Dy value
%            d = [0,0,1]: Dz value
%
% OUTPUTS:
% rhs --- global rhs vector

% Last Modified: 08/07/2020 by Xu Zhang

%% 1. RHS on non-interface elements
dof = size(fem.g2ldof,2);  nloc = dof;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);
AN = fem.area(ntID); gw = fem.gw;
gxN = fem.gx(ntID,:); gyN = fem.gy(ntID,:); gzN = fem.gz(ntID,:);
X = zeros(nloc*ntN, 1);

feEvalBas = @EvalNed1Bas3D;
IfeEvalBas = @evalNed1IFEBas3D;

fN = feval(fun,gxN,gyN,gzN);
ind = 0;
I = reshape(femI.g2ldof(ntID,1:6),nloc*ntN,1);
for i = 1:dof
    ibas = feEvalBas(fem.bas, ntID, gxN, gyN, gzN, i, dind, vind);
    X(ind+1:ind+ntN) = AN.*sum((ibas.*fN).*gw',2).*fem.t_e_orit(ntID,i);
    ind = ind + ntN;
end
rhsN = sparse(I,1,X,length(femI.gdof),1);

%% 2. RHS on interface elements
AI1 = femI.area1; gw = femI.gw; gxI1 = femI.gx1; gyI1 = femI.gy1; gzI1 = femI.gz1;
ntI1 = size(femI.tType1,1); % not number of interface element, but quadrature element
nloc1 = 12; X1 = zeros(nloc1*ntI1, 1);
fI1 = feval(fun,gxI1,gyI1,gzI1);
ind = 0;
I1 = reshape(femI.tType1,nloc1*ntI1,1);
for i = 1:nloc1
    ibas = femI.basUType1(:,vind,i);
    X1(ind+1:ind+ntI1) = AI1.*sum((ibas.*fI1).*gw',2);
    ind = ind + ntI1;
end
rhsI1 = sparse(I1,1,X1,size(femI.gdof,1),1);

AI2 = femI.area2; gw = femI.gw; gxI2 = femI.gx2; gyI2 = femI.gy2; gzI2 = femI.gz2;
ntI2 = size(femI.tType2,1); % not number of interface element, but quadrature element
nloc2 = 14; X2 = zeros(nloc2*ntI2, 1);
fI2 = feval(fun,gxI2,gyI2,gzI2);
ind = 0;
I2 = reshape(femI.tType2,nloc2*ntI2,1);
for i = 1:nloc2
    ibas = femI.basUType2(:,vind,i);
    X2(ind+1:ind+ntI2) = AI2.*sum((ibas.*fI2).*gw',2);
    ind = ind + ntI2;
end
rhsI2 = sparse(I2,1,X2,size(femI.gdof,1),1);

rhs = rhsN + rhsI1 + rhsI2;


function [ar,arp,aru,g]=v_lpcrf2ar(rf)
%V_LPCRF2AR Convert reflection coefs to autoregressive coefs [AR,ARP,ARU,G]=(RF)
%
% Input:  RF(:,p+1) gives reflection coefficients of one or more p-section lossless tubes
% Ouputs: G is the gain of the all-pole AR filter
%         AR/G is the transfer function from U_in to the glottal input wave, U_g.
%               AR(:,1)=1 always.
%         ARP*K is the transfer function from U_in to the pressure just after the glottis
%               where K = rho*c/Alips: rho = air density 1.23 kg/m^3, c=sound speed 340 m/s,
%               Alips = effective area of free space beyond the lips.
%         ARU is the transfer function from U_in to the total volume velocity through the glottis
%
%              where U_in=z^(p/2)*U_lips is the time-advanced volume velocity at the lips
%
%         Energy into the vcal tract is equal to K*filter(ARP,1,Ulips).*filter(ARU,1,Ulips)
%              reverse glottal flows divided by 1-r0 where r0 is the glottal reflection coefficient.
%              The scale factor is included to avoid a zero answer when the glottis is closed giving r0=1.
%
% The transfer functions have ar(:,1)=art(:,1)=1
% They should both be multiplied by z^(p/2)/prod(1+rf) to correct the absolute
% gain and to compensate for the delay of p/2 samples along the length of the tube.
%
% The energy into the vocal tract is given by ars(speech) * are(speech)
%
% Ref: D. M. Brookes and H. P. Loke. "Modelling energy flow in the vocal tract with
%           applications to glottal closure and opening detection." In Proc ICASSP'99, pages 213-216, Mar 1999.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_lpcrf2ar.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(rf);
p2=p1+1;
p=p1-1;
pm=p-1;
arf=[ones(nf,1) zeros(nf,p)];
arr=[zeros(nf,p) rf(:,p1)];
cr=zeros(nf,p);
for k=1:p-1
  rk=rf(:,(p1-k)*ones(1,k));
  cr(:,1:k)=arr(:,p2-k:p1);
  arr(:,p1-k:p)=arr(:,p1-k:p)+rk.*arf(:,1:k);
  arf(:,2:k+1)=arf(:,2:k+1)+rk.*cr(:,1:k);
end
r1=rf(:,1);
ar=arf+r1(:,ones(1,p1)).*arr;
if nargout>1
   kp=prod(1-rf(:,2:p1),2);
   arp=(arf-arr)./kp(:,ones(1,p1));
   if nargout>2
      g=prod(1+rf(:,2:p1),2);
      aru=(arf+arr)./g(:,ones(1,p1));
      if nargout>3
         g=g.*(1+rf(:,1));
      end
   end
end


function [b, dev, stat] = glm_multilevel(D, Yvarname, Xvarnames, wh_keep)
% Predict Y from X using GLM
%
% :Usage:
% ::
%
%    [b, dev, stat] = glm_multilevel(D, Yvarname, Xvarnames, wh_keep)
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2013 Tor Wager
%
%     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/>.
% ..
%
% :Inputs:
%
%   **D:**
%        a canlab_dataset object
%
%   **Yvarname:**
%        the name of a variable to predict. must be event level
%
%   **Xvarnames:**
%        the name(s) of predictor variables. if multiple, put in
%        cell array. must be event level
%
%   **wh_keep:**
%        a logical vector of 1/0 values
%
% :Outputs:
%   **b:**
%       a vector of coefficient estimates (same as for glmfit())
%
%   **dev:**
%       the deviance of the fit (same as for glmfit())
%
%   **stat:**
%       structure containing stats fields (see glmfit() documentation)
%

[Y, ~, levelY] = get_var(D, Yvarname, wh_keep);
Y = Y';

%% MUST IMPLEMENT GET_VAR FOR MULTIPLE VARS AT AN EVENT LEVEL.  RETURNS A CELL ARRAY FOR EACH PERSON.  DAT BECOMES WARNING STRING, DATCELL IS OF INTEREST.

[X, ~, levelX] = get_var(D, Xvarnames, wh_keep);

if levelY ~= 2 || levelX ~= 2, error('Vars must be event level'); end

n=size(Y,2);
X1 = cell(1,n);
X2 = mean(Y)'; % matrix of 2nd level preds

%{
if isstruct(varargin{1}) % the "alternative format" described above
    xstruct = varargin{1};
    fields = varargin{2};
    for i = 1:n % each subject
        clear myX
        for j = 1:length(fields) %all the fields
            myX(:, j) = xstruct.(fields{j}){i};
            X1{i} = myX;
        end
    getvif(X1{i})
    end

else
%}
    for i = 1:n % each subject
        clear myX
        for j = 1:length(varargin)
            myX(:, j) = varargin{j}{i};
            X1{i} = myX;
        end
    end
%end

stats = glmfit_multilevel(Y, X1, scale(X2, 1), 'weighted', 'noplots', ...
    'names', {'Intrcpt' names{2:end}}, 'beta_names', {'Avg within-ss relationship' ['Effect of Avg. ' names{1}]});

end


%	Bloch simulator using mex file
%   [mx,my,mz] = blochCim(b1,gr,tp,T1,T2,freq,pr,mode,sens,mx0,my0,mz0);
%
%	Bloch simulation of rotations due to B1, gradient and
%	off-resonance, including relaxation effects.  At each time
%	point, the rotation matrix and decay matrix are calculated.
%	Simulation can simulate the steady-state if the sequence
%	is applied repeatedly.This code can also simulate the parallel transmit.
%
%   INPUT:
%   b1 = (ntime * ncoil) RF pulse, complex number in (Gauss).
%   gr = (ntime * 3) gradient in (Gauss/cm).
%   tp = (ntime * 1) time duration of each b1 and gr point, in (sec),
%				or (1 * 1) time step if constant for all points
%				or monotonically INCREASING endtime of each
%				interval..
%   T1, T2 = (1 * 1) relaxation time in (sec)
%   freq = (nx * ny * nz) or (npos * 1) off-resonance freq. in (Hz)
%   pr = (npos * 3) array of spatial positions in (cm)
%   mode= Bitmask simulation mode: (default 0)
%		Bit 0:  0-Simulate from start, 1-Steady State
%		Bit 1:  1-Record m at time points.  0-just end time.%
%       Bit 2:  0-display simulation information. 1-only print out warnings.
%
%
%   (OPTIONAL)
%   sens = (ncoils * npos or ncoils * nx*ny*nz) sensitivities matrix.
%   default is 1 for all position. Set to default if you put 1 here.
%   mx0,my0,mz0 = (npos * 1) initial magnetization
%   default is (0,0,1) for all position
%
%	OUTPUT:
%		mx,my,mz = npos x 1 arrays of the resulting magnetization
%				components at each position.
%
%	The code is originally downloaded from Brian Hargreaves's website.
%
%   Hao Sun made the following main changes:
%   (1)match field map with position
%   (2)correct the minus sign error in the 'b1imag' in the blochsimfz sub
%   function in Brian's code
%   (3)add parallel simulation function
%
%
%   Hao Sun, the University of Michigan, Jul 22,2012


function [cg,theta] = cgmo(im,radius,norient,varargin)
% function [cg] = cgmo(im,radius,norient,...)
%
% Compute the color gradient at a single scale and multiple
% orientations.
%
% INPUT
%	im		Grayscale or RGB image, values in [0,1].
%	radius		Radius of disc for cg.
%	norient		Number of orientations for cg.
%	'nbins'		Number of bins; should be > 1/sigmaSim.
%	'sigmaSim'	For color similarity function.
%	'gamma'		Gamma correction for LAB [2.5].
%	'smooth'	Smoothing method, one of
%			{'gaussian','savgol','none'}, default 'none'.
%	'sigmaSmo'	Sigma for smoothing, default to radius.
%
% OUTPUT
%	cg		Size [h,w,d,norient] array of cg images,
%			where d is the dimensionality of the image.
%
% The input parameters {radius,nbins,sigmaSim,sigmaSmo} should be
% scalars when the input image is grayscale, and can be either scalars
% or 3-element vectors when the image is RGB.
%
% See also cgmo.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% April 2003

% process options
nbins = 32;
sigmaSim = 0.1;
gamma = 2.5;
smooth = 'none';
sigmaSmo = radius;
for i = 1:2:numel(varargin),
  opt = varargin{i};
  if ~ischar(opt), error('option names not a string'); end
  if i==numel(varargin), error(sprintf('option ''%s'' has no value',opt)); end
  val = varargin{i+1};
  switch opt,
   case 'nbins', nbins=val;
   case 'sigmaSim', sigmaSim=val;
   case 'gamma', gamma=val;
   case 'smooth',
    switch val,
     case {'none','gaussian','savgol'}, smooth=val;
     otherwise, error(sprintf('invalid option smooth=''%s''',val));
    end
   case 'sigmaSmo', sigmaSmo=val;
   otherwise, error(sprintf('invalid option ''%s''',opt));
  end
end

% check arguments
if ndims(im)==2, % grayscale image
  if numel(radius)~=1, error('radius should have 1 element'); end
  if numel(nbins)~=1, error('nbins should have 1 element'); end
  if numel(sigmaSim)~=1, error('sigmaSim should have 1 element'); end
  if numel(sigmaSmo)~=1, error('sigmaSim should have 1 element'); end
elseif ndims(im)==3, % RGB image
  if numel(radius)==1, radius = radius*ones(3,1); end
  if numel(nbins)==1, nbins = nbins*ones(3,1); end
  if numel(sigmaSim)==1, sigmaSim = sigmaSim*ones(3,1); end
  if numel(sigmaSmo)==1, sigmaSmo = sigmaSmo*ones(3,1); end
  if numel(radius)~=3, error('radius should have 1 or 3 elements'); end
  if numel(nbins)~=3, error('nbins should have 1 or 3 elements'); end
  if numel(sigmaSim)~=3, error('sigmaSim should have 1 or 3 elements'); end
  if numel(sigmaSmo)~=3, error('sigmaSmo should have 1 or 3 elements'); end
  radius = radius(:);
  nbins = nbins(:);
  sigmaSim = sigmaSim(:);
  sigmaSmo = sigmaSmo(:);
else
  error('image not of valid dimension');
end
norient = max(1,norient);
nbins = max(1,nbins);

% min and max values for a,b channels of LAB
% used to scale values into the unit interval
abmin = -73;
abmax = 95;

% make sure nbins is large enough with respect to sigmaSim
if any( nbins < 1./sigmaSim ),
  warning('nbins < 1/sigmaSim is suspect');
end

% check pixel valies
if min(im(:)) < 0 | max(im(:))>1,
  error('pixel values out of range [0,1]');
end

if ndims(im)==2, % grayscale image

  % compute cg from gray values
  cmap = max(1,ceil(im*nbins));
  csim = colorsim(nbins,sigmaSim);
  [cg,theta] = tgmo(...
      cmap,nbins,radius,norient,...
      'tsim',csim,'smooth',smooth,'sigma',sigmaSmo);

else, % RGB image

  % convert gamma-corrected image to LAB and scale values into [0,1]
  lab = RGB2Lab(im.^gamma);
  lab(:,:,1) = lab(:,:,1) ./ 100;
  lab(:,:,2) = (lab(:,:,2) - abmin) ./ (abmax-abmin);
  lab(:,:,3) = (lab(:,:,3) - abmin) ./ (abmax-abmin);
  lab(:,:,2) = max(0,min(1,lab(:,:,2)));
  lab(:,:,3) = max(0,min(1,lab(:,:,3)));

  % compute cg from LAB values
  cg = zeros([size(im) norient]);
  for i = 1:3,
    cmap = max(1,ceil(lab(:,:,i)*nbins(i)));
    csim = colorsim(nbins(i),sigmaSim(i));
    [cg(:,:,i,:),theta] = tgmo(...
        cmap,nbins(i),radius(i),norient,...
        'tsim',csim,'smooth',smooth,'sigma',sigmaSmo(i));
  end

end

% compute color similarity matrix assuming colors are in [0,1]
function m = colorsim(nbins,sigma)
bc = ((1:nbins)-0.5)/nbins; % bin centers
[x,y] = meshgrid(bc,bc);
m = 1.0 - exp(-abs(x-y).^2./(2*sigma.^2));