function []=uw_sb_unwrap_space_time(day,ifgday_ix,unwrap_method,time_win,la_flag,bperp,n_trial_wraps,prefilt_win,scf_flag,temp,n_temp_wraps,max_bperp_for_temp_est)
%UW_SB_UNWRAP_SPACE_TIME smooth and unwrap phase diffs between neighboring data points in time
%
%   Andy Hooper, June 2007
%
%   ======================================================================
%   07/2008 AH: Allow isolated images, not included in any interferogram
%   01/2010 AH: Allow for non-positive definite inverse var/covar matrix
%   01/2012 AH: New option la_flag that estimates LA error for each arc
%   02/2012 AH: New method 3D_NEW
%   02/2012 AH: New method 3D_FULL
%   03/2012 AH: version updated for subsets of full network
%   10/2012 AH: Bug corrected in 3D_FULL
%   11/2012 AH: Add temperature option and scf_flag
%   09/2013 DB: Fix compatibility issue with matlab 2009
%   10/2013 DB: Fix introduced bug on them variable
%   09/2014 DB: Fix the order in which isolated dates are removed. Start
%               from big to small.
%   02/2014 AH: Add predefined ph_uw option
%   ======================================================================
%
%
disp('Unwrapping in time-space...')

uw=load('uw_grid');
ui=load('uw_interp');

n_ifg=uw.n_ifg;
n_ps=uw.n_ps;
nzix=uw.nzix;
ij=uw.ij;

if isempty(uw.ph_uw_predef)
    predef_flag='n';
else
    predef_flag='y';
end

n_image=size(day,1);
master_ix=find(day==0);
[nrow,ncol]=size(ui.Z);

day_pos_ix=find(day>0);
[tempdummy,I]=min(day(day_pos_ix));
dph_space=((uw.ph(ui.edgs(:,3),:).*conj(uw.ph(ui.edgs(:,2),:))));
if predef_flag=='y'
    dph_space_uw=uw.ph_uw_predef(ui.edgs(:,3),:)-uw.ph_uw_predef(ui.edgs(:,2),:);
    predef_ix=~isnan(dph_space_uw);
    dph_space_uw=dph_space_uw(predef_ix);
else
    predef_ix=[];
end
clear uw tempdummy

dph_space=dph_space./abs(dph_space);

ifreq_ij=[];
jfreq_ij=[];

G=zeros(n_ifg,n_image);
for i=1:n_ifg
    G(i,ifgday_ix(i,1))=-1;
    G(i,ifgday_ix(i,2))=1;
end

nzc_ix=sum(abs(G))~=0; % non-zero column index
day=day(nzc_ix);
n_image=size(day,1);
G=G(:,nzc_ix);
zc_ix=find(nzc_ix==0);
zc_ix = sort(zc_ix,'descend');              % fix to make sure the order goes from largest to smallest
for i=1:length(zc_ix)
    ifgday_ix(ifgday_ix>zc_ix(i))=ifgday_ix(ifgday_ix>zc_ix(i))-1;
end
n=size(G,2);

if ~isempty(temp)
    temp_flag='y';
else
    temp_flag='n';
end

if strcmpi(temp_flag,'y')
    fprintf('   Estimating temperature correlation (elapsed time=%ds)\n',round(toc))
    ix=abs(bperp)<max_bperp_for_temp_est;
    temp_sub=temp(ix);
    temp_range=max(temp)-min(temp);
    temp_range_sub=max(temp_sub)-min(temp_sub);
    dph_sub=dph_space(:,ix); % only ifgs using ith image
    n_temp_wraps=n_temp_wraps*(temp_range_sub/temp_range);

    trial_mult=[-ceil(8*n_temp_wraps):ceil(8*n_temp_wraps)];
    n_trials=length(trial_mult);
    trial_phase=temp_sub/temp_range_sub*pi/4;
    trial_phase_mat=exp(-j*trial_phase*trial_mult);
    Kt=zeros(ui.n_edge,1,'single');
    coh=zeros(ui.n_edge,1,'single');
    for i=1:ui.n_edge
        cpxphase=dph_sub(i,:).';
        cpxphase_mat=repmat(cpxphase,1,n_trials);
        phaser=trial_phase_mat.*cpxphase_mat;
        phaser_sum=sum(phaser);
        coh_trial=abs(phaser_sum)/sum(abs(cpxphase));
        [coh_max,coh_max_ix]=max(coh_trial);
        falling_ix=find(diff(coh_trial(1:coh_max_ix))<0); % segemnts prior to peak where falling
        if ~isempty(falling_ix)
            peak_start_ix=falling_ix(end)+1;
        else
            peak_start_ix=1;
        end
        rising_ix=find(diff(coh_trial(coh_max_ix:end))>0); % segemnts after peak where rising
        if ~isempty(rising_ix)
            peak_end_ix=rising_ix(1)+coh_max_ix-1;
        else
            peak_end_ix=n_trials;
        end
        coh_trial(peak_start_ix:peak_end_ix)=0;
        if coh_max-max(coh_trial)>0.1 % diff between peak and next peak at least 0.1
            K0=pi/4/temp_range_sub*trial_mult(coh_max_ix);
            resphase=cpxphase.*exp(-1i*(K0*temp_sub)); % subtract approximate fit
            offset_phase=sum(resphase);
            resphase=angle(resphase*conj(offset_phase)); % subtract offset, take angle (unweighted)
            weighting=abs(cpxphase);
            mopt=double(weighting.*temp_sub)\double(weighting.*resphase);
            Kt(i)=K0+mopt;
            phase_residual=cpxphase.*exp(-1i*(Kt(i)*temp_sub));
            mean_phase_residual=sum(phase_residual);
            coh(i)=abs(mean_phase_residual)/sum(abs(phase_residual));
        end
    end

    clear cpxphase_mat trial_phase_mat phaser
    Kt(coh<0.31)=0; % not to be trusted;
    dph_space=dph_space.*exp(-1i*Kt*temp');
    if predef_flag=='y'
        dph_temp=Kt*temp';
        dph_space_uw=dph_space_uw-dph_temp(predef_ix);
        clear dph_temp
    end
    dph_sub=dph_sub.*exp(-1i*Kt*temp_sub');

end

if strcmpi(la_flag,'y')

    fprintf('   Estimating look angle error (elapsed time=%ds)\n',round(toc))

    bperp_range=max(bperp)-min(bperp);
    ix=find(abs(diff(ifgday_ix,[],2))==1);

    if length(ix)>=length(day)-1 % if almost full chain is present
        fprintf('   using sequential daisy chain of interferograms\n')
        dph_sub=dph_space(:,ix); % only ifgs using ith image
        bperp_sub=bperp(ix);
        bperp_range_sub=max(bperp_sub)-min(bperp_sub);
        n_trial_wraps=n_trial_wraps*(bperp_range_sub/bperp_range);
    else
        ifgs_per_image=sum(abs(G));
        [max_ifgs_per_image,max_ix]=max(ifgs_per_image);
        if max_ifgs_per_image>=length(day)-2; % can create chain from (almost) complete single master series
            fprintf('   Using sequential daisy chain of interferograms\n')
            ix=G(:,max_ix)~=0;
            gsub=G(ix,max_ix);
            sign_ix=-sign(single(gsub'));
            dph_sub=dph_space(:,ix); % only ifgs using chosen master image
            bperp_sub=[bperp(ix)];
            bperp_sub(sign_ix==-1)=-bperp_sub(sign_ix==-1);
            bperp_sub=[bperp_sub;0]; % add master
            sign_ix=repmat(sign_ix,ui.n_edge,1);
            dph_sub(sign_ix==-1)=conj(dph_sub(sign_ix==-1)); % flip sign if necessary to make ith image master
            dph_sub=[dph_sub,mean(abs(dph_sub),2)]; % add zero phase master
            slave_ix=sum(ifgday_ix(ix,:),2)-max_ix;
            day_sub=day([slave_ix;max_ix]); % extract days for subset
            [day_sub,sort_ix]=sort(day_sub); % sort ascending day
            dph_sub=dph_sub(:,sort_ix); % sort ascending day
            bperp_sub=bperp_sub(sort_ix);
            bperp_sub=diff(bperp_sub);
            bperp_range_sub=max(bperp_sub)-min(bperp_sub);
            n_trial_wraps=n_trial_wraps*(bperp_range_sub/bperp_range);
            n_sub=length(day_sub);
            dph_sub=dph_sub(:,[2:end]).*conj(dph_sub(:,1:end-1)); % sequential dph, to reduce influence of defo
            dph_sub=dph_sub./abs(dph_sub); % normalise
        else % just use all available
            dph_sub=dph_space;
            bperp_sub=bperp;
            bperp_range_sub=bperp_range;
        end
    end

    trial_mult=[-ceil(8*n_trial_wraps):ceil(8*n_trial_wraps)];
    n_trials=length(trial_mult);
    trial_phase=bperp_sub/bperp_range_sub*pi/4;
    trial_phase_mat=exp(-j*trial_phase*trial_mult);
    K=zeros(ui.n_edge,1,'single');
    coh=zeros(ui.n_edge,1,'single');
    for i=1:ui.n_edge
        cpxphase=dph_sub(i,:).';
        cpxphase_mat=repmat(cpxphase,1,n_trials);
        phaser=trial_phase_mat.*cpxphase_mat;
        phaser_sum=sum(phaser);
        coh_trial=abs(phaser_sum)/sum(abs(cpxphase));
        [coh_max,coh_max_ix]=max(coh_trial);
        falling_ix=find(diff(coh_trial(1:coh_max_ix))<0); % segemnts prior to peak where falling
        if ~isempty(falling_ix)
            peak_start_ix=falling_ix(end)+1;
        else
            peak_start_ix=1;
        end
        rising_ix=find(diff(coh_trial(coh_max_ix:end))>0); % segemnts after peak where rising
        if ~isempty(rising_ix)
            peak_end_ix=rising_ix(1)+coh_max_ix-1;
        else
            peak_end_ix=n_trials;
        end
        coh_trial(peak_start_ix:peak_end_ix)=0;
        if coh_max-max(coh_trial)>0.1 % diff between peak and next peak at least 0.1
            K0=pi/4/bperp_range_sub*trial_mult(coh_max_ix);
            resphase=cpxphase.*exp(-1i*(K0*bperp_sub)); % subtract approximate fit
            offset_phase=sum(resphase);
            resphase=angle(resphase*conj(offset_phase)); % subtract offset, take angle (unweighted)
            weighting=abs(cpxphase);
            mopt=double(weighting.*bperp_sub)\double(weighting.*resphase);
            K(i)=K0+mopt;
            phase_residual=cpxphase.*exp(-1i*(K(i)*bperp_sub));
            mean_phase_residual=sum(phase_residual);
            coh(i)=abs(mean_phase_residual)/sum(abs(phase_residual));
        end
    end

    clear cpxphase_mat trial_phase_mat phaser dph_sub
    K(coh<0.31)=0; % not to be trusted;
    if strcmpi(temp_flag,'y')
        dph_space(K==0,:)=dph_space(K==0,:).*exp(1i*Kt(K==0)*temp'); % add back temp correction if not able to estimate DEM reliably
        Kt(K==0)=0;
        K(Kt==0)=0;
    end
    dph_space=dph_space.*exp(-1i*K*bperp');
    if predef_flag=='y'
        dph_scla=K*bperp';
        dph_space_uw=dph_space_uw-dph_scla(predef_ix);
        clear dph_scla
    end

end

spread=sparse(zeros(ui.n_edge,n_ifg));

if strcmpi(unwrap_method,'2D')
    dph_space_uw=angle(dph_space);
    if strcmpi(la_flag,'y')
        dph_space_uw=dph_space_uw+K*bperp';   % equal to dph_space + integer cycles
    end
    if strcmpi(temp_flag,'y')
        dph_space_uw=dph_space_uw+Kt*temp';   % equal to dph_space + integer cycles
    end
    dph_noise=[];
    save('uw_space_time','dph_space_uw','spread','dph_noise');
elseif strcmpi(unwrap_method,'3D_NO_DEF')
    dph_noise=angle(dph_space);
    dph_space_uw=angle(dph_space);
    if strcmpi(la_flag,'y')
        dph_space_uw=dph_space_uw+K*bperp';   % equal to dph_space + integer cycles
    end
    if strcmpi(temp_flag,'y')
        dph_space_uw=dph_space_uw+Kt*temp';   % equal to dph_space + integer cycles
    end
    save('uw_space_time','dph_space_uw','dph_noise','spread');
else
    fprintf('   Smoothing in time (elapsed time=%ds)\n',round(toc))

    if strcmpi(unwrap_method,'3D_FULL')

       dph_smooth_ifg=nan(size(dph_space),'single');

       for i=1:n_image
         ix=G(:,i)~=0;
         if sum(ix)>=n_image-2 % allow only 1 missing ifg for each image as master
           gsub=G(ix,i);
           dph_sub=dph_space(:,ix); % only ifgs using ith image
           sign_ix=repmat(-sign(single(gsub')),ui.n_edge,1);
           dph_sub(sign_ix==-1)=conj(dph_sub(sign_ix==-1)); % flip sign if necessary to make ith image master
           slave_ix=sum(ifgday_ix(ix,:),2)-i;
           day_sub=day(slave_ix); % extract days for subset
           [day_sub,sort_ix]=sort(day_sub); % sort ascending day
           dph_sub=dph_sub(:,sort_ix); % sort ascending day
           dph_sub_angle=angle(dph_sub);
           n_sub=length(day_sub);
           dph_smooth=zeros(ui.n_edge,n_sub,'single');
           for i1=1:n_sub
               time_diff=(day_sub(i1)-day_sub)';
               weight_factor=exp(-(time_diff.^2)/2/time_win^2);
               weight_factor=weight_factor/sum(weight_factor);

               dph_mean=sum(dph_sub.*repmat(weight_factor,ui.n_edge,1),2);
               %dph_mean_adj=angle(dph_sub.*repmat(conj(dph_mean),1,n_sub)); % subtract weighted mean
               dph_mean_adj=mod(dph_sub_angle-repmat(angle(dph_mean),1,n_sub)+pi,2*pi)-pi;
               GG=[ones(n_sub,1),time_diff'];
               if size(GG,1)>1
                   m=lscov(GG,double(dph_mean_adj)',weight_factor);
               else
                   m=zeros(size(GG,1),ui.n_edge);
               end
               %dph_mean_adj=mod(dph_mean_adj-(GG*m)'+pi,2*pi)-pi; % subtract first estimate
               %m2=lscov(GG,double(dph_mean_adj)',weight_factor);
               %dph_smooth(:,i1)=dph_mean.*exp(1i*(m(1,:)'+m2(1,:)')); % add back weighted mean
               dph_smooth(:,i1)=dph_mean.*exp(1i*(m(1,:)')); % add back weighted mean
            end
            dph_smooth_sub=cumsum([angle(dph_smooth(:,1)),angle(dph_smooth(:,2:end).*conj(dph_smooth(:,1:end-1)))],2);
            close_master_ix=find(slave_ix-i>0);
            if isempty(close_master_ix)
                close_master_ix=n_sub;
            else
                close_master_ix=close_master_ix(1);
                if close_master_ix>1
                    close_master_ix=[close_master_ix-1;close_master_ix];
                end
            end
            dph_close_master=mean(dph_smooth_sub(:,close_master_ix),2);
            dph_smooth_sub=dph_smooth_sub-repmat(dph_close_master-angle(exp(j*dph_close_master)),1,n_sub);
            dph_smooth_sub=dph_smooth_sub.*sign_ix;
            already_sub_ix=find(~isnan(dph_smooth_ifg(1,ix))); % already unwrapped index
            ix=find(ix);
            already_ix=ix(already_sub_ix);
            std_noise1=std(angle(dph_space(:,already_ix).*exp(-1i*dph_smooth_ifg(:,already_ix))));
            std_noise2=std(angle(dph_space(:,already_ix).*exp(-1i*dph_smooth_sub(:,already_sub_ix))));
            keep_ix=true(n_sub,1);
            keep_ix(already_sub_ix(std_noise1<std_noise2))=false; % keep least noisy
            dph_smooth_ifg(:,ix(keep_ix))=dph_smooth_sub(:,keep_ix);
          end
        end

        dph_noise=angle(dph_space.*exp(-1i*dph_smooth_ifg));
        dph_noise(std(dph_noise,0,2)>1.2,:)=nan;

    else
        x=(day-day(1))*(n-1)/(day(end)-day(1)); % use dates for smoothing
        if predef_flag=='y'
            n_dph=size(dph_space,1);
            dph_space_angle=double(angle(dph_space));
            dph_space_angle(predef_ix)=dph_space_uw;
            dph_space_series=zeros(n,n_dph);
            for i=1:n_dph
                 W=predef_ix(i,:)+0.01; % give more weight to the predfined unwrapped
                 dph_space_series(2:end,i)=lscov(double(G(:,2:end)),dph_space_angle(i,:)',W);
            end
        else
            dph_space_series=[zeros(1,ui.n_edge);double(G(:,2:end))\double(angle(dph_space))'];
        end

        dph_smooth_series=zeros(size(G,2),ui.n_edge,'single');

        for i1=1:n
            time_diff_sq=(day(i1)-day).^2;
            weight_factor=exp(-time_diff_sq/2/time_win^2);
            weight_factor=weight_factor/sum(weight_factor);
            dph_smooth_series(i1,:)=sum(dph_space_series.*repmat(weight_factor,1,ui.n_edge));
        end

        dph_smooth_ifg=(G*dph_smooth_series)';
        dph_noise=angle(dph_space.*exp(-1i*dph_smooth_ifg));

        if strcmpi(unwrap_method,'3D_SMALL_DEF')|...
           strcmpi(unwrap_method,'3D_QUICK')
          not_small_ix=find(std(dph_noise,0,2)>1.3)';
          fprintf('   %d edges with high std dev in time (elapsed time=%ds)\n',length(not_small_ix),round(toc))
          dph_noise(not_small_ix,:)=nan;
        else % 3D
          uw=load('uw_grid');
          ph_noise=angle(uw.ph.*conj(uw.ph_lowpass));
          clear uw
          dph_noise_sf=((ph_noise(ui.edgs(:,3),:)-(ph_noise(ui.edgs(:,2),:))));
          m_minmax=repmat([-pi,pi],5,1).*repmat([0.5;0.25;1;0.25;1],1,2);
          anneal_opts=[1;15;0;0;0;0;0];
          covm=cov((dph_noise_sf)); % estimate of covariance
          [W,P]=chol(inv(covm)); % weighting matrix
          if P~=0
              W=diag(1./sqrt(diag(covm)));
          end
          not_small_ix=find(std(dph_noise,0,2)>1)';
          fprintf('   Performing complex smoothing on %d edges (elapsed time=%ds)\n',length(not_small_ix),round(toc))

          n_proc=0;
          for i=not_small_ix
            dph=angle(dph_space(i,:))';
            dph_smooth_series(:,i)=uw_sb_smooth_unwrap(m_minmax,anneal_opts,G,W,dph,x);

            n_proc=n_proc+1;
            if round(n_proc/1000)==n_proc/1000
                save('uw_unwrap_time','G','dph_space','dph_smooth_series');
                fprintf('%d edges of %d reprocessed (elapsed time=%ds)\n',n_proc,length(not_small_ix),round(toc))
            end
          end
          dph_smooth_ifg=(G*dph_smooth_series)';
          dph_noise=angle(dph_space.*exp(-1i*dph_smooth_ifg));
        end
    end
    clear dph_space
    dph_space_uw=dph_smooth_ifg+dph_noise;
    clear dph_smooth_ifg

    if strcmpi(la_flag,'y')
        dph_space_uw=dph_space_uw+K*bperp';   % equal to dph_space + integer cycles
    end
    if strcmpi(temp_flag,'y')
        dph_space_uw=dph_space_uw+Kt*temp';   % equal to dph_space + integer cycles
    end

    if strcmpi(scf_flag,'y')

        fprintf('   Calculating local phase gradients (elapsed time=%ds)\n',round(toc))
        ifreq_ij=nan(n_ps,n_ifg,'single');
        jfreq_ij=nan(n_ps,n_ifg,'single');
        ifgw=zeros(nrow,ncol);
        uw=load('uw_grid');
        for i=1:n_ifg
            ifgw(nzix)=uw.ph(:,i);
            [ifreq,jfreq,grad_ij,Hmag]=gradient_filt(ifgw,prefilt_win);
            ix=~isnan(ifreq)&Hmag./((abs(ifreq))+1)>3;
            if sum(ix(:))>2
                ifreq_ij(:,i)=griddata(grad_ij(ix,2),grad_ij(ix,1),ifreq(ix),ij(:,2),ij(:,1),'linear');
            end
            ix=~isnan(jfreq)&Hmag./((abs(jfreq))+1)>3;
            if sum(ix(:))>2
                jfreq_ij(:,i)=griddata(grad_ij(ix,2),grad_ij(ix,1),jfreq(ix),ij(:,2),ij(:,1),'linear');
            end
        end
        clear uw

        spread2=zeros(size(spread),'single');
        dph_smooth_uw2=nan(ui.n_edge,n_ifg,'single');

        fprintf('   Smoothing using local phase gradients (elapsed time=%ds)\n',round(toc))
        for i=1:ui.n_edge
            nodes_ix=ui.edgs(i,[2:3]);
            ifreq_edge=mean(ifreq_ij(nodes_ix,:));
            jfreq_edge=mean(jfreq_ij(nodes_ix,:));
            diff_i=diff(ij(nodes_ix,1));
            diff_j=diff(ij(nodes_ix,2));
            dph_smooth_uw2(i,:)=diff_i*ifreq_edge+diff_j*jfreq_edge;
%           spread2(i,:)=diff_i*diff(ifreq_ij(nodes_ix,:))+diff_j*diff(jfreq_ij(nodes_ix,:));
            spread2(i,:)=diff(ifreq_ij(nodes_ix,:))+diff(jfreq_ij(nodes_ix,:));
        end
        fprintf('   Choosing between time and phase gradient smoothing (elapsed time=%ds)\n',round(toc))
        std_noise=std(dph_noise,0,2);
        dph_noise2=angle(exp(j*(dph_space_uw-dph_smooth_uw2)));
        std_noise2=std(dph_noise2,0,2);
        dph_noise2(std_noise2>1.3,:)=nan;
        shaky_ix=isnan(std_noise) | std_noise>std_noise2; % spatial smoothing works better index

        fprintf('   %d arcs smoothed in time, %d in space (elapsed time=%ds)\n',ui.n_edge-sum(shaky_ix),sum(shaky_ix),round(toc))

        dph_noise(shaky_ix,:)=dph_noise2(shaky_ix,:);
        dph_space_uw(shaky_ix,:)=dph_smooth_uw2(shaky_ix,:)+dph_noise2(shaky_ix,:);
        spread(shaky_ix,:)=spread2(shaky_ix,:);
    else
        shaky_ix=[];
    end

    save('uw_space_time','dph_space_uw','dph_noise','G','spread','ifreq_ij','jfreq_ij','shaky_ix','predef_ix');
end


function [DEM] = spm_ADEM(DEM)
% Dynamic expectation maximisation:  Active inversion
% FORMAT DEM   = spm_ADEM(DEM)
%
% DEM.G  - generative process
% DEM.M  - recognition  model
% DEM.C  - causes
% DEM.U  - prior expectation of causes
%__________________________________________________________________________
%
% This implementation of DEM is the same as spm_DEM but integrates both the
% generative process and model inversion in parallel. Its functionality is
% exactly the same apart from the fact that confounds are not accommodated
% explicitly.  The generative model is specified by DEM.G and the veridical
% causes by DEM.C; these may or may not be used as priors on the causes for
% the inversion model DEM.M (i.e., DEM.U = DEM.C).  Clearly, DEM.G does not
% require any priors or precision components; it will use the values of the
% parameters specified in the prior expectation fields.
%
% This routine is not used for model inversion per se but to simulate the
% dynamical inversion of models.  Critically, it includes action
% variables a - that couple the model back to the generative process
% This enables active inference (c.f., action-perception) or embodied
% inference.
%
% hierarchical models M(i)
%--------------------------------------------------------------------------
%   M(i).g  = y(t)  = g(x,v,P)    {inline function, string or m-file}
%   M(i).f  = dx/dt = f(x,v,P)    {inline function, string or m-file}
%
%   M(i).pE = prior expectation of p model-parameters
%   M(i).pC = prior covariances of p model-parameters
%   M(i).hE = prior expectation of h hyper-parameters (cause noise)
%   M(i).hC = prior covariances of h hyper-parameters (cause noise)
%   M(i).gE = prior expectation of g hyper-parameters (state noise)
%   M(i).gC = prior covariances of g hyper-parameters (state noise)
%   M(i).Q  = precision components (input noise)
%   M(i).R  = precision components (state noise)
%   M(i).V  = fixed precision (input noise)
%   M(i).W  = fixed precision (state noise)
%   M(i).xP = precision (states)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(i).n  = number of states x(i)
%   M(i).l  = number of output v(i)
%   M(i).k  = number of action a(i)

% hierarchical process G(i)
%--------------------------------------------------------------------------
%   G(i).g  = y(t)  = g(x,v,a,P)    {inline function, string or m-file}
%   G(i).f  = dx/dt = f(x,v,a,P)    {inline function, string or m-file}
%
%   G(i).pE = model-parameters
%   G(i).U  = precision (action)
%   G(i).V  = precision (input noise)
%   G(i).W  = precision (state noise)
%
%   G(1).R  = restriction or rate matrix for action [default: 1];
%   G(i).aP = precision (action)   [default: exp(-2)]
%
%   G(i).m  = number of inputs v(i + 1);
%   G(i).n  = number of states x(i)
%   G(i).l  = number of output v(i)
%   G(i).k  = number of action a(i)
%
%
% Returns the following fields of DEM
%--------------------------------------------------------------------------
%
% true model-states - u
%--------------------------------------------------------------------------
%   pU.x    = true hidden states
%   pU.v    = true causal states v{1} = response (Y)
%   pU.C    = prior covariance: cov(v)
%   pU.S    = prior covariance: cov(x)
%
% model-parameters - p
%--------------------------------------------------------------------------
%   pP.P    = parameters for each level
%
% hyper-parameters (log-transformed) - h,g
%--------------------------------------------------------------------------
%   pH.h    = cause noise
%   pH.g    = state noise
%
% conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.a    = Action
%   qU.x    = Conditional expectation of hidden states
%   qU.v    = Conditional expectation of causal states
%   qU.z    = Conditional prediction errors (v)
%   qU.C    = Conditional covariance: cov(v)
%   qU.S    = Conditional covariance: cov(x)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
% conditional moments of hyper-parameters (log-transformed) - q(h)
%--------------------------------------------------------------------------
%   qH.h    = Conditional expectation (cause noise)
%   qH.g    = Conditional expectation (state noise)
%   qH.C    = Conditional covariance
%
% F         = log evidence = log marginal likelihood = negative free energy
%__________________________________________________________________________
%
% spm_ADEM implements a variational Bayes (VB) scheme under the Laplace
% approximation to the conditional densities of states (u), parameters (p)
% and hyperparameters (h) of any analytic nonlinear hierarchical dynamic
% model, with additive Gaussian innovations.  It comprises three
% variational steps (D,E and M) that update the conditional moments of u, p
% and h respectively
%
%                D: qu.u = max <L>q(p,h)
%                E: qp.p = max <L>q(u,h)
%                M: qh.h = max <L>q(u,p)
%
% where qu.u corresponds to the conditional expectation of hidden states x
% and causal states v and so on.  L is the ln p(y,u,p,h|M) under the model
% M. The conditional covariances obtain analytically from the curvature of
% L with respect to u, p and h.
%
% The D-step is embedded in the E-step because q(u) changes with each
% sequential observation.  The dynamical model is transformed into a static
% model using temporal derivatives at each time point.  Continuity of the
% conditional trajectories q(u,t) is assured by a continuous ascent of F(t)
% in generalised co-ordinates.  This means DEM can deconvolve online and
% represents an alternative to Kalman filtering or alternative Bayesian
% update procedures.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ADEM.m 7679 2019-10-24 15:54:07Z spm $

% check model, data, priors and unpack
%--------------------------------------------------------------------------
DEM   = spm_ADEM_set(DEM);
M     = DEM.M;
G     = DEM.G;
C     = DEM.C;
U     = DEM.U;

% check whether to print
%--------------------------------------------------------------------------
try
    db = DEM.db;
catch
    db = 1;
end

% find or create a DEM figure
%--------------------------------------------------------------------------
if db
    Fdem = spm_figure('GetWin','DEM');
end

% ensure embedding dimensions are compatible
%--------------------------------------------------------------------------
G(1).E.n = M(1).E.n;
G(1).E.d = M(1).E.n;

% order parameters (d = n = 1 for static models) and checks
%==========================================================================
d    = M(1).E.d + 1;                      % embedding order of q(v)
n    = M(1).E.n + 1;                      % embedding order of q(x)
s    = M(1).E.s;                          % smoothness - s.d. (bins)

% number of states and parameters - generative model
%--------------------------------------------------------------------------
nY   = size(C,2);                         % number of samples
nl   = size(M,2);                         % number of levels
nv   = sum(spm_vec(M.m));                 % number of v (causal states)
nx   = sum(spm_vec(M.n));                 % number of x (hidden states)
ny   = M(1).l;                            % number of y (inputs)
nc   = M(end).l;                          % number of c (prior causes)
nu   = nv*d + nx*n;                       % number of generalised states

% number of states and parameters - generative process
%--------------------------------------------------------------------------
gr   = sum(spm_vec(G.l));                 % number of v (outputs)
ga   = sum(spm_vec(G.k));                 % number of a (active states)
gx   = sum(spm_vec(G.n));                 % number of x (hidden states)
gy   = G(1).l;                            % number of y (inputs)
na   = ga;                                % number of a (action)

% number of iterations
%--------------------------------------------------------------------------
try, nE = M(1).E.nE; catch, nE = 16; end
try, nM = M(1).E.nM; catch, nM = 8;  end
try, dt = M(1).E.dt; catch, dt = 1;  end

% initialise regularisation parameters
%--------------------------------------------------------------------------
te = 2;                                   % log integration time for E-Step
global t

% precision (roughness) of generalised fluctuations
%--------------------------------------------------------------------------
iV    = spm_DEM_R(n,s);
iG    = spm_DEM_R(n,s);

% time-delay operators (absorb motor delays into motor gain matrix)
%--------------------------------------------------------------------------
try
    nG = norm(iG);
    iG = iG*spm_DEM_T(n,-M(1).Ta);
    iG = iG*nG/norm(iG);
end
try
    Ty = spm_DEM_T(n,-M(1).Ty);
    Ty = kron(Ty,speye(ny,ny));
end

% precision components Q{} requiring [Re]ML estimators (M-Step)
%==========================================================================
Q     = {};
for i = 1:nl
    q0{i,i} = sparse(M(i).l,M(i).l); %#ok<AGROW>
    r0{i,i} = sparse(M(i).n,M(i).n);
end
Q0    = kron(iV,spm_cat(q0));
R0    = kron(iV,spm_cat(r0));
for i = 1:nl
    for j = 1:length(M(i).Q)
        q          = q0;
        q{i,i}     = M(i).Q{j};
        Q{end + 1} = blkdiag(kron(iV,spm_cat(q)),R0);
    end
    for j = 1:length(M(i).R)
        q          = r0;
        q{i,i}     = M(i).R{j};
        Q{end + 1} = blkdiag(Q0,kron(iV,spm_cat(q)));
    end
end

% and fixed components P
%--------------------------------------------------------------------------
Q0    = kron(iV,spm_cat(spm_diag({M.V})));
R0    = kron(iV,spm_cat(spm_diag({M.W})));
Qp    = blkdiag(Q0,R0);
nh    = length(Q);                           % number of hyperparameters
iR    = [zeros(1,ny),ones(1,nv),ones(1,nx)]; % for empirical priors
iR    = kron(speye(n,n),diag(iR));

% restriction or rate matrices - in terms of precision
%--------------------------------------------------------------------------
q0{1} = G(1).U;
Q0    = kron(iG,spm_cat(q0));
R0    = kron(iG,spm_cat(r0));
iG    = blkdiag(Q0,R0);

% restriction or rate matrices - in terms of dE/da
%--------------------------------------------------------------------------
try
    R         = sparse(sum(spm_vec(G.l)),na);
    R(1:ny,:) = G(1).R;
    R         = kron(spm_speye(n,1,0),R);
catch
    R = 1;
end

% fixed priors on action (a)
%--------------------------------------------------------------------------
try
    aP = G(1).aP;
catch
    aP = exp(-2);
end

% fixed priors on states (u)
%--------------------------------------------------------------------------
xP    = spm_cat(spm_diag({M.xP}));
Px    = kron(iV(1:n,1:n),speye(nx,nx)*exp(-8) + xP);
Pv    = kron(iV(1:d,1:d),speye(nv,nv)*exp(-8));
Pa    = spm_speye(na,na)*aP;
Pu    = spm_cat(spm_diag({Px Pv}));

% hyperpriors
%--------------------------------------------------------------------------
ph.h  = spm_vec({M.hE M.gE});             % prior expectation of h
ph.c  = spm_cat(spm_diag({M.hC M.gC}));   % prior covariances of h
qh.h  = ph.h;                             % conditional expectation
qh.c  = ph.c;                             % conditional covariance
ph.ic = spm_inv(ph.c);                    % prior precision

% priors on parameters (in reduced parameter space)
%==========================================================================
pp.c  = cell(nl,nl);
qp.p  = cell(nl,1);
for i = 1:(nl - 1)

    % eigenvector reduction: p <- pE + qp.u*qp.p
    %----------------------------------------------------------------------
    qp.u{i}   = spm_svd(M(i).pC);                    % basis for parameters
    M(i).p    = size(qp.u{i},2);                     % number of qp.p
    qp.p{i}   = sparse(M(i).p,1);                    % initial qp.p
    pp.c{i,i} = qp.u{i}'*M(i).pC*qp.u{i};            % prior covariance

    try
        qp.e{i} = qp.p{i} + qp.u{i}'*(spm_vec(M(i).P) - spm_vec(M(i).pE));
    catch
        qp.e{i} = qp.p{i};                           % initial qp.e
    end

end
Up    = spm_cat(spm_diag(qp.u));

% initialise and augment with confound parameters B; with flat priors
%--------------------------------------------------------------------------
np    = sum(spm_vec(M.p));                  % number of model parameters
pp.c  = spm_cat(pp.c);
pp.ic = spm_inv(pp.c);

% initialise conditional density q(p) (for D-Step)
%--------------------------------------------------------------------------
qp.e  = spm_vec(qp.e);
qp.c  = sparse(np,np);

% initialise cell arrays for D-Step; e{i + 1} = (d/dt)^i[e] = e[i]
%==========================================================================
qu.x      = cell(n,1);
qu.v      = cell(n,1);
qu.a      = cell(1,1);
qu.y      = cell(n,1);
qu.u      = cell(n,1);
pu.v      = cell(n,1);
pu.x      = cell(n,1);
pu.z      = cell(n,1);
pu.w      = cell(n,1);

[qu.x{:}] = deal(sparse(nx,1));
[qu.v{:}] = deal(sparse(nv,1));
[qu.a{:}] = deal(sparse(na,1));
[qu.y{:}] = deal(sparse(ny,1));
[qu.u{:}] = deal(sparse(nc,1));
[pu.v{:}] = deal(sparse(gr,1));
[pu.x{:}] = deal(sparse(gx,1));
[pu.z{:}] = deal(sparse(gr,1));
[pu.w{:}] = deal(sparse(gx,1));

% initialise cell arrays for hierarchical structure of x[0] and v[0]
%--------------------------------------------------------------------------
qu.x{1}   = spm_vec({M(1:end - 1).x});
qu.v{1}   = spm_vec({M(1 + 1:end).v});
qu.a{1}   = spm_vec({G.a});
pu.x{1}   = spm_vec({G.x});
pu.v{1}   = spm_vec({G.v});

% derivatives for Jacobian of D-step
%--------------------------------------------------------------------------
Dx    = kron(spm_speye(n,n,1),spm_speye(nx,nx,0));
Dv    = kron(spm_speye(d,d,1),spm_speye(nv,nv,0));
Dc    = kron(spm_speye(d,d,1),spm_speye(nc,nc,0));
Da    = kron(spm_speye(1,1,1),sparse(na,na));
Du    = spm_cat(spm_diag({Dx,Dv}));
Dq    = spm_cat(spm_diag({Dx,Dv,Dc,Da}));

Dx    = kron(spm_speye(n,n,1),spm_speye(gx,gx,0));
Dv    = kron(spm_speye(n,n,1),spm_speye(gr,gr,0));
Dp    = spm_cat(spm_diag({Dv,Dx,Dv,Dx}));
dfdw  = kron(speye(n,n),speye(gx,gx));
dydv  = kron(speye(n,n),speye(gy,gr));

% and null blocks
%--------------------------------------------------------------------------
dVdc  = sparse(d*nc,1);

% gradients and curvatures for conditional uncertainty
%--------------------------------------------------------------------------
dWdu  = sparse(nu,1);
dWduu = sparse(nu,nu);

% preclude unnecessary iterations
%--------------------------------------------------------------------------
if ~np && ~nh, nE = 1; end

% create innovations (and add causes)
%--------------------------------------------------------------------------
[z,w]  = spm_DEM_z(G,nY);
z{end} = C + z{end};
a      = {G.a};
Z      = spm_cat(z(:));
W      = spm_cat(w(:));
A      = spm_cat(a(:));

% Iterate DEM
%==========================================================================
F      = -Inf;
for iE = 1:nE

    % get time and clear persistent variables in evaluation routines
    %----------------------------------------------------------------------
    tic; clear spm_DEM_eval

    % E-Step: (with embedded D-Step)
    %======================================================================

    % [re-]set accumulators for E-Step
    %----------------------------------------------------------------------
    dFdp  = zeros(np,1);
    dFdpp = zeros(np,np);
    EE    = sparse(0);
    ECE   = sparse(0);
    EiSE  = sparse(0);
    qp.ic = sparse(0);
    Hqu.c = sparse(0);

    % [re-]set precisions using [hyper]parameter estimates
    %----------------------------------------------------------------------
    iS    = Qp;
    for i = 1:nh
       iS = iS + Q{i}*exp(qh.h(i));
    end

    % precision for empirical priors
    %----------------------------------------------------------------------
    iP    = iR*iS*iR;

    % [re-]set states & their derivatives
    %----------------------------------------------------------------------
    try
        qu = qU(1);
        pu = pU(1);
    end

    % D-Step: (nY samples)
    %======================================================================
    for iY = 1:nY

        % time (GLOBAL variable for non-automomous systems)
        %------------------------------------------------------------------
        t      = iY/nY;

        % pass action to pu.a (external states)
        %==================================================================
        try, A = spm_cat({qU.a qu.a}); end

        % derivatives of responses and random fluctuations
        %------------------------------------------------------------------
        pu.z = spm_DEM_embed(Z,n,iY);
        pu.w = spm_DEM_embed(W,n,iY);
        pu.a = spm_DEM_embed(A,n,iY);
        qu.u = spm_DEM_embed(U,n,iY);

        % evaluate generative process
        %------------------------------------------------------------------
        [pu,dg,df] = spm_ADEM_diff(G,pu);

        % and pass response to qu.y
        %==================================================================
        for i = 1:n
            y       = spm_unvec(pu.v{i},{G.v});
            qu.y{i} = y{1};
        end

        % sensory delays
        %------------------------------------------------------------------
        try, qu.y = spm_unvec(Ty*spm_vec(qu.y),qu.y); end

        % evaluate generative model
        %------------------------------------------------------------------
        [E,dE] = spm_DEM_eval(M,qu,qp);

        % conditional covariance [of states {u}]
        %------------------------------------------------------------------
        qu.c   = spm_inv(dE.du'*iS*dE.du + Pu);
        pu.c   = spm_inv(dE.du'*iP*dE.du + Pu);
        Hqu.c  = Hqu.c + spm_logdet(qu.c);

        % save at qu(t)
        %------------------------------------------------------------------
        qE{iY} = E;
        qC{iY} = qu.c;
        pC{iY} = pu.c;
        qU(iY) = qu;
        pU(iY) = pu;

        % and conditional precision
        %------------------------------------------------------------------
        if nh
            ECEu  = dE.du*qu.c*dE.du';
            ECEp  = dE.dp*qp.c*dE.dp';
        end

        % uncertainty about parameters dWdv, ... ; W = ln(|qp.c|)
        %==================================================================
        if np
            for i = 1:nu
                CJp(:,i)   = spm_vec(qp.c*dE.dpu{i}'*iS);
                dEdpu(:,i) = spm_vec(dE.dpu{i}');
            end
            dWdu  = CJp'*spm_vec(dE.dp');
            dWduu = CJp'*dEdpu;
        end

        % tensor products for Jacobian (generative process)
        %------------------------------------------------------------------
        Dgda  = kron(spm_speye(n,1,1),dg.da);
        Dgdv  = kron(spm_speye(n,n,1),dg.dv);
        Dgdx  = kron(spm_speye(n,n,1),dg.dx);
        dfda  = kron(spm_speye(n,1,0),df.da);
        dfdv  = kron(spm_speye(n,n,0),df.dv);
        dfdx  = kron(spm_speye(n,n,0),df.dx);

        dgda  = kron(spm_speye(n,1,0),dg.da);
        dgdx  = kron(spm_speye(n,n,0),dg.dx);

        % change in error w.r.t. action
        %------------------------------------------------------------------
        Dfdx  = 0;
        for i = 1:n
            Dfdx = Dfdx + kron(spm_speye(n,n,-i),df.dx^(i - 1));
        end

        % dE/da with restriction (R)
        %------------------------------------------------------------------
        dE.dv = dE.dy*dydv;
        dE.da = dE.dv*((dgda + dgdx*Dfdx*dfda).*R);

        % first-order derivatives
        %------------------------------------------------------------------
        dVdu  = -dE.du'*iS*E - Pu*spm_vec({qu.x{1:n} qu.v{1:d}}) - dWdu/2;
        dVda  = -dE.da'*iG*E - Pa*spm_vec( qu.a{1:1});

        % and second-order derivatives
        %------------------------------------------------------------------
        dVduu = -dE.du'*iS*dE.du - Pu - dWduu/2 ;
        dVdaa = -dE.da'*iG*dE.da - Pa;
        dVduv = -dE.du'*iS*dE.dv;
        dVduc = -dE.du'*iS*dE.dc;
        dVdua = -dE.du'*iS*dE.da;
        dVdav = -dE.da'*iG*dE.dv;
        dVdau = -dE.da'*iG*dE.du;
        dVdac = -dE.da'*iG*dE.dc;

        % D-step update: of causes v{i}, and hidden states x(i)
        %==================================================================

        % states and conditional modes
        %------------------------------------------------------------------
        p     = {pu.v{1:n} pu.x{1:n} pu.z{1:n} pu.w{1:n}};
        q     = {qu.x{1:n} qu.v{1:d} qu.u{1:d} qu.a{1:1}};
        u     = [p q];

        % gradient
        %------------------------------------------------------------------
        dFdu  = [                              Dp*spm_vec(p);
                 spm_vec({dVdu; dVdc; dVda}) + Dq*spm_vec(q)];

        % Jacobian (variational flow)
        %------------------------------------------------------------------
        dFduu = spm_cat(...
                {Dgdv  Dgdx Dv   []   []       []    Dgda;
                 dfdv  dfdx []   dfdw []       []    dfda;
                 []    []   Dv   []   []       []    [];
                 []    []   []   Dx   []       []    [];
                 dVduv []   []   []   Du+dVduu dVduc dVdua;
                 []    []   []   []   []       Dc    []
                 dVdav []   []   []   dVdau    dVdac dVdaa});

        % update states q = {x,v,z,w} and conditional modes
        %==================================================================
        du    = spm_dx(dFduu,dFdu,dt);
        u     = spm_unvec(spm_vec(u) + du,u);

        % and save them
        %------------------------------------------------------------------
        pu.v(1:n) = u((1:n));
        pu.x(1:n) = u((1:n) + n);
        qu.x(1:n) = u((1:n) + n + n + n + n);
        qu.v(1:d) = u((1:d) + n + n + n + n + n);
        qu.a(1:1) = u((1:1) + n + n + n + n + n + d + d);

        % Gradients and curvatures for E-Step: W = tr(C*J'*iS*J)
        %==================================================================
        if np
            for i = 1:np
                CJu(:,i)   = spm_vec(qu.c*dE.dup{i}'*iS);
                dEdup(:,i) = spm_vec(dE.dup{i}');
            end
            dWdp  = CJu'*spm_vec(dE.du');
            dWdpp = CJu'*dEdup;

            % Accumulate; dF/dP = <dL/dp>, dF/dpp = ...
            %--------------------------------------------------------------
            dFdp  = dFdp  - dWdp/2  - dE.dp'*iS*E;
            dFdpp = dFdpp - dWdpp/2 - dE.dp'*iS*dE.dp;
            qp.ic = qp.ic           + dE.dp'*iS*dE.dp;

        end

        % accumulate SSE
        %------------------------------------------------------------------
        EiSE = EiSE + E'*iS*E;

        % and quantities for M-Step
        %------------------------------------------------------------------
        if nh
            EE  = E*E'+ EE;
            ECE = ECE + ECEu + ECEp;
        end

        if nE == 1

            % evaluate objective function (F)
            %======================================================================
            J(iY) = - trace(E'*iS*E)/2  ...            % states (u)
                    + spm_logdet(qu.c)  ...            % entropy q(u)
                    + spm_logdet(iS)/2;                % entropy - error
        end

    end % sequence (nY)

    % augment with priors
    %----------------------------------------------------------------------
    dFdp   = dFdp  - pp.ic*qp.e;
    dFdpp  = dFdpp - pp.ic;
    qp.ic  = qp.ic + pp.ic;
    qp.c   = spm_inv(qp.ic);

    % E-step: update expectation (p)
    %======================================================================

    % update conditional expectation
    %----------------------------------------------------------------------
    dp     = spm_dx(dFdpp,dFdp,{te});
    qp.e   = qp.e + dp;
    qp.p   = spm_unvec(qp.e,qp.p);

    % M-step - hyperparameters (h = exp(l))
    %======================================================================
    mh     = zeros(nh,1);
    dFdh   = zeros(nh,1);
    dFdhh  = zeros(nh,nh);
    for iM = 1:nM

        % [re-]set precisions using [hyper]parameter estimates
        %------------------------------------------------------------------
        iS    = Qp;
        for i = 1:nh
            iS = iS + Q{i}*exp(qh.h(i));
        end
        S     = spm_inv(iS);
        dS    = ECE + EE - S*nY;

        % 1st-order derivatives: dFdh = dF/dh
        %------------------------------------------------------------------
        for i = 1:nh
            dPdh{i}        =  Q{i}*exp(qh.h(i));
            dFdh(i,1)      = -trace(dPdh{i}*dS)/2;
        end

        % 2nd-order derivatives: dFdhh
        %------------------------------------------------------------------
        for i = 1:nh
            for j = 1:nh
                dFdhh(i,j) = -trace(dPdh{i}*S*dPdh{j}*S*nY)/2;
            end
        end

        % hyperpriors
        %------------------------------------------------------------------
        qh.e  = qh.h  - ph.h;
        dFdh  = dFdh  - ph.ic*qh.e;
        dFdhh = dFdhh - ph.ic;

        % update ReML estimate of parameters
        %------------------------------------------------------------------
        dh    = spm_dx(dFdhh,dFdh);
        qh.h  = qh.h + dh;
        mh    = mh   + dh;

        % conditional covariance of hyperparameters
        %------------------------------------------------------------------
        qh.c  = -spm_inv(dFdhh);

        % convergence (M-Step)
        %------------------------------------------------------------------
        if (dFdh'*dh < 1e-2) || (norm(dh,1) < exp(-8)), break, end

    end % M-Step

    % evaluate objective function (F)
    %======================================================================
    L   = - trace(EiSE)/2  ...               % states (u)
        - trace(qp.e'*pp.ic*qp.e)/2  ...     % parameters (p)
        - trace(qh.e'*ph.ic*qh.e)/2  ...     % hyperparameters (h)
        + Hqu.c/2             ...            % entropy q(u)
        + spm_logdet(qp.c)/2  ...            % entropy q(p)
        + spm_logdet(qh.c)/2  ...            % entropy q(h)
        - spm_logdet(pp.c)/2  ...            % entropy - prior p
        - spm_logdet(ph.c)/2  ...            % entropy - prior h
        + spm_logdet(iS)*nY/2 ...            % entropy - error
        - n*ny*nY*log(2*pi)/2;

    % if F is increasing, save expansion point and derivatives
    %----------------------------------------------------------------------
    if L > F(end) || iE < 3

        % save model-states (for each time point)
        %==================================================================
        for t = 1:length(qU)

            % states
            %--------------------------------------------------------------
            a     = spm_unvec(qU(t).a{1},{G.a});
            v     = spm_unvec(pU(t).v{1},{G.v});
            x     = spm_unvec(pU(t).x{1},{G.x});
            z     = spm_unvec(pU(t).z{1},{G.v});
            w     = spm_unvec(pU(t).w{1},{G.x});
            for i = 1:nl
                try
                    PU.v{i}(:,t) = spm_vec(v{i});
                    PU.z{i}(:,t) = spm_vec(z{i});
                end
                try
                    PU.x{i}(:,t) = spm_vec(x{i});
                    PU.w{i}(:,t) = spm_vec(w{i});
                end
                try
                    QU.a{i}(:,t) = spm_vec(a{i});
                end
            end

            % conditional modes
            %--------------------------------------------------------------
            v     = spm_unvec(qU(t).v{1},{M(1 + 1:end).v});
            x     = spm_unvec(qU(t).x{1},{M(1:end - 1).x});
            z     = spm_unvec(qE{t}(1:(ny + nv)),{M.v});
            w     = spm_unvec(qE{t}((1:nx) + (ny + nv)*n),{M.x});
            for i = 1:(nl - 1)
                if M(i).m, QU.v{i + 1}(:,t) = spm_vec(v{i}); end
                if M(i).l, QU.z{i}(:,t)     = spm_vec(z{i}); end
                if M(i).n, QU.x{i}(:,t)     = spm_vec(x{i}); end
                if M(i).n, QU.w{i}(:,t)     = spm_vec(w{i}); end
            end
            QU.v{1}(:,t)  = spm_vec(qU(t).y{1}) - spm_vec(z{1});
            QU.z{nl}(:,t) = spm_vec(z{nl});

            % and conditional covariances
            %--------------------------------------------------------------
            i       = (1:nx);
            QU.S{t} = qC{t}(i,i);
            PU.S{t} = pC{t}(i,i);
            i       = (1:nv) + nx*n;
            QU.C{t} = qC{t}(i,i);
            PU.C{t} = pC{t}(i,i);
        end

        % save conditional densities
        %------------------------------------------------------------------
        B.QU  = QU;
        B.PU  = PU;
        B.qp  = qp;
        B.qh  = qh;

        % decrease regularisation
        %------------------------------------------------------------------
        F(iE) = L;
        te    = min(te + 1,8);

    else

        % otherwise, return to previous expansion point and break
        %------------------------------------------------------------------
        QU    = B.QU;
        PU    = B.PU;
        qp    = B.qp;
        qh    = B.qh;

        % increase regularisation
        %------------------------------------------------------------------
        F(iE) = F(end);
        te    = min(te - 1,0);

    end

    % report and break if convergence
    %======================================================================
    if db
        figure(Fdem)
        spm_DEM_qU(QU)
        if np
            subplot(nl,4,4*nl)
            bar(full(Up*qp.e))
            xlabel({'parameters';'{minus prior}'})
            axis square, grid on
        end
        if length(F) > 2
            subplot(nl,4,4*nl - 1)
            plot(F - F(1))
            xlabel('updates')
            title('log-evidence')
            axis square, grid on
        end
        drawnow

        % report (EM-Steps)
        %------------------------------------------------------------------
        str{1} = sprintf('ADEM: %i (%i)',iE,iM);
        str{2} = sprintf('F:%.4e',full(L - F(1)));
        str{3} = sprintf('p:%.2e',full(dp'*dp));
        str{4} = sprintf('h:%.2e',full(mh'*mh));
        str{5} = sprintf('(%.2e sec)',full(toc));

        fprintf('%-16s%-16s%-14s%-14s%-16s\n',str{:})
    end

    if (norm(dp,1) < exp(-8)) && (norm(mh,1) < exp(-8)), break, end

end

% assemble output arguments
%==========================================================================

% conditional moments of model-parameters (rotated into original space)
%--------------------------------------------------------------------------
qP.P   = spm_unvec(Up*qp.e + spm_vec(M.pE),M.pE);
qP.C   = Up*qp.c*Up';
qP.V   = spm_unvec(diag(qP.C),M.pE);

% conditional moments of hyper-parameters (log-transformed)
%--------------------------------------------------------------------------
qH.h   = spm_unvec(qh.h,{{M.hE} {M.gE}});
qH.g   = qH.h{2};
qH.h   = qH.h{1};
qH.C   = qh.c;
qH.V   = spm_unvec(diag(qH.C),{{M.hE} {M.gE}});
qH.W   = qH.V{2};
qH.V   = qH.V{1};

% Fill in DEM with response and its causes
%--------------------------------------------------------------------------
DEM.pP.P = {G.pE};            % parameters encoding process

DEM.M  = M;                   % generative model
DEM.U  = U;                   % causes
DEM.Y  = PU.v{1};             % response

DEM.pU = PU;                  % prior moments of model-states
DEM.qU = QU;                  % conditional moments of model-states
DEM.qP = qP;                  % conditional moments of model-parameters
DEM.qH = qH;                  % conditional moments of hyper-parameters

DEM.F  = F;                   % [-ve] Free energy
try
    DEM.J  = J;               % [-ve] Free energy (over samples)
end


function dat=readinr(fname)
%
% vol=readinr(fname)
%
% load a volume from an INR file
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
% date: 2009/05/03
%
% input:
%      fname: input file name
%
% output:
%      dat: output, data read from the inr file
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

fid=fopen(fname,'rb');
s=fread(fid,256,'uchar');

s=char(s)';

if(regexp(s,'#INRIMAGE-4')~=1)
   error('INRIMAGE header was not found')
end

nx=regexp(s,'XDIM\s*=\s*([0-9]+)','tokens');
if(length(nx))
   nx=str2num(nx{1}{1});
else
   error('no XDIM found');
end

ny=regexp(s,'YDIM\s*=\s*([0-9]+)','tokens');
if(length(ny))
   ny=str2num(ny{1}{1});
else
   error('no YDIM found');
end

nz=regexp(s,'ZDIM\s*=\s*([0-9]+)','tokens');
if(length(nz))
   nz=str2num(nz{1}{1});
else
   error('no ZDIM found');
end

nv=regexp(s,'VDIM\s*=\s*([0-9]+)','tokens');
if(length(nv))
   nv=str2num(nv{1}{1});
else
   nv=1;
end

type=regexp(s,'TYPE=([a-z ]+)','tokens');
if(length(type))
   type=type{1}{1};
else
   error('no TYPE found');
end

pixel=regexp(s,'PIXSIZE=([0-9]+)','tokens');
if(length(pixel))
   pixel=str2num(pixel{1}{1});
else
   error('no PIXSIZE found');
end

%header=sprintf(['#INRIMAGE-4#{\nXDIM=%d\nYDIM=%d\nZDIM=%d\nVDIM=1\nTYPE=%s\n' ...
%  'PIXSIZE=%d bits\nCPU=decm\nVX=1\nVY=1\nVZ=1\n'],size(vol),btype,bitlen);

if(strcmp(type,'unsigned fixed') & pixel==8)
   dtype='uint8';
elseif(strcmp(type,'unsigned fixed') & pixel==16)
   dtype='uint16';
elseif(strcmp(type,'float') & pixel==32)
   dtype='float';
elseif(strcmp(type,'float') & pixel==64)
   dtype='double';
else
   error('volume format not supported');
end

dat=fread(fid,nx*ny*nz*nv,dtype);
fclose(fid);

if(nv==1)
   dat=reshape(dat,[nx,ny,nz]);
else
   dat=reshape(dat,[nx,ny,nz,nv]);
end


function [RIR_cell] = ISM_RIR_bank_GPU(setupstruc,RIRFileName,varargin)
%ISM_RIR_bank  Bank of RIRs using Lehmann & Johansson's image-source method
%
% [RIR_CELL] = ISM_RIR_bank(SETUP_STRUC,RIR_FILE_NAME)
% [RIR_CELL] = ISM_RIR_bank( ... ,'arg1',val1,'arg2',val2,...)
%
% This function generates a bank of room impulse responses (RIRs) for a
% particular user-defined room setup, using Lehmann and Johansson's
% implementation of the image-source method (see: "Prediction of energy
% decay in room impulse responses simulated with an image-source model", J.
% Acoust. Soc. Am., vol. 124(1), pp. 269-277, July 2008). The input
% SETUP_STRUC is a structure of enviromental parameters containing the
% following fields:
%
%          Fs: sampling frequency (in Hz).
%        room: 1-by-3 vector of enclosure dimensions (in m),
%              [x_length y_length z_length].
%     mic_pos: N-by-3 matrix, [x1 y1 z1; x2 y2 z2; ...] positions of N
%              microphones (in m).
%    src_traj: M-by-3 matrix, [x1 y1 z1; x2 y2 z2; ...] positions of M
%              source trajectory points (in m).
%  T20 or T60: scalar value (in s), desired reverberation time.
%           c: (optional) sound velocity (in m/s).
% abs_weights: (optional) 1-by-6 vector of absorption coefficients weights,
%              [w_x1 w_x2 w_y1 w_y2 w_z1 w_z2].
%
% If the field SETUP_STRUC.c is undefined, the function assumes a default
% value of sound velocity of 343 m/s.
%
% The field 'abs_weight' corresponds to the relative weights of each of the
% six absorption coefficients resulting from the desired reverberation time.
% For instance, defining 'abs_weights' as [1 1 0.8 0.8 0.6 0.6] will result
% in the absorption coefficients (alpha) for the walls in the y-dimension
% being 20% smaller compared to the x-dimension walls, whereas the floor
% and ceiling will end up with absorption coefficients 40% smaller (e.g.,
% to simulate the effects of a concrete floor and ceiling). If this field
% is omitted, the parameter 'abs_weight' will default to [1 1 1 1 1 1],
% which leads to uniform absorption coefficients for all room boundaries.
%
% The structure SETUP_STRUC may contain one of the two fields 'T60' or
% 'T20'. This function will automatically determine which reverberation
% type is used and compute the desired room absorption coefficients
% accordingly. T20 is defined as the time required for the impulse response
% energy to decay from -5 to -25dB, whereas T60 corresponds to the time
% required by the impulse response energy to decay by 60dB. Setting the
% corresponding field value to 0 achieves anechoic impulse responses
% (direct path only).
%
% In addition, a number of other (optional) parameters can be set using a
% series of 'argument'--value pairs. The following parameters (arguments)
% can be used:
%
%   'Delta_dB': scalar (in dB), parameter determining how much the resulting
%               impulse response is cropped: the impulse response is
%               computed until the time index where its overall energy
%               content has decreased by 'Delta_dB' decibels, after which
%               the computations stop. Not relevant if the reverberation
%               time is set to 0 (anechoic case). Defaults to 50.
% 'SilentFlag': set to 1 to disable this function's on-screen messages.
%               Defaults to 0.
%
% This function returns a 2-dimensional cell array RIR_CELL containing the
% RIRs for each source trajectory point and each microphone, organised as
% follows: RIR_CELL{mic_index,traj_index}. The resulting filter length
% may differ slightly in each computed RIR.
%
% This function also saves the computation results on file. The argument
% RIR_FILE_NAME determines the name of the .mat file where the variable
% RIR_CELL is to be saved. If a file already exists with the same name as
% the input argument, the user will be prompted to determine whether the
% file is to be overwritten or not. The given parameter RIR_FILE_NAME can
% be a full access path to the desired file. If no access path is given,
% the file is saved in the current working directory.

% Release date: November 2009
% Author: Eric A. Lehmann, Perth, Australia (www.eric-lehmann.com)
%
% Copyright (C) 2009 Eric A. Lehmann
%
% 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/>.

VarList = {'Delta_dB'       50;         % maximum attenuation in RIR computation
           'SilentFlag'     0;          % set to 1 to disable on-screen messages
           'useGPU'         0};         % whether to use GPU
eval(SetUserVars(VarList,varargin));    % set user-definable variables

if isempty(RIRFileName)
    DoNotSave = 1;
else
    DoNotSave = 0;
    if length(RIRFileName)<=4 || ~strcmpi(RIRFileName(end-3:end),'.mat'),
        RIRFileName = [RIRFileName '.mat'];
    end
end
if exist(RIRFileName,'file')==2,
    foo = input(' [ISM_RIR_bank] The file name passed as input already exists. Overwrite? [yes/no]: ','s');
    if ~strcmpi(foo,'yes');
        fprintf(' [ISM_RIR_bank] Terminating execution now (no data was saved).\n');
        return
    end
end

Fs = setupstruc.Fs;
room = setupstruc.room;
micpos = setupstruc.mic_pos;
straj = setupstruc.src_traj;

if isfield(setupstruc,'abs_weights'),
    weights = setupstruc.abs_weights;
else
    weights = ones(1,6);
end
if isfield(setupstruc,'c'),
    cc = setupstruc.c;
else
    cc = 343;
end
if isfield(setupstruc,'T60'),
    if isfield(setupstruc, 'reflect_weights')
        beta = setupstruc.reflect_weights;
    else
        alpha = ISM_AbsCoeff('t60',setupstruc.T60,room,weights,'LehmannJohansson','c',cc);
    end
    rttype = 'T60'; rtval = setupstruc.T60;
elseif isfield(setupstruc,'T20'),
    alpha = ISM_AbsCoeff('t20',setupstruc.T20,room,weights,'LehmannJohansson','c',cc);
    rttype = 'T20'; rtval = setupstruc.T20;
else
    error('Missing T60 or T20 field.');
end
if isfield(setupstruc, 'reflect_weights')==0
    beta = sqrt(1-alpha);
end

nMics = size(micpos,1);     % number of microphones
nSPts = size(straj,1);      % number of source trajectory points

%-=:=- Compute RIR bank -=:=-
RIR_cell = cell(nMics,nSPts); % pre-allocate cell array
if ~SilentFlag, PrintLoopPCw(' [ISM_RIR_bank] Computing room impulse responses. '); end;
for mm=1:nMics,
    X_rcv = micpos(mm,:);
    for tt=1:nSPts,         % compute ISM room impulse response for each source-receiver combinations
        if ~SilentFlag, PrintLoopPCw((mm-1)*nSPts+tt,nMics*nSPts); end;
        X_src = straj(tt,:);
        if strcmpi(rttype, 'T60')       % modified by Xiong Xiao: if we use T60, simply set RIR length to T60 length
            RIR_cell{mm,tt} = ISM_RoomResp_GPU(Fs,beta,rttype,rtval,X_src,X_rcv,room,'SilentFlag',1,'c',cc,'Delta_dB',Delta_dB, 'MaxDelay', rtval, 'useGPU', useGPU);
        else
            RIR_cell{mm,tt} = ISM_RoomResp_GPU(Fs,beta,rttype,rtval,X_src,X_rcv,room,'SilentFlag',1,'c',cc,'Delta_dB',Delta_dB, 'useGPU', useGPU);
        end
    end
end

%-=:=- Save results into .mat file -=:=-
if DoNotSave==0
    save(RIRFileName,'RIR_cell');
    if ~SilentFlag, fprintf(' [ISM_RIR_bank] RIR bank parameter ''RIR_cell'' saved in file ''%s''\n',RIRFileName); end;
end


function [y]=multifuncrs(X, funs, eps, varargin)
% Cross approximation of a (vector-)function of several TT-tensors.
%   [Y]=MULTIFUNCRS(X,FUNS,EPS, VARARGIN)
%   Computes approximation to the functions FUNS(X{1},...,X{N}) with accuracy EPS
%   X should be a cell array of nx TT-tensors of equal sizes.
%   The function FUNS should receive a 2d array V of sizes I x N, where the
%   first dimension stays for the reduced set of spatial indices, and  the
%   second is the enumerator of X.
%   The returned sizes should be I x D2, where D2 is the number of
%   components in FUNS. D2 should be either provided as the last (d+1)-th
%   TT-rank of the initial guess, or given explicitly as an option (see
%   below).
%   For example, a linear combination reads FUNS=@(x)(x*W), W is a N x D2
%   matrix.
%
%   Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o y0 - initial approximation [random rank-2 tensor]
%       o nswp - maximal number of DMRG sweeps [10]
%       o rmax - maximal TT rank [Inf]
%       o verb - verbosity level, 0-silent, 1-sweep info, 2-block info [1]
%       o kickrank - the rank-increasing parameter [5]
%       o d2 - the last rank of y, that is dim(FUNS) [1]
%       o qr - do (or not) qr before maxvol [false]
%
%   The method is based on the alternating approximation, with
%   the one-block enrichment via KICKRANK random vectors or randomized AMR.
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%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
%---------------------------

nswp = 10;
kickrank = 5;
y = [];
verb = 1;
% kicktype = 'rand';
kicktype = 'amr-two';
pcatype = 'svd';
%pcatype = 'uchol';
rmax = Inf;
d2 = 1;
wasrand = false;
trunctype = 'fro';
do_qr = false;
% trunctype = 'cross';

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'y0'
            y=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case 'rmax'
            rmax=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'kicktype'
            kicktype=varargin{i+1};
        case 'pcatype'
            pcatype=varargin{i+1};
        case 'trunctype'
            trunctype=varargin{i+1};
        case 'd2'
            d2=varargin{i+1};
        case 'qr'
            do_qr = varargin{i+1};
        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

nx = numel(X);
d = X{1}.d;
n = X{1}.n;
rx = zeros(d+1,nx);
crX = cell(d,nx);
for i=1:nx
    rx(:,i) = X{i}.r;
    crX(:,i) = core2cell(X{i});
end;

if (isempty(y))
    ry = d2*ones(d+1,1); ry(1)=1;
    y = tt_rand(n, d, ry);
    wasrand = true;
end;
ry = y.r;
cry = core2cell(y);

Ry = cell(d+1,1);
Ry{1} = 1; Ry{d+1}=1;
Rx = cell(d+1,nx);
for i=1:nx
    Rx{1,i}=1; Rx{d+1,i}=1;
end;

block_order = [+(d), -(d)];

% Orth
for i=1:d-1
    cr = cry{i}; % r1,n,d2,r2
    cr = reshape(cr, ry(i)*n(i), ry(i+1));
    [cr, rv]=qr(cr, 0);
    cr2 = cry{i+1};
    cr2 = reshape(cr2, ry(i+1), n(i+1)*ry(i+2));
    cr2 = rv*cr2;
    ry(i+1) = size(cr, 2);
    cr = reshape(cr, ry(i), n(i), ry(i+1));
    cry{i+1} = reshape(cr2, ry(i+1), n(i+1), ry(i+2));
    cry{i} = cr;

    % Interface matrix for Y
    Ry{i+1} = Ry{i}*reshape(cr, ry(i), n(i)*ry(i+1));
    Ry{i+1} = reshape(Ry{i+1}, ry(i)*n(i), ry(i+1));
    if (wasrand)
        curind = [];
        while numel(curind)<ry(i+1)
            curind = [curind; ceil(rand(ry(i+1), 1)*(n(i)*ry(i)))];
            curind = unique(curind);
        end;
        curind = curind(1:ry(i+1));
    else

        curind = maxvol2(Ry{i+1},'qr',do_qr);
    end;
    Ry{i+1} = Ry{i+1}(curind, :);
    % Interface matrices for X
    for j=1:nx
        Rx{i+1,j} = reshape(crX{i,j}, rx(i,j), n(i)*rx(i+1,j));
        Rx{i+1,j} = Rx{i,j}*Rx{i+1,j};
        Rx{i+1,j} = reshape(Rx{i+1,j}, ry(i)*n(i), rx(i+1,j));
        Rx{i+1,j} = Rx{i+1,j}(curind, :);
    end;
end;

d2 = ry(d+1);
ry(d+1) = 1;
cry{d} = permute(cry{d}, [3,1,2]); % d2, rd, nd

last_sweep = false;
swp = 1;

% dy_old = ones(d,1);
dy = zeros(d,1);
max_dy = 0;
% For extra-rank addition
% dpows = ones(d,1)*min_dpow;
% dranks = zeros(d,1);

cur_order = block_order;
order_index = 2;
i = d;
dir = sign(cur_order(order_index));

% DMRG sweeps
while (swp<=nswp)||(dir>0)

    oldy = reshape(cry{i}, d2*ry(i)*n(i)*ry(i+1), 1);

    if (~last_sweep)
        % Compute the X superblocks
        curbl = zeros(ry(i)*n(i)*ry(i+1), nx);
        for j=1:nx
            cr = reshape(crX{i,j}, rx(i,j), n(i)*rx(i+1,j));
            cr = Rx{i,j}*cr;
            cr = reshape(cr, ry(i)*n(i), rx(i+1,j));
            cr = cr*Rx{i+1,j};
            curbl(:,j) = cr(:);
        end;
        % Call the function
        newy = funs(curbl); % sizes: rnr x nx -> rnr x d2
        % Multiply with inverted Ry
        newy = reshape(newy, ry(i), n(i)*ry(i+1)*d2);
        newy = (Ry{i}) \ newy;
        newy = reshape(newy, ry(i)*n(i)*ry(i+1), d2);
        newy = reshape(newy.', d2*ry(i)*n(i), ry(i+1));
        newy = newy / (Ry{i+1});
        newy = reshape(newy, d2*ry(i)*n(i)*ry(i+1), 1);
    else
        newy = oldy;
    end;

    dy(i) = norm(newy(:)-oldy)/norm(newy(:));
    max_dy = max(max_dy, dy(i));

    % Truncation
    if (dir>0) % left-to-right
        newy = reshape(newy, d2, ry(i)*n(i)*ry(i+1));
        newy = reshape(newy.', ry(i)*n(i), ry(i+1)*d2);
    else
        newy = reshape(newy, d2*ry(i), n(i)*ry(i+1));
    end;

    if (kickrank>=0)
        [u,s,v]=svd(newy, 'econ');
        s = diag(s);
        if (strcmp(trunctype, 'fro'))||(last_sweep)
            r = my_chop2(s, eps/sqrt(d)*norm(s));
        else
            % Truncate taking into account the (r+1) overhead in the cross
            cums = (s.*(2:numel(s)+1)').^2;
            cums = cumsum(cums(end:-1:1));
            cums = cums(end:-1:1)./cums(1);
            r = find(cums<(eps^2/d), 1);
            if (isempty(r))
                r = numel(s);
            end;
        end;
        r = min(r, rmax);
        r = min(r, numel(s));
    else
        if (dir>0)
            [u,v]=qr(newy, 0);
            v=v';
            r = size(u,2);
            s = ones(r,1);
        else
            [v,u]=qr(newy.', 0);
            v=conj(v);
            u=u.';
            r = size(u,2);
            s = ones(r,1);
        end;
    end;

    if (verb>1)
    	fprintf('=multifuncrs=   block %d{%d}, dy: %3.3e, r: %d\n', i, dir, dy(i), r);
    end;

    % Kicks and interfaces
    if (dir>0)&&(i<d) % left-to-right, kickrank, etc
        u = u(:,1:r);
        v = v(:,1:r)*diag(s(1:r));

        % kick
        radd = 0; rv = 1;
        if (~last_sweep)&&(kickrank>0)
            if (strcmp(kicktype, 'amr-two'))
                % AMR(two)-like kick. See also the M.Sc.Thesis by D. Zheltkov.
                % The left indices are nested, but the right are chosen
                % randomly. In Zheltkov's work, from all possible n^(d-k)
                % values. However, in the functional-cross it would result
                % in a d^2 complexity. Here, I use only the neighbouring
                % core for randomization. Additionally, the actual kick is
                % performed via the Z=PCA(supercore), since otherwise the
                % rank grows too high.

                % Compute the X superblocks
                ind2 = unique(ceil(rand(ry(i+1), 1)*(ry(i+2)*n(i+1))));
                rkick = numel(ind2);
                curbl = zeros(ry(i)*n(i)*rkick, nx);
                for j=1:nx
                    cr1 = reshape(crX{i,j}, rx(i,j), n(i)*rx(i+1,j));
                    cr1 = Rx{i,j}*cr1;
                    cr1 = reshape(cr1, ry(i)*n(i), rx(i+1,j));
                    cr2 = reshape(crX{i+1,j}, rx(i+1,j)*n(i+1), rx(i+2,j));
                    cr2 = cr2*Rx{i+2,j}; % now its size rx
                    cr2 = reshape(cr2, rx(i+1,j), n(i+1)*ry(i+2));
                    cr2 = cr2(:, ind2);
                    curbl(:,j) = reshape(cr1*cr2, ry(i)*n(i)*rkick, 1);
                end;
                % Call the function
                uk = funs(curbl); % rnr, d2
                uk = reshape(uk, ry(i), n(i)*rkick*d2);
                uk = Ry{i} \ uk;
                uk = reshape(uk, ry(i)*n(i), rkick*d2);
                if (strcmp(pcatype, 'svd'))
                    [uk,sk,vk]=svd(uk, 'econ');
                    uk = uk(:,1:min(kickrank, size(uk,2)));
                else
                    uk = uchol(uk.', kickrank+1);
                    uk = uk(:,end:-1:max(end-kickrank+1,1));
                end;
            else
                uk = rand(ry(i)*n(i), kickrank);
            end;
            [u,rv]=qr([u,uk], 0);
            radd = size(uk,2);
        end;
        v = [v, zeros(ry(i+1)*d2, radd)];
        v = rv*(v');
        r = size(u,2);

        cr2 = cry{i+1};
        cr2 = reshape(cr2, ry(i+1), n(i+1)*ry(i+2));
        v = reshape(v, r*ry(i+1), d2);
        v = reshape(v.', d2*r, ry(i+1));
        v = v*cr2; % size r+radd, n2, r3

        ry(i+1) = r;

        u = reshape(u, ry(i), n(i), r);
        v = reshape(v, d2, r, n(i+1), ry(i+2));

        % Stuff back
        cry{i} = u;
        cry{i+1} = v;

        % Recompute left interface matrices
        % Interface matrix for Y
        Ry{i+1} = Ry{i}*reshape(u, ry(i), n(i)*ry(i+1));
        Ry{i+1} = reshape(Ry{i+1}, ry(i)*n(i), ry(i+1));
        curind = maxvol2(Ry{i+1},'qr',do_qr);
        Ry{i+1} = Ry{i+1}(curind, :);
        % Interface matrices for X
        for j=1:nx
            Rx{i+1,j} = reshape(crX{i,j}, rx(i,j), n(i)*rx(i+1,j));
            Rx{i+1,j} = Rx{i,j}*Rx{i+1,j};
            Rx{i+1,j} = reshape(Rx{i+1,j}, ry(i)*n(i), rx(i+1,j));
            Rx{i+1,j} = Rx{i+1,j}(curind, :);
        end;
    elseif (dir<0)&&(i>1) % right-to-left
        u = u(:,1:r)*diag(s(1:r));
        v = conj(v(:,1:r));
        % kick
        radd = 0; rv = 1;
        if (~last_sweep)&&(kickrank>0)
            if (strcmp(kicktype, 'amr-two'))
                % Compute the X superblocks
                ind2 = unique(ceil(rand(ry(i), 1)*(ry(i-1)*n(i-1))));
                rkick = numel(ind2);
                curbl = zeros(rkick*n(i)*ry(i+1), nx);
                for j=1:nx
                    cr1 = reshape(crX{i,j}, rx(i,j)*n(i), rx(i+1,j));
                    cr1 = cr1*Rx{i+1,j};
                    cr1 = reshape(cr1, rx(i,j), n(i)*ry(i+1));
                    cr2 = reshape(crX{i-1,j}, rx(i-1,j), n(i-1)*rx(i,j));
                    cr2 = Rx{i-1,j}*cr2; % now its size rx
                    cr2 = reshape(cr2, ry(i-1)*n(i-1), rx(i,j));
                    cr2 = cr2(ind2, :);
                    curbl(:,j) = reshape(cr2*cr1, rkick*n(i)*ry(i+1), 1);
                end;
                % Call the function
                uk = funs(curbl); % rnr x d2
                uk = reshape(uk, rkick*n(i)*ry(i+1), d2);
                uk = reshape(uk.', d2*rkick*n(i), ry(i+1));
                uk = uk / Ry{i+1};
                uk = reshape(uk, d2*rkick, n(i)*ry(i+1));
                if (strcmp(pcatype, 'svd'))
                    [vk,sk,uk]=svd(uk, 'econ');
                    uk = uk(:,1:min(kickrank, size(uk,2)));
                else
                    uk = uchol(uk, kickrank+1);
                    uk = uk(:,end:-1:max(end-kickrank+1,1));
                end;
            else
                uk = rand(n(i)*ry(i+1), kickrank);
            end;
%             uk = rand(n(i)*ry(i+1), kickrank);
            [v,rv]=qr([v,uk], 0);
            radd = size(uk,2);
        end;
        u = [u, zeros(d2*ry(i), radd)];
        u = u*(rv.');
        r = size(v,2);

        cr2 = cry{i-1};
        cr2 = reshape(cr2, ry(i-1)*n(i-1), ry(i));
        u = reshape(u, d2, ry(i)*r);
        u = reshape(u.', ry(i), r*d2);
        u = cr2*u;

        u = reshape(u, ry(i-1)*n(i-1)*r, d2);
        u = reshape(u.', d2, ry(i-1), n(i-1), r);
        v = reshape(v.', r, n(i), ry(i+1));

        % Stuff back
        ry(i) = r;
        cry{i-1} = u;
        cry{i} = v;

        % Recompute left interface matrices
        % Interface matrix for Y
        Ry{i} = reshape(v, ry(i)*n(i), ry(i+1))*Ry{i+1};
        Ry{i} = reshape(Ry{i}, ry(i), n(i)*ry(i+1));
        curind = maxvol2(Ry{i}.','qr',do_qr);
        Ry{i} = Ry{i}(:, curind);
        % Interface matrices for X
        for j=1:nx
            Rx{i,j} = reshape(crX{i,j}, rx(i,j)*n(i), rx(i+1,j));
            Rx{i,j} = Rx{i,j}*Rx{i+1,j};
            Rx{i,j} = reshape(Rx{i,j}, rx(i,j), n(i)*ry(i+1));
            Rx{i,j} = Rx{i,j}(:, curind);
        end;
    elseif ((dir>0)&&(i==d))
        % Just stuff back the last core
        newy = u(:,1:r)*diag(s(1:r))*(v(:,1:r)');
        newy = reshape(newy, ry(i)*n(i)*ry(i+1), d2);
        cry{i} = reshape(newy.', d2, ry(i), n(i), ry(i+1));
    elseif ((dir<0)&&(i==1))
        % Just stuff back the last core
        newy = u(:,1:r)*diag(s(1:r))*(v(:,1:r)');
        newy = reshape(newy, d2, ry(i), n(i), ry(i+1));
        cry{i} = newy;
    end;

    i = i+dir;
    % Reversing, residue check, etc
    cur_order(order_index) = cur_order(order_index) - dir;
    % New direction
    if (cur_order(order_index)==0)
        order_index = order_index+1;

        if (verb>0)
            fprintf('=multifuncrs= sweep %d{%d}, max_dy: %3.3e, erank: %g\n', swp, order_index-1, max_dy, sqrt(ry(1:d)'*(n.*ry(2:d+1))/sum(n)));
        end;

        if (last_sweep)
            break;
        end;

        if (max_dy<eps)&&(dir<0)
            last_sweep=true;
            kickrank=0;
        end;

        if (order_index>numel(cur_order)) % New global sweep
            cur_order = block_order;
            order_index = 1;
            %residue
            if (last_sweep)
                cur_order = d-1;
            end;

            max_dy = 0;
            swp = swp+1;
        end;

        dir = sign(cur_order(order_index));
        i = i+dir;
    end;
end

cry{d} = permute(cry{d}, [2,3,1]); % d2 is r(d+1)
y = cell2core(y, cry);

end


function g = linKernGradient(kern, x, varargin)

% LINKERNGRADIENT Gradient of LIN kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% linear
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO linKernParamInit, kernGradient, linKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2009

% KERN

if nargin < 4
  [k, sk] = linKernCompute(kern, x);
else
  [k, sk] = linKernCompute(kern, x, varargin{1});
end
g(1) = sum(sum(varargin{end}.*sk));


function [p,priors]=create_parameters(start_from_mode)
p=struct();
%--- Targeted steady state values
p.alpha = 0.30; %(1) the average labor income share is 70% (page 15 of paper)
p.r = 1.01; %(2) the average real prime loan rate is 4% per annum (page 15 of paper)
p.ky = 4.6194; %(3) the capital-output ratio is on average 1.15 at the annual frequency (page 15 of paper)
p.ik = 0.2093/4; % (4) the investment-capital ratio is on average 0.209 at the annual frequency
p.qlLeOY = 2.60; %(5) the average land-output ratio is 0.65 at the annual frequency
p.theta_bar = 0.75;  %(6) the average nonfarm and nonfiancial businesses' loan-asset ratio is 0.75 at the annual frequency (page 15 of paper)
p.qlLhOY = 5.8011; %(7) the average housing output ratio is 1.45 at the annual frequency
p.n = 1/4; %(8) average market hours is 25% of time endowment
p.lbar = 1; %arbitrary

priors=struct();
priors.gamma_h          ={ 0.7  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.gamma_e          ={ 0.7  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.omega            ={ 3.0  , 0.1020, 5.994  , 'gamma_pdf(0.9)'    , .00001        , 10          };
priors.g_trans          ={ 0.375, 0.1   , 1.5    , 'gamma_pdf(0.9)'    , .00001        , 10          };
priors.lambda_qbar_trans={ 1.25 , 0.1   , 1.5    , 'gamma_pdf(0.9)'    , .00001        , 10          };
priors.rho_a            ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_z            ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_v            ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_q            ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_mu           ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_phi          ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_psi          ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.rho_xitheta      ={ 0.9  , 0.0256, 0.7761 , 'beta_pdf(0.9)'     , .00000001     , .99999999999};
priors.sig_Eps_a        ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_z        ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_v        ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_q        ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_mu       ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_phi      ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_psi      ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };
priors.sig_Eps_xitheta  ={ 0.01 , 0.0001, 2      , 'inv_gamma_pdf(0.9)', 0.000000000001, 100         };

if start_from_mode
    priors.gamma_e{1}= 0.658435028;
    priors.gamma_h{1}= 0.497555761;
    priors.omega{1}= 0.175347074;
    priors.g_trans{1}= 0.422109209;
    priors.lambda_qbar_trans{1}= 1.21261837;
    priors.rho_a{1}=  0.905471687;
    priors.rho_v{1}= 0.00947706131;	%rho_{nu_z}
    priors.rho_mu{1}= 0.294889827;	   %rho_{nu_q}
    priors.rho_psi{1}=  0.982918278;
    priors.rho_q{1}= 0.561965864;
    priors.rho_xitheta{1}= 0.980427788;
    priors.rho_phi{1}=  0.999758433;
    priors.rho_z{1}= 0.426298781;	 %rho_z
    priors.sig_Eps_z{1}=     0.00418908791;
    priors.sig_Eps_v{1}=   0.00365910563;
    priors.sig_Eps_q{1}=     0.00415238812;
    priors.sig_Eps_mu{1}=   0.00288990925;
    priors.sig_Eps_a{1}=     0.101281626;
    priors.sig_Eps_phi{1}=   0.0462334747;
    priors.sig_Eps_psi{1}=    0.00731583619;
    priors.sig_Eps_xitheta{1}= 0.0111777605;
end

% add the initial conditions of the priors to the parameters
%------------------------------------------------------------
fields=fieldnames(priors);
for ip=1:numel(fields)
    name=fields{ip};
    p.(name)=priors.(name){1};
end

end


classdef form_acc < mp.form_ac
%MP.FORM_ACC  MATPOWER Formulation class for AC cartesian voltage formulations
%   Each concrete Network Model Element class must inherit, at least
%   indirectly, from both MP.NM_ELEMENT and MP.FORM.
%
%   Subclass of MP.FORM_AC.
%   MP.FORM provides properties and methods related to the specific
%   formulation (e.g. DC version, AC polar power version, etc.)
%
%   Properties
%       (model parameters inherited from MP.FORM_AC)
%
%   Methods
%       form_name() - returns string w/name of formulation ('AC-cartesian formulation')
%       form_tag() - returns string w/short label for formulation ('acc')

%   MATPOWER
%   Copyright (c) 2019-2021, 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.

%     properties
%     end

    methods
        function name = form_name(obj)
            name = 'AC-cartesian';
        end

        function tag = form_tag(obj)
            tag = 'acc';
        end

        function vtypes = model_vvars(obj)
            vtypes = {'vr', 'vi'};
        end

        function [Iu, Iw] = port_inj_current_jac(obj, ...
                n, v_, Y, M, invdiagvic, diagSlincJ)
            % [Iu, Iw] = obj.port_inj_current_jac(...)

            %% intermediate terms
            E = invdiagvic * (conj(M) - invdiagvic * diagSlincJ);

%             %% linear current term
%             Iu = Y;
%             Iw = 1j * Y;
%
%             %% + current from linear power term
%             Iu = Iu + E;
%             Iw = Iw - 1j * E;

            Iu = Y + E;
            Iw = 1j * (Y - E);
        end

        function [Iuu, Iuw, Iww] = port_inj_current_hess_v(obj, x_, lam, v_, z_, diaginvic, Y, M, diagSlincJ, dlamJ)
            % [Iuu, Iuw, Iww] = obj.port_inj_current_hess_v(x_, lam)
            % [Iuu, Iuw, Iww] = obj.port_inj_current_hess_v(x_, lam, sysx)
            % [Iuu, Iuw, Iww] = obj.port_inj_current_hess_v(x_, lam, sysx, idx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, v_, z_, diaginvic, Y, M, diagSlincJ, dlamJ)

            if nargin < 10
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                [Y, M, N, s] = obj.get_params(idx, {'Y', 'M', 'N', 's'});
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                %% compute linear power injections
                if isempty(z_)
                    Slin = M*v_ + s;
                else
                    Slin = M*v_ + N*z_ + s;
                end

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diaginvic = sparse(1:ni, 1:ni, 1 ./ conj(vi_), ni, ni);
                if isempty(idx)     %% all ports
                    diagSlincJ = sparse(1:n, 1:n, conj(Slin), n, n);
                    dlamJ      = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    diagSlincJ = sparse(1:ni, idx, conj(Slin), ni, n);
                    dlamJ      = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            % A = diaginvic * conj(M);
            % B = diaginvic * conj(N);
            % C = diaginvic * diaginvic * diagSlincJ;
            D = (diaginvic * dlamJ).';
            E = diaginvic * (conj(M) - diaginvic * diagSlincJ);
            % E = A - C;
            F = D * E;
            G = -(F.' + F);
            % H = -D * B;

            %% linear current term
            %% second derivatives all zero

            %% current from linear power term
            Iuu = G;
            Iuw = -1j * G;
            Iww = -G;
        end

        function [Iuzr, Iuzi, Iwzr, Iwzi] = port_inj_current_hess_vz(obj, x_, lam, v_, z_, diaginvic, N, dlamJ)
            % [Iuzr, Iuzi, Iwzr, Iwzi] = obj.port_inj_current_hess_vz(x_, lam)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, sysx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, sysx, idx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, v_, z_, diaginvic, N, dlamJ)

            if nargin < 8
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                N = obj.get_params(idx, 'N');
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diaginvic = sparse(1:ni, 1:ni, 1 ./ conj(vi_), ni, ni);
                if isempty(idx)     %% all ports
                    dlamJ = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    dlamJ = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            % A = diaginvic * conj(M);
            B = diaginvic * conj(N);
            % C = diaginvic * diaginvic * diagSlincJ;
            D = (diaginvic * dlamJ).';
            % E = diaginvic * (conj(M) - diaginvic * diagSlincJ);
            % E = A - C;
            % F = D * E;
            % G = -(F.' + F);
            H = -D * B;

            %% current from linear power term
            Iuzr = H;
            Iuzi = -1j * H;
            Iwzr = Iuzi;
            Iwzi = -H;
        end

        function [Su, Sw] = port_inj_power_jac(obj, ...
                n, v_, Y, M, diagv, diagvi, diagIlincJ)
            % [Su, Sw] = obj.port_inj_power_jac(...)

            %% intermediate terms
%             A = diagIlincJ;
            B = diagvi * conj(Y);

%             %% linear power term
%             Su = M;
%             Sw = 1j * M;
%
%             %% + power from linear current term
%             Su = Su + A + B;
%             Sw = Sw + 1j * (A - B);

            A = M + diagIlincJ;
            Su = A + B;
            Sw = 1j * (A - B);
        end

        function [Suu, Suw, Sww] = port_inj_power_hess_v(obj, x_, lam, v_, z_, diagvi, Y, M, diagIlincJ, dlamJ)
            % [Suu, Suw, Sww] = obj.port_inj_power_hess_v(x_, lam)
            % [Suu, Suw, Sww] = obj.port_inj_power_hess_v(x_, lam, sysx)
            % [Suu, Suw, Sww] = obj.port_inj_power_hess_v(x_, lam, sysx, idx)
            % [...] = obj.port_inj_power_hess_v(x_, lam, v_, z_, diagvi, Y, M, diagIlincJ, dlamJ)

            if nargin < 10
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                [Y, L, M, i] = obj.get_params(idx, {'Y', 'L', 'M', 'i'});
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                %% compute linear current injections
                if isempty(z_)
                    Ilin = Y*v_ + i;
                else
                    Ilin = Y*v_ + L*z_ + i;
                end

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diagvi = sparse(1:ni, 1:ni, vi_, ni, ni);
                if isempty(idx)     %% all ports
                    diagIlincJ = sparse(1:n, 1:n, conj(Ilin), n, n);
                    dlamJ      = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    diagIlincJ = sparse(1:ni, idx, conj(Ilin), ni, n);
                    dlamJ      = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            % D = dlamJ.';
            E = dlamJ.' * conj(Y);  %% D * conj(Y);
            % F = E + E.';
            % G = 1j * (E - E.');

            %% linear power term
            %% second derivatives all zero

            %% power from linear current term
            Suu = E + E.';          %% F
            Suw = 1j * (E.' - E);   %% G.'
            Sww = Suu;
        end

        function [Suzr, Suzi, Swzr, Swzi] = port_inj_power_hess_vz(obj, x_, lam, v_, z_, diagvi, L, dlamJ)
            % [Suzr, Suzi, Swzr, Swzi] = obj.port_inj_power_hess_vz(x_, lam)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, sysx)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, sysx, idx)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, v_, z_, diagvi, L, dlamJ)

            if nargin < 8
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                L = obj.get_params(idx, 'L');
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diagvi = sparse(1:ni, 1:ni, vi_, ni, ni);
                if isempty(idx)     %% all ports
                    dlamJ = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    dlamJ = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            D = dlamJ.';
            H = D * conj(L);

            %% power from linear current term
            Suzr = H;
            Suzi = -1j * H;
            Swzr = 1j * H;
            Swzi = H;
        end

        function [va, vm] = aux_data_va_vm(obj, ad)
            v_ = ad.vr + 1j * ad.vi;
            va = angle(v_);
            vm = abs(v_);
        end

        function [g, dg] = va_fcn(obj, xx, idx, lim)
            %% lim can be a vector value for equality constraint or
            %% upper bound on va, or a cell array with {vamin, vamax}
            %% for double bounds

            %% unpack data
            [vr, vi] = deal(xx{:});

            %% compute voltage angle mismatch
            if isempty(idx)
                va = angle(vr + 1j * vi);
            else
                va = angle(vr(idx) + 1j * vi(idx));
            end
            if iscell(lim)
                g = [ lim{1} - va;
                      va - lim{2}   ];
            else
                g = va - lim;
            end

            if nargout > 1
                %% compute partials of voltage angle w.r.t vr and vi
                nn = length(vr);
                if isempty(idx)
                    idx = 1:nn;
                end
                n = length(idx);
                vm2 = vr(idx).^2 + vi(idx).^2;
                dva_dvr = sparse(1:n, idx, -vi(idx) ./ vm2, n, nn);
                dva_dvi = sparse(1:n, idx,  vr(idx) ./ vm2, n, nn);
                if iscell(lim)
                    dg = [ -dva_dvr -dva_dvi;       %% va w.r.t vr, vi
                            dva_dvr  dva_dvi  ];    %% va w.r.t vr, vi
                else
                    dg = [dva_dvr dva_dvi];         %% va w.r.t vr, vi
                end
            end
        end

        function d2G = va_hess(obj, xx, lam, idx)
            %% unpack data
            [vr, vi] = deal(xx{:});
            nn = length(vr);

            %% evaluate Hessian of voltage angle function
            if isempty(idx)
                vvr = vr;
                vvi = vi;
                idx = 1:nn;
            else
                vvr = vr(idx);
                vvi = vi(idx);
            end
            vvr2 = vvr.^2;
            vvi2 = vvi.^2;
            vvm4 = (vvr2 + vvi2).^2;
            n = length(idx);
            if length(lam) == n     %% upper bound or equality
                lamvm4 = lam ./ vvm4;
            else                    %% doubly bounded (use lam_ub-lam_lb)
                lamvm4 = (lam(n+1:2*n) - lam(1:n)) ./ vvm4;
            end
            d2vref_rr = sparse(idx, idx, 2 * lamvm4 .*  vvr .* vvi,   nn, nn);
            d2vref_ri = sparse(idx, idx,     lamvm4 .* (vvi2 - vvr2), nn, nn);

            %% construct Hessian
            d2G = [ d2vref_rr  d2vref_ri;
                    d2vref_ri -d2vref_rr ];
        end

        function [g, dg] = vm2_fcn(obj, xx, idx, lim)
            %% lim can be a scalar value for equality constraint or
            %% upper bound on vm^2, or a cell array with {vm2min, vm2max}
            %% for double bounds

            %% unpack data
            [vr, vi] = deal(xx{:});

            %% compute voltage magnitude^2 mismatch
            if isempty(idx)
                vm2 = vr.^2 + vi.^2;
            else
                vm2 = vr(idx).^2 + vi(idx).^2;
            end
            if iscell(lim)
                g = [ lim{1} - vm2;
                      vm2 - lim{2}   ];
            else
                g = vm2 - lim;
            end

            if nargout > 1
                %% compute partials of voltage magnitude^2 w.r.t vr and vi
                nn = length(vr);
                if isempty(idx)
                    idx = 1:nn;
                end
                n = length(idx);
                dvm_dvr = sparse(1:n, idx, 2 * vr(idx), n, nn);
                dvm_dvi = sparse(1:n, idx, 2 * vi(idx), n, nn);
                if iscell(lim)
                    dg = [ -dvm_dvr -dvm_dvi;
                            dvm_dvr  dvm_dvi  ];    %% vm2 w.r.t vr, vi
                else
                    dg = [ dvm_dvr dvm_dvi ];       %% vm2 w.r.t vr, vi
                end
            end
        end

        function d2G = vm2_hess(obj, xx, lam, idx)
            %% unpack data
            [vr, vi] = deal(xx{:});
            nn = length(vr);

            %% evaluate Hessian of voltage magnitude^2 function
            if isempty(idx)
                idx = 1:nn;
            end
            n = length(idx);
            if length(lam) == n     %% upper bound or equality
                dlam = sparse(idx, idx, 2*lam, nn, nn);
            else                    %% doubly bounded (use lam_ub-lam_lb)
                dlam = sparse(idx, idx, 2*(lam(n+1:2*n) - lam(1:n)), nn, nn);
            end

            %% construct Hessian
            zz = sparse(nn, nn);
            d2G = [dlam zz; zz dlam];
        end
    end     %% methods
end         %% classdef