function [VO,M] = pm_segment(VF,PG,flags)
% Segment an MR image into Gray, White & CSF.
%
% FORMAT VO = pm_segment(PF,PG,flags)
% PF    - name(s) of image(s) to segment (must have same dimensions).
% PG    - name(s) of template image(s) for realignment.
%       - or a 4x4 transformation matrix which maps from the image to
%         the set of templates.
% flags - a structure normally based on defaults.segment
% VO    - optional output volume
% M     - affine transformation between template and image to segment
%
%                      The algorithm is four step:
%
% 1) Determine the affine transform which best matches the image with a
%    template image. If the name of more than one image is passed, then
%    the first image is used in this step. This step is not performed if
%    no template images are specified.
%
% 2) Perform Cluster Analysis with a modified Mixture Model and a-priori
%    information about the likelihoods of each voxel being one of a
%    number of different tissue types. If more than one image is passed,
%    then they they are all assumed to be in register, and the voxel
%    values are fitted to multi-normal distributions.
%
% 3) Perform morphometric operations on the grey and white partitions
%    in order to more accurately identify brain tissue. This is then used
%    to clean up the grey and white matter segments.
%
% 4) If no or 2 output arguments is/are specified, then the segmented
%    images are written to disk. The names of these images have "c1",
%    "c2" & "c3" appended to the name of the first image passed. The
%    'brainmask' is also created with "BrMsk_" as an appendix.
%
%_______________________________________________________________________
% Refs:
%
% Ashburner J & Friston KJ (1997) Multimodal Image Coregistration and
% Partitioning - a Unified Framework. NeuroImage 6:209-217
%
%_______________________________________________________________________
%
% The template image, and a-priori likelihood images are modified
% versions of those kindly supplied by Alan Evans, MNI, Canada
% (ICBM, NIH P-20 project, Principal Investigator John Mazziotta).
%_______________________________________________________________________
%
% This is a renamed version of the original spm_segment which has been
% removed from the main spm distribution, but copied into the FieldMap
% toolbox where it is still used.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: pm_segment.m 4873 2012-08-30 19:06:26Z john $

% Create some suitable default values
%-----------------------------------------------------------------------

dr = fileparts(mfilename('fullpath'));
def_flags.estimate.priors = char(...
        fullfile(dr,'grey.nii'),...
        fullfile(dr,'white.nii'),...
        fullfile(dr,'csf.nii'));
def_flags.estimate.reg    = 0.01;
def_flags.estimate.cutoff = 30;
def_flags.estimate.samp   = 3;
def_flags.estimate.bb     =  [[-88 88]' [-122 86]' [-60 95]'];
def_flags.estimate.affreg.smosrc = 8;
def_flags.estimate.affreg.regtype = 'mni';
def_flags.estimate.affreg.weight = '';
def_flags.write.cleanup   = 1;
def_flags.write.wrt_cor   = 1;
def_flags.write.wrt_brV   = 0;
def_flags.graphics        = 1;

if nargin<3, flags = def_flags; end;
if ~isfield(flags,'estimate'),        flags.estimate        = def_flags.estimate;        end;
if ~isfield(flags.estimate,'priors'), flags.estimate.priors = def_flags.estimate.priors; end;
if ~isfield(flags.estimate,'reg'),    flags.estimate.reg    = def_flags.estimate.reg;    end;
if ~isfield(flags.estimate,'cutoff'), flags.estimate.cutoff = def_flags.estimate.cutoff; end;
if ~isfield(flags.estimate,'samp'),   flags.estimate.samp   = def_flags.estimate.samp;   end;
if ~isfield(flags.estimate,'bb'),     flags.estimate.bb     = def_flags.estimate.bb;     end;
if ~isfield(flags.estimate,'affreg'), flags.estimate.affreg = def_flags.estimate.affreg; end;
if ~isfield(flags.estimate.affreg,'smosrc'),
    flags.estimate.affreg.smosrc = def_flags.estimate.affreg.smosrc;
end;
if ~isfield(flags.estimate.affreg,'regtype'),
    flags.estimate.affreg.regtype = def_flags.estimate.affreg.regtype;
end;
if ~isfield(flags.estimate.affreg,'weight'),
    flags.estimate.affreg.weight = def_flags.estimate.affreg.weight;
end;
if ~isfield(flags,'write'),         flags.write         = def_flags.write;         end;
if ~isfield(flags.write,'cleanup'), flags.write.cleanup = def_flags.write.cleanup; end;
if ~isfield(flags.write,'wrt_cor'), flags.write.wrt_cor = def_flags.write.wrt_cor; end;
if ~isfield(flags.write,'wrt_brV'), flags.write.wrt_brV = def_flags.write.wrt_brV; end;
if ~isfield(flags,'graphics'),      flags.graphics      = def_flags.graphics;      end;

%-----------------------------------------------------------------------

if ischar(VF), VF= spm_vol(VF); end;

SP         = init_sp(flags.estimate,VF,PG);
[x1,x2,x3] = get_sampling(SP.MM,VF,flags.estimate.samp,flags.estimate.bb);
BP         = init_bp(VF, flags.estimate.cutoff, flags.estimate.reg);
CP         = init_cp(VF,x3);
sums       = zeros(8,1);

for pp=1:length(x3),
    [raw,msk] = get_raw(VF,x1,x2,x3(pp));
    s         = get_sp(SP,x1,x2,x3(pp));
    CP        = update_cp_est(CP,s,raw,msk,pp);
    sums      = sums + reshape(sum(sum(s,1),2),8,1);
end;
sums = sums/sum(sums);
CP   = shake_cp(CP);

[CP,BP,SP] = run_segment(CP,BP,SP,VF,sums,x1,x2,x3);

%save segmentation_results.mat CP BP SP VF sums

[g,w,c] = get_gwc(VF,BP,SP,CP,sums,flags.write.wrt_cor);
if flags.write.cleanup, [g,w,c,b] = clean_gwc(g,w,c); end;

% Create the segmented images + the brain mask.
%-----------------------------------------------------------------------
%offs  = cumsum(repmat(prod(VF(1).dim(1:2)),1,VF(1).dim(3)))-prod(VF(1).dim(1:2));
%pinfo = [repmat([1/255 0]',1,VF(1).dim(3)) ; offs];
[pth,nm,xt] = fileparts(deblank(VF(1).fname));

if flags.write.wrt_brV
    Nwrt = 4;
else
    Nwrt = 3;
end
for j=1:Nwrt,
    tmp   = fullfile(pth,['c', num2str(j), nm, xt]);
    if j==4, tmp = fullfile(pth,['BrMsk_', nm  xt]); end

    VO(j) = struct(...
        'fname',tmp,...
        'dim',    VF(1).dim(1:3),...
        'dt',     [spm_type('uint8'), spm_platform('bigend')],...
        'mat',    VF(1).mat,...
        'pinfo',  [1/255 0 0]',...
        'descrip','Segmented image');
end;

if nargout==0 || nargout==2,
    VO = spm_create_vol(VO);

    spm_progress_bar('Init',VF(1).dim(3),'Writing Segmented','planes completed');
    for pp=1:VF(1).dim(3),
        VO(1) = spm_write_plane(VO(1),double(g(:,:,pp))/255,pp);
        VO(2) = spm_write_plane(VO(2),double(w(:,:,pp))/255,pp);
        VO(3) = spm_write_plane(VO(3),double(c(:,:,pp))/255,pp);
        if flags.write.wrt_brV
            VO(4) = spm_write_plane(VO(4),double(b(:,:,pp))/255,pp);
        end
        spm_progress_bar('Set',pp);
    end;
    spm_progress_bar('Clear');
end;

VO(1).dat = g; VO(1).pinfo = VO(1).pinfo(1:2,:);
VO(2).dat = w; VO(2).pinfo = VO(2).pinfo(1:2,:);
VO(3).dat = c; VO(3).pinfo = VO(3).pinfo(1:2,:);
if flags.write.wrt_brV
    VO(4).dat = b; VO(4).pinfo = VO(4).pinfo(1:2,:);
end
if nargout==2
    M = SP.MM/VF(1).mat;
end

if flags.graphics, display_graphics(VF,VO,CP.mn,CP.cv,CP.mg); end;

return;
%=======================================================================

%=======================================================================
function [y1,y2,y3] = affine_transform(x1,x2,x3,M)
    y1 = M(1,1)*x1 + M(1,2)*x2 + M(1,3)*x3 + M(1,4);
    y2 = M(2,1)*x1 + M(2,2)*x2 + M(2,3)*x3 + M(2,4);
    y3 = M(3,1)*x1 + M(3,2)*x2 + M(3,3)*x3 + M(3,4);
return;
%=======================================================================

%=======================================================================
function display_graphics(VF,VS,mn,cv,mg)
% Do the graphics
nb = 3;
spm_figure('Clear','Graphics');
fg = spm_figure('FindWin','Graphics');
if ~isempty(fg),
    % Show some text
    %-----------------------------------------------------------------------
    ax = axes('Position',[0.05 0.8 0.9 0.2],'Visible','off','Parent',fg);
    text(0.5,0.80, 'Segmentation','FontSize',16,'FontWeight','Bold',...
        'HorizontalAlignment','center','Parent',ax);

    text(0,0.65, ['Image:  ' spm_file(VF(1).fname,'short50')],...
        'FontSize',14,'FontWeight','Bold','Parent',ax);

    text(0,0.40, 'Means:','FontSize',12,'FontWeight','Bold','Parent',ax);
    text(0,0.30, 'Std devs:' ,'FontSize',12,'FontWeight','Bold','Parent',ax);
    text(0,0.20, 'N vox:','FontSize',12,'FontWeight','Bold','Parent',ax);
    for j=1:nb,
        text((j+0.5)/(nb+1),0.40, num2str(mn(1,j)),...
            'FontSize',12,'FontWeight','Bold',...
            'HorizontalAlignment','center','Parent',ax);
        text((j+0.5)/(nb+1),0.30, num2str(sqrt(cv(1,1,j))),...
            'FontSize',12,'FontWeight','Bold',...
            'HorizontalAlignment','center','Parent',ax);
        text((j+0.5)/(nb+1),0.20, num2str(mg(1,j)/sum(mg(1,:))),...
            'FontSize',12,'FontWeight','Bold',...
            'HorizontalAlignment','center','Parent',ax);
    end;
    if length(VF) > 1,
        text(0,0.10,...
        'Note: only means and variances for the first image are shown',...
        'Parent',ax,'FontSize',12);
    end;

    M1 = VS(1).mat;
    M2 = VF(1).mat;
    for i=1:5,
        M   = spm_matrix([0 0 i*VF(1).dim(3)/6]);
        img = spm_slice_vol(VF(1),M,VF(1).dim(1:2),1);
        img(1,1) = eps;
        ax = axes('Position',...
            [0.05 0.75*(1-i/5)+0.05 0.9/(nb+1) 0.75/5],...
            'Visible','off','Parent',fg);
        imagesc(rot90(img), 'Parent', ax);
        set(ax,'Visible','off','DataAspectRatio',[1 1 1]);

        for j=1:3,
            img = spm_slice_vol(VS(j),M2\M1*M,VF(1).dim(1:2),1);
            ax  = axes('Position',...
                [0.05+j*0.9/(nb+1) 0.75*(1-i/5)+0.05 0.9/(nb+1) 0.75/5],...
                'Visible','off','Parent',fg);
            image(rot90(img*64), 'Parent', ax);
            set(ax,'Visible','off','DataAspectRatio',[1 1 1]);
        end;
    end;

    spm_print;
    drawnow;
end;
return;
%=======================================================================

%=======================================================================
function M = get_affine_mapping(VF,VG,aflags)

if ~isempty(VG) && ischar(VG), VG = spm_vol(VG); end;

if ~isempty(VG) && isstruct(VG),
    % Affine registration so that a priori images match the image to
    % be segmented.
    %-----------------------------------------------------------------------

    VFS = spm_smoothto8bit(VF(1),aflags.smosrc);

    % Scale all images approximately equally
    % ---------------------------------------------------------------
    for i=1:length(VG),
        VG(i).pinfo(1:2,:) = VG(i).pinfo(1:2,:)/spm_global(VG(i));
    end;
    VFS(1).pinfo(1:2,:) = VFS(1).pinfo(1:2,:)/spm_global(VFS(1));

    spm_plot_convergence('Init','Affine Registration','Mean squared difference','Iteration');
    flags     = struct('sep',aflags.smosrc, 'regtype',aflags.regtype,'WG',[],'globnorm',0,'debug',0);
    M         = eye(4);
    [M,scal]  = spm_affreg(VG, VFS, flags, M);

    if ~isempty(aflags.weight), flags.WG = spm_vol(aflags.weight); end;

    flags.sep = aflags.smosrc/2;
    M         = spm_affreg(VG, VFS, flags, M,scal);
    spm_plot_convergence('Clear');

elseif all(size(VG) == [4 4])
    % Assume that second argument is a matrix that will do the job
    %-----------------------------------------------------------------------
    M = VG;
else
    % Assume that image is normalized
    %-----------------------------------------------------------------------
    M = eye(4);
end
return;
%=======================================================================

%=======================================================================
function [x1,x2,x3] = get_sampling(MM,VF,samp,bb1)
% Voxels to sample during the cluster analysis
%-----------------------------------------------------------------------

% A bounding box for the brain in Talairach space.
%bb = [ [-88 88]' [-122 86]' [-60 95]'];
%c = [bb(1,1) bb(1,2) bb(1,3) 1
%     bb(1,1) bb(1,2) bb(2,3) 1
%     bb(1,1) bb(2,2) bb(1,3) 1
%     bb(1,1) bb(2,2) bb(2,3) 1
%     bb(2,1) bb(1,2) bb(1,3) 1
%     bb(2,1) bb(1,2) bb(2,3) 1
%     bb(2,1) bb(2,2) bb(1,3) 1
%     bb(2,1) bb(2,2) bb(2,3) 1]';
%tc = MM\c;
%tc = tc(1:3,:)';
%mx = max(tc);
%mn = min(tc);
%bb = [mn ; mx];
%vx = sqrt(sum(VF(1).mat(1:3,1:3).^2));
%samp = round(max(abs([4 4 4]./vx), [1 1 1]));
%x1 = bb(1,1):samp(1):bb(2,1);
%x2 = bb(1,2):samp(2):bb(2,2);
%x3 = bb(1,3):samp(3):bb(2,3);
%return;

% A bounding box for the brain in Talairach space.
if nargin<4, bb1 = [ [-88 88]' [-122 86]' [-60 95]']; end;

% A mapping from a unit radius sphere to a hyper-ellipse
% that is just enclosed by the bounding box in Talairach
% space.
M0 = [diag(diff(bb1)/2) mean(bb1)';[0 0 0 1]];

% The mapping from voxels to Talairach space is MM,
% so the ellipse in the space of the image becomes:
M0 = MM\M0;

% So to work out the bounding box in the space of the
% image that just encloses the hyper-ellipse.
tmp = M0(1:3,1:3);
tmp = diag(tmp*tmp'/diag(sqrt(diag(tmp*tmp'))));
bb  = round([M0(1:3,4)-tmp M0(1:3,4)+tmp])';
bb  = min(max(bb,[1 1 1 ; 1 1 1]),[VF(1).dim(1:3) ; VF(1).dim(1:3)]);

% Want to sample about every 3mm
tmp  = sqrt(sum(VF(1).mat(1:3,1:3).^2))';
samp = round(max(abs(tmp.^(-1)*samp), [1 1 1]'));

x1 = bb(1,1):samp(1):bb(2,1);
x2 = bb(1,2):samp(2):bb(2,2);
x3 = bb(1,3):samp(3):bb(2,3);

return;
%=======================================================================

%=======================================================================
function [CP,BP,SP] = run_segment(CP,BP,SP,VF,sums,x1,x2,x3)
oll = -Inf;
spm_plot_convergence('Init','Segmenting','Log-likelihood','Iteration #');

for iter = 1:64,
    ll= 0;
    for pp = 1:length(x3), % Loop over planes
        bf        = get_bp(BP,x1,x2,x3(pp));
        [raw,msk] = get_raw(VF,x1,x2,x3(pp));
        s         = get_sp(SP,x1,x2,x3(pp));
        cor       = bf.*raw;
        [P,ll0]   = get_p(cor,msk,s,sums,CP,bf);
        ll        = ll + ll0;
        CP        = update_cp_est(CP,P,cor,msk,pp);
        BP        = update_bp_est(BP,P,cor,CP,msk,x1,x2,x3(pp));
    end;

    BP = update_bp(BP);
    if iter>1, spm_plot_convergence('Set',ll); end;
    %fprintf('\t%g\n', ll);

    % Stopping criterion
    %-----------------------------------------------------------------------
    if iter == 2,
        ll2 = ll;
    elseif iter > 2 && abs((ll-oll)/(ll-ll2)) < 0.0001
        break;
    end;
    oll = ll;
end;
spm_plot_convergence('Clear');
return;
%=======================================================================

%=======================================================================
function BP = init_bp(VF,co,reg)
m        = length(VF);
tmp      = sqrt(sum(VF(1).mat(1:3,1:3).^2));
BP.nbas  = max(round((VF(1).dim(1:3).*tmp)/co),[1 1 1]);
BP.B1    = spm_dctmtx(VF(1).dim(1),BP.nbas(1));
BP.B2    = spm_dctmtx(VF(1).dim(2),BP.nbas(2));
BP.B3    = spm_dctmtx(VF(1).dim(3),BP.nbas(3));

nbas     = BP.nbas;
if prod(BP.nbas)>1,
    % Set up a priori covariance matrix
    vx = sqrt(sum(VF(1).mat(1:3,1:3).^2));
    kx=(pi*((1:nbas(1))'-1)*pi/vx(1)/VF(1).dim(1)*10).^2;
    ky=(pi*((1:nbas(2))'-1)*pi/vx(2)/VF(1).dim(2)*10).^2;
    kz=(pi*((1:nbas(3))'-1)*pi/vx(3)/VF(1).dim(3)*10).^2;

    % Cost function based on sum of squares of 4th derivatives
    IC0 =  (1*kron(kz.^4,kron(ky.^0,kx.^0)) +...
            1*kron(kz.^0,kron(ky.^4,kx.^0)) +...
            1*kron(kz.^0,kron(ky.^0,kx.^4)) +...
            4*kron(kz.^3,kron(ky.^1,kx.^0)) +...
            4*kron(kz.^3,kron(ky.^0,kx.^1)) +...
            4*kron(kz.^1,kron(ky.^3,kx.^0)) +...
            4*kron(kz.^0,kron(ky.^3,kx.^1)) +...
            4*kron(kz.^1,kron(ky.^0,kx.^3)) +...
            4*kron(kz.^0,kron(ky.^1,kx.^3)) +...
            6*kron(kz.^2,kron(ky.^2,kx.^0)) +...
            6*kron(kz.^2,kron(ky.^0,kx.^2)) +...
            6*kron(kz.^0,kron(ky.^2,kx.^2)) +...
           12*kron(kz.^2,kron(ky.^1,kx.^1)) +...
           12*kron(kz.^1,kron(ky.^2,kx.^1)) +...
           12*kron(kz.^1,kron(ky.^1,kx.^2)) )*reg;

    %IC0(1) = max(IC0);
    BP.IC0 = diag(IC0(2:end));

    % Initial estimate for intensity modulation field
    BP.T   = zeros(nbas(1),nbas(2),nbas(3),length(VF));
    %-----------------------------------------------------------------------
else
    BP.T   = zeros([1 1 1 length(VF)]);
    BP.IC0 = [];
end;
BP.Alpha = zeros(prod(BP.nbas(1:3)),prod(BP.nbas(1:3)),m);
BP.Beta  = zeros(prod(BP.nbas(1:3)),m);
return;
%=======================================================================

%=======================================================================
function BP = update_bp_est(BP,p,cor,CP,msk,x1,x2,x3)
if prod(BP.nbas)<=1, return; end;
B1 = BP.B1(x1,:);
B2 = BP.B2(x2,:);
B3 = BP.B3(x3,:);
for j=1:size(BP.Alpha,3),
    cr  = cor(:,:,j);
    w1 = zeros(size(cr));
    w2 = zeros(size(cr));
    for i=[1 2 3 4 5 6 7 8],
        tmp = p(:,:,i)*CP.cv(j,j,i)^(-1);
        w1  = w1 + tmp.*(CP.mn(j,i) - cr);
        w2  = w2 + tmp;
    end;
    wt1       = 1 + cr.*w1;
    wt2       = cr.*(cr.*w2 - w1);
    wt1(~msk) = 0;
    wt2(~msk) = 0;

    BP.Beta(:,j)    = BP.Beta(:,j)    + kron(B3',spm_krutil(wt1,B1,B2,0));
    BP.Alpha(:,:,j) = BP.Alpha(:,:,j) + kron(B3'*B3,spm_krutil(wt2,B1,B2,1));
end;
return;
%=======================================================================

%=======================================================================
function BP = update_bp(BP)
if prod(BP.nbas)<=1, return; end;
for j=1:size(BP.Alpha,3),
    x     = BP.T(:,:,:,j);
    x     = x(:);
    x     = x(2:end);
    Alpha = BP.Alpha(2:end,2:end,j);
    Beta  = BP.Beta(2:end,j);
    x     = (Alpha + BP.IC0)\(Alpha*x + Beta);

    BP.T(:,:,:,j) = reshape([0 ; x],BP.nbas(1:3));
    BP.Alpha      = zeros(size(BP.Alpha));
    BP.Beta       = zeros(size(BP.Beta));
end;
return;
%=======================================================================

%=======================================================================
function bf = get_bp(BP,x1,x2,x3)
bf = ones(length(x1),length(x2),size(BP.Alpha,3));
if prod(BP.nbas)<=1, return; end;
B1 = BP.B1(x1,:);
B2 = BP.B2(x2,:);
B3 = BP.B3(x3,:);
for i=1:size(BP.Alpha,3),
    t = reshape(reshape(BP.T(:,:,:,i),...
        BP.nbas(1)*BP.nbas(2),BP.nbas(3))*B3', BP.nbas(1), BP.nbas(2));
    bf(:,:,i) = exp(B1*t*B2');
end;
return;
%=======================================================================

%=======================================================================
function [dat,msk] = get_raw(VF,x1,x2,x3)
[X1,X2,X3] = ndgrid(x1,x2,x3);
for i=1:length(VF),
    [Y1,Y2,Y3] = affine_transform(X1,X2,X3,VF(i).mat\VF(1).mat);
    dat(:,:,i) = spm_sample_vol(VF(i),Y1,Y2,Y3,1);
end;
msk = all(dat,3) & all(isfinite(double(dat)),3);
return;
%=======================================================================

%=======================================================================
function CP = init_cp(VF,x3)
n = 8;
m = length(VF);
p = length(x3);
CP.mom0 = zeros(1,n,p)+eps;
CP.mom1 = zeros(m,n,p);
CP.mom2 = zeros(m,m,n,p)+eps;

% Occasionally the dynamic range of the images is such that many voxels
% all have the same intensity.  Adding cv0 is an attempt to improve the
% stability of the algorithm if this occurs. The value 0.083 was obtained
% from var(rand(1000000,1)).  It prbably isn't the best way of doing
% things, but it appears to work.
CP.cv0 = zeros(m,m);
for i=1:m,
    if spm_type(VF(i).dt(1),'intt'),
        CP.cv0(i,i)=0.083*mean(VF(i).pinfo(1,:));
    end;
end;
return;
%=======================================================================

%=======================================================================
function CP = shake_cp(CP)
CP.mom0(:,5,:)   = CP.mom0(:,1,:);
CP.mom0(:,6,:)   = CP.mom0(:,2,:);
CP.mom0(:,7,:)   = CP.mom0(:,3,:);
CP.mom1(:,5,:)   = CP.mom1(:,1,:);
CP.mom1(:,6,:)   = CP.mom1(:,2,:);
CP.mom1(:,7,:)   = CP.mom1(:,3,:);
CP.mom1(:,8,:)   = 0;
CP.mom2(:,:,5,:) = CP.mom2(:,:,1,:);
CP.mom2(:,:,6,:) = CP.mom2(:,:,2,:);
CP.mom2(:,:,7,:) = CP.mom2(:,:,3,:);
return;
%=======================================================================

%=======================================================================
function CP = update_cp_est(CP,P,dat,msk,p)
m   = size(dat,3);
d   = size(P);
P   = reshape(P,[d(1)*d(2),d(3)]);
dat = reshape(dat,[d(1)*d(2),m]);
P(~msk(:),:)   = [];
dat(~msk(:),:) = [];
for i=1:size(CP.mom0,2),
    CP.mom0(1,i,p)   = sum(P(:,i));
    CP.mom1(:,i,p)   = sum((P(:,i)*ones(1,m)).*dat)';
    CP.mom2(:,:,i,p) = ((P(:,i)*ones(1,m)).*dat)'*dat;
end;

for i=1:size(CP.mom0,2),
    CP.mg(1,i)   = sum(CP.mom0(1,i,:),3);
    CP.mn(:,i)   = sum(CP.mom1(:,i,:),3)/CP.mg(1,i);

    tmp          = (CP.mg(1,i).*CP.mn(:,i))*CP.mn(:,i)';
    tmp          = tmp-eye(size(tmp))*eps*1e6;
    CP.cv(:,:,i) = (sum(CP.mom2(:,:,i,:),4) - tmp)/CP.mg(1,i) + CP.cv0;
end;
CP.mg   = CP.mg/sum(CP.mg);
return;
%=======================================================================

%=======================================================================
function [p,ll] = get_p(cor,msk,s,sums,CP,bf)
d   = [size(cor) 1 1];
n   = size(CP.mg,2);
cor = reshape(cor,d(1)*d(2),d(3));
cor = cor(msk,:);
p   = zeros(d(1)*d(2),n);
if ~any(msk), p  = reshape(p,d(1),d(2),n); ll=0; return; end;

for i=1:n,
    amp       = 1/sqrt((2*pi)^d(3) * det(CP.cv(:,:,i)));
    dst       = (cor-ones(size(cor,1),1)*CP.mn(:,i)')/sqrtm(CP.cv(:,:,i));
    dst       = sum(dst.*dst,2);
    tmp       = s(:,:,i);
    p(msk,i)  = (amp*CP.mg(1,i)/sums(i))*exp(-0.5*dst).*tmp(msk) +eps;
end;
sp = sum(p,2);
ll = sum(log(sp(msk).*bf(msk)+eps));
sp(~msk) = Inf;
for i=1:n, p(:,i) = p(:,i)./sp; end;
p  = reshape(p,d(1),d(2),n);
return;
%=======================================================================

%=======================================================================
function SP = init_sp(flags,VF,PG)
SP.VB       = spm_vol(flags.priors);
MM          = get_affine_mapping(VF,PG,flags.affreg);
%VF          = spm_vol(PF);
SP.MM       = MM*VF(1).mat;
SP.w        = 0.98;
return;
%=======================================================================

%=======================================================================
function s = get_sp(SP,x1,x2,x3)
[X1,X2,X3] = ndgrid(x1,x2,x3);
[Y1,Y2,Y3] = affine_transform(X1,X2,X3,SP.VB(1).mat\SP.MM);
w1  = SP.w;
w2  = (1-w1)/2;
s   = zeros([size(Y1),4]);
for i=1:3,
    s(:,:,i) = spm_sample_vol(SP.VB(i),Y1,Y2,Y3,1)*w1+w2;
end;
s(:,:,4:8)   = repmat(abs(1-sum(s(:,:,1:3),3))/5,[1 1 5]);
return;
%=======================================================================

%=======================================================================
function [g,w,c] = get_gwc(VF,BP,SP,CP,sums,wc)

if wc,
    VC = VF;
    for j=1:length(VF),
        [pth,nm,xt,vr] = spm_fileparts(deblank(VF(j).fname));
        VC(j).fname    = fullfile(pth,['m' nm xt vr]);
        VC(j).descrip  = 'Bias corrected image';
    end;
    VC = spm_create_vol(VC);
end;

spm_progress_bar('Init',VF(1).dim(3),'Creating Segmented','planes completed');
x1 = 1:VF(1).dim(1);
x2 = 1:VF(1).dim(2);
x3 = 1:VF(1).dim(3);

g = uint8(0); g(VF(1).dim(1),VF(1).dim(2),VF(1).dim(3)) = 0;
w = uint8(0); w(VF(1).dim(1),VF(1).dim(2),VF(1).dim(3)) = 0;
c = uint8(0); c(VF(1).dim(1),VF(1).dim(2),VF(1).dim(3)) = 0;

for pp=1:length(x3),
    bf        = get_bp(BP,x1,x2,x3(pp));
    [raw,msk] = get_raw(VF,x1,x2,x3(pp));
    cor       = raw.*bf;
    if wc,
        for j=1:length(VC),
            VC(j) = spm_write_plane(VC(j),cor(:,:,j),pp);
        end;
    end;
    s         = get_sp(SP,x1,x2,x3(pp));
    p         = get_p(cor,msk,s,sums,CP,bf);
    g(:,:,pp) = uint8(round(p(:,:,1)*255));
    w(:,:,pp) = uint8(round(p(:,:,2)*255));
    c(:,:,pp) = uint8(round(p(:,:,3)*255));

    spm_progress_bar('Set',pp);
end;
spm_progress_bar('Clear');

return;
%=======================================================================

%=======================================================================
function [g,w,c,b] = clean_gwc(g,w,c)
b    = w;
b(1) = w(1);

% Build a 3x3x3 seperable smoothing kernel
%-----------------------------------------------------------------------
kx=[0.75 1 0.75];
ky=[0.75 1 0.75];
kz=[0.75 1 0.75];
sm=sum(kron(kron(kz,ky),kx))^(1/3);
kx=kx/sm; ky=ky/sm; kz=kz/sm;

% Erosions and conditional dilations
%-----------------------------------------------------------------------
niter = 32;
spm_progress_bar('Init',niter,'Extracting Brain','Iterations completed');
for j=1:niter,
    if j>2, th=0.15; else th=0.6; end; % Dilate after two its of erosion.
    for i=1:size(b,3),
        gp = double(g(:,:,i));
        wp = double(w(:,:,i));
        bp = double(b(:,:,i))/255;
        bp = (bp>th).*(wp+gp);
        b(:,:,i) = uint8(round(bp));
    end;
    spm_conv_vol(b,b,kx,ky,kz,-[1 1 1]);
    spm_progress_bar('Set',j);
end;
th = 0.05;
for i=1:size(b,3),
    gp       = double(g(:,:,i))/255;
    wp       = double(w(:,:,i))/255;
    cp       = double(c(:,:,i))/255;
    bp       = double(b(:,:,i))/255;
    bp       = ((bp>th).*(wp+gp))>th;
    g(:,:,i) = uint8(round(255*gp.*bp./(gp+wp+cp+eps)));
    w(:,:,i) = uint8(round(255*wp.*bp./(gp+wp+cp+eps)));
    c(:,:,i) = uint8(round(255*(cp.*bp./(gp+wp+cp+eps)+cp.*(1-bp))));
    b(:,:,i) = uint8(round(255*bp));
end;
spm_progress_bar('Clear');
return;


% function [Results, Gt, F1, Fs2, Fs3, Ft2, Ft3] = GenerativeTriTL(Train_Data,Test_Data,Parameter_Setting)
function [Results, pzd_t] = GenerativeTriTL(TrainData, TestData, TrainLabel, TestLabel, numIdentical, numAlike, numDistinct, numIter, numSource, numTrain, numTarget, numTest)
% function [Results, pz_d] = TriTL(Train_Data,Test_Data,Parameter_Setting)

% The common program for CD_PLSA, which can deal with multiple classes,
% multiple source domains and multiple target domains

%%%% Input:
% The parameter Train_data stores the file pathes of training data and the
% corresponding labels
% The parameter Test_data stores the file pathes of test data and the
% corresponding labels
% The parameter Parameterfile stores the parameter setting information

%%%% Output
% The variable Results is a matrix with size numIteration x numTarget, where
% numIteration is the number of iterations, numTarget is the number of
% target domains. Results record the detailed results of each iteration.

% The variable pz_d is a matrix with size n x c, where n is the number of
% instances in all target domains, specifically, n = n_1 + ... + nt (n_t is
% the number of instances in t-th target domain), c is the number of
% classes
%
% Note that if you want to deal with large data set, you should set larget
% memory for Matlab. You can set it in the file C:\boot.ini (This may not
% be true in your system), change '/fastdetect' to '/fastdetect /3GB'.
%
% Be good luck for your research, if you have any questions, you can
% contact the email: zhuangfz@ics.ict.ac.cn

%read the parameters
numK_1 = double(numIdentical);
numK_2 = double(numAlike);
numK_3 = double(numDistinct);
numIteration = double(numIter);

TrainX = TrainData;
TrainY = TrainLabel;
TestX = TestData;
TestY = TestLabel;
labelset = union(TestY,[]);

numC = length(labelset);
numFeature = size(TestX,1);

start = 1;
% if numK_3 == 0
%     start = 0;
% end
if start == 1
    DataSetX = [TrainX TestX];
    Learn.Verbosity = 1;
    Learn.Max_Iterations = 20;
    Learn.heldout = .1; % for tempered EM only, percentage of held out data
    Learn.Min_Likelihood_Change = 1;
    Learn.Folding_Iterations = 20; % for TEM only: number of fiolding in iterations
    Learn.TEM = 0; %tempered or not tempered
    [Pw_z,Pz_d,Pd,Li,perp,eta] = pLSA(DataSetX,[],numK_1+numK_2,Learn); %start PLSA
    %xlswrite(strcat('pwz_','common_selected','.xls'),Pw_z);
    csvwrite(strcat('pzw_','common_selected','.plsa'),Pw_z);
end
%pwy = xlsread(strcat('pwz_','common_selected','.xls'));

%% Following are Initializaitons
pzw = csvread(strcat('pzw_','common_selected','.plsa'));
pwy_a = pzw(:,1:numK_1); % the common topics using the same words
pwy_b_s = []; % the common topics using different words
pwy_b_t = []; % the common topics using different words
for i = 1:numSource
    pwy_b_s = [pwy_b_s, pzw(:,1+numK_1:numK_1+numK_2)];
end
for i = 1:numTarget
    pwy_b_t = [pwy_b_t, pzw(:,1+numK_1:numK_1+numK_2)];
end
% pwy_c_s = []; % different topics using different words
% pwy_c_t = []; % different topics using different words
pwy_c_s = ones(numFeature,numK_3*numSource)/numFeature;
pwy_c_t = ones(numFeature,numK_3*numTarget)/numFeature;
clear pzw;
pdz_s = zeros(sum(numTrain),numC);
for i = 1:size(pdz_s,1)
    pdz_s(i,TrainY(i)) = 1;
end
for i = 1:numSource
    pos = 0;
    if i > 1
        for t = 1:i-1
            pos = pos + numTrain(t);
        end
    end
    if i == 1
        pos = 0;
    end
    for j = 1:numC
        pdz_s(pos+1:pos+numTrain(i),j) = pdz_s(pos+1:pos+numTrain(i),j)/sum(pdz_s(pos+1:pos+numTrain(i),j));
    end
end

% In our paper, pdz_t is assigned as the predicted results by supervised
% classifiers
% The initialization of the target-domain label
pdz_t = zeros(sum(numTest),numC);
flag = 1;
if flag == 1
    w_models = [];
    for i = 1:numSource
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTrain(t);
            end
        end
        if i == 1
            pos = 0;
        end
        TempTrainX = TrainX(:,pos+1:pos+numTrain(i));
        TempTrainY = TrainY(:,pos+1:pos+numTrain(i));
        for v = 1:length(TempTrainY)
            if TempTrainY(v) > 1
                TempTrainY(v) = -1;
            end
        end

        TempTrainXY = scale_cols(TempTrainX,TempTrainY);
        fprintf('.....................................\n');
        w00 = zeros(size(TempTrainXY,1),1);
        lambda = exp(linspace(-0.5,6,20));
        wbest = [];
        f1max = -inf;
        for j = 1:length(lambda)
            w_0 = train_cg(TempTrainXY,w00,lambda(j));
            f1 = logProb(TempTrainXY,w_0);
            if f1 > f1max
                f1max = f1;
                wbest = w_0;
                %se_lambda = lambda(j);
            end
        end
        w_models = [w_models wbest];
        clear TempTrainX;
        clear TempTrainY;
        clear TempTrainXY;
    end
%     csvwrite(strcat('lg_models/','model_lg.model'),w_models);
end

TempGt = zeros(size(pdz_t));
% w_models = csvread(strcat('lg_models/','model_lg.model'));
for i = 1:numTarget
    pos = 0;
    if i > 1
        for t = 1:i-1
            pos = pos + numTest(t);
        end
    end
    if i == 1
        pos = 0;
    end
    TempTestX = TestX(:,pos+1:pos+numTest(i));
    for j = 1:numSource
        wbest = w_models(:,j);
        ptemp = 1./(1 + exp(-wbest'*TempTestX));
    end
    TempGt(pos+1:pos+numTest(i),:) = TempGt(pos+1:pos+numTest(i),:) + [(ptemp'+0.5)/2 ((1-ptemp)'+0.5)/2];
    clear TempTestX;
end
TempGt = TempGt/numSource;
pdz_t = TempGt; % not yet normalize
%% The initialization pyz
pyz_a = ones(numK_1,numC)/numK_1;
pyz_b = ones(numK_2,numC)/numK_2;
pyz_c_s = ones(numK_3,numC*numSource)/numK_3;
pyz_c_t = ones(numK_3,numC*numTarget)/numK_3;

%% the initial accuracy
iter_results = [];
for i = 1:numTarget
    pos = 0;
    if i > 1
        for t = 1:i-1
            pos = pos + numTest(t);
        end
    end
    if i == 1
        pos = 0;
    end
    pzd = pdz_t(pos+1:pos+numTest(i),:);
    nCorrect = 0;
    for j = 1:size(pzd,1)
        [va vi] = max(pzd(j,:));
        if labelset(vi) == TestY(pos+j)
            nCorrect = nCorrect + 1;
        end
    end
    iter_results(1,i+1) = nCorrect/(numTest(i));
    iter_results(1,1) = 0;
end

% the normalization of pdz_t
for i = 1:numTarget
    pos = 0;
    if i > 1
        for t = 1:i-1
            pos = pos + numTest(t);
        end
    end
    if i == 1
        pos = 0;
    end
    for j = 1:numC
        pdz_t(pos+1:pos+numTest(i),j) = pdz_t(pos+1:pos+numTest(i),j)/sum(pdz_t(pos+1:pos+numTest(i),j));
    end
end

pzr_s = ones(1,numC*numSource)/numC;
pzr_t = ones(1,numC*numTarget)/numC;
pr = ones(1,numSource+numTarget)/(numSource+numTarget);

for i = 1:numSource
    pos = 0;
    if i > 1
        for t = 1:i-1
            pos = pos + numTrain(t);
        end
    end
    if i == 1
        pos = 0;
    end
    pr(i) = sum(sum(TrainX(:,pos+1:pos+numTrain(i))));
end
for i = 1:numTarget
    pos = 0;
    if i > 1
        for t = 1:i-1
            pos = pos + numTest(t);
        end
    end
    if i == 1
        pos = 0;
    end
    pr(numSource+i) = sum(sum(TestX(:,pos+1:pos+numTest(i))));
end
pr = pr/sum(pr);
%% stepLen
stepLen = 1000;
% fprintf('the 0 iteration,the value of objective is %g\n',fvalue);
%% Start to interate
% update all variables
% pwy_a; pwy_b_s; pwy_b_t; pwy_c_s; pwy_c_t; pdz_s; pdz_t;
% pyz_a; pyz_b; pyz_c_s; pyz_c_t; pzr_s; pzr_t; pr;
for iterID = 1:numIteration

    % update pwy_a; pwy_b_s; pwy_b_t; pwy_c_s; pwy_c_t;
    temp_pwy_a = zeros(size(pwy_a));
    temp_pwy_b_s = [];
    temp_pwy_b_t = [];
    temp_pwy_c_s = [];
    temp_pwy_c_t = [];
    % update pdz_t;
    temp_pdz_t = [];
    % update pzr_s; pzr_t; pr
    temp_pzr_s = [];
    temp_pzr_t = [];
    temp_pr = [];
    for i = 1:numSource
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTrain(t);
            end
        end
        if i == 1
            pos = 0;
        end
        A = pyz_a;
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_s(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_a*A*pdz_s(pos+1:pos+numTrain(i),:)';
        tempsum2 = tempsum2*pr(i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        temp_pwy_a = temp_pwy_a + pwy_a.*(MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,pdz_s(pos+1:pos+numTrain(i),:),stepLen)*A'*pr(i));
        I = sum(MatrixProduce(pwy_a',MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,pdz_s(pos+1:pos+numTrain(i),:),stepLen),stepLen).*pyz_a)*pr(i);

        %%-----------------%%
        A = pyz_b;
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_s(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_b_s(:,(i-1)*numK_2+1:i*numK_2)*A*pdz_s(pos+1:pos+numTrain(i),:)';
        tempsum2 = tempsum2*pr(i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        B = pwy_b_s(:,(i-1)*numK_2+1:i*numK_2).*(MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,pdz_s(pos+1:pos+numTrain(i),:),stepLen)*A'*pr(i));
        temp_pwy_b_s = [temp_pwy_b_s B];
        I = I + sum(MatrixProduce(pwy_b_s(:,(i-1)*numK_2+1:i*numK_2)',MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,pdz_s(pos+1:pos+numTrain(i),:),stepLen),stepLen).*pyz_b)*pr(i);

        A = pyz_c_s(:,(i-1)*numC+1:i*numC);
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_s(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_c_s(:,(i-1)*numK_3+1:i*numK_3)*A*pdz_s(pos+1:pos+numTrain(i),:)';
        tempsum2 = tempsum2*pr(i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        B = pwy_c_s(:,(i-1)*numK_3+1:i*numK_3).*(MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,pdz_s(pos+1:pos+numTrain(i),:),stepLen)*A'*pr(i));
        temp_pwy_c_s = [temp_pwy_c_s B];
        I = I + sum(MatrixProduce(pwy_c_s(:,(i-1)*numK_3+1:i*numK_3)',MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,pdz_s(pos+1:pos+numTrain(i),:),stepLen),stepLen).*pyz_c_s(:,(i-1)*numC+1:i*numC))*pr(i);
        temp_pzr_s = [temp_pzr_s I];
        temp_pr = [temp_pr sum(I)];
    end
    for i = 1:numTarget
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTest(t);
            end
        end
        if i == 1
            pos = 0;
        end
        A = pyz_a;
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_t(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_a*A*pdz_t(pos+1:pos+numTest(i),:)';
        tempsum2 = tempsum2*pr(numSource+i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        temp_pwy_a = temp_pwy_a + pwy_a.*(MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,pdz_t(pos+1:pos+numTest(i),:),stepLen)*A'*pr(numSource+i));
        H = (MatrixProduce([TestX(:,pos+1:pos+numTest(i))./tempsum2]',pwy_a,stepLen)*A*pr(numSource+i));
        I = sum(MatrixProduce(pwy_a',MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,pdz_t(pos+1:pos+numTest(i),:),stepLen),stepLen).*pyz_a)*pr(numSource+i);
        %%-----------------%%
        A = pyz_b;
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_t(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_b_t(:,(i-1)*numK_2+1:i*numK_2)*A*pdz_t(pos+1:pos+numTest(i),:)';
        tempsum2 = tempsum2*pr(numSource+i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        B = pwy_b_t(:,(i-1)*numK_2+1:i*numK_2).*(MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,pdz_t(pos+1:pos+numTest(i),:),stepLen)*A'*pr(numSource+i));
        temp_pwy_b_t = [temp_pwy_b_t B];
        H = H + (MatrixProduce([TestX(:,pos+1:pos+numTest(i))./tempsum2]',pwy_b_t(:,(i-1)*numK_2+1:i*numK_2),stepLen)*A*pr(numSource+i));
        I = I + sum(MatrixProduce(pwy_b_t(:,(i-1)*numK_2+1:i*numK_2)',MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,pdz_t(pos+1:pos+numTest(i),:),stepLen),stepLen).*pyz_b)*pr(numSource+i);

        A = pyz_c_t(:,(i-1)*numC+1:i*numC);
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_t(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_c_t(:,(i-1)*numK_3+1:i*numK_3)*A*pdz_t(pos+1:pos+numTest(i),:)';
        tempsum2 = tempsum2*pr(numSource+i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        B = pwy_c_t(:,(i-1)*numK_3+1:i*numK_3).*(MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,pdz_t(pos+1:pos+numTest(i),:),stepLen)*A'*pr(numSource+i));
        temp_pwy_c_t = [temp_pwy_c_t B];
        H = H + (MatrixProduce([TestX(:,pos+1:pos+numTest(i))./tempsum2]',pwy_c_t(:,(i-1)*numK_3+1:i*numK_3),stepLen)*A*pr(numSource+i));
        H = pdz_t(pos+1:pos+numTest(i),:).*H;
        temp_pdz_t = [temp_pdz_t; H];
        I = I + sum(MatrixProduce(pwy_c_t(:,(i-1)*numK_3+1:i*numK_3)',MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,pdz_t(pos+1:pos+numTest(i),:),stepLen),stepLen).*pyz_c_t(:,(i-1)*numC+1:i*numC))*pr(numSource+i);
        temp_pzr_t = [temp_pzr_t I];
        temp_pr = [temp_pr sum(I)];
    end

    % update pyz_a; pyz_b; pyz_c_s; pyz_c_t;
    temp_pyz_a = zeros(size(pyz_a));
    temp_pyz_b = zeros(size(pyz_b));
    temp_pyz_c_s = [];
    temp_pyz_c_t = [];
    for i = 1:numSource
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTrain(t);
            end
        end
        if i == 1
            pos = 0;
        end
        A = pdz_s(pos+1:pos+numTrain(i),:);
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_s(1,j+(i-1)*numC);
        end

        tempsum2 = pwy_a*pyz_a*A';
        tempsum2 = tempsum2*pr(i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        temp_pyz_a = temp_pyz_a + pyz_a.*(MatrixProduce(pwy_a',MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,A,stepLen),stepLen)*pr(i));

        %%-----------------%%
        tempsum2 = pwy_b_s(:,(i-1)*numK_2+1:i*numK_2)*pyz_b*A';
        tempsum2 = tempsum2*pr(i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        temp_pyz_b = temp_pyz_b + pyz_b.*(MatrixProduce(pwy_b_s(:,(i-1)*numK_2+1:i*numK_2)',MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,A,stepLen),stepLen)*pr(i));

        tempsum2 = pwy_c_s(:,(i-1)*numK_3+1:i*numK_3)*pyz_c_s(:,(i-1)*numC+1:i*numC)*A';
        tempsum2 = tempsum2*pr(i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        B = pyz_c_s(:,(i-1)*numC+1:i*numC).*(MatrixProduce(pwy_c_s(:,(i-1)*numK_3+1:i*numK_3)',MatrixProduce(TrainX(:,pos+1:pos+numTrain(i))./tempsum2,A,stepLen),stepLen)*pr(i));
        temp_pyz_c_s = [temp_pyz_c_s B];
    end
    for i = 1:numTarget
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTest(t);
            end
        end
        if i == 1
            pos = 0;
        end
        A = pdz_t(pos+1:pos+numTest(i),:);
        for j = 1:numC
            A(:,j) = A(:,j)*pzr_t(1,j+(i-1)*numC);
        end
        tempsum2 = pwy_a*pyz_a*A';
        tempsum2 = tempsum2*pr(numSource+i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        temp_pyz_a = temp_pyz_a + pyz_a.*(MatrixProduce(pwy_a',MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,A,stepLen),stepLen)*pr(numSource+i));

        %%-----------------%%
        tempsum2 = pwy_b_t(:,(i-1)*numK_2+1:i*numK_2)*pyz_b*A';
        tempsum2 = tempsum2*pr(numSource+i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        temp_pyz_b = temp_pyz_b + pyz_b.*(MatrixProduce(pwy_b_t(:,(i-1)*numK_2+1:i*numK_2)',MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,A,stepLen),stepLen)*pr(numSource+i));

        tempsum2 = pwy_c_t(:,(i-1)*numK_3+1:i*numK_3)*pyz_c_t(:,(i-1)*numC+1:i*numC)*A';
        tempsum2 = tempsum2*pr(numSource+i);
        [xs ys] = find(tempsum2 < 10^(-20));
        for q = 1:size(xs,1)
            tempsum2(xs(q,1),ys(q,1)) = 1;
        end
        B = pyz_c_t(:,(i-1)*numC+1:i*numC).*(MatrixProduce(pwy_c_t(:,(i-1)*numK_3+1:i*numK_3)',MatrixProduce(TestX(:,pos+1:pos+numTest(i))./tempsum2,A,stepLen),stepLen)*pr(numSource+i));
        temp_pyz_c_t = [temp_pyz_c_t B];
    end

    % normalize all the variables
    pwy_a = temp_pwy_a;
    pwy_b_s = temp_pwy_b_s;
    pwy_b_t = temp_pwy_b_t;
    pwy_c_s = temp_pwy_c_s;
    pwy_c_t = temp_pwy_c_t;
    % pdz_s = temp_pdz_s;
    pdz_t = temp_pdz_t;
    pyz_a = temp_pyz_a;
    pyz_b = temp_pyz_b;
    pyz_c_s = temp_pyz_c_s;
    pyz_c_t = temp_pyz_c_t;
    pzr_s = temp_pzr_s;
    pzr_t = temp_pzr_t;
    pr = temp_pr;

    for t = 1:numK_1
        pwy_a(:,t) = pwy_a(:,t)/sum(pwy_a(:,t));
    end
    for t = 1:numC
        pyz_a(:,t) = pyz_a(:,t)/sum(pyz_a(:,t));
        pyz_b(:,t) = pyz_b(:,t)/sum(pyz_b(:,t));
    end
    pr = pr/sum(pr);
    for t = 1:numK_2*numSource
        pwy_b_s(:,t) = pwy_b_s(:,t)/sum(pwy_b_s(:,t));
    end
    for t = 1:numK_3*numSource
        pwy_c_s(:,t) = pwy_c_s(:,t)/sum(pwy_c_s(:,t));
    end
    for t = 1:numC*numSource
        pyz_c_s(:,t) = pyz_c_s(:,t)/sum(pyz_c_s(:,t));
    end
    for t = 1:numK_2*numTarget
        pwy_b_t(:,t) = pwy_b_t(:,t)/sum(pwy_b_t(:,t));
    end
    for t = 1:numK_3*numTarget
        pwy_c_t(:,t) = pwy_c_t(:,t)/sum(pwy_c_t(:,t));
    end
    for t = 1:numC*numTarget
        pyz_c_t(:,t) = pyz_c_t(:,t)/sum(pyz_c_t(:,t));
    end
    for i = 1:numTarget
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTest(t);
            end
        end
        if i == 1
            pos = 0;
        end
        for t = 1:numC
            pdz_t(pos+1:pos+numTest(i),t) = pdz_t(pos+1:pos+numTest(i),t)/sum(pdz_t(pos+1:pos+numTest(i),t));
        end
    end
    for i = 1:numSource
        pzr_s(1,(i-1)*numC+1:i*numC) = pzr_s(1,(i-1)*numC+1:i*numC)/sum(pzr_s(1,(i-1)*numC+1:i*numC));
    end
    for i = 1:numTarget
        pzr_t(1,(i-1)*numC+1:i*numC) = pzr_t(1,(i-1)*numC+1:i*numC)/sum(pzr_t(1,(i-1)*numC+1:i*numC));
    end

    %%%%%% The output results
    pzd_t = [];
    for i = 1:numTarget
        A = pdz_t(pos+1:pos+numTest(i),:);
        for t = 1:numC
            A(:,t) = A(:,t)*pzr_t(1,(i-1)*numC+t);
        end
        A = A*pr(numSource+i);
        pzd_t = [pzd_t; A];
    end
    iter_results(iterID+1,1) = iterID;
    for i = 1:numTarget
        pos = 0;
        if i > 1
            for t = 1:i-1
                pos = pos + numTest(t);
            end
        end
        if i == 1
            pos = 0;
        end
        pzd = pzd_t(pos+1:pos+numTest(i),:);
        nCorrect = 0;
        for j = 1:size(pzd,1)
            [va vi] = max(pzd(j,:));
            if labelset(vi) == TestY(pos+j)
                nCorrect = nCorrect + 1;
            end
        end
        iter_results(iterID+1,i+1) = nCorrect/(numTest(i));
    end
end
%% output
Results = iter_results
end


function [y,swp]=mvk2(a,x,eps,nswp,z,rmax,varargin)
%Two-sided DMRG fast matrix-by-vector product
%   [Y,SWP]=MVK2(A,X,EPS,[NSWP],[Y],[RMAX],[OPTIONS]) Two-sided DMRG (mvk
%   is one-sided). Matrix-by-vector product of a TT-matrix A
%   by a TT-tensor X with accuracy EPS. Also, one can specify the number of
%   sweeps NSWP, initial approximation Z and the maximal TT-rank RMAX (if
%   they become too large)
%
%
% 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
%---------------------------
n=a.n;
m=a.m;
att=a.tt;
corea=att.core;
psa=att.ps;
ra=att.r;
d=att.d;
%start_val='fast_mv';
start_val='rough_mv';
%start_val='random';
rmax_loc=8; %For the "rough" matrix-by-vector product
if ( nargin <= 5 || isempty(rmax) )
   rmax=1000;
end
if ( nargin <= 4 || isempty(z) )

    if ( strcmp(start_val,'random') )
    rz1=rank(a,1)*rank(x,1);
    rzd=rank(a,d+1)*rank(x,d+1);
    kf=5;
    rz=[rz1;kf*ones(d-1,1);rzd];
    z=tt_rand(n,ndims(x),rz);
    elseif (strcmp(start_val,'rough_mv'))
        rmax_loc=8; %
        xloc=round(x,0,rmax_loc);
        aloc=round(a,0,rmax_loc);
        z=round(aloc*xloc,eps);
        %z=aloc*xloc;
    elseif (strcmp(start_val,'fast_mv') )
         rz1=rank(a,1)*rank(x,1);
      rzd=rank(a,d+1)*rank(x,d+1);
      kf=5;
      rz=[rz1;kf*ones(d-1,1);rzd];
      z=tt_rand(n,ndims(x),rz);
       z=mvk2(a,x,max(eps,1e-2),10,z,rmax); %First, do it with bad accuracy
    end
end
if ( nargin <= 3 || isempty(nswp) )
  nswp = 40;
end
y=z;

%Parameters section
kick_rank=6;
verb=false;

%Warmup is to orthogonalize Y from right-to-left and compute psi-matrices
%for Ax
psi=cell(d+1,1); %Psi-matrices
%psi{d+1}=1; psi{1}=1;
psi{d+1}=eye(rank(y,d+1));
psi{1}=eye(rank(y,1));
%Here we will add convergence test
%Warmup: right-to-left QR + computation of psi matrices

corex=x.core;
psx=x.ps;
rx=x.r;

swp=1;
converged=false;
while (swp <= nswp && ~converged)
psy=y.ps;
  ry=y.r;
  corey=y.core;
     pos1=psy(d+1);
cr1=corey(psy(d):psy(d+1)-1);

for i=d:-1:2
   cr2=corey(psy(i-1):psy(i)-1);
   cr1=reshape(cr1,[ry(i),n(i)*ry(i+1)]);
   cr2=reshape(cr2,[ry(i-1)*n(i-1),ry(i)]);
   cr1=cr1.';
   [q,rm]=qr(cr1,0); rn=size(q,2); rm=rm.';
   q=q.';
   ry(i)=rn;
   corey(pos1-ry(i+1)*n(i)*ry(i):pos1-1)=q(:);
   %Convolution is now performed for psi(i) using psi(i+1) and corea, and
   %(new) core q
   cra=corea(psa(i):psa(i+1)-1); cra=reshape(cra,[ra(i),n(i),m(i),ra(i+1)]);
   cry=reshape(conj(q),[ry(i),n(i),ry(i+1)]);
   crx=corex(psx(i):psx(i+1)-1); crx=reshape(crx,[rx(i),m(i),rx(i+1)]);
   pscur=psi{i+1}; pscur=reshape(pscur,[ra(i+1),rx(i+1),ry(i+1)]); %ra,rx,ry
   %First, convolve over rx(i+1)
   crx=reshape(crx,[rx(i)*m(i),rx(i+1)]);
   pscur=permute(pscur,[2,1,3]); pscur=reshape(pscur,[rx(i+1),ra(i+1)*ry(i+1)]);
   pscur=crx*pscur; %pscur is now rx(i)*m(i)*ra(i+1)*ry(i+1)
   %Convolve over m(i),ra(i+1),n(i),ry(i+1)
    pscur=reshape(pscur,[rx(i),m(i)*ra(i+1),ry(i+1)]);
    pscur=permute(pscur,[1,3,2]);
    pscur=reshape(pscur,[rx(i)*ry(i+1),m(i)*ra(i+1)]);
    cra=reshape(cra,[ra(i)*n(i),m(i)*ra(i+1)]); cra=cra.';
    pscur=pscur*cra;
    %pscur is now rx(i)*ry(i+1)*ra(i)*n(i), it is left to convolve over
    %n(i)*ry(i+1)
    pscur=reshape(pscur,[rx(i),ry(i+1),ra(i),n(i)]);
    pscur=permute(pscur,[3,1,4,2]);
    pscur=reshape(pscur,[rx(i)*ra(i),n(i)*ry(i+1)]);
    cry=reshape(cry,[ry(i),n(i)*ry(i+1)]); cry=cry.';
    pscur=pscur*cry;
    psi{i}=pscur;
   %End of psi-block
   pos1=pos1-ry(i+1)*n(i)*ry(i);
   cr1=cr2*rm;

end
corey(pos1-ry(2)*n(1)*ry(1):pos1-1)=cr1(:);
pos1=pos1-ry(2)*n(1)*ry(1);
corey=corey(pos1:numel(corey)); %Truncate unused elements

  %left-to-right dmrg sweep
  pos1=1;
  cry_old=corey;
  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
  converged=true;
  ermax=0;
  for i=1:d-1
     %We care for two cores, with number i & number i+1, and use
     %psi(i) and psi(i+2) as a basis; also we will need to recompute
     %psi(i+1)
     ps1=psi{i}; ps2=psi{i+2};
     cra1=corea(psa(i):psa(i+1)-1); cra2=corea(psa(i+1):psa(i+2)-1);
     crx1=corex(psx(i):psx(i+1)-1); crx2=corex(psx(i+1):psx(i+2)-1);
     %our convolution is
     %ps1(ra(i),rx(i),ry(i))*cra1(ra(i),n(i),m(i),ra(i+1))*
     %cra2(ra(i+1),n(i+1),m(i+1),ra(i+2))*
     %*ps2(ra(i+2),rx(i+2),ry(i+2))
     %*crx1(rx(i),m(i),rx(i+1))*cr2x(rx(i+1)*m(i+1)*rx(i+2))
     %Scheme ps1*crx1 over rx(i)

     ps1=reshape(ps1,[ra(i),rx(i),ry(i)]);
     ps1=permute(ps1,[1,3,2]); ps1=reshape(ps1,[ra(i)*ry(i),rx(i)]);
     crx1=reshape(crx1,[rx(i),m(i)*rx(i+1)]);
     ps1=ps1*crx1; %ps1 is now ra(i)*ry(i)*m(i)*rx(i+1)
     %Now convolve with matrix A over ra(i)*m(i)
     ps1=reshape(ps1,[ra(i),ry(i),m(i),rx(i+1)]);
     ps1=permute(ps1,[2,4,1,3]);
     ps1=reshape(ps1,[ry(i)*rx(i+1),ra(i)*m(i)]);
     cra1=reshape(cra1,[ra(i),n(i),m(i),ra(i+1)]);
     cra1=permute(cra1,[1,3,2,4]);
     cra1=reshape(cra1,[ra(i)*m(i),n(i)*ra(i+1)]);
     ps1=ps1*cra1; %ps1 is now ry(i)*rx(i+1)*n(i)*ra(i+1)
     %Then the ``same'' convolution is carried over for second pair of
     %cores
     ps2=reshape(ps2,[ra(i+2),rx(i+2),ry(i+2)]);
     ps2=permute(ps2,[2,1,3]);
     ps2=reshape(ps2,[rx(i+2),ra(i+2)*ry(i+2)]);
     crx2=reshape(crx2,[rx(i+1)*m(i+1),rx(i+2)]);
     ps2=crx2*ps2; %ps2 is now rx*(i+1)*m(i+1)*ra(i+2)*ry(i+2)
     %Convolve over m(i+1)*ra(i+2)
     ps2=reshape(ps2,[rx(i+1),m(i+1)*ra(i+2),ry(i+2)]);
     ps2=permute(ps2,[2,1,3]);
     ps2=reshape(ps2,[m(i+1)*ra(i+2),rx(i+1)*ry(i+2)]);
     cra2=reshape(cra2,[ra(i+1)*n(i+1),m(i+1)*ra(i+2)]);
     ps2=cra2*ps2;
     %ps2 is now ra(i+1)*n(i+1)*rx(i+1)*ry(i+2)
     %Now form superblock by contraction ps1 & ps2
     %over ra(i+1)*rx(i+1)
     ps0=reshape(ps1,[ry(i),rx(i+1),n(i),ra(i+1)]);
     ps0=permute(ps0,[1,3,2,4]);
     ps0=reshape(ps0,[ry(i)*n(i),rx(i+1)*ra(i+1)]);
     ps2=reshape(ps2,[ra(i+1),n(i+1),rx(i+1),ry(i+2)]);
     ps2=permute(ps2,[3,1,2,4]);
     ps2=reshape(ps2,[rx(i+1)*ra(i+1),n(i+1)*ry(i+2)]);
     super_core=ps0*ps2; %super_core is ry(i)*n(i)*n(i+1)*ry(i+2)
    if ( i > 1 )
     %Compute previous supercore
        cr1=corey(pos1:pos1+ry(i)*n(i)*ry(i+1)-1);
        cr2=cry_old(psy(i+1):psy(i+2)-1);
        cr1=reshape(cr1,[ry(i)*n(i),ry(i+1)]);
        cr2=reshape(cr2,[ry(i+1),n(i+1)*ry(i+2)]);
         super_core_old=cr1*cr2;
         er=norm(super_core_old(:)-super_core(:))/norm(super_core(:));

        if ( er > eps )
            converged=false;
        end
        ermax=max(er,ermax);
     %if ( verb )
     %  fprintf('i=%d er=%3.2e \n',i,er);
     %end
     end
     [u,s,v]=svd(super_core,'econ');
     s=diag(s);
     r=my_chop2(s,eps/sqrt(d-1)*norm(s)); r=min(r,rmax);
     u=u(:,1:r); s=s(1:r); v=v(:,1:r); v=v*diag(s);

     %Kick rank

     ur=randn(size(u,1),kick_rank);
     %Orthogonalize ur to u by Golub-Kahan reorth
     u=reort(u,ur);
     radd=size(u,2)-r;
     if ( radd > 0 )
        vr=zeros(size(v,1),radd);
        v=[v,vr];
     end
     r=size(u,2);

     ry(i+1)=r;
     %u is ry(i)*n(i)*ry(i+1)
     %core_new(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=u(:);
     corey(pos1:pos1+ry(i)*n(i)*ry(i+1)-1)=u(:);

     u=reshape(u,[ry(i),n(i),ry(i+1)]);

     %Compute new psi
     %ps1 is ry(i)*rx(i+1)*n(i)*ra(i+1) with u over ry(i)*n(i)
     u=conj(u);
     ps1=reshape(ps1,[ry(i),rx(i+1),n(i),ra(i+1)]);
     ps1=permute(ps1,[4,2,1,3]);
     ps1=reshape(ps1,[ra(i+1)*rx(i+1),ry(i)*n(i)]);
     u=reshape(u,[ry(i)*n(i),ry(i+1)]);
     ps1=ps1*u;
     psi{i+1}=ps1;
     %Compute (?) new v
     pos1=pos1+ry(i)*n(i)*ry(i+1);
     v=v';
     %v=reshape(v,[ry(i+1),n(i+1),ry(i+2)]);
     corey(pos1:pos1+ry(i+1)*n(i+1)*ry(i+2)-1)=v(:);
  end
  psy=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
y.core=corey;
y.r=ry;
y.ps=psy;
swp=swp+1;
if ( verb )
fprintf('swp=%d er=%3.2e trunk=%3.2e \n',swp,ermax,eps/sqrt(d-1));
end
end
if ( swp == nswp )
  fprintf('mvk2 warning: error is not fixed for maximal number of sweeps %d\n', swp);
end%end
%p1=tt_mvdot(core(a),core(x),y0);
%p2=dot(y,tt_tensor(y0));
%abs(p1-p2)/abs(p1)
%keyboard;


%%*************************************************************************
%% sqlp: main solver
%%
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

  function [obj,X,y,Z,info,runhist] = sqlpmain(blk,At,C,b,par,parbarrier,X0,y0,Z0);

   global spdensity smallblkdim  printlevel msg
   global solve_ok  use_LU  exist_analytic_term numpertdiagschur
   global schurfun  schurfun_par
%%
   randstate = rand('state');  randnstate = randn('state');
   rand('state',0);   randn('state',0);
%%
   vers          = par.vers;
   predcorr      = par.predcorr;
   gam           = par.gam;
   expon         = par.expon;
   gaptol        = par.gaptol;
   inftol        = par.inftol;
   steptol       = par.steptol;
   maxit         = par.maxit;
   printlevel    = par.printlevel;
   stoplevel     = par.stoplevel;
   scale_data    = par.scale_data;
   spdensity     = par.spdensity;
   rmdepconstr   = par.rmdepconstr;
   smallblkdim   = par.smallblkdim;
   schurfun      = par.schurfun;
   schurfun_par  = par.schurfun_par;
   ublksize      = par.ublksize;
%%
   tstart = clock;
   X = X0; y = y0; Z = Z0;
   for p = 1:size(blk,1)
      if strcmp(blk{p,1},'u'); Z{p} = zeros(blk{p,2},1); end
   end
%%
%%-----------------------------------------
%% convert unrestricted blk to linear blk.
%%-----------------------------------------
%%
   convertlen = 0;
   [blk,At,C,X,Z,u2lblk,ublkidx] = sqlpu2lblk(blk,At,C,X,Z,par,convertlen);
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      if (u2lblk(p) == 1)
         n = 2*blk{p,2};
         blk{p,1} = 'l';  blk{p,2} = n;
         parbarrier{p} = zeros(1,n);
         At{p} = [At{p}; -At{p}];
         tau = max(1,norm(C{p}));
         C{p} = [C{p}; -C{p}];
         msg = 'convert ublk to lblk';
         if (printlevel); fprintf(' *** %s',msg); end
         b2 = 1 + abs(b');
         normCtmp = 1+norm(C{p});
         normAtmp = 1+sqrt(sum(At{p}.*At{p}));
         if (n > 1000)
            const = sqrt(n);
         else
	    const = n;
         end
         if (par.startpoint == 1)
            X{p} = const* max([1,b2./normAtmp]) *ones(n,1);
            Z{p} = const* max([1,normAtmp/sqrt(n),normCtmp/sqrt(n)]) *ones(n,1);
            X{p} = X{p}.*(1+1e-10*rand(n,1));
            Z{p} = Z{p}.*(1+1e-10*rand(n,1));
	 else
            const = max(abs(X{p})) + 100;
            X{p} = [X{p}+const; const*ones(n/2,1)];
            %%old: const = 100; Z{p} = [const*ones(n/2,1); const*ones(n/2,1)];
            Z{p} = [abs(Z0{p}); abs(Z0{p})] + 1e-4;
         end
      end
   end
%%-----------------------------------------
%% check whether {A1,...,Am} is
%% linearly independent.
%%-----------------------------------------
%%
   m0 = length(b);
   [At,b,y,indeprows,par.depconstr,feasible,par.AAt] = ...
    checkdepconstr(blk,At,b,y,rmdepconstr);
   if (~feasible)
      obj = []; X = cell(size(blk,1),1); y = []; Z = cell(size(blk,1),1);
      runhist = [];
      msg = 'SQLP is not feasible';
      if (printlevel); fprintf('\n %s \n',msg); end
      return;
   end
   par.normAAt = norm(par.AAt,'fro');
%%
%%-----------------------------------------
%% scale SQLP data. Note: must be done only
%% after checkdepconstr
%%-----------------------------------------
%%
   normA2 = 1+ops(At,'norm');
   normb2 = 1+norm(b);
   normC2 = 1+ops(C,'norm');
   normX0 = 1+ops(X0,'norm');
   normZ0 = 1+ops(Z0,'norm');
   if (scale_data)
      [At,C,b,normA,normC,normb,X,y,Z] = scaling(blk,At,C,b,X,y,Z);
   else
      normA = 1; normC = 1; normb = 1;
   end
%%
%%-----------------------------------------
%% find the combined list of non-zero
%% elements of Aj, j = 1:k, for each k.
%% IMPORTANT NOTE: Ak, C are permuted.
%%-----------------------------------------
%%
   par.numcolAt = length(b);
   [At,C,X,Z,par.permA,par.permZ] = sortA(blk,At,C,b,X,Z);
   [par.isspA,par.nzlistA,par.nzlistAsum,par.isspAy,par.nzlistAy] = nzlist(blk,At,par);
%%
%%-----------------------------------------
%% create an artifical non-negative block
%% for a purely log-barrier problem
%%-----------------------------------------
%%
   numblkold = size(blk,1);
   nn = 0;
   for p = 1:size(blk,1);
      pblk = blk(p,:);
      idx = find(parbarrier{p}==0);
      if ~isempty(idx);
         if strcmp(pblk{1},'l')
            nn = nn + length(idx);
         elseif strcmp(pblk{1},'s') | strcmp(pblk{1},'q')
            nn = nn + sum(pblk{2}(idx));
         end
      end
   end
   if (nn==0)
      analytic_prob = 1;
      numblk = size(blk,1)+1;
      blk{numblk,1} = 'l'; blk{numblk,2} = 1;
      At{numblk,1} = sparse(1,length(b));
      C{numblk,1} = 1;
      X{numblk,1} = 1e3;
      Z{numblk,1} = 1e3;
      parbarrier{numblk,1} = 0;
      u2lblk(numblk,1) = 0;
      nn = nn + 1;
   else
      analytic_prob = 0;
   end
%%
   exist_analytic_term = 0;
   for p = 1:size(blk,1);
      idx = find(parbarrier{p} > 0);
      if ~isempty(idx);
         exist_analytic_term = 1;
      end
   end
%%-----------------------------------------
%% initialization
%%-----------------------------------------
%%
   EE = ops(blk,'identity');
   normE2 = ops(EE,'norm'); Zpertold = 1;
   for p = 1:size(blk,1)
      normCC(p) = 1+ops(C(p),'norm');
      normEE(p) = 1+ops(EE(p),'norm');
   end
   [Xchol,indef(1)] = blkcholfun(blk,X);
   [Zchol,indef(2)] = blkcholfun(blk,Z);
   if any(indef)
      msg = 'stop: X or Z not positive definite';
      if (printlevel); fprintf('\n  %s\n',msg); end
      info.termcode = -3;
      info.msg1 = msg;
      obj = []; X = cell(size(blk,1),1); y = []; Z = cell(size(blk,1),1);
      runhist = [];
      return;
   end
   AX = AXfun(blk,At,par.permA,X);
   rp = b-AX;
   ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
   ZpATynorm = ops(ZpATy,'norm');
   Rd = ops(C,'-',ZpATy);
   objadd0 = 0;
   if (scale_data)
      for p = 1:size(blk,1)
         pblk = blk(p,:);
         objadd0 = objadd0 + sum(parbarrier{p}.*pblk{2})*log(normA{p});
      end
   end
   objadd = blkbarrier(blk,X,Z,Xchol,Zchol,parbarrier) + objadd0;
   obj = (normb*normC)*[blktrace(blk,C,X), b'*y] + objadd;
   gap = (normb*normC)*blktrace(blk,X,Z) - diff(objadd);
   relgap = gap/(1+sum(abs(obj)));
   prim_infeas = norm(rp)/normb2;
   dual_infeas = ops(Rd,'norm')/normC2;
   infeas = max(prim_infeas,dual_infeas);
   if (scale_data)
      infeas_org(1) = prim_infeas*normb;
      infeas_org(2) = dual_infeas*normC;
   else
      infeas_org = [0,0];
   end
   trXZ = blktrace(blk,X,Z,parbarrier);
   if (nn > 0); mu  = trXZ/nn; else; mu = gap/ops(X,'getM'); end
   normX = ops(X,'norm');
%%
   termcode = 0; restart = 0;
   pstep = 1; dstep = 1; pred_convg_rate = 1; corr_convg_rate = 1;
   prim_infeas_min  = prim_infeas;
   dual_infeas_min  = dual_infeas;
   prim_infeas_best = prim_infeas;
   dual_infeas_best = dual_infeas;
   infeas_best = infeas;
   relgap_best = relgap;
   homRd = inf; homrp = inf; dy = zeros(length(b),1);
   msg = []; msg2 = []; msg3 = [];
   runhist.pobj    = obj(1);
   runhist.dobj    = obj(2);
   runhist.gap     = gap;
   runhist.relgap  = relgap;
   runhist.pinfeas = prim_infeas;
   runhist.dinfeas = dual_infeas;
   runhist.infeas  = infeas;
   runhist.step    = 0;
   runhist.normX   = normX;
   runhist.cputime = etime(clock,tstart);
   ttime.preproc   = runhist.cputime;
   ttime.pred = 0; ttime.pred_pstep = 0; ttime.pred_dstep = 0;
   ttime.corr = 0; ttime.corr_pstep = 0; ttime.corr_dstep = 0;
   ttime.pchol = 0; ttime.dchol = 0; ttime.misc = 0;
%%
%%-----------------------------------------
%% display parameters and initial info
%%-----------------------------------------
%%
   if (printlevel >= 2)
      fprintf('\n********************************************');
      fprintf('***********************\n');
      fprintf('   SDPT3: Infeasible path-following algorithms');
      fprintf('\n********************************************');
      fprintf('***********************\n');
      [hh,mm,ss] = mytime(ttime.preproc);
      if (printlevel>=3)
         fprintf(' version  predcorr  gam  expon  scale_data\n');
         if (vers == 1); fprintf('   HKM '); elseif (vers == 2); fprintf('    NT '); end
         fprintf('     %1.0f      %4.3f',predcorr,gam);
         fprintf('   %1.0f        %1.0f    %1.0f\n',expon,scale_data);
         fprintf('\nit pstep dstep pinfeas dinfeas  gap')
         fprintf('      mean(obj)   cputime\n');
         fprintf('------------------------------------------------');
         fprintf('-------------------\n');
         fprintf('%2.0f|%4.3f|%4.3f|%2.1e|%2.1e|',0,0,0,prim_infeas,dual_infeas);
         fprintf('%2.1e|%- 7.6e| %s:%s:%s|',gap,mean(obj),hh,mm,ss);
      end
   end
%%
%%---------------------------------------------------------------
%% start main loop
%%---------------------------------------------------------------
%%
   param.termcode    = termcode;
   param.iter        = 0;
   param.obj         = obj;
   param.relgap      = relgap;
   param.prim_infeas = prim_infeas;   param.dual_infeas = dual_infeas;
   param.homRd       = homRd;         param.homrp       = homrp;
   param.AX          = AX;            param.ZpATynorm   = ZpATynorm;
   param.normA       = normA;
   param.normb       = normb;         param.normC       = normC;
   param.normX0      = normX0;        param.normZ0      = normZ0;
   param.m0          = m0;            param.indeprows   = indeprows;
   param.prim_infeas_bad = 0;
   param.dual_infeas_bad = 0;
   param.prim_infeas_min = prim_infeas;
   param.dual_infeas_min = dual_infeas;
   param.gaptol      = gaptol;
   param.inftol      = inftol;
   param.maxit       = maxit;
   param.scale_data  = scale_data;
   param.printlevel  = printlevel;
   param.ublksize    = ublksize;
   Xbest = X; ybest = y; Zbest = Z;
%%
   for iter = 1:maxit;
      tstart  = clock;
      timeold = tstart;
      update_iter = 0; breakyes = 0;
      pred_slow = 0; corr_slow = 0; step_short = 0;
      par.parbarrier = parbarrier;
      par.iter    = iter;
      par.obj     = obj;
      par.relgap  = relgap;
      par.pinfeas = prim_infeas;
      par.dinfeas = dual_infeas;
      par.rp      = rp;
      par.y       = y;
      par.dy      = dy;
      par.normX   = normX;
      par.ZpATynorm = ZpATynorm;
      %%if (printlevel > 2); fprintf(' %2.1e',par.normX); end
      if (iter == 1 | restart); Cpert = min(1,normC2/ops(EE,'norm')); end
      if (runhist.dinfeas(1) > 1e-3) & (~exist_analytic_term) ...
         & (relgap > 1e-4)
         if (par.normX > 5e3 & iter < 20)
            Cpert = Cpert*0.5;
         elseif (par.normX > 5e2 & iter < 20);
            Cpert = Cpert*0.3;
         else;
            Cpert = Cpert*0.1;
         end
         Rd = ops(Rd,'+',EE,Cpert);
         %%if (printlevel > 2); fprintf('|%2.1e',Cpert); end
      end
%%---------------------------------------------------------------
%% predictor step.
%%---------------------------------------------------------------
%%
      if (predcorr)
         sigma = 0;
      else
         sigma = 1-0.9*min(pstep,dstep);
         if (iter == 1); sigma = 0.5; end;
      end
      sigmu = cell(size(blk,1),1);
      for p = 1:size(blk,1)
         sigmu{p} = max(sigma*mu, parbarrier{p}');
      end
      invXchol = cell(size(blk,1),1);
      invZchol = ops(Zchol,'inv');
      if (vers == 1);
         [par,dX,dy,dZ,coeff,L,hRd] = ...
          HKMpred(blk,At,par,rp,Rd,sigmu,X,Z,invZchol);
      elseif (vers == 2);
         [par,dX,dy,dZ,coeff,L,hRd] = ...
          NTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);
      end
      if (solve_ok <= 0)
         msg = 'stop: difficulty in computing predictor directions';
         if (printlevel); fprintf('\n  %s',msg); end
         runhist.pinfeas(iter+1) = runhist.pinfeas(iter);
         runhist.dinfeas(iter+1) = runhist.dinfeas(iter);
         runhist.relgap(iter+1)  = runhist.relgap(iter);
         runhist.cputime(iter+1) = etime(clock,tstart);
         termcode = -4;
         break; %% do not ues breakyes = 1
      end
      timenew = clock;
      ttime.pred = ttime.pred + etime(timenew,timeold); timeold = timenew;
%%
%%-----------------------------------------
%% step-lengths for predictor step
%%-----------------------------------------
%%
      if (gam == 0)
         gamused = 0.9 + 0.09*min(pstep,dstep);
      else
         gamused = gam;
      end
      [Xstep,invXchol] = steplength(blk,X,dX,Xchol,invXchol);
      pstep = min(1,gamused*full(Xstep));
      timenew = clock;
      ttime.pred_pstep = ttime.pred_pstep + etime(timenew,timeold); timeold = timenew;
      Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
      dstep = min(1,gamused*full(Zstep));
      trXZnew = trXZ + pstep*blktrace(blk,dX,Z,parbarrier) ...
                 + dstep*blktrace(blk,X,dZ,parbarrier) ...
                 + pstep*dstep*blktrace(blk,dX,dZ,parbarrier);
      if (nn > 0); mupred  = trXZnew/nn; else; mupred = 1e-16; end
      mupredhist(iter) = mupred;
      timenew = clock;
      ttime.pred_dstep = ttime.pred_dstep + etime(timenew,timeold); timeold = timenew;
%%
%%-----------------------------------------
%%  stopping criteria for predictor step.
%%-----------------------------------------
%%
      if (min(pstep,dstep) < steptol) & (stoplevel) & (iter > 10)
         msg = 'stop: steps in predictor too short';
         if (printlevel)
            fprintf('\n  %s',msg);
            fprintf(': pstep = %3.2e,  dstep = %3.2e\n',pstep,dstep);
         end
         runhist.cputime(iter+1) = etime(clock,tstart);
         termcode = -2;
         breakyes = 1;
      end
      if (~predcorr)
         if (iter >= 2)
            idx = [max(2,iter-2) : iter];
            pred_slow = all(mupredhist(idx)./mupredhist(idx-1) > 0.4);
            idx = [max(2,iter-5) : iter];
            pred_convg_rate = mean(mupredhist(idx)./mupredhist(idx-1));
            pred_slow = pred_slow + (mupred/mu > 5*pred_convg_rate);
         end
         if (max(mu,infeas) < 1e-6) & (pred_slow) & (stoplevel)
            msg = 'stop: lack of progress in predictor';
            if (printlevel)
               fprintf('\n  %s',msg);
               fprintf(': mupred/mu = %3.2f, pred_convg_rate = %3.2f.',...
               mupred/mu,pred_convg_rate);
            end
            runhist.cputime(iter+1) = etime(clock,tstart);
            termcode = -2;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%---------------------------------------------------------------
%% corrector step.
%%---------------------------------------------------------------
%%
      if (predcorr) & (~breakyes)
         step_pred = min(pstep,dstep);
         if (mu > 1e-6)
            if (step_pred < 1/sqrt(3));
               expon_used = 1;
            else
               expon_used = max(expon,3*step_pred^2);
            end
         else
            expon_used = max(1,min(expon,3*step_pred^2));
         end
         if (nn==0)
             sigma = 0.2;
         elseif (mupred < 0)
             sigma = 0.8;
         else
            sigma = min(1, (mupred/mu)^expon_used);
         end
         sigmu = cell(size(blk,1),1);
         for p = 1:size(blk,1)
            sigmu{p} = max(sigma*mu, parbarrier{p}');
         end
         if (vers == 1)
            [dX,dy,dZ] = HKMcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         elseif (vers == 2)
            [dX,dy,dZ] = NTcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         end
         if (solve_ok <= 0)
            msg = 'stop: difficulty in computing corrector directions';
            if (printlevel); fprintf('\n  %s',msg); end
            runhist.pinfeas(iter+1) = runhist.pinfeas(iter);
            runhist.dinfeas(iter+1) = runhist.dinfeas(iter);
            runhist.relgap(iter+1)  = runhist.relgap(iter);
            runhist.cputime(iter+1) = etime(clock,tstart);
            termcode = -4;
            break; %% do not ues breakyes = 1
         end
         timenew = clock;
         ttime.corr = ttime.corr + etime(timenew,timeold); timeold = timenew;
%%
%%-----------------------------------
%% step-lengths for corrector step
%%-----------------------------------
%%
         if (gam == 0)
            gamused = 0.9 + 0.09*min(pstep,dstep);
         else
            gamused = gam;
         end
         Xstep = steplength(blk,X,dX,Xchol,invXchol);
         pstep = min(1,gamused*full(Xstep));
         timenew = clock;
         ttime.corr_pstep = ttime.corr_pstep+etime(timenew,timeold); timeold = timenew;
         Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
         dstep = min(1,gamused*full(Zstep));
         trXZnew = trXZ + pstep*blktrace(blk,dX,Z,parbarrier) ...
                    + dstep*blktrace(blk,X,dZ,parbarrier)...
                    + pstep*dstep*blktrace(blk,dX,dZ,parbarrier);
         if (nn > 0); mucorr  = trXZnew/nn; else; mucorr = 1e-16; end
         timenew = clock;
         ttime.corr_dstep = ttime.corr_dstep+etime(timenew,timeold); timeold = timenew;
%%
%%-----------------------------------------
%%  stopping criteria for corrector step
%%-----------------------------------------
         if (iter >= 2)
            idx = [max(2,iter-2) : iter];
            corr_slow = all(runhist.gap(idx)./runhist.gap(idx-1) > 0.8);
            idx = [max(2,iter-5) : iter];
            corr_convg_rate = mean(runhist.gap(idx)./runhist.gap(idx-1));
            corr_slow = corr_slow + (mucorr/mu > max(min(1,5*corr_convg_rate),0.8));
         end
	 if (max(relgap,infeas) < 1e-6) & (iter > 20) ...
            & (corr_slow > 1) & (stoplevel)
            msg = 'stop: lack of progress in corrector';
   	    if (printlevel)
               fprintf('\n  %s',msg);
               fprintf(': mucorr/mu = %3.2f, corr_convg_rate = %3.2f',...
               mucorr/mu,corr_convg_rate);
            end
            runhist.cputime(iter+1) = etime(clock,tstart);
            termcode = -2;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%---------------------------------------------------------------
%% udpate iterate
%%---------------------------------------------------------------
      indef = [1,1];
      if (update_iter)
         for t = 1:5
            [Xchol,indef(1)] = blkcholfun(blk,ops(X,'+',dX,pstep));
            timenew = clock;
            ttime.pchol = ttime.pchol + etime(timenew,timeold); timeold = timenew;
            if (indef(1)); pstep = 0.8*pstep; else; break; end
         end
	 if (t > 1); pstep = gamused*pstep; end
	 for t = 1:5
            [Zchol,indef(2)] = blkcholfun(blk,ops(Z,'+',dZ,dstep));
            timenew = clock;
            ttime.dchol = ttime.dchol + etime(timenew,timeold); timeold = timenew;
            if (indef(2)); dstep = 0.8*dstep; else; break; end
         end
	 if (t > 1); dstep = gamused*dstep; end
         %%-------------------------------------------
         AXtmp = AX + pstep*AXfun(blk,At,par.permA,dX);
         prim_infeasnew = norm(b-AXtmp)/normb2;
         if (relgap < 5*infeas); alpha = 1e2; else; alpha = 1e3; end
         if any(indef)
            if indef(1); msg = 'stop: X not positive definite'; end
            if indef(2); msg = 'stop: Z not positive definite'; end
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -3;
            breakyes = 1;
         elseif (prim_infeasnew > max([1e-8,relgap,20*prim_infeas]) & iter > 10) ...
            | (prim_infeasnew > max([1e-7,1e3*prim_infeas,0.1*relgap]) & relgap < 1e-2) ...
            | (prim_infeasnew > alpha*max([1e-9,param.prim_infeas_min]) ...
               & (prim_infeasnew > max([3*prim_infeas,0.1*relgap])) ...
               & (iter > 25) & (dual_infeas < 1e-6) & (relgap < 0.1)) ...
            | ((prim_infeasnew > 1e3*prim_infeas & prim_infeasnew > 1e-12) ...
               & (max(relgap,dual_infeas) < 1e-8))
            if (stoplevel)
               msg = 'stop: primal infeas has deteriorated too much';
               if (printlevel); fprintf('\n  %s, %2.1e',msg,prim_infeasnew); end
               termcode = -7;
               breakyes = 1;
            end
         elseif (trXZnew > 1.05*runhist.gap(iter)) & (~exist_analytic_term) ...
	    & ((infeas < 1e-5) & (relgap < 1e-4) & (iter > 20) ...
	       | (max(infeas,relgap) < 1e-7) & (iter > 10))
            if (stoplevel)
               msg = 'stop: progress in duality gap has deteriorated';
               if (printlevel); fprintf('\n  %s, %2.1e',msg,trXZnew); end
               termcode = -8;
               breakyes = 1;
            end
         else
            X = ops(X,'+',dX,pstep);
            y = y + dstep*dy;
            Z = ops(Z,'+',dZ,dstep);
         end
      end
%%---------------------------------------------------------------
%% adjust linear blk arising from unrestricted blk
%%---------------------------------------------------------------
      if (~breakyes)
         for p = 1:size(blk,1)
            if (u2lblk(p) == 1)
               len = blk{p,2}/2;
               xtmp = min(X{p}([1:len]),X{p}(len+[1:len]));
               alpha = 0.8;
               X{p}([1:len])     = X{p}([1:len]) - alpha*xtmp;
               X{p}(len+[1:len]) = X{p}(len+[1:len]) - alpha*xtmp;
               if (mu < 1e-4) %% old: (mu < 1e-7)
                  Z{p} = 0.5*mu./max(1,X{p}); %% good to keep this step
               else
                  ztmp = min(1,max(Z{p}([1:len]),Z{p}(len+[1:len])));
                  if (dual_infeas > 1e-4 & dstep < 0.2)
                     beta = 0.3;
                  else
                     beta = 0.0;
                  end
                  %% important to set beta = 0 at later stage.
                  Z{p}([1:len])     = Z{p}([1:len]) + beta*ztmp;
                  Z{p}(len+[1:len]) = Z{p}(len+[1:len]) + beta*ztmp;
               end
            end
         end
      end
%%--------------------------------------------------
%% perturb Z: do this step before checking for break
%%--------------------------------------------------
      if (~breakyes) & (~exist_analytic_term)
         trXZtmp = blktrace(blk,X,Z);
         trXE  = blktrace(blk,X,EE);
         Zpert = max(1e-12,0.2*min(relgap,prim_infeas)).*normC2./normE2;
         Zpert = min(Zpert,0.1*trXZtmp./trXE);
         Zpert = min([1,Zpert,1.5*Zpertold]);
         if (infeas < 0.1)
            Z = ops(Z,'+',EE,Zpert);
            [Zchol,indef(2)] = blkcholfun(blk,Z);
            if any(indef(2))
               msg = 'stop: Z not positive definite';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -3;
               breakyes = 1;
            end
            %%if (printlevel > 2); fprintf(' %2.1e',Zpert); end
         end
         Zpertold = Zpert;
      end
%%---------------------------------------------------------------
%% compute rp, Rd, infeasibities, etc
%%---------------------------------------------------------------
%%
      AX  = AXfun(blk,At,par.permA,X);
      rp  = b-AX;
      ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
      ZpATynorm = ops(ZpATy,'norm');
      Rd  = ops(C,'-',ZpATy);
      objadd = blkbarrier(blk,X,Z,Xchol,Zchol,parbarrier) + objadd0;
      obj = (normb*normC)*[blktrace(blk,C,X), b'*y] + objadd;
      gap = (normb*normC)*blktrace(blk,X,Z) - diff(objadd);
      relgap = gap/(1+sum(abs(obj)));
      prim_infeas = norm(rp)/normb2;
      dual_infeas = ops(Rd,'norm')/normC2;
      infeas = max(prim_infeas,dual_infeas);
      if (scale_data)
         infeas_org(1) = prim_infeas*normb;
         infeas_org(2) = dual_infeas*normC;
      end
      homRd = inf; homrp = inf;
      if (ops(parbarrier,'norm') == 0)
         if (obj(2) > 0); homRd = ZpATynorm/(obj(2)); end
         if (obj(1) < 0); homrp = norm(AX)/(-obj(1))/(normC); end
      end
      trXZ = blktrace(blk,X,Z,parbarrier);
      if (nn > 0); mu = trXZ/nn; else; mu = gap/ops(X,'getM'); end
      normX = ops(X,'norm');
%%
      runhist.pobj(iter+1)  = obj(1);
      runhist.dobj(iter+1)  = obj(2);
      runhist.gap(iter+1)   = gap;
      runhist.relgap(iter+1)  = relgap;
      runhist.pinfeas(iter+1) = prim_infeas;
      runhist.dinfeas(iter+1) = dual_infeas;
      runhist.infeas(iter+1)  = infeas;
      runhist.step(iter+1)    = min(pstep,dstep);
      runhist.normX(iter+1)   = normX;
      runhist.cputime(iter+1) = etime(clock,tstart);
      timenew = clock;
      ttime.misc = ttime.misc + etime(timenew,timeold); timeold = timenew;
      [hh,mm,ss] = mytime(sum(runhist.cputime));
      if (printlevel>=3)
         fprintf('\n%2.0f|%4.3f|%4.3f',iter,pstep,dstep);
         fprintf('|%2.1e|%2.1e|%2.1e|',prim_infeas,dual_infeas,gap);
         fprintf('%- 7.6e| %s:%s:%s|',mean(obj),hh,mm,ss);
      end
%%--------------------------------------------------
%% check convergence
%%--------------------------------------------------
      param.use_LU      = use_LU;
      param.stoplevel   = stoplevel;
      param.termcode    = termcode;
      param.iter        = iter;
      param.obj         = obj;
      param.gap         = gap;
      param.relgap      = relgap;
      param.prim_infeas = prim_infeas;
      param.dual_infeas = dual_infeas;
      param.mu        = mu;
      param.homRd     = homRd;
      param.homrp     = homrp;
      param.AX        = AX;
      param.ZpATynorm = ZpATynorm;
      param.normX     = ops(X,'norm');
      param.normZ     = ops(Z,'norm');
      param.numpertdiagschur = numpertdiagschur;
      if (~breakyes)
         [param,breakyes,restart,msg2] = sqlpcheckconvg(param,runhist);
      end
      if (restart)
         [X,y,Z] = infeaspt(blk,At,C,b,2,1e5);
         rp  = b-AXfun(blk,At,par.permA,X);
         ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
         Rd  = ops(C,'-',ZpATy);
         trXZ = blktrace(blk,X,Z,parbarrier);
         mu   = trXZ/nn;
         gap  =  (normb*normC)*blktrace(blk,X,Z) - diff(objadd);
         prim_infeas = norm(rp)/normb2;
         dual_infeas = ops(Rd,'norm')/normC2;
         infeas = max(prim_infeas,dual_infeas);
         [Xchol,indef(1)] = blkcholfun(blk,X);
         [Zchol,indef(2)] = blkcholfun(blk,Z);
         stoplevel = 3;
      end
%%--------------------------------------------------
%% check for break
%%--------------------------------------------------
      if ((prim_infeas < 1.5*prim_infeas_best) ...
         | (max(relgap,infeas) < 0.8*max(relgap_best,infeas_best))) ...
         & (max(relgap,dual_infeas) < 0.8*max(relgap_best,dual_infeas_best))
         Xbest = X; ybest = y; Zbest = Z;
         prim_infeas_best = prim_infeas;
         dual_infeas_best = dual_infeas;
         relgap_best = relgap; infeas_best = infeas;
         update_best(iter+1) = 1;
         %%fprintf('#')
      else
         update_best(iter+1) = 0;
      end
      if (max(relgap_best,infeas_best) < 1e-4 ...
          & norm(update_best(max(1,iter-1):iter+1)) == 0)
         msg = 'lack of progress in infeas';
         if (printlevel); fprintf('\n  %s',msg); end
         termcode = -9;
         breakyes = 1;
      end
      if (breakyes); break; end
   end
%%---------------------------------------------------------------
%% end of main loop
%%---------------------------------------------------------------
%%
   use_bestiter = 1;
   if (use_bestiter) & (param.termcode <= 0)
      X = Xbest; y = ybest; Z = Zbest;
      Xchol = blkcholfun(blk,X);
      Zchol = blkcholfun(blk,Z);
      AX = AXfun(blk,At,par.permA,X);
      rp = b-AX;
      ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
      Rd = ops(C,'-',ZpATy);
      objadd = blkbarrier(blk,X,Z,Xchol,Zchol,parbarrier) + objadd0;
      obj = (normb*normC)*[blktrace(blk,C,X), b'*y] + objadd;
      gap = (normb*normC)*blktrace(blk,X,Z) - diff(objadd);
      relgap = gap/(1+sum(abs(obj)));
      prim_infeas = norm(rp)/normb2;
      dual_infeas = ops(Rd,'norm')/normC2;
      infeas = max(prim_infeas,dual_infeas);
      runhist.pobj(iter+1)  = obj(1);
      runhist.dobj(iter+1)  = obj(2);
      runhist.gap(iter+1)   = gap;
      runhist.relgap(iter+1)  = relgap;
      runhist.pinfeas(iter+1) = prim_infeas;
      runhist.dinfeas(iter+1) = dual_infeas;
      runhist.infeas(iter+1)  = infeas;
   end
%%---------------------------------------------------------------
%% unscale and produce infeasibility certificates if appropriate
%%---------------------------------------------------------------
   if (iter >= 1)
      [X,y,Z,termcode,resid,reldist,msg3] = ...
      sqlpmisc(blk,At,C,b,X,y,Z,par.permZ,param);
   end
%%---------------------------------------------------------------
%% recover unrestricted blk from linear blk
%%---------------------------------------------------------------
%%
   for p = 1:size(blk,1)
      if (u2lblk(p) == 1)
         n = blk{p,2}/2;
         X{p} = X{p}(1:n)-X{p}(n+[1:n]);
         Z{p} = Z{p}(1:n);
      end
   end
   for p = 1:size(ublkidx,1)
      if ~isempty(ublkidx{p,2})
         n0 = ublkidx{p,1}; idxB = setdiff([1:n0]',ublkidx{p,2});
         tmp = zeros(n0,1); tmp(idxB) = X{p}; X{p} = tmp;
         tmp = zeros(n0,1); tmp(idxB) = Z{p}; Z{p} = tmp;
      end
   end
   if (analytic_prob)
      X = X(1:numblkold); Z = Z(1:numblkold);
   end
%%---------------------------------------------------------------
%% print summary
%%---------------------------------------------------------------
%%
   maxC = 1+ops(ops(C,'abs'),'max');
   maxb = 1+max(abs(b));
   if (scale_data)
      dimacs = [infeas_org(1)*normb2/maxb; 0; infeas_org(2)*normC2/maxC; 0];
   else
      dimacs = [prim_infeas*normb2/maxb; 0; dual_infeas*normC2/maxC; 0];
   end
   dimacs = [dimacs; [-diff(obj); gap]/(1+sum(abs(obj)))];
   info.dimacs   = dimacs;
   info.termcode = termcode;
   info.iter     = iter;
   info.obj      = obj;
   info.gap      = gap;
   info.relgap   = relgap;
   info.pinfeas  = prim_infeas;
   info.dinfeas  = dual_infeas;
   info.cputime  = sum(runhist.cputime);
   info.time     = ttime;
   info.resid    = resid;
   info.reldist  = reldist;
   info.normX    = ops(X,'norm');
   info.normy    = norm(y);
   info.normZ    = ops(Z,'norm');
   info.normb    = normb2; info.maxb = maxb;
   info.normC    = normC2; info.maxC = maxC;
   info.normA    = normA2;
   info.msg1     = msg;
   info.msg2     = msg2;
   info.msg3     = msg3;
   sqlpsummary(info,ttime,infeas_org,printlevel);
   rand('state',randstate);
   randn('state',randnstate);
%%*****************************************************************************


function Rob = motion(Rob, Tim)
%MOTION Robot motion.
%   ROB = MOTION(ROB, TIM) performs one EKF-prediction motion step to robot
%   Rob in the global map Map, following the motion model in Rob.motion.
%   Both Rob and Map are updated. The time information Tim is used only if
%   the motion model requires it, but it has to be provided because MOTION
%   is a generic method.
%
%   The following motion models are supported:
%       'odometry'   uses function odo3()
%       'constVel'   uses function constVel()
%   Edit this file to add new motion models.
%
%   See also SIMMOTION, CONSTVEL, ODO3, UPDATEFRAME.

%   Copyright 2009 David Marquez @ LAAS-CNRS.

global Map

% Update rob and sen info from map
Rob = map2rob(Rob);

% robot state range
r = Rob.state.r;

switch Rob.motion

    case  {'constVel'} % constant velocity

        % motion model of the robot: mean and Jacobians
        [Map.x(r), F_x, F_u] = constVel(Map.x(r),Rob.con.u,Tim.dt);

        % update Rob and Map structures - mean only
        Rob = map2rob(Rob);

        % Covariances matrix update
        predictBlockEkf(r, F_x, Rob.con.U, F_u);

    case  {'odometry'}  % 3D odometry

        % motion model of the robot: mean and Jacobians
        [Rob.frame, F_x, F_u]   = odo3(Rob.frame,Rob.con.u);

        % update Rob and Map structures - mean only
        Map.x(Rob.frame.r) = Rob.frame.x;

        % Covariances matrix update
        predictBlockEkf(r, F_x, Rob.con.U, F_u);

    otherwise

        error('??? Unknown motion model ''%s''.',Rob.motion);
end

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [baseMVA, bus, gen, branch, areas, gencost] = case57
%CASE57    Power flow data for IEEE 57 bus test case.
%   Please see 'help caseformat' for details on the case file format.
%   This data was converted from IEEE Common Data Format
%   (ieee57cdf.txt) on 20-Sep-2004 by cdf2matp, rev. 1.11
%   See end of file for warnings generated during conversion.
%
%   Converted from IEEE CDF file from:
%       http://www.ee.washington.edu/research/pstca/
%
%   Manually modified Qmax, Qmin on generator 1 to 200, -140, respectively.
%
%  08/25/93 UW ARCHIVE           100.0  1961 W IEEE 57 Bus Test Case

%   MATPOWER
%   $Id: case57.m,v 1.5 2004/09/21 01:47:48 ray Exp $

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

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
bus = [
	1	3	55	17	0	0	1	1.04	0	0	1	1.06	0.94;
	2	2	3	88	0	0	1	1.01	-1.18	0	1	1.06	0.94;
	3	2	41	21	0	0	1	0.985	-5.97	0	1	1.06	0.94;
	4	1	0	0	0	0	1	0.981	-7.32	0	1	1.06	0.94;
	5	1	13	4	0	0	1	0.976	-8.52	0	1	1.06	0.94;
	6	2	75	2	0	0	1	0.98	-8.65	0	1	1.06	0.94;
	7	1	0	0	0	0	1	0.984	-7.58	0	1	1.06	0.94;
	8	2	150	22	0	0	1	1.005	-4.45	0	1	1.06	0.94;
	9	2	121	26	0	0	1	0.98	-9.56	0	1	1.06	0.94;
	10	1	5	2	0	0	1	0.986	-11.43	0	1	1.06	0.94;
	11	1	0	0	0	0	1	0.974	-10.17	0	1	1.06	0.94;
	12	2	377	24	0	0	1	1.015	-10.46	0	1	1.06	0.94;
	13	1	18	2.3	0	0	1	0.979	-9.79	0	1	1.06	0.94;
	14	1	10.5	5.3	0	0	1	0.97	-9.33	0	1	1.06	0.94;
	15	1	22	5	0	0	1	0.988	-7.18	0	1	1.06	0.94;
	16	1	43	3	0	0	1	1.013	-8.85	0	1	1.06	0.94;
	17	1	42	8	0	0	1	1.017	-5.39	0	1	1.06	0.94;
	18	1	27.2	9.8	0	10	1	1.001	-11.71	0	1	1.06	0.94;
	19	1	3.3	0.6	0	0	1	0.97	-13.2	0	1	1.06	0.94;
	20	1	2.3	1	0	0	1	0.964	-13.41	0	1	1.06	0.94;
	21	1	0	0	0	0	1	1.008	-12.89	0	1	1.06	0.94;
	22	1	0	0	0	0	1	1.01	-12.84	0	1	1.06	0.94;
	23	1	6.3	2.1	0	0	1	1.008	-12.91	0	1	1.06	0.94;
	24	1	0	0	0	0	1	0.999	-13.25	0	1	1.06	0.94;
	25	1	6.3	3.2	0	5.9	1	0.982	-18.13	0	1	1.06	0.94;
	26	1	0	0	0	0	1	0.959	-12.95	0	1	1.06	0.94;
	27	1	9.3	0.5	0	0	1	0.982	-11.48	0	1	1.06	0.94;
	28	1	4.6	2.3	0	0	1	0.997	-10.45	0	1	1.06	0.94;
	29	1	17	2.6	0	0	1	1.01	-9.75	0	1	1.06	0.94;
	30	1	3.6	1.8	0	0	1	0.962	-18.68	0	1	1.06	0.94;
	31	1	5.8	2.9	0	0	1	0.936	-19.34	0	1	1.06	0.94;
	32	1	1.6	0.8	0	0	1	0.949	-18.46	0	1	1.06	0.94;
	33	1	3.8	1.9	0	0	1	0.947	-18.5	0	1	1.06	0.94;
	34	1	0	0	0	0	1	0.959	-14.1	0	1	1.06	0.94;
	35	1	6	3	0	0	1	0.966	-13.86	0	1	1.06	0.94;
	36	1	0	0	0	0	1	0.976	-13.59	0	1	1.06	0.94;
	37	1	0	0	0	0	1	0.985	-13.41	0	1	1.06	0.94;
	38	1	14	7	0	0	1	1.013	-12.71	0	1	1.06	0.94;
	39	1	0	0	0	0	1	0.983	-13.46	0	1	1.06	0.94;
	40	1	0	0	0	0	1	0.973	-13.62	0	1	1.06	0.94;
	41	1	6.3	3	0	0	1	0.996	-14.05	0	1	1.06	0.94;
	42	1	7.1	4.4	0	0	1	0.966	-15.5	0	1	1.06	0.94;
	43	1	2	1	0	0	1	1.01	-11.33	0	1	1.06	0.94;
	44	1	12	1.8	0	0	1	1.017	-11.86	0	1	1.06	0.94;
	45	1	0	0	0	0	1	1.036	-9.25	0	1	1.06	0.94;
	46	1	0	0	0	0	1	1.05	-11.89	0	1	1.06	0.94;
	47	1	29.7	11.6	0	0	1	1.033	-12.49	0	1	1.06	0.94;
	48	1	0	0	0	0	1	1.027	-12.59	0	1	1.06	0.94;
	49	1	18	8.5	0	0	1	1.036	-12.92	0	1	1.06	0.94;
	50	1	21	10.5	0	0	1	1.023	-13.39	0	1	1.06	0.94;
	51	1	18	5.3	0	0	1	1.052	-12.52	0	1	1.06	0.94;
	52	1	4.9	2.2	0	0	1	0.98	-11.47	0	1	1.06	0.94;
	53	1	20	10	0	6.3	1	0.971	-12.23	0	1	1.06	0.94;
	54	1	4.1	1.4	0	0	1	0.996	-11.69	0	1	1.06	0.94;
	55	1	6.8	3.4	0	0	1	1.031	-10.78	0	1	1.06	0.94;
	56	1	7.6	2.2	0	0	1	0.968	-16.04	0	1	1.06	0.94;
	57	1	6.7	2	0	0	1	0.965	-16.56	0	1	1.06	0.94;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin
gen = [
	1	128.9	-16.1	200	-140	1.04	100	1	575.88	0;
	2	0	-0.8	50	-17	1.01	100	1	100	0;
	3	40	-1	60	-10	0.985	100	1	140	0;
	6	0	0.8	25	-8	0.98	100	1	100	0;
	8	450	62.1	200	-140	1.005	100	1	550	0;
	9	0	2.2	9	-3	0.98	100	1	100	0;
	12	310	128.5	155	-150	1.015	100	1	410	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status
branch = [
	1	2	0.0083	0.028	0.129	9900	0	0	0	0	1;
	2	3	0.0298	0.085	0.0818	9900	0	0	0	0	1;
	3	4	0.0112	0.0366	0.038	9900	0	0	0	0	1;
	4	5	0.0625	0.132	0.0258	9900	0	0	0	0	1;
	4	6	0.043	0.148	0.0348	9900	0	0	0	0	1;
	6	7	0.02	0.102	0.0276	9900	0	0	0	0	1;
	6	8	0.0339	0.173	0.047	9900	0	0	0	0	1;
	8	9	0.0099	0.0505	0.0548	9900	0	0	0	0	1;
	9	10	0.0369	0.1679	0.044	9900	0	0	0	0	1;
	9	11	0.0258	0.0848	0.0218	9900	0	0	0	0	1;
	9	12	0.0648	0.295	0.0772	9900	0	0	0	0	1;
	9	13	0.0481	0.158	0.0406	9900	0	0	0	0	1;
	13	14	0.0132	0.0434	0.011	9900	0	0	0	0	1;
	13	15	0.0269	0.0869	0.023	9900	0	0	0	0	1;
	1	15	0.0178	0.091	0.0988	9900	0	0	0	0	1;
	1	16	0.0454	0.206	0.0546	9900	0	0	0	0	1;
	1	17	0.0238	0.108	0.0286	9900	0	0	0	0	1;
	3	15	0.0162	0.053	0.0544	9900	0	0	0	0	1;
	4	18	0	0.555	0	9900	0	0	0.97	0	1;
	4	18	0	0.43	0	9900	0	0	0.978	0	1;
	5	6	0.0302	0.0641	0.0124	9900	0	0	0	0	1;
	7	8	0.0139	0.0712	0.0194	9900	0	0	0	0	1;
	10	12	0.0277	0.1262	0.0328	9900	0	0	0	0	1;
	11	13	0.0223	0.0732	0.0188	9900	0	0	0	0	1;
	12	13	0.0178	0.058	0.0604	9900	0	0	0	0	1;
	12	16	0.018	0.0813	0.0216	9900	0	0	0	0	1;
	12	17	0.0397	0.179	0.0476	9900	0	0	0	0	1;
	14	15	0.0171	0.0547	0.0148	9900	0	0	0	0	1;
	18	19	0.461	0.685	0	9900	0	0	0	0	1;
	19	20	0.283	0.434	0	9900	0	0	0	0	1;
	21	20	0	0.7767	0	9900	0	0	1.043	0	1;
	21	22	0.0736	0.117	0	9900	0	0	0	0	1;
	22	23	0.0099	0.0152	0	9900	0	0	0	0	1;
	23	24	0.166	0.256	0.0084	9900	0	0	0	0	1;
	24	25	0	1.182	0	9900	0	0	1	0	1;
	24	25	0	1.23	0	9900	0	0	1	0	1;
	24	26	0	0.0473	0	9900	0	0	1.043	0	1;
	26	27	0.165	0.254	0	9900	0	0	0	0	1;
	27	28	0.0618	0.0954	0	9900	0	0	0	0	1;
	28	29	0.0418	0.0587	0	9900	0	0	0	0	1;
	7	29	0	0.0648	0	9900	0	0	0.967	0	1;
	25	30	0.135	0.202	0	9900	0	0	0	0	1;
	30	31	0.326	0.497	0	9900	0	0	0	0	1;
	31	32	0.507	0.755	0	9900	0	0	0	0	1;
	32	33	0.0392	0.036	0	9900	0	0	0	0	1;
	34	32	0	0.953	0	9900	0	0	0.975	0	1;
	34	35	0.052	0.078	0.0032	9900	0	0	0	0	1;
	35	36	0.043	0.0537	0.0016	9900	0	0	0	0	1;
	36	37	0.029	0.0366	0	9900	0	0	0	0	1;
	37	38	0.0651	0.1009	0.002	9900	0	0	0	0	1;
	37	39	0.0239	0.0379	0	9900	0	0	0	0	1;
	36	40	0.03	0.0466	0	9900	0	0	0	0	1;
	22	38	0.0192	0.0295	0	9900	0	0	0	0	1;
	11	41	0	0.749	0	9900	0	0	0.955	0	1;
	41	42	0.207	0.352	0	9900	0	0	0	0	1;
	41	43	0	0.412	0	9900	0	0	0	0	1;
	38	44	0.0289	0.0585	0.002	9900	0	0	0	0	1;
	15	45	0	0.1042	0	9900	0	0	0.955	0	1;
	14	46	0	0.0735	0	9900	0	0	0.9	0	1;
	46	47	0.023	0.068	0.0032	9900	0	0	0	0	1;
	47	48	0.0182	0.0233	0	9900	0	0	0	0	1;
	48	49	0.0834	0.129	0.0048	9900	0	0	0	0	1;
	49	50	0.0801	0.128	0	9900	0	0	0	0	1;
	50	51	0.1386	0.22	0	9900	0	0	0	0	1;
	10	51	0	0.0712	0	9900	0	0	0.93	0	1;
	13	49	0	0.191	0	9900	0	0	0.895	0	1;
	29	52	0.1442	0.187	0	9900	0	0	0	0	1;
	52	53	0.0762	0.0984	0	9900	0	0	0	0	1;
	53	54	0.1878	0.232	0	9900	0	0	0	0	1;
	54	55	0.1732	0.2265	0	9900	0	0	0	0	1;
	11	43	0	0.153	0	9900	0	0	0.958	0	1;
	44	45	0.0624	0.1242	0.004	9900	0	0	0	0	1;
	40	56	0	1.195	0	9900	0	0	0.958	0	1;
	56	41	0.553	0.549	0	9900	0	0	0	0	1;
	56	42	0.2125	0.354	0	9900	0	0	0	0	1;
	39	57	0	1.355	0	9900	0	0	0.98	0	1;
	57	56	0.174	0.26	0	9900	0	0	0	0	1;
	38	49	0.115	0.177	0.003	9900	0	0	0	0	1;
	38	48	0.0312	0.0482	0	9900	0	0	0	0	1;
	9	55	0	0.1205	0	9900	0	0	0.94	0	1;
];

%%-----  OPF Data  -----%%
%% area data
areas = [
	1	1;
];

%% generator cost data
%	1	startup	shutdown	n	x0	y0	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
gencost = [
	2	0	0	3	0.0775795	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.25	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.0222222	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.0322581	20	0;
];

return;

% Warnings from cdf2matp conversion:
%
% ***** Qmax = Qmin at generator at bus    1 (Qmax set to Qmin + 10)
% ***** area data conversion not yet implemented (creating dummy area data)
% ***** Insufficient generation, setting Pmax at slack bus (bus 1) to 575.88
% ***** MVA limit of branch 1 - 2 not given, set to 9900
% ***** MVA limit of branch 2 - 3 not given, set to 9900
% ***** MVA limit of branch 3 - 4 not given, set to 9900
% ***** MVA limit of branch 4 - 5 not given, set to 9900
% ***** MVA limit of branch 4 - 6 not given, set to 9900
% ***** MVA limit of branch 6 - 7 not given, set to 9900
% ***** MVA limit of branch 6 - 8 not given, set to 9900
% ***** MVA limit of branch 8 - 9 not given, set to 9900
% ***** MVA limit of branch 9 - 10 not given, set to 9900
% ***** MVA limit of branch 9 - 11 not given, set to 9900
% ***** MVA limit of branch 9 - 12 not given, set to 9900
% ***** MVA limit of branch 9 - 13 not given, set to 9900
% ***** MVA limit of branch 13 - 14 not given, set to 9900
% ***** MVA limit of branch 13 - 15 not given, set to 9900
% ***** MVA limit of branch 1 - 15 not given, set to 9900
% ***** MVA limit of branch 1 - 16 not given, set to 9900
% ***** MVA limit of branch 1 - 17 not given, set to 9900
% ***** MVA limit of branch 3 - 15 not given, set to 9900
% ***** MVA limit of branch 4 - 18 not given, set to 9900
% ***** MVA limit of branch 4 - 18 not given, set to 9900
% ***** MVA limit of branch 5 - 6 not given, set to 9900
% ***** MVA limit of branch 7 - 8 not given, set to 9900
% ***** MVA limit of branch 10 - 12 not given, set to 9900
% ***** MVA limit of branch 11 - 13 not given, set to 9900
% ***** MVA limit of branch 12 - 13 not given, set to 9900
% ***** MVA limit of branch 12 - 16 not given, set to 9900
% ***** MVA limit of branch 12 - 17 not given, set to 9900
% ***** MVA limit of branch 14 - 15 not given, set to 9900
% ***** MVA limit of branch 18 - 19 not given, set to 9900
% ***** MVA limit of branch 19 - 20 not given, set to 9900
% ***** MVA limit of branch 21 - 20 not given, set to 9900
% ***** MVA limit of branch 21 - 22 not given, set to 9900
% ***** MVA limit of branch 22 - 23 not given, set to 9900
% ***** MVA limit of branch 23 - 24 not given, set to 9900
% ***** MVA limit of branch 24 - 25 not given, set to 9900
% ***** MVA limit of branch 24 - 25 not given, set to 9900
% ***** MVA limit of branch 24 - 26 not given, set to 9900
% ***** MVA limit of branch 26 - 27 not given, set to 9900
% ***** MVA limit of branch 27 - 28 not given, set to 9900
% ***** MVA limit of branch 28 - 29 not given, set to 9900
% ***** MVA limit of branch 7 - 29 not given, set to 9900
% ***** MVA limit of branch 25 - 30 not given, set to 9900
% ***** MVA limit of branch 30 - 31 not given, set to 9900
% ***** MVA limit of branch 31 - 32 not given, set to 9900
% ***** MVA limit of branch 32 - 33 not given, set to 9900
% ***** MVA limit of branch 34 - 32 not given, set to 9900
% ***** MVA limit of branch 34 - 35 not given, set to 9900
% ***** MVA limit of branch 35 - 36 not given, set to 9900
% ***** MVA limit of branch 36 - 37 not given, set to 9900
% ***** MVA limit of branch 37 - 38 not given, set to 9900
% ***** MVA limit of branch 37 - 39 not given, set to 9900
% ***** MVA limit of branch 36 - 40 not given, set to 9900
% ***** MVA limit of branch 22 - 38 not given, set to 9900
% ***** MVA limit of branch 11 - 41 not given, set to 9900
% ***** MVA limit of branch 41 - 42 not given, set to 9900
% ***** MVA limit of branch 41 - 43 not given, set to 9900
% ***** MVA limit of branch 38 - 44 not given, set to 9900
% ***** MVA limit of branch 15 - 45 not given, set to 9900
% ***** MVA limit of branch 14 - 46 not given, set to 9900
% ***** MVA limit of branch 46 - 47 not given, set to 9900
% ***** MVA limit of branch 47 - 48 not given, set to 9900
% ***** MVA limit of branch 48 - 49 not given, set to 9900
% ***** MVA limit of branch 49 - 50 not given, set to 9900
% ***** MVA limit of branch 50 - 51 not given, set to 9900
% ***** MVA limit of branch 10 - 51 not given, set to 9900
% ***** MVA limit of branch 13 - 49 not given, set to 9900
% ***** MVA limit of branch 29 - 52 not given, set to 9900
% ***** MVA limit of branch 52 - 53 not given, set to 9900
% ***** MVA limit of branch 53 - 54 not given, set to 9900
% ***** MVA limit of branch 54 - 55 not given, set to 9900
% ***** MVA limit of branch 11 - 43 not given, set to 9900
% ***** MVA limit of branch 44 - 45 not given, set to 9900
% ***** MVA limit of branch 40 - 56 not given, set to 9900
% ***** MVA limit of branch 56 - 41 not given, set to 9900
% ***** MVA limit of branch 56 - 42 not given, set to 9900
% ***** MVA limit of branch 39 - 57 not given, set to 9900
% ***** MVA limit of branch 57 - 56 not given, set to 9900
% ***** MVA limit of branch 38 - 49 not given, set to 9900
% ***** MVA limit of branch 38 - 48 not given, set to 9900
% ***** MVA limit of branch 9 - 55 not given, set to 9900