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 [norm] = electroderealign(cfg);

% ELECTRODEREALIGN rotates and translates electrode positions to
% template electrode positions or towards the head surface. It can
% either perform a rigid body transformation, in which only the
% coordinate system is changed, or it can apply additional deformations
% to the input electrodes.
%
% Use as
%   [elec] = electroderealign(cfg)
%
% Three different methods for aligning the input electrodes are implemented:
% based on a warping method, based on the fiducials or interactive with a
% graphical user interface. Each of these approaches is described below.
%
% 1) You can apply a spatial deformation method (i.e. 'warp') that
% automatically minimizes the distance between the electrodes and the
% averaged standard. The warping methods use a non-linear search to
% optimize the error between input and template electrodes or the
% head surface.
%
% 2) You can apply a rigid body realignment based on three fiducial locations.
% Realigning using the fiducials only ensures that the fiducials (typically
% nose, left and right ear) are along the same axes in the input electrode
% set as in the template electrode set.
%
% 3) You can display the electrode positions together with the skin surface,
% and manually (using the graphical user interface) adjust the rotation,
% translation and scaling parameters, so that the two match.
%
% The configuration can contain the following options
%   cfg.method         = different methods for aligning the electrodes
%                        'rigidbody'       apply a rigid-body warp
%                        'globalrescale'   apply a rigid-body warp with global rescaling
%                        'traditional'     apply a rigid-body warp with individual axes rescaling
%                        'nonlin1'         apply a 1st order non-linear warp
%                        'nonlin2'         apply a 2nd order non-linear warp
%                        'nonlin3'         apply a 3rd order non-linear warp
%                        'nonlin4'         apply a 4th order non-linear warp
%                        'nonlin5'         apply a 5th order non-linear warp
%                        'realignfiducial' realign the fiducials
%                        'interactive'     manually using graphical user interface
%   cfg.channel        = Nx1 cell-array with selection of channels (default = 'all'),
%                        see CHANNELSELECTION for details
%   cfg.fiducial       = cell-array with the name of three fiducials used for
%                        realigning (default = {'nasion', 'lpa', 'rpa'})
%   cfg.casesensitive  = 'yes' or 'no', determines whether string comparisons
%                        between electrode labels are case sensitive (default = 'yes')
%   cfg.feedback       = 'yes' or 'no' (default = 'no')
%
% The electrode set that will be realigned is specified as
%   cfg.elecfile       = string with filename, or alternatively
%   cfg.elec           = structure with electrode definition
%
% If you want to align the electrodes to a single template electrode set
% or to multiple electrode sets (which will be averaged), you should
% specify the template electrode sets as
%   cfg.template       = single electrode set that serves as standard
% or
%   cfg.template{1..N} = list of electrode sets that are averaged into the standard
% The template electrode sets can be specified either as electrode
% structures (i.e. when they are already read in memory) or as electrode
% files.
%
% If you want to align the electrodes to the head surface as obtained from
% an anatomical MRI (using one of the warping methods), you should specify
% the head surface
%   cfg.headshape      = a filename containing headshape, a structure containing a
%                        single triangulated boundary, or a Nx3 matrix with surface
%                        points
%
% In case you only want to realign the fiducials, the template electrode
% set only has to contain the three fiducials, e.g.
%   cfg.template.pnt(1,:) = [110 0 0]  % location of the nose
%   cfg.template.pnt(2,:) = [0  90 0]  % left ear
%   cfg.template.pnt(3,:) = [0 -90 0]  % right ear
%   cfg.template.label    = {''nasion', 'lpa', 'rpa'}
%
% See also READ_FCDC_ELEC, VOLUMEREALIGN

% Copyright (C) 2005-2006, Robert Oostenveld
%
% $Log: electroderealign.m,v $
% Revision 1.1  2009/01/30 04:02:02  arno
% *** empty log message ***
%
% Revision 1.6  2007/08/06 09:20:14  roboos
% added support for bti_hs
%
% Revision 1.5  2007/07/26 08:00:09  roboos
% also deal with cfg.headshape if specified as surface, set of points or ctf_hs file.
% the construction of the tri is now done consistently for all headshapes if tri is missing
%
% Revision 1.4  2007/02/13 15:12:51  roboos
% removed cfg.plot3d option
%
% Revision 1.3  2006/12/12 11:28:33  roboos
% moved projecttri subfunction into seperate function
%
% Revision 1.2  2006/10/04 07:10:07  roboos
% updated documentation
%
% Revision 1.1  2006/09/13 07:20:06  roboos
% renamed electrodenormalize to electroderealign, added "deprecated"-warning to the old function
%
% Revision 1.10  2006/09/13 07:09:24  roboos
% Implemented support for cfg.method=interactive, using GUI for specifying and showing transformations. Sofar only for electrodes+headsurface.
%
% Revision 1.9  2006/09/12 15:26:06  roboos
% implemented support for aligning electrodes to the skin surface, extended and improved documentation
%
% Revision 1.8  2006/04/20 09:58:34  roboos
% updated documentation
%
% Revision 1.7  2006/04/19 15:42:53  roboos
% replaced call to warp_pnt with new function name warp_optim
%
% Revision 1.6  2006/03/14 08:16:00  roboos
% changed function call to warp3d into warp_apply (thanks to Arno)
%
% Revision 1.5  2005/05/17 17:50:37  roboos
% changed all "if" occurences of & and | into && and ||
% this makes the code more compatible with Octave and also seems to be in closer correspondence with Matlab documentation on shortcircuited evaluation of sequential boolean constructs
%
% Revision 1.4  2005/03/21 15:49:43  roboos
% added cfg.casesensitive for string comparison of electrode labels
% added cfg.feedback and cfg.plot3d option for debugging
% changed output: now ALL electrodes of the input are rerurned, after applying the specified transformation
% fixed small bug in feedback regarding distarnce prior/after realignfiducials)
% added support for various warping strategies, a.o. traditional, rigidbody, nonlin1-5, etc.
%
% Revision 1.3  2005/03/16 09:18:56  roboos
% fixed bug in fprintf feedback, instead of giving mean squared distance it should give mean distance before and after normalization
%
% Revision 1.2  2005/01/18 12:04:39  roboos
% improved error handling of missing fiducials
% added other default fiducials
% changed debugging output
%
% Revision 1.1  2005/01/17 14:56:06  roboos
% new implementation
%

% set the defaults
if ~isfield(cfg, 'channel'),       cfg.channel = 'all';       end
if ~isfield(cfg, 'feedback'),      cfg.feedback = 'no';       end
if ~isfield(cfg, 'casesensitive'), cfg.casesensitive = 'yes'; end
if ~isfield(cfg, 'headshape'),     cfg.headshape = [];        end
if ~isfield(cfg, 'template'),      cfg.template = [];         end

% this is a common mistake which can be accepted
if strcmp(cfg.method, 'realignfiducials')
  cfg.method = 'realignfiducial';
end

if strcmp(cfg.method, 'warp')
  % rename the default warp to one of the method recognized by the warping toolbox
  cfg.method = 'traditional';
end

if strcmp(cfg.feedback, 'yes')
  % use the global fb field to tell the warping toolbox to print feedback
  global fb
  fb = 1;
else
  global fb
  fb = 0;
end

usetemplate  = isfield(cfg, 'template')  && ~isempty(cfg.template);
useheadshape = isfield(cfg, 'headshape') && ~isempty(cfg.headshape);

if usetemplate
  % get the template electrode definitions
  if ~iscell(cfg.template)
    cfg.template = {cfg.template};
  end
  Ntemplate = length(cfg.template);
  for i=1:Ntemplate
    if isstruct(cfg.template{i})
      template(i) = cfg.template{i};
    else
      template(i) = read_fcdc_elec(cfg.template{i});
    end
  end
elseif useheadshape
  % get the surface describing the head shape
  if isstruct(cfg.headshape) && isfield(cfg.headshape, 'pnt')
    % use the headshape surface specified in the configuration
    headshape = cfg.headshape;
  elseif isnumeric(cfg.headshape) && size(cfg.headshape,2)==3
    % use the headshape points specified in the configuration
    headshape.pnt = cfg.headshape;
  elseif ischar(cfg.headshape) && filetype(cfg.headshape, 'ctf_shape')
    % read the headshape from file
    headshape = read_ctf_shape(cfg.headshape);
  elseif ischar(cfg.headshape) && filetype(cfg.headshape, '4d_hs')
    % read the headshape from file
    headshape     = [];
    headshape.pnt = read_bti_hs(cfg.headshape);
  else
    error('cfg.headshape is not specified correctly')
  end
  if ~isfield(headshape, 'tri')
    % generate a closed triangulation from the surface points
    headshape.tri = projecttri(headshape.pnt);
  end
else
  error('you should either specify template electrode positions, template fiducials or a head shape');
end

% get the electrode definition that should be warped
if isfield(cfg, 'elec')
  elec = cfg.elec;
else
  elec = read_fcdc_elec(cfg.elecfile);
end

% remember the original electrode locations and labels
orig = elec;

% convert all labels to lower case for string comparisons
% this has to be done AFTER keeping the original labels and positions
if strcmp(cfg.casesensitive, 'no')
  for i=1:length(elec.label)
    elec.label{i} = lower(elec.label{i});
  end
  for j=1:length(template)
    for i=1:length(template(j).label)
      template(j).label{i} = lower(template(j).label{i});
    end
  end
end

if strcmp(cfg.feedback, 'yes')
  % create an empty figure, continued below...
  figure
  axis equal
  axis vis3d
  hold on
  xlabel('x')
  ylabel('y')
  zlabel('z')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if usetemplate && any(strcmp(cfg.method, {'rigidbody', 'globalrescale', 'traditional', 'nonlin1', 'nonlin2', 'nonlin3', 'nonlin4', 'nonlin5'}))
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % determine electrode selection and overlapping subset for warping
  cfg.channel = channelselection(cfg.channel, elec.label);
  for i=1:Ntemplate
    cfg.channel = channelselection(cfg.channel, template(i).label);
  end

  % make subselection of electrodes
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label = elec.label(datsel);
  elec.pnt   = elec.pnt(datsel,:);
  for i=1:Ntemplate
    [cfgsel, datsel] = match_str(cfg.channel, template(i).label);
    template(i).label = template(i).label(datsel);
    template(i).pnt   = template(i).pnt(datsel,:);
  end

  % compute the average of the template electrode positions
  all = [];
  for i=1:Ntemplate
    all = cat(3, all, template(i).pnt);
  end
  avg    = mean(all,3);
  stderr = std(all, [], 3);

  fprintf('warping electrodes to template... '); % the newline comes later
  [norm.pnt, norm.m] = warp_optim(elec.pnt, avg, cfg.method);
  norm.label = elec.label;

  dpre  = mean(sqrt(sum((avg - elec.pnt).^2, 2)));
  dpost = mean(sqrt(sum((avg - norm.pnt).^2, 2)));
  fprintf('mean distance prior to warping %f, after warping %f\n', dpre, dpost);

  if strcmp(cfg.feedback, 'yes')
    % plot all electrodes before warping
    my_plot3(elec.pnt, 'r.');
    my_plot3(elec.pnt(1,:), 'r*');
    my_plot3(elec.pnt(2,:), 'r*');
    my_plot3(elec.pnt(3,:), 'r*');
    my_text3(elec.pnt(1,:), elec.label{1}, 'color', 'r');
    my_text3(elec.pnt(2,:), elec.label{2}, 'color', 'r');
    my_text3(elec.pnt(3,:), elec.label{3}, 'color', 'r');

    % plot all electrodes after warping
    my_plot3(norm.pnt, 'm.');
    my_plot3(norm.pnt(1,:), 'm*');
    my_plot3(norm.pnt(2,:), 'm*');
    my_plot3(norm.pnt(3,:), 'm*');
    my_text3(norm.pnt(1,:), norm.label{1}, 'color', 'm');
    my_text3(norm.pnt(2,:), norm.label{2}, 'color', 'm');
    my_text3(norm.pnt(3,:), norm.label{3}, 'color', 'm');

    % plot the template electrode locations
    my_plot3(avg,      'b.');
    my_plot3(avg(1,:), 'b*');
    my_plot3(avg(2,:), 'b*');
    my_plot3(avg(3,:), 'b*');
    my_text3(avg(1,:), norm.label{1}, 'color', 'b');
    my_text3(avg(2,:), norm.label{2}, 'color', 'b');
    my_text3(avg(3,:), norm.label{3}, 'color', 'b');

    % plot lines connecting the input/warped electrode locations with the template locations
    my_line3(elec.pnt, avg, 'color', 'r');
    my_line3(norm.pnt, avg, 'color', 'm');
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif useheadshape && any(strcmp(cfg.method, {'rigidbody', 'globalrescale', 'traditional', 'nonlin1', 'nonlin2', 'nonlin3', 'nonlin4', 'nonlin5'}))
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % determine electrode selection and overlapping subset for warping
  cfg.channel = channelselection(cfg.channel, elec.label);

  % make subselection of electrodes
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label = elec.label(datsel);
  elec.pnt   = elec.pnt(datsel,:);

  fprintf('warping electrodes to head shape... '); % the newline comes later
  [norm.pnt, norm.m] = warp_optim(elec.pnt, headshape, cfg.method);
  norm.label = elec.label;

  dpre  = warp_error([],     elec.pnt, headshape, cfg.method);
  dpost = warp_error(norm.m, elec.pnt, headshape, cfg.method);
  fprintf('mean distance prior to warping %f, after warping %f\n', dpre, dpost);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'realignfiducial')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % try to determine the fiducials automatically if not specified
  option1 = {'nasion' 'left' 'right'};
  option2 = {'nasion' 'lpa' 'rpa'};
  option3 = {'nz' 'lpa' 'rpa'};
  if ~isfield(cfg, 'fiducial')
    if length(match_str(elec.label, option1))==3
      cfg.fiducial = option1;
    elseif length(match_str(elec.label, option2))==3
      cfg.fiducial = option2;
    elseif length(match_str(elec.label, option3))==3
      cfg.fiducial = option3;
    else
      error('could not determine three fiducials, please specify cfg.fiducial')
    end
  end
  fprintf('using fiducials {''%s'', ''%s'', ''%s''}\n', cfg.fiducial{1}, cfg.fiducial{2}, cfg.fiducial{3});

  % determine electrode selection
  cfg.channel = channelselection(cfg.channel, elec.label);
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label = elec.label(datsel);
  elec.pnt   = elec.pnt(datsel,:);

  if length(cfg.fiducial)~=3
    error('you must specify three fiducials');
  end

  % do case-insensitive search for fiducial locations
  nas_indx = match_str(lower(elec.label), lower(cfg.fiducial{1}));
  lpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{2}));
  rpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{3}));
  if length(nas_indx)~=1 || length(lpa_indx)~=1 || length(rpa_indx)~=1
    error('not all fiducials were found in the electrode set');
  end
  elec_nas = elec.pnt(nas_indx,:);
  elec_lpa = elec.pnt(lpa_indx,:);
  elec_rpa = elec.pnt(rpa_indx,:);

  % find the matching fiducials in the template and average them
  templ_nas = [];
  templ_lpa = [];
  templ_rpa = [];
  for i=1:Ntemplate
    nas_indx = match_str(lower(template(i).label), lower(cfg.fiducial{1}));
    lpa_indx = match_str(lower(template(i).label), lower(cfg.fiducial{2}));
    rpa_indx = match_str(lower(template(i).label), lower(cfg.fiducial{3}));
    if length(nas_indx)~=1 || length(lpa_indx)~=1 || length(rpa_indx)~=1
      error(sprintf('not all fiducials were found in template %d', i));
    end
    templ_nas(end+1,:) = template(i).pnt(nas_indx,:);
    templ_lpa(end+1,:) = template(i).pnt(lpa_indx,:);
    templ_rpa(end+1,:) = template(i).pnt(rpa_indx,:);
  end
  templ_nas = mean(templ_nas,1);
  templ_lpa = mean(templ_lpa,1);
  templ_rpa = mean(templ_rpa,1);

  % realign both to a common coordinate system
  elec2common  = headcoordinates(elec_nas, elec_lpa, elec_rpa);
  templ2common = headcoordinates(templ_nas, templ_lpa, templ_rpa);

  % compute the combined transform and realign the electrodes to the template
  norm       = [];
  norm.m     = elec2common * inv(templ2common);
  norm.pnt   = warp_apply(norm.m, elec.pnt, 'homogeneous');
  norm.label = elec.label;

  nas_indx = match_str(lower(elec.label), lower(cfg.fiducial{1}));
  lpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{2}));
  rpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{3}));
  dpre  = mean(sqrt(sum((elec.pnt([nas_indx lpa_indx rpa_indx],:) - [templ_nas; templ_lpa; templ_rpa]).^2, 2)));
  nas_indx = match_str(lower(norm.label), lower(cfg.fiducial{1}));
  lpa_indx = match_str(lower(norm.label), lower(cfg.fiducial{2}));
  rpa_indx = match_str(lower(norm.label), lower(cfg.fiducial{3}));
  dpost = mean(sqrt(sum((norm.pnt([nas_indx lpa_indx rpa_indx],:) - [templ_nas; templ_lpa; templ_rpa]).^2, 2)));
  fprintf('mean distance between fiducials prior to realignment %f, after realignment %f\n', dpre, dpost);

  if strcmp(cfg.feedback, 'yes')
    % plot the first three electrodes before transformation
    my_plot3(elec.pnt(1,:), 'r*');
    my_plot3(elec.pnt(2,:), 'r*');
    my_plot3(elec.pnt(3,:), 'r*');
    my_text3(elec.pnt(1,:), elec.label{1}, 'color', 'r');
    my_text3(elec.pnt(2,:), elec.label{2}, 'color', 'r');
    my_text3(elec.pnt(3,:), elec.label{3}, 'color', 'r');

    % plot the template fiducials
    my_plot3(templ_nas, 'b*');
    my_plot3(templ_lpa, 'b*');
    my_plot3(templ_rpa, 'b*');
    my_text3(templ_nas, ' nas', 'color', 'b');
    my_text3(templ_lpa, ' lpa', 'color', 'b');
    my_text3(templ_rpa, ' rpa', 'color', 'b');

    % plot all electrodes after transformation
    my_plot3(norm.pnt, 'm.');
    my_plot3(norm.pnt(1,:), 'm*');
    my_plot3(norm.pnt(2,:), 'm*');
    my_plot3(norm.pnt(3,:), 'm*');
    my_text3(norm.pnt(1,:), norm.label{1}, 'color', 'm');
    my_text3(norm.pnt(2,:), norm.label{2}, 'color', 'm');
    my_text3(norm.pnt(3,:), norm.label{3}, 'color', 'm');
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'interactive')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % open a figure
  fig = figure;
  % add the data to the figure
  set(fig, 'CloseRequestFcn', @cb_close);
  setappdata(fig, 'elec', elec);
  setappdata(fig, 'transform', eye(4));
  if useheadshape
    setappdata(fig, 'surf', headshape);
  end
  if usetemplate
    % FIXME interactive realigning to template electrodes is not yet supported
    % this requires a consistent handling of channel selection etc.
    setappdata(fig, 'template', template);
  end
  % add the GUI elements
  cb_creategui(gca);
  cb_redraw(gca);
  rotate3d on
  waitfor(fig);
  % get the data from the figure that was left behind as global variable
  global norm
  tmp = norm;
  clear global norm
  norm = tmp;
  clear tmp

else
  error('unknown method');
end

% apply the spatial transformation to all electrodes, and replace the
% electrode labels by their case-sensitive original values
if any(strcmp(cfg.method, {'rigidbody', 'globalrescale', 'traditional', 'nonlin1', 'nonlin2', 'nonlin3', 'nonlin4', 'nonlin5'}))
  norm.pnt   = warp_apply(norm.m, orig.pnt, cfg.method);
else
  norm.pnt   = warp_apply(norm.m, orig.pnt, 'homogenous');
end
norm.label = orig.label;

% add version information to the configuration
try
  % get the full name of the function
  cfg.version.name = mfilename('fullpath');
catch
  % required for compatibility with Matlab versions prior to release 13 (6.5)
  [st, i] = dbstack;
  cfg.version.name = st(i);
end
cfg.version.id = '$Id: electroderealign.m,v 1.1 2009/01/30 04:02:02 arno Exp $';

% remember the configuration
norm.cfg = cfg;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% some simple SUBFUNCTIONs that facilitate 3D plotting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = my_plot3(xyz, varargin)
h = plot3(xyz(:,1), xyz(:,2), xyz(:,3), varargin{:});
function h = my_text3(xyz, varargin)
h = text(xyz(:,1), xyz(:,2), xyz(:,3), varargin{:});
function my_line3(xyzB, xyzE, varargin)
for i=1:size(xyzB,1)
  line([xyzB(i,1) xyzE(i,1)], [xyzB(i,2) xyzE(i,2)], [xyzB(i,3) xyzE(i,3)], varargin{:})
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION to layout a moderately complex graphical user interface
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = layoutgui(fig, geometry, position, style, string, value, tag, callback);
horipos  = geometry(1); % lower left corner of the GUI part in the figure
vertpos  = geometry(2); % lower left corner of the GUI part in the figure
width    = geometry(3); % width  of the GUI part in the figure
height   = geometry(4); % height of the GUI part in the figure
horidist = 0.05;
vertdist = 0.05;
options  = {'units', 'normalized', 'HorizontalAlignment', 'center'}; %  'VerticalAlignment', 'middle'
Nrow     = size(position,1);
h        = cell(Nrow,1);
for i=1:Nrow
  if isempty(position{i})
    continue;
  end
  position{i} = position{i} ./ sum(position{i});
  Ncol = size(position{i},2);
  ybeg = (Nrow-i  )/Nrow + vertdist/2;
  yend = (Nrow-i+1)/Nrow - vertdist/2;
  for j=1:Ncol
    xbeg    = sum(position{i}(1:(j-1))) + horidist/2;
    xend    = sum(position{i}(1:(j  ))) - horidist/2;
    pos(1) = xbeg*width  + horipos;
    pos(2) = ybeg*height + vertpos;
    pos(3) = (xend-xbeg)*width;
    pos(4) = (yend-ybeg)*height;
    h{i}{j} = uicontrol(fig, ...
      options{:}, ...
      'position', pos, ...
      'style',    style{i}{j}, ...
      'string',   string{i}{j}, ...
      'tag',      tag{i}{j}, ...
      'value',    value{i}{j}, ...
      'callback', callback{i}{j} ...
      );
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cb_creategui(hObject, eventdata, handles);
% define the position of each GUI element
position = {
  [2 1 1 1]
  [2 1 1 1]
  [2 1 1 1]
  [1]
  [1]
  [1]
  [1]
  [1 1]
  };

% define the style of each GUI element
style = {
  {'text' 'edit' 'edit' 'edit'}
  {'text' 'edit' 'edit' 'edit'}
  {'text' 'edit' 'edit' 'edit'}
  {'pushbutton'}
  {'pushbutton'}
  {'toggle'}
  {'toggle'}
  {'text' 'edit'}
  };

% define the descriptive string of each GUI element
string = {
  {'rotate'    0 0 0}
  {'translate' 0 0 0}
  {'scale'     1 1 1}
  {'redisplay'}
  {'apply'}
  {'toggle grid'}
  {'toggle axes'}
  {'alpha' 0.7}
  };

% define the value of each GUI element
value = {
  {[] [] [] []}
  {[] [] [] []}
  {[] [] [] []}
  {[]}
  {[]}
  {0}
  {0}
  {[] []}
  };

% define a tag for each GUI element
tag = {
  {'' 'rx' 'ry' 'rz'}
  {'' 'tx' 'ty' 'tz'}
  {'' 'sx' 'sy' 'sz'}
  {''}
  {''}
  {'toggle grid'}
  {'toggle axes'}
  {'' 'alpha'}
  };

% define the callback function of each GUI element
callback = {
  {[] @cb_redraw @cb_redraw @cb_redraw}
  {[] @cb_redraw @cb_redraw @cb_redraw}
  {[] @cb_redraw @cb_redraw @cb_redraw}
  {@cb_redraw}
  {@cb_apply}
  {@cb_redraw}
  {@cb_redraw}
  {[] @cb_redraw}
  };

fig = get(hObject, 'parent');
layoutgui(fig, [0.7 0.05 0.25 0.50], position, style, string, value, tag, callback);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cb_redraw(hObject, eventdata, handles);
fig = get(hObject, 'parent');
surf = getappdata(fig, 'surf');
elec = getappdata(fig, 'elec');
template = getappdata(fig, 'template');
% get the transformation details
rx = str2num(get(findobj(fig, 'tag', 'rx'), 'string'));
ry = str2num(get(findobj(fig, 'tag', 'ry'), 'string'));
rz = str2num(get(findobj(fig, 'tag', 'rz'), 'string'));
tx = str2num(get(findobj(fig, 'tag', 'tx'), 'string'));
ty = str2num(get(findobj(fig, 'tag', 'ty'), 'string'));
tz = str2num(get(findobj(fig, 'tag', 'tz'), 'string'));
sx = str2num(get(findobj(fig, 'tag', 'sx'), 'string'));
sy = str2num(get(findobj(fig, 'tag', 'sy'), 'string'));
sz = str2num(get(findobj(fig, 'tag', 'sz'), 'string'));
R = rotate   ([rx ry rz]);
T = translate([tx ty tz]);
S = scale    ([sx sy sz]);
H = S * T * R;
elec.pnt = warp_apply(H, elec.pnt);
axis vis3d; cla
xlabel('x')
ylabel('y')
zlabel('z')
if ~isempty(surf)
  triplot(surf.pnt, surf.tri,  [], 'faces_skin');
  alpha(str2num(get(findobj(fig, 'tag', 'alpha'), 'string')));
end
if ~isempty(template)
  triplot(template.pnt, [], [], 'nodes_blue')
end
triplot(elec.pnt, [], [], 'nodes');
if isfield(elec, 'line')
  triplot(elec.pnt, elec.line, [], 'edges');
end
if get(findobj(fig, 'tag', 'toggle axes'), 'value')
  axis on
else
  axis off
end
if get(findobj(fig, 'tag', 'toggle grid'), 'value')
  grid on
else
  grid off
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cb_apply(hObject, eventdata, handles);
fig = get(hObject, 'parent');
elec      = getappdata(fig, 'elec');
transform = getappdata(fig, 'transform');
% get the transformation details
rx = str2num(get(findobj(fig, 'tag', 'rx'), 'string'));
ry = str2num(get(findobj(fig, 'tag', 'ry'), 'string'));
rz = str2num(get(findobj(fig, 'tag', 'rz'), 'string'));
tx = str2num(get(findobj(fig, 'tag', 'tx'), 'string'));
ty = str2num(get(findobj(fig, 'tag', 'ty'), 'string'));
tz = str2num(get(findobj(fig, 'tag', 'tz'), 'string'));
sx = str2num(get(findobj(fig, 'tag', 'sx'), 'string'));
sy = str2num(get(findobj(fig, 'tag', 'sy'), 'string'));
sz = str2num(get(findobj(fig, 'tag', 'sz'), 'string'));
R = rotate   ([rx ry rz]);
T = translate([tx ty tz]);
S = scale    ([sx sy sz]);
H = S * T * R;
elec.pnt = warp_apply(H, elec.pnt);
transform = H * transform;
set(findobj(fig, 'tag', 'rx'), 'string', 0);
set(findobj(fig, 'tag', 'ry'), 'string', 0);
set(findobj(fig, 'tag', 'rz'), 'string', 0);
set(findobj(fig, 'tag', 'tx'), 'string', 0);
set(findobj(fig, 'tag', 'ty'), 'string', 0);
set(findobj(fig, 'tag', 'tz'), 'string', 0);
set(findobj(fig, 'tag', 'sx'), 'string', 1);
set(findobj(fig, 'tag', 'sy'), 'string', 1);
set(findobj(fig, 'tag', 'sz'), 'string', 1);
setappdata(fig, 'elec', elec);
setappdata(fig, 'transform', transform);
cb_redraw(hObject);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cb_close(hObject, eventdata, handles);
% make the current transformation permanent and subsequently allow deleting the figure
cb_apply(gca);
% get the updated electrode from the figure
fig    = hObject;
% hmmm, this is ugly
global norm
norm   = getappdata(fig, 'elec');
norm.m = getappdata(fig, 'transform');
set(fig, 'CloseRequestFcn', @delete);
delete(fig);


function output = calllindo_nlp(interfacedata)

global MY_LICENSE_FILE

persistent iEnv

% Instead of calling lindo, we define the parameters we need. This is
% needed to speed up repeated calls
%lindo
LSERR_NO_ERROR                                               = 0000;
LS_IPARAM_NLP_PRINTLEVEL                                     = 203;
LS_IPARAM_NLP_SOLVER                                         = 201;
LS_IPARAM_NLP_MAXLOCALSEARCH                                 = 221;
LS_STATUS_OPTIMAL                                            = 1;
LS_STATUS_BASIC_OPTIMAL                                      = 2;
LS_STATUS_INFEASIBLE                                         = 3;
LS_STATUS_UNBOUNDED                                          = 4;
LS_STATUS_FEASIBLE                                           = 5;
LS_STATUS_INFORUNB                                           = 6;
LS_STATUS_NEAR_OPTIMAL                                       = 7;
LS_STATUS_LOCAL_OPTIMAL                                      = 8;
LS_STATUS_LOCAL_INFEASIBLE                                   = 9;
LS_STATUS_CUTOFF                                             = 10;
LS_STATUS_NUMERICAL_ERROR                                    = 11;
LS_STATUS_UNKNOWN                                            = 12;
LS_STATUS_UNLOADED                                           = 13;
LS_STATUS_LOADED                                             = 14;
LS_METHOD_FREE                                               = 0;
LS_METHOD_PSIMPLEX                                           = 1;
LS_METHOD_DSIMPLEX                                           = 2;
LS_METHOD_BARRIER                                            = 3;
LS_METHOD_NLP                                                = 4;
LS_NMETHOD_FREE                                              = 4;
LS_NMETHOD_CONOPT                                            = 7;
LS_NMETHOD_MSW_GRG                                           = 9;

if isempty(iEnv)
    % This call is mighty slow, so we do it only once, unless uses clears
    % everything
    [MY_LICENSE_KEY,Err] = mxlindo('LSloadLicenseString',MY_LICENSE_FILE);
    [iEnv,nErr]=mxlindo('LScreateEnv',MY_LICENSE_KEY);
    if nErr ~= LSERR_NO_ERROR;output = returnempty(-5); return; end;
end

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
x0      = interfacedata.x0;
Q       = interfacedata.Q;
lb      = interfacedata.lb;
ub      = interfacedata.ub;
monomtable = interfacedata.monomtable;

% Do some pre-calc to be used in callbacks
nonlinearindicies = find(interfacedata.variabletype~=0);
nonlinearindicies = unionstripped(nonlinearindicies,interfacedata.evalVariables);
linearindicies    = find(interfacedata.variabletype==0);
linearindicies    = setdiff1D(linearindicies,nonlinearindicies);
interfacedata.nonlinearindicies = nonlinearindicies;
interfacedata.linearindicies    = linearindicies;

% % Move nonlinear bounds to constraints
if ~isempty(lb)
    finite = find(~isinf(lb(nonlinearindicies)));
    if ~isempty(finite)
        temp = F_struc(1:K.f,:);
        F_struc(1:K.f,:) = [];
        n = length(c);

        for i = 1:length(finite)
            j = nonlinearindicies(i);
            F_struc = [-lb(j) sparse(1,j,1,1,n);F_struc];
        end
        K.l = K.l + length(finite);
        F_struc = [temp;F_struc];
        interfacedata.K = K;
        interfacedata.F_struc = F_struc;
    end
end
if ~isempty(ub)
    finite = find(~isinf(ub(nonlinearindicies)));
    if ~isempty(finite)
        temp = F_struc(1:K.f,:);
        F_struc(1:K.f,:) = [];
        n = length(c);

        for i = 1:length(finite)
            j = nonlinearindicies(i);
            F_struc = [ub(j) -sparse(1,j,1,1,n);F_struc];
        end
        K.l = K.l + length(finite);
        F_struc = [temp;F_struc];
        interfacedata.K = K;
        interfacedata.F_struc = F_struc;
    end
end
%
% Init model size
m  = K.l + K.f;
n  = length(c);

% Specifying variable types...
vtype = repmat('C',1,length(c(linearindicies)));
vtype(interfacedata.integer_variables) = 'I';

oshift = interfacedata.f;

if m == 0
    interfacedata.F_struc = [1e6 -ones(1,length(c))];
    K.l = 1;
    F_struc = [1e6 -ones(1,length(c))];
    m = 1;
    csense = [repmat('E',1,K.f) repmat('L',1,K.l)];
end

[Nbegcol,Nlencol,Nrowndx,Nobjcnt,Nobjndx,Apatt] = jacSparsity(interfacedata);
A = -F_struc(:,1+linearindicies);
b = full(F_struc(:,1));
csense = [repmat('E',1,K.f) repmat('L',1,K.l)];
A = A.*(~Apatt);
b(any(Apatt,2)) = 0;

%[MY_LICENSE_KEY,nErr] = mxlindo('LSloadLicenseString',MY_LICENSE_FILE);

%[iEnv,nErr]=mxlindo('LScreateEnv',MY_LICENSE_KEY);
%if nErr ~= LSERR_NO_ERROR;output = returnempty(-5); return; end;
[iModel,nErr]=mxlindo('LScreateModel',iEnv);
if nErr ~= LSERR_NO_ERROR;output = returnempty(11); return; end;
constant_data = setup_fmincon_params(interfacedata);
constant_data.F_struc = F_struc;
lindo_fun([],[],[],[],[],[],constant_data);
[nErr] = mxlindo('LSsetFuncalc', iModel, 'lindo_fun',constant_data);
if nErr ~= LSERR_NO_ERROR;output = returnempty(11); return; end;
[nErr] = mxlindo('LSsetModelIntParameter', iModel, LS_IPARAM_NLP_PRINTLEVEL, options.verbose+1);
if nErr ~= LSERR_NO_ERROR;output = returnempty(11); return; end;

% Set NLP solver
[nErr] = mxlindo('LSsetModelIntParameter', iModel, LS_IPARAM_NLP_SOLVER, eval(options.lindo.LS_IPARAM_NLP_SOLVER));
[nErr] = mxlindo('LSsetModelIntParameter', iModel, LS_IPARAM_NLP_MAXLOCALSEARCH,options.lindo.LS_IPARAM_NLP_MAXLOCALSEARCH);

% Load the LP portion of  model
[nErr] = mxlindo('LSXloadLPData', iModel, 1, 0, c(linearindicies), b, csense,sparse(A), lb(linearindicies), ub(linearindicies));
if nErr ~= LSERR_NO_ERROR;output = createoutput(11); return; end;

nErr = mxlindo('LSloadVarType',iModel,vtype);
if nErr ~= LSERR_NO_ERROR;output = createoutput(11); return; end;

% Load the NLP portion of the model
[nErr] = mxlindo('LSloadNLPData', iModel, Nbegcol, Nlencol,[], Nrowndx, Nobjcnt,Nobjndx,[]);
if nErr ~= LSERR_NO_ERROR;output = createoutput(11); return; end;

% Optimize model
solvertime = tic;

if isempty(interfacedata.integer_variables)
    solver = 2;
else
    solver = 1;
end
solvertime = tic;
switch solver
    case 1
        [solstat,nErr] = mxlindo('LSsolveMIP', iModel);
        if ~ismember(solstat,[2009 LS_STATUS_INFEASIBLE])
            [x,nErr] = mxlindo('LSgetMIPPrimalSolution',iModel);
        else
            x = zeros(length(linearindicies),1);
        end
    case 2
        [solstat,nErr] = mxlindo('LSoptimize', iModel, eval(options.lindo.LS_METHOD));
        if ~ismember(solstat,[2009 LS_STATUS_INFEASIBLE])
            [x,nErr] = mxlindo('LSgetPrimalSolution',iModel);
        else
            x = zeros(length(linearindicies),1);
        end
    case 3
        [solStatus,nErr] = mxlindo('LSsolveGOP', iModel);
        [x,nErr] = mxlindo('LSgetPrimalSolution',iModel);
    otherwise
end
solvertime = toc(solvertime);

w = zeros(length(c),1);w(linearindicies) =x;
y = [];

%[nErr]=mxlindo('LSdeleteEnv',iEnv);
[nErr]=mxlindo('LSdeleteModel',iModel);

switch solstat
    case {LS_STATUS_OPTIMAL,LS_STATUS_BASIC_OPTIMAL,7,8}
        problem = 0;
    case {LS_STATUS_INFEASIBLE,LS_STATUS_LOCAL_INFEASIBLE}
        problem = 1;
    case {LS_STATUS_UNBOUNDED}
        problem = 2;
    otherwise
        problem = 11;
end
infostr = yalmiperror(problem,'LINDO-QP');

% Save all data sent to solver?
if options.savesolverinput
    solverinput.solstat = solstat;
    solverinput.nErr = nErr;
    solverinput.x = x;
else
    solverinput = [];
end

% Save all data from the solver?
if options.savesolveroutput
    solveroutput.x = x;
    solveroutput.fmin = fmin;
    solveroutput.flag = flag;
    solveroutput.output=output;
    solveroutput.lambda=lambda;
else
    solveroutput = [];
end

% Standard interface
output = createoutput(w,y,[],problem,'LINDO',solverinput,solveroutput,solvertime);


% Run this in order to construct the results table for the CE-CLM paper
Extract_table_results_68;

file_out = fopen('results/300W_68.txt', 'w');

fprintf(file_out, 'Errors with outline (68 points)\n');
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'Method\tcomm\tdiff\n');
fprintf(file_out, 'CLNF\t%.2f\t%.2f\n', median(clnf_error_comm)*100, median(clnf_error_ibug)*100);
fprintf(file_out, 'CFAN\t--- \t%.2f\n', median(cfan_error_ibug)*100);
fprintf(file_out, 'DRMF\t%.2f\t%.2f\n', median(drmf_error_comm)*100, median(drmf_error_ibug)*100);
fprintf(file_out, 'CFSS\t%.2f\t%.2f\n', median(cfss_error_comm)*100, median(cfss_error_ibug)*100);
fprintf(file_out, 'TCDCN\t%.2f\t%.2f\n', median(tcdcn_error_comm)*100, median(tcdcn_error_ibug)*100);
fprintf(file_out, '3DDFA\t%.2f\t%.2f\n', median(error_3ddfa_comm)*100, median(error_3ddfa_ibug)*100);
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'CE-CLM\t%.2f\t%.2f\n', median(ceclm_error_comm)*100, median(ceclm_error_ibug)*100);
fclose(file_out);

Extract_table_results_49;
file_out = fopen('results/300W_49.txt', 'w');

fprintf(file_out, 'Errors without outline (49 points)\n');
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'Method\tcomm\tdiff\n');
fprintf(file_out, 'CLNF\t%.2f\t%.2f\n', median(clnf_error_comm)*100, median(clnf_error_ibug)*100);
fprintf(file_out, 'SDM \t%.2f\t%.2f\n', median(sdm_error_comm)*100, median(sdm_error_ibug)*100);
fprintf(file_out, 'CFAN\t--- \t%.2f\n', median(cfan_error_ibug)*100);
fprintf(file_out, 'DRMF\t%.2f\t%.2f\n', median(drmf_error_comm)*100, median(drmf_error_ibug)*100);
fprintf(file_out, 'CFSS\t%.2f\t%.2f\n', median(cfss_error_comm)*100, median(cfss_error_ibug)*100);
fprintf(file_out, 'TCDCN\t%.2f\t%.2f\n', median(tcdcn_error_comm)*100, median(tcdcn_error_ibug)*100);
fprintf(file_out, '3DDFA\t%.2f\t%.2f\n', median(error_3ddfa_comm)*100, median(error_3ddfa_ibug)*100);
fprintf(file_out, '------------------------------\n');
fprintf(file_out, 'CE-CLM\t%.2f\t%.2f\n', median(ceclm_error_comm)*100, median(ceclm_error_ibug)*100);

fclose(file_out);


% 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 fx = jdfunc (jdin)

% objective function for tdb2utc

% input

%  jdin = current value for UTC julian date

% output

%  fx = delta julian date

% Orbital Mechanics with MATLAB

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

global jdsaved

tai_utc = findleap(jdin);

fx = utc2tdb (jdin, tai_utc) - jdsaved;

end


function [displacements,rois_dic,seedinfo,outstate] = ncorr_alg_dicanalysis(imgs,radius,spacing,cutoff_diffnorm,cutoff_iteration,total_threads,enabled_stepanalysis,subsettrunc,num_img,total_imgs,pos_parent,params_init)
% This function performs RG-DIC.
%
% Inputs -----------------------------------------------------------------%
%   imgs - struct; Contains struct('imginfo',{},'roi',{}). Contains the
%   reference and current images packaged together. The imginfo field
%   contains an ncorr_class_img and the ROI field contains an ncorr_class_roi.
%   radius - integer; subset radius
%   spacing - integer; subset spacing
%   cutoff_diffnorm - double; cutoff for the norm of difference vector
%   cutoff_iteration - integer; cutoff for the number of IC-GN iterations
%   total_threads - integer; total number of threads
%   enabled_stepanalysis - logical; if true, then process as many seeds as
%   possible. If false, process all the seeds.
%   subsettrunc - logical; if true, then subset truncation is enabled
%   num_img - integer; reference image number
%   total_imgs - integer; total number of images
%   pos_parent - integer array; this is the position of the parent figure
%   which determines where to position this figure
%   params_init - struct; contains struct('paramvector',{},'num_region',{},
%   'num_thread',{},'computepoints',{}). If its not empty, it contains last
%   set of seeds used for the previous iteration
%
% Outputs ----------------------------------------------------------------%
%   displacements - struct; contains
%   struct('plot_u',{},'plot_v',{},'plot_corrcoef',{},'plot_validpoints',{})
%   rois_dic - ncorr_class_img; ROIs after being unioned with reference roi
%   and validpoints from DIC analysis.
%   seedinfo - struct; contains struct('paramvector',{},'num_region',{},
%   'num_thread',{},'computepoints',{})
%   outstate - integer; returns either out.cancelled, out.failed, or
%   out.success.
%
% outstate will only return failed if ncorr_alg_rgdic throws an exception.

    % Initialize outputs
    outstate = out.cancelled;
    displacements = struct('plot_u',{},'plot_v',{},'plot_corrcoef',{},'plot_validpoints',{});
    rois_dic = ncorr_class_roi.empty;
    seedinfo = struct('paramvector',{},'num_region',{},'num_thread',{},'computepoints',{});

    % --------------------------------------------------------------------%
    % Get Seeds ----------------------------------------------------------%
    % --------------------------------------------------------------------%

    if (isempty(params_init))
        % Get new seeds manually through GUI. outstate_seeds will either be
        % success or cancelled.
        [seedinfo_prelim,threaddiagram,outstate_seeds] = ncorr_gui_seedanalysis(imgs(1).imginfo, ...
                                                                                [imgs(2:end).imginfo], ...
                                                                                imgs(1).roi, ...
                                                                                radius, ...
                                                                                spacing, ...
                                                                                cutoff_diffnorm, ...
                                                                                cutoff_iteration, ...
                                                                                total_threads, ...
                                                                                enabled_stepanalysis, ...
                                                                                subsettrunc, ...
                                                                                num_img, ...
                                                                                total_imgs, ...
                                                                                pos_parent);
        % See if analysis was cancelled
        if (outstate_seeds ~= out.success)
            return;
        end
    else
        % Use params init to place seeds directly
        % Initialize seedinfo_prelim
        seedinfo_prelim = struct('paramvector',{},'num_region',{},'num_thread',{},'computepoints',{});

        % Initialize Thread diagram buffers - some buffers are unused
        ref_reduced = imgs(1).imginfo.reduce(spacing); % Unused
        roi_reduced = imgs(1).roi.reduce(spacing); % Unused
        preview_threaddiagram = zeros(size(roi_reduced.mask)); % Unused
        threaddiagram_buffer = -ones(size(roi_reduced.mask));
        threaddiagram = -ones(size(roi_reduced.mask)); % This must be negative 1

        % Calculate seeds - cycle over regions
        num_imgs_success = inf; % Initialize to impossibly high value
        manualseed = false; % Set this parameter true if there is trouble. This causes the user to reselect the seeds manually through a GUI.
        % Cycle over regions
        for i = 0:size(params_init,2)-1
            % Get updated seed positions based on seed locations of the
            % last successfully seeded image for this region
            % Initialize
            pos_seed = zeros(size(params_init,1),2);
            % Cycle over threads
            for j = 0:size(params_init,1)-1
                % Must round displacements so that the updated position
                % lies properly on "spaced grid" - THIS IS IMPORTANT!!!
                pos_seed(j+1,1) = params_init(j+1,i+1,end).paramvector(1) + round(params_init(j+1,i+1,end).paramvector(3)/(spacing+1))*(spacing+1);
                pos_seed(j+1,2) = params_init(j+1,i+1,end).paramvector(2) + round(params_init(j+1,i+1,end).paramvector(4)/(spacing+1))*(spacing+1);
            end

            % Get num_region - just use seed from the first thread
            num_region = params_init(1,i+1,end).num_region;

            % Make sure seed positions are within the ROI and are unique.
            % If they arent then prompt the user and have them replace the
            % seeds manually.
            regionmask = roi_reduced.get_regionmask(num_region);
            if (size(pos_seed,1) == size(unique(pos_seed,'rows'),1) && ...
                all(pos_seed(:,1)./(spacing+1) >= 0) && all(pos_seed(:,1)./(spacing+1) < size(regionmask,2)) && ...
                all(pos_seed(:,2)./(spacing+1) >= 0) && all(pos_seed(:,2)./(spacing+1) < size(regionmask,1)) && ...
                all(regionmask(sub2ind(size(regionmask),pos_seed(:,2)/(spacing+1)+1,pos_seed(:,1)/(spacing+1)+1))))
                % Get seeds - outstate will be successful if at least one
                % image is seeded
                [seedinfo_buffer,convergence_buffer,outstate_seeds] = ncorr_alg_seedanalysis(imgs(1).imginfo, ...
                                                                                [imgs(2:end).imginfo], ...
                                                                                imgs(1).roi, ...
                                                                                num_region, ...
                                                                                pos_seed, ...
                                                                                radius, ...
                                                                                cutoff_diffnorm, ...
                                                                                cutoff_iteration, ...
                                                                                enabled_stepanalysis, ...
                                                                                subsettrunc, ...
                                                                                num_img, ...
                                                                                total_imgs); %#ok<ASGLU>
                % See if analysis was cancelled
                if (outstate_seeds == out.cancelled)
                    return;
                end

                % See if analysis was successful or failed
                if (outstate_seeds == out.success)
                    % Form thread diagram for these seeds
                    ncorr_alg_formthreaddiagram(threaddiagram_buffer,preview_threaddiagram,int32(pos_seed/(spacing+1)),regionmask,ref_reduced.formatted());

                    % Get compute points
                    for j = 0:size(seedinfo_buffer,3)-1
                        for k = 0:size(seedinfo_buffer,1)-1
                            seedinfo_buffer(k+1,1,j+1).computepoints = length(find(threaddiagram_buffer == k));
                        end
                    end
                else
                    % Seed analysis failed. Alert user and then ask him/her
                    % to manually place seeds.
                    h_error = errordlg('Not a single image was seeded correctly; please replace seeds manually.','Error','modal');
                    uiwait(h_error);

                    manualseed = true;
                end
            else
                % Alert user
                h_error = errordlg('One or more seeds went outside of the ROI **OR** converged on top of each other, please replace manually. If this happens often then dont place seeds near each other or near the boundary.','Error','modal');
                uiwait(h_error);
                manualseed = true;
            end

            if (~manualseed)
                % Take minimum of num_imgs_success and
                % seedinfo. Buffer can be more or less than
                % num_imgs_success
                num_imgs_success = min(num_imgs_success, size(seedinfo_buffer,3));

                % Clear out other images in buffer and prelim
                if (~isempty(seedinfo_prelim))
                    seedinfo_prelim = seedinfo_prelim(:,:,1:num_imgs_success);
                end
                seedinfo_buffer = seedinfo_buffer(:,:,1:num_imgs_success);

                % Append seedinfo_buffer - append along 2nd dimension
                seedinfo_prelim = horzcat(seedinfo_prelim,seedinfo_buffer); %#ok<AGROW>

                % Merge threaddiagram from previous iteration
                threaddiagram(threaddiagram_buffer ~= -1) = threaddiagram_buffer(threaddiagram_buffer ~= -1);
            else
                % If there's a problem, just ask user to manually reset
                % seeds. Outstate will either be success or cancelled.
                [seedinfo_prelim,threaddiagram,outstate_seeds] = ncorr_gui_seedanalysis(imgs(1).imginfo, ...
                                                                                [imgs(2:end).imginfo], ...
                                                                                imgs(1).roi, ...
                                                                                radius, ...
                                                                                spacing, ...
                                                                                cutoff_diffnorm, ...
                                                                                cutoff_iteration, ...
                                                                                total_threads, ...
                                                                                enabled_stepanalysis, ...
                                                                                subsettrunc, ...
                                                                                num_img, ...
                                                                                total_imgs, ...
                                                                                pos_parent);

                % Check if analysis was cancelled
                if (outstate_seeds ~= out.success)
                    return;
                end

                % Break since seeds for all regions are placed in GUI.
                break;
            end
        end

        % Debug ----------------------------------------------------------%
        %{
        figure, imshow(threaddiagram,[]); hold on;
        for i = 0:size(seedinfo_prelim,2)-1
            for j = 0:size(seedinfo_prelim,1)-1
                plot(seedinfo_prelim(j+1,i+1,1).paramvector(1)/(spacing+1),seedinfo_prelim(j+1,i+1,1).paramvector(2)/(spacing+1),'ro');
            end
        end
        hold off;
        %}
        % ----------------------------------------------------------------%
    end

    % --------------------------------------------------------------------%
    % Perform DIC Analysis -----------------------------------------------%
    % --------------------------------------------------------------------%

    % Format Seeds ---------------------------------------------------%
    seedinfo_prelim_f = seedinfo_prelim;
    for i = 0:size(seedinfo_prelim,1)-1
        for j = 0:size(seedinfo_prelim,2)-1
            for k = 0:size(seedinfo_prelim,3)-1
                seedinfo_prelim_f(i+1,j+1,k+1).num_region = int32(seedinfo_prelim(i+1,j+1,k+1).num_region);
                seedinfo_prelim_f(i+1,j+1,k+1).num_thread = int32(seedinfo_prelim(i+1,j+1,k+1).num_thread);
                seedinfo_prelim_f(i+1,j+1,k+1).computepoints = int32(seedinfo_prelim(i+1,j+1,k+1).computepoints);
            end
        end
    end

    % Begin DIC analysis ---------------------------------------------%
    displacements_prelim = struct('plot_u',{},'plot_v',{},'plot_corrcoef',{},'plot_validpoints',{});
    rois_dic_prelim = ncorr_class_roi.empty;
    for i = 0:size(seedinfo_prelim,3)-1
        % Put in try block because DIC can run
        % out of memory
        try
            tic
            % No need to send in the number of regions or total number
            % of threads because this is encoded in the size of the
            % seeds.
            % FYI: seeinfo_prelim(i,j,k) where i refers to the thread
            % number, j refers to the region, and k refers to the image.
            % ncorr_alg_rgdic will either return success or cancelled or
            % return an exception.
            [displacements_prelim(i+1),outstate_dic] = ncorr_alg_rgdic(imgs(1).imginfo.formatted(), ...
                                                                       imgs(i+2).imginfo.formatted(), ...
                                                                       imgs(1).roi.formatted(), ...
                                                                       seedinfo_prelim_f(:,:,i+1), ...
                                                                       int32(threaddiagram), ...
                                                                       int32(radius), ...
                                                                       int32(spacing), ...
                                                                       cutoff_diffnorm, ...
                                                                       int32(cutoff_iteration), ...
                                                                       logical(subsettrunc), ...
                                                                       int32(num_img+i), ...
                                                                       int32(total_imgs));
            toc
        catch %#ok<CTCH>
            % Only time an exception is returned is either if rgdic has
            % incorrect inputs, ran out of memory, or there is a bug in
            % the code. The first option never happens if rgdic
            % is called through the program. The second option can
            % happen and is thus the only real handleable exception
            % which will get thrown by rgdic. The last case was not
            % intended, so it is not handled here.
            h_error = errordlg('Ncorr most likely ran out of memory while performing DIC. Please clear memory in workspace, restart Ncorr, or crop/use smaller images before doing analysis.','Error','modal');
            uiwait(h_error);
            return;
        end

        % See if analysis was cancelled
        if (outstate_dic ~= out.success)
            return;
        end

        % Take union of reference ROI with validpoints to get the new
        % ROI.
        rois_dic_prelim(i+1) = imgs(1).roi.get_union(displacements_prelim(i+1).plot_validpoints,spacing);
    end

    % Set outputs
    for i = 0:length(displacements_prelim)-1
        displacements(i+1) = displacements_prelim(i+1);
        rois_dic(i+1) = rois_dic_prelim(i+1);
    end
    for i = 0:size(seedinfo_prelim,1)-1
        for j = 0:size(seedinfo_prelim,2)-1
            for k = 0:size(seedinfo_prelim,3)-1
                seedinfo(i+1,j+1,k+1) = seedinfo_prelim(i+1,j+1,k+1);
            end
        end
    end
    outstate = out.success;
end