function [FluxCorrelations, PValues, TaxonomyInfo] = correlateFluxWithTaxonAbundance(abundancePath, fluxPath, infoFilePath, corrMethod)
% Part of the Microbiome Modeling Toolbox. This function calculates and
% plots the correlations between fluxes for one or more reactions of
% interest in a number of microbiome samples and the relative microbe
% abundance on different taxonomical levels in the same samples.
% The function should be used after running mgPipe to identify correlations
% between the computed metabolic profiles and specific taxa in the samples.
%
% USAGE
%
%     [FluxCorrelations, PValues, TaxonomyInfo] = correlateFluxWithTaxonAbundance(abundancePath, fluxPath, taxonomy, corrMethod)
%
% INPUTS:
%    abundancePath:     Path to the .csv file with the abundance data.
%                       Needs to be in same format as example file
%                       'cobratoolbox/papers/018_microbiomeModelingToolbox/examples/normCoverage.csv'
%     fluxPath:         Path to the .csv file with the fluxes for reactions
%                       of interest with sample IDs as rows and reaction
%                       IDs in microbiome community models as columns
%
% OPTIONAL INPUTS:
%    infoFilePath:      Path to the spreadsheet with the taxonomy information
%                       on organisms (default: AGORA_infoFile.xlsx)
%     corrMethod:       Method to compute the linear correlation
%                       coefficient. Allowed inputs: 'Pearson' (default),
%                       'Kendall', 'Spearman'.
%
% OUTPUTS:
%     FluxCorrelations: Structure with correlations between fluxes for each
%                       reaction and abundances on taxon levels
%     PValues:          p-values corresponding to each calculated
%                       correlation
%     TaxonomyInfo:     Taxonomical information on each taxon level
%
% .. Author: Almut Heinken, 03/2018
%                           10/2018:  changed input to location of the csv file with the
%                                     abundance data
%                           01/2020:  adapted to be suitable for pan-models, and
%                                     changed flux input to a csv file.

% read the csv file with the abundance data
abundance = readInputTableForPipeline(abundancePath);
if isnumeric(abundance{2, 1})
    abundance(:, 1) = [];
end
% adapt IDs if neccessary
abundance(1,2:end) = strrep(abundance(1,2:end),'-','_');

fluxes = readInputTableForPipeline(fluxPath);
fluxes(1,:) = strrep(fluxes(1,:),'microbiota_model_diet_','');
fluxes(1,:) = strrep(fluxes(1,:),'microbiota_model_samp_','');
fluxes(:,1) = strrep(fluxes(:,1),'microbiota_model_diet_','');
fluxes(:,1) = strrep(fluxes(:,1),'microbiota_model_samp_','');

% check if data is from same samples
if ~isempty(setdiff(fluxes(1,2:end),abundance(1,2:end)))
    fluxes=fluxes';
    % if it still does not match
    if ~isempty(setdiff(fluxes(1,2:end),abundance(1,2:end)))
        error('Sample IDs in abundance and flux files do not agree!')
    end
end

% load database
database=loadVMHDatabase;

fluxes(:,1)=strrep(fluxes(:,1),'EX_','');
fluxes(:,1)=strrep(fluxes(:,1),'(e)','');
fluxes(:,1)=strrep(fluxes(:,1),'[fe]','');
% for i=2:size(fluxes,1)
% fluxes{i,1}=database.metabolites{find(strcmp(database.metabolites(:,1),fluxes{i,1})),2};
% end

% Get the taxonomy information
if exist('infoFilePath','var')
    taxonomy = readInputTableForPipeline(infoFilePath);
else
    taxonomy = readInputTableForPipeline('AGORA_infoFile.xlsx');
end

if ~exist('corrMethod', 'var')  % Define correlation coefficient method if not entered
    corrMethod = 'Pearson';
end

% Calculate the abundance in each sample on all taxon levels
TaxonomyLevels = {
    'Phylum'
    'Class'
    'Order'
    'Family'
    'Genus'
    'Species'
};
% extract the list of entries on each taxonomical level and prepare the
% summarized abundance table
fprintf('Calculating the relative abundances on all taxon levels. \n')
for t = 1:size(TaxonomyLevels, 1)
    % find the columns corresponding to each taxonomy level and the list of
    % unique taxa
    taxonCol = find(strcmp(taxonomy(1, :), TaxonomyLevels{t}));
    % find and save all entries
    taxa = unique(taxonomy(2:end, taxonCol));
    % exclude unclassified entries
    taxa(strncmp('unclassified', taxa, taxonCol)) = [];
    TaxonomyLevels{t, 2} = taxa;
    for i = 1:length(taxa)
        SampleAbundance.(TaxonomyLevels{t}){1, i + 1} = taxa{i};
        for j = 2:size(abundance, 2)
            SampleAbundance.(TaxonomyLevels{t}){j, 1} = abundance{1, j};
            SampleAbundance.(TaxonomyLevels{t}){j, i + 1} = 0;
        end
    end
end

% Go through the abundance data and summarize taxon abundances for each
% strain in at least one sample

% Find the right column for the input data (strains, species,..)
if sum(strncmp(abundance(:,1),'pan',3))==size(abundance,1)-1
abundance(:,1)=regexprep(abundance(:,1),'pan','','once');
end
inputTaxa={};
for i=1:size(taxonomy,2)
    if ~isnumeric(taxonomy{2,i})
        taxa=strrep(taxonomy(:,i),' ','_');
        taxa=strrep(taxa,'.','_');
        taxa=strrep(taxa,'/','_');
        taxa=strrep(taxa,'-','_');
        taxa=strrep(taxa,'-','_');
        taxa=strrep(taxa,'[','');
        taxa=strrep(taxa,']','');
        taxa=strrep(taxa,')','');
        taxa=strrep(taxa,'__','_');
        if length(intersect(abundance(2:end,1),taxa))==size(abundance,1)-1
            inputTaxa=taxa;
            inputCol=i;
            break
        end
    end
end
if isempty(inputTaxa)
    error('Some taxa in the abundance file are not found in the taxonomy file!')
end

for i = 2:size(abundance, 2)
    for j = 2:size(abundance, 1)
        for t = 1:size(TaxonomyLevels, 1)
            % find the taxon for the current strain
            taxonCol = find(strcmp(taxonomy(1, :), TaxonomyLevels{t}));
            if taxonCol >= inputCol
                findTax = taxonomy(find(strcmp(abundance{j, 1}, inputTaxa)), taxonCol);
                if isempty(strfind(findTax{1}, 'unclassified'))
                    % find the taxon for the current strain in the sample abundance
                    % variable
                    findinSampleAbun = find(strcmp(findTax{1}, SampleAbundance.(TaxonomyLevels{t})(1, :)));
                    % sum up the relative abundance
                    if contains(version,'(R202') % for Matlab R2020a and newer
                        SampleAbundance.(TaxonomyLevels{t}){i, findinSampleAbun} = SampleAbundance.(TaxonomyLevels{t}){i, findinSampleAbun} + abundance{j, i};
                    else
                        SampleAbundance.(TaxonomyLevels{t}){i, findinSampleAbun} = SampleAbundance.(TaxonomyLevels{t}){i, findinSampleAbun} + str2double(abundance{j, i});
                    end
                end
            end
        end
    end
end
% remove the taxa not present in samples or only present in small abundances
for t = 1:size(TaxonomyLevels, 1)
    delArray = [];
    cnt = 1;
    for i = 2:size(SampleAbundance.(TaxonomyLevels{t}), 2)
        for j = 2:size(SampleAbundance.(TaxonomyLevels{t}), 1)
            abun(j - 1, 1) = SampleAbundance.(TaxonomyLevels{t}){j, i};
        end
        if sum(abun) < 0.005
            delArray(cnt, 1) = i;
            cnt = cnt + 1;
        end
    end
    SampleAbundance.(TaxonomyLevels{t})(:, delArray) = [];
end
% find the flux data for each reaction
fprintf('Calculating the correlations between fluxes and abundances. \n')
for i = 2:size(fluxes, 1)
    data = [];
    for m = 2:size(fluxes, 2)
        data(m - 1, 1) = str2double(string(fluxes{i, m}));
    end
    for t = 1:size(TaxonomyLevels, 1)
        FluxCorrelations.(TaxonomyLevels{t}){1, i} = fluxes{i, 1};
        PValues.(TaxonomyLevels{t}){1, i} = fluxes{i, 1};
        % find the abundance data for each taxon
        for j = 2:size(SampleAbundance.(TaxonomyLevels{t}), 2)
            FluxCorrelations.(TaxonomyLevels{t}){j, 1} = SampleAbundance.(TaxonomyLevels{t}){1, j};
            PValues.(TaxonomyLevels{t}){j, 1} = SampleAbundance.(TaxonomyLevels{t}){1, j};
            % find the abundance data for each sample
            dataTaxa = data;
            for k = 2:size(SampleAbundance.(TaxonomyLevels{t}), 1)
                % match with correct individual in flux table
                sampleInFluxes = find(strcmp(fluxes(1, :), SampleAbundance.(TaxonomyLevels{t}){k, 1}));
                dataTaxa(sampleInFluxes - 1, 2) = SampleAbundance.(TaxonomyLevels{t}){k, j};
            end
            % exclude NaNs
            dataTaxa(find(isnan(dataTaxa(:,1))),:)=[];
            dataTaxa(find(isnan(dataTaxa(:,2))),:)=[];
            % calculate the correlation with the given correlation coefficient method
            [RHO, PVAL] = corr(dataTaxa(:, 1), dataTaxa(:, 2), 'type', corrMethod);
            if isnan(RHO)
                RHO = 0;
            end
            if abs(RHO) < 0.0000000001
                RHO = 0;
            end
            FluxCorrelations.(TaxonomyLevels{t}){j, i} = RHO;
            PValues.(TaxonomyLevels{t}){j, i} = PVAL;
        end
    end
end

% remove entries that are only weak correlations and empty taxonomy entries
for t = 1:size(TaxonomyLevels, 1)
    cnt=1;
    delArray=[];
    for j=2:size(FluxCorrelations.(TaxonomyLevels{t}),2)
        if ~any(abs(cell2mat(FluxCorrelations.(TaxonomyLevels{t})(2:end,j))) > 0.3)
            delArray(cnt,1)=j;
            cnt=cnt+1;
        end
    end
    FluxCorrelations.(TaxonomyLevels{t})(:,delArray)=[];

    cnt=1;
    delArray=[];
    for j=2:size(FluxCorrelations.(TaxonomyLevels{t}),1)
        if ~any(abs(cell2mat(FluxCorrelations.(TaxonomyLevels{t})(j,2:end))) > 0.3)
            delArray(cnt,1)=j;
            cnt=cnt+1;
        end
    end
    FluxCorrelations.(TaxonomyLevels{t})(delArray,:)=[];

    FluxCorrelations.(TaxonomyLevels{t})(find(strcmp(FluxCorrelations.(TaxonomyLevels{t})(:,1),'')),:)=[];
end

% translate to metabolite descriptions
for t = 2:size(TaxonomyLevels, 1)
    for i=2:size(FluxCorrelations.(TaxonomyLevels{t}),2)
        try
            FluxCorrelations.(TaxonomyLevels{t}){1,i}=database.metabolites{find(strcmp(database.metabolites(:,1),FluxCorrelations.(TaxonomyLevels{t}){1,i})),2};
        catch
            warning('Flux label could not be translated to metabolite name.')
        end
    end
end

% export taxonomical information
taxonCol = 'Phylum';
% remove unnecessary columns
taxonomy(:,taxonCol+1:end)=[];

for t = 2:size(TaxonomyLevels, 1)
    TaxonomyReduced=taxonomy;
    taxonCol = find(strcmp(taxonomy(1, :), TaxonomyLevels{t}));
    TaxonomyReduced(:,1:taxonCol-1)=[];
    % remove duplicate entries
    [C,IA] = unique(TaxonomyReduced(:,1),'stable');
    % remove unclassified taxa
    findUncl=find(contains(C,'unclassified'));
    IA(findUncl,:)=[];
    TaxonomyInfo.(TaxonomyLevels{t})=TaxonomyReduced(IA,:);
end

set(0, 'DefaultTextInterpreter', 'none')

% Plot the calculated correlations.
for t = 1:length(TaxonomyLevels)
    data = string(FluxCorrelations.(TaxonomyLevels{t})(2:end, 2:end));
    data = str2double(data);

    if size(FluxCorrelations.(TaxonomyLevels{t}),2) > 5 && size(FluxCorrelations.(TaxonomyLevels{t}),1) > 5
        if size(FluxCorrelations.(TaxonomyLevels{t}),2) < 50
            cgo = clustergram(data,...
                'RowLabels', FluxCorrelations.(TaxonomyLevels{t})(2:end,1),...
                'ColumnLabels', FluxCorrelations.(TaxonomyLevels{t})(1,2:end),...
                'ColumnLabelsRotate', 45, ...
                'Cluster', 'all', ...
                'ShowDendrogram','True', ...
                'symmetric','False', ...
                'colormap', 'turbo' ...
                );
        else
            cgo = clustergram(data,...
                'RowLabels', FluxCorrelations.(TaxonomyLevels{t})(2:end,1),...
                'Cluster', 'all', ...
                'ShowDendrogram','True', ...
                'symmetric','False', ...
                'colormap', 'turbo' ...
                );
        end
        h = plot(cgo);
        set(h,'TickLabelInterpreter','none');
        colorbar(h)
    end
end

end


function [sampft, sampyt] = gp_rnd(gp, x, y, varargin)
%GP_RND  Random draws from the posterior Gaussian process
%
%  Description
%    [SAMPFT, SAMPYT] = GP_RND(GP, X, Y, XT, OPTIONS) takes a
%    Gaussian process structure, record structure (from gp_mc) or
%    array (from gp_ia) GP together with a matrix XT of input
%    vectors, matrix X of training inputs and vector Y of training
%    targets, and returns a random sample SAMPFT and SAMPYT from
%    the posterior distribution p(ft|x,y,xt) and the predictive
%    distribution p(yt|x,y,xt) at locations XT.
%
%    OPTIONS is optional parameter-value pair
%      nsamp     - determines the number of samples (default = 1).
%      predcf    - index vector telling which covariance functions are
%                  used for prediction. Default is all (1:gpcfn)
%      tstind    - a vector defining, which rows of X belong to which
%                  training block in *IC type sparse models. Default is [].
%                  See also GP_PRED.
%      z         - optional observed quantity in triplet (x_i,y_i,z_i)
%                  Some likelihoods may use this. For example, in case of
%                  Poisson likelihood we have z_i=E_i, that is, expected
%                  value
%                  for ith case.
%      fcorr     - Method used for latent marginal posterior corrections.
%                  Default is 'off'. Possible methods are 'fact' for EP
%                  and either 'fact', 'cm2' or 'lr' for Laplace. If method is
%                  'on', 'fact' is used for EP and 'cm2' for Laplace.
%                  See GP_PREDCM and Cseke & Heskes (2011) for more information.
%      splitnormal - determines if the samples are drawn from
%                  split-Normal approximation (Geweke, 1989) in the
%                  case of non-Gaussian likelihood.
%                  Possible values are 'on' (default) and 'off'.
%      n_scale   - the maximum number of the most significant principal
%                  component directories to scale in the split-Normal
%                  approximation (default 50).
%
%    If likelihood is non-Gaussian and gp.latent_method is Laplace the
%    samples are drawn from split-Normal approximation (see option
%    splitnormal), and if gp.latent_method is EP the samples are drawn
%    from the normal approximation.
%
%  Reference
%    Cseke & Heskes (2011). Approximate Marginals in Latent Gaussian
%    Models. Journal of Machine Learning Research 12 (2011), 417-454
%
%    Geweke, J. (1989).  Bayesian inference in econometric models using
%    Monte Carlo integration. Econometrica 57:1317-1339.
%
%  See also
%    GP_PRED, GP_PAK, GP_UNPAK

%  Internal comments
%    - sampling with FIC, PIC and CS+FIC forms full nxn matrix and
%       works only when sampling for the training inputs
%    - The code is not optimized
%
% Copyright (c) 2007-2010 Jarno Vanhatalo
% Copyright (c) 2008      Jouni Hartikainen
% Copyright (c) 2011      Aki Vehtari

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

ip=inputParser;
ip.FunctionName = 'GP_RND';
ip.addRequired('gp',@(x) isstruct(x) || iscell(x));
ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addOptional('xt', [], @(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))))
ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('predcf', [], @(x) isempty(x) || ...
                 isvector(x) && isreal(x) && all(isfinite(x)&x>0))
ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                 (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
ip.addParamValue('nsamp', 1, @(x) isreal(x) && isscalar(x))
ip.addParamValue('fcorr', 'off', @(x) ismember(x, {'fact','cm2','off','on','lr'}))
ip.addParamValue('splitnormal', 'on', @(x) (islogical(x) && isscalar(x))|| ...
                 ismember(x,{'on' 'off' 'full'}))
ip.addParamValue('n_scale',50, @(x) isnumeric(x) && x>=0);
if numel(varargin)==0 || isnumeric(varargin{1})
  % inputParser should handle this, but it doesn't
  ip.parse(gp, x, y, varargin{:});
else
  ip.parse(gp, x, y, [], varargin{:});
end
xt=ip.Results.xt;
z=ip.Results.z;
zt=ip.Results.zt;
tn = size(x,1);
predcf=ip.Results.predcf;
tstind=ip.Results.tstind;
nsamp=ip.Results.nsamp;
fcorr=ip.Results.fcorr;
splitnormal = ip.Results.splitnormal;
n_scale = min(tn, floor(ip.Results.n_scale));
if isempty(xt)
  xt=x;
  if isempty(tstind)
    if iscell(gp)
      gptype=gp{1}.type;
    else
      gptype=gp.type;
    end
    switch gptype
      case {'FULL' 'VAR' 'DTC' 'SOR'}
        tstind = [];
      case {'FIC' 'CS+FIC'}
        tstind = 1:size(x,1);
      case 'PIC'
        if iscell(gp)
          tstind = gp{1}.tr_index;
        else
          tstind = gp.tr_index;
        end
    end
  end
end

if isfield(gp, 'latent_method')
  if iscell(gp)
      gplatmet=gp{1}.latent_method;
  else
      gplatmet=gp.latent_method;
  end
  if ~strcmp(gplatmet, 'Laplace') && strcmp(splitnormal,'on')
      % splitnormal is applicable only with Laplace
      splitnormal='off';
  end
end

sampyt=[];
if isstruct(gp) && numel(gp.jitterSigma2)==1
  % Single GP
  if isfield(gp, 'monotonic') && gp.monotonic
      [gp,x,y,z,xt,zt] = gp.fh.setUpDataForMonotonic(gp,x,y,z,xt,zt);
  end

  if (isfield(gp.lik.fh,'trcov') && ~isfield(gp,'lik_mono')) ...
      || isfield(gp, 'latentValues')
    % ====================================================
    %    Gaussian likelihood or MCMC with latent values
    % ====================================================

    if nargout > 1
      [Eft, Covft, ~, Eyt, Covyt] ...
        = gp_jpred(gp,x,y,xt,'z',z,'predcf',predcf,'tstind',tstind);
    else
      [Eft, Covft] = gp_jpred(gp,x,y,xt,'z',z, ...
                              'predcf',predcf,'tstind',tstind);
    end
    rr = randn(size(Eft,1),nsamp);
    sampft = bsxfun(@plus, Eft, chol(Covft,'lower')*rr);
    if nargout > 1
      sampyt = bsxfun(@plus, Eyt, chol(Covyt,'lower')*rr);
    end

  else
    % =============================
    %    non-Gaussian likelihood
    % =============================

    if nargout > 1
      warning('Sampling of yt is not implemented for non-Gaussian likelihoods');
    end
    if strcmp(splitnormal,'on') && isfield(gp,'meanf')
        warning('Split-Normal is not implemented for mean functions');
        splitnormal='off';
    end
    if strcmp(splitnormal,'on') && isfield(gp.lik, 'nondiagW')
        warning('Split-Normal is not implemented for likelihoods with nondiagonal Hessian')
        splitnormal='off';
    end
    if strcmp(splitnormal,'on') && ~isequal(fcorr, 'off')
        warning('Split-Normal is not used when latent marginal posterior corrections are used')
        splitnormal='off';
    end

    if strcmp(splitnormal,'on')
      % ------------------
      %    Splitnormal on
      % ------------------

      switch gp.type

        % ---------------------------
        case 'FULL'

          [e,~,~, param] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
          if isnan(e)
            error('Laplace-algorithm returned NaN');
          end
          [Ef, L, W] = deal(param.f, param.L, param.La2);

          % Evaluate the covariance
          K = gp_trcov(gp,x);
          K_ss = gp_trcov(gp,xt,predcf);
          K_nf = gp_cov(gp,xt,x,predcf);
          if W >= 0
            % Likelihood is log concave
            if issparse(K) && issparse(L)
              if issparse(W)
                sqrtWK = sqrt(W)*K;
              else
                sqrtWK = sparse(1:tn,1:tn,sqrt(W),tn,tn)*K;
              end
              Covf = K - sqrtWK'*ldlsolve(L,sqrtWK);
            else
              V = linsolve(L,bsxfun(@times,sqrt(W),K),struct('LT',true));
              Covf = K - V'*V;
            end
          else
            % Likelihood is not log concave
            V = bsxfun(@times,L,W');
            Covf = K - K*(diag(W) - V'*V)*K;
          end

        % ---------------------------
        case 'FIC'

          if ~isempty(tstind) && length(tstind) ~= tn
            error('tstind (if provided) has to be of same length as x.')
          end

          % Ensure the direction of tstind
          tstind = tstind(:);

          [e,~,~, param] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
          if isnan(e)
            error('Laplace-algorithm returned NaN');
          end
          Ef = param.f;

          u = gp.X_u;
          K_uu = gp_trcov(gp,u,predcf);
          K_uu = (K_uu+K_uu')./2;          % ensure the symmetry
          Luu = chol(K_uu, 'lower');

          B = Luu\gp_cov(gp,u,x,predcf);
          B2 = Luu\gp_cov(gp,u,xt,predcf);
          K = B'*B;
          K_ss = B2'*B2;
          K_nf = B2'*B;
          clear K_uu Luu B B2

          % If the prediction is made for training set, evaluate Lav
          % also for prediction points.
          if ~isempty(tstind)
            kss = gp_trvar(gp,xt,predcf);
            % Replace diagonals
            K(1:length(K)+1:numel(K)) = kss(tstind);
            K_ss(1:length(K_ss)+1:numel(K_ss)) = kss;
            % Replace common samples in K_nf
            K_nf(tstind+(0:size(xt,1):(tn-1)*size(xt,1))') = kss(tstind);
            clear kss
          else
            % Add lambda
            % Replace diagonals
            K(1:length(K)+1:numel(K)) = gp_trvar(gp,x,predcf);
            K_ss(1:length(K_ss)+1:numel(K_ss)) = gp_trvar(gp,xt,predcf);
          end

          Wsqrt = -gp.lik.fh.llg2(gp.lik, y, Ef, 'latent', z);
          if any(Wsqrt < 0)
            error('FIC not implemented for non-log-concave likelihoods')
          end
          Wsqrt = sqrt(Wsqrt);
          L = chol(eye(size(K)) + bsxfun(@times,bsxfun(@times,Wsqrt,K),Wsqrt'), 'lower');

          % Evaluate the covariance for the posterior of f
          V = linsolve(L,bsxfun(@times,Wsqrt,K),struct('LT',true));
          Covf = K - V'*V;

        % ---------------------------
        case {'PIC' 'PIC_BLOCK'}

          u = gp.X_u;
          K_fu = gp_cov(gp,x,u,predcf);         % f x u
          K_uu = gp_trcov(gp,u,predcf);         % u x u, noiseles covariance K_uu
          K_uu = (K_uu+K_uu')./2;               % ensure the symmetry of K_uu

          ind = gp.tr_index;
          Luu = chol(K_uu)';
          B=Luu\(K_fu');
          B2 = Luu\(gp_cov(gp,u,xt,predcf));
          clear Luu

          [e,~,~, p] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
          if isnan(e)
            error('Laplace-algorithm returned NaN');
          end
          [Ef, La2] = deal(p.f, p.La2);

          % Evaluate the variance
          sqrtW = -gp.lik.fh.llg2(gp.lik, y, Ef, 'latent', z);
          if any(sqrtW < 0)
            error('PIC not implemented for non-log-concave likelihoods')
          end
          sqrtW = sqrt(sqrtW);
          % Components for (I + W^(1/2)*(Qff + La2)*W^(1/2))^(-1) = Lahat^(-1) - L2*L2'
          Lahat = cell(length(ind),1);
          for i=1:length(ind)
            Lahat{i} = eye(length(ind{i})) + bsxfun(@times,bsxfun(@times,sqrtW(ind{i}),La2{i}),sqrtW(ind{i})');
          end
          sKfu = bsxfun(@times, sqrtW, K_fu);
          iLasKfu = zeros(size(K_fu));
          for i=1:length(ind)
            iLasKfu(ind{i},:) = Lahat{i}\sKfu(ind{i},:);
          end
          A2 = K_uu + sKfu'*iLasKfu; A2=(A2+A2')./2;
          L2 = iLasKfu/chol(A2);

          % NOTE!
          % This is done with full matrices at the moment.
          % Needs to be rewritten.
          K_ss = B2'*B2;
          K_nf = B2'*B;
          K = B'*B;
          C = -L2*L2';
          for i=1:length(ind)
            % Originally
            % >> La = gp_trcov(gp, xt(tstind{i},:), predcf) - B2(:,tstind{i})'*B2(:,tstind{i});
            % >> K_ss(ind{i},ind{i}) =  K_ss(ind{i},ind{i}) + La;
            % changed into (works if xt is not the same as x)
            % >> La = gp_trcov(gp, xt(tstind{i},:), predcf) - B2(:,tstind{i})'*B2(:,tstind{i});
            % >> K_ss(tstind{i},tstind{i}) =  K_ss(tstind{i},tstind{i}) + La;
            % which is implemented in the line bellow
            K_ss(tstind{i},tstind{i}) =  gp_trcov(gp, xt(tstind{i},:), predcf);
            K_nf(tstind{i},ind{i}) = gp_cov(gp, xt(tstind{i},:), x(ind{i},:),predcf);
            K(ind{i},ind{i}) = K(ind{i},ind{i}) + La2{i};
            C(ind{i},ind{i}) =  C(ind{i},ind{i}) + inv(Lahat{i});
          end

          Covf = K - K * bsxfun(@times,bsxfun(@times,sqrtW,C),sqrtW') * K;

        % ---------------------------
        case 'CS+FIC'

          if ~isempty(tstind) && length(tstind) ~= tn
            error('tstind (if provided) has to be of same length as x.')
          end

          % Ensure the direction of tstind
          tstind = tstind(:);

          % Indexes to all non-compact support and compact support covariances.
          cscf = cellfun(@(x) isfield(x,'cs'), gp.cf);
          predcf1 = find(~cscf);
          predcf2 = find(cscf);
          if ~isempty(predcf)
            predcf1 = intersect(predcf1,predcf);
            predcf2 = intersect(predcf2,predcf);
          end

          % Laplace approximation
          [e,~,~, param] = gpla_e(gp_pak(gp), gp, x, y, 'z', z);
          if isnan(e)
            error('Laplace-algorithm returned NaN');
          end
          Ef = param.f;

          u = gp.X_u;

          K_uu = gp_trcov(gp,u,predcf1);
          K_uu = (K_uu+K_uu')./2;
          Luu = chol(K_uu)';

          B=Luu\(gp_cov(gp,u,x,predcf1));
          B2=Luu\(gp_cov(gp,u,xt,predcf1));
          K = B'*B;
          K_ss = B2'*B2;
          K_nf = B2'*B;
          clear K_uu Luu B B2

          if ~isempty(tstind)
            % Add Lambda
            kss = gp_trvar(gp,xt,predcf1);
            % Replace diagonals
            K(1:length(K)+1:numel(K)) = kss(tstind);
            K_ss(1:length(K_ss)+1:numel(K_ss)) = kss;
            % Replace common samples in K_nf
            K_nf(tstind+(0:size(xt,1):(tn-1)*size(xt,1))') = kss(tstind);

            % Add CS covariance
            kss = gp_trcov(gp, xt, predcf2);
            K = K + kss(tstind,tstind);
            K_ss = K_ss + kss;
            clear kss

          else
            % Add lambda
            % Replace diagonals
            K(1:length(K)+1:numel(K)) = gp_trvar(gp,x,predcf1);
            K_ss(1:length(K_ss)+1:numel(K_ss)) = gp_trvar(gp,xt,predcf1);

            % Add CS covariance
            K = K + gp_trcov(gp, x, predcf2);
            K_ss = K_ss + gp_trcov(gp, xt, predcf2);

          end

          % Add CS covariance for K_nf
          K_nf = K_nf + gp_cov(gp, xt, x, predcf2);

          sqrtW = -gp.lik.fh.llg2(gp.lik, y, Ef, 'latent', z);
          if any(sqrtW < 0)
            error('CS+FIC not implemented for non-log-concave likelihoods')
          end
          sqrtW = sqrt(sqrtW);
          L = chol(eye(size(K)) + bsxfun(@times,bsxfun(@times,sqrtW,K),sqrtW'), 'lower');
          V = linsolve(L,bsxfun(@times,sqrtW,K),struct('LT',true));
          Covf = K - V'*V;

        otherwise
          error('Split-Normal not implemented for %s', gp.type)

      end

      % Scaling n_scale principal component directions
      % Split-normal approximation: Geweke (1989)

      % N.B. svd of sparse matrix is not generally sparse (unless Covf is
      % block diagonal or can be made such by reordering the dimensions).
      % Scaling using the Cholesky factorisation could possibly be done
      % sparsely but the directions would be different (not split-Normal).
      % Thus full matrices has to be used.
      [V, D, ~] = svd(full(Covf));
      T = real(V) * sqrt(real(D)); % Ensuring the real

      L = chol(K,'lower');

      % Optimise the skewness
      delta = [-6:0.5:-0.5, 0.5:0.5:6];
      D = bsxfun(@plus, kron(delta,T(:,1:n_scale)), Ef);
      ll = zeros(1,n_scale*length(delta)); % Here looping is faster than cellfun
      for i = 1:n_scale*length(delta)
        ll(i) = 2*gp.lik.fh.ll(gp.lik,y,D(:,i),z);
      end
      ll = ll - sum(D.*(L'\(L\D)));
      ll0 = 2*gp.lik.fh.ll(gp.lik, y, Ef, z) - Ef'*(L'\(L\Ef));
      ll(ll >= ll0) = NaN;
      f = bsxfun(@rdivide, abs(delta), reshape(sqrt(ll0-ll),n_scale,length(delta)));
      clear D V ll ll0
      q = max(f(:,delta>0),[],2);
      r = max(f(:,delta<0),[],2);
      q(isnan(q)) = 1;
      r(isnan(r)) = 1;
      % Safety limits [1/10, 10]
      q = min(max(q,0.1),10);
      r = min(max(r,0.1),10);
      if n_scale < tn
        q = [q;ones(tn-n_scale,1)];
        r = [r;ones(tn-n_scale,1)];
      end

      % Draw samples from split-Normal approximated posterior of f and
      % sample from the conditional distribution of ft for each sample of f
      u = rand(tn,nsamp);
      c = r./(r+q);
      Covft = K_ss - K_nf*(L'\(L\K_nf'));
      Covft_ind = find(diag(Covft)>gp.jitterSigma2);
      if length(Covft_ind) == size(xt,1)
        % The following evaluates samples of ft directly by
        % Eft + chol(Covft)*randn,
        % where Eft = K_nf*K\f and Covft = K_ss - K_nf*K\K_nf'
        sampft = K_nf*(L'\(L\ ...
          bsxfun(@plus,Ef,T*((bsxfun(@times,q,double(bsxfun(@ge,u,c))) ...
          +bsxfun(@times,-r,double(bsxfun(@lt,u,c)))).*abs(randn(tn,nsamp)))) ...
          )) + chol(Covft,'lower')*randn(size(xt,1),nsamp);
      else
        % Zero variance dimensions in p(ft|X,xt,f)
        % Evaluate first Eft for each f
        sampft = K_nf*(L'\(L\ ...
            bsxfun(@plus,Ef,T*((bsxfun(@times,q,double(bsxfun(@ge,u,c))) ...
            +bsxfun(@times,-r,double(bsxfun(@lt,u,c)))).*abs(randn(tn,nsamp)))) ));
        % Add variability only from non-zero variance dimensions
        sampft(Covft_ind,:) = sampft(Covft_ind,:) ...
          + chol(Covft(Covft_ind,Covft_ind),'lower')*randn(length(Covft_ind),nsamp);
      end

    else
      % -------------------
      %    split-Normal off
      % -------------------
      [Eft, Covft] = gp_jpred(gp,x,y,xt,'z',z, ...
        'predcf',predcf, 'tstind',tstind, 'fcorr','off');
      sampft = bsxfun(@plus, Eft, chol(Covft,'lower')*randn(size(xt,1),nsamp));

      if ~isequal(fcorr, 'off')
        % Do marginal corrections for samples
        [pc_predm, fvecm] = gp_predcm(gp, x, y, xt, 'z', z, 'ind', 1:size(xt,1), 'fcorr', fcorr);
        for i=1:size(xt,1)
          % Remove NaNs and zeros
          pc_pred=pc_predm(:,i);
          dii=isnan(pc_pred)|pc_pred==0;
          pc_pred(dii)=[];
          fvec=fvecm(:,i);
          fvec(dii)=[];
          % compute cdf
          cumsumpc = cumsum(pc_pred)/sum(pc_pred);
          % Remove non-unique values from grid vector & distribution
          [cumsumpc, inds] = unique(cumsumpc);
          fvec = fvec(inds);
          % use inverse cdf to make marginal transformation
          fsnc = normcdf(sampft(i,:), Eft(i,1), sqrt(diag(Covft(i,i))));
          sampft(i,:) = interp1(cumsumpc,fvec,fsnc);
        end
      end
    end

  end

elseif isstruct(gp) && numel(gp.jitterSigma2)>1
  % MCMC
  nmc=size(gp.jitterSigma2,1);
  % resample nsamp cases from nmc samples
  % deterministic resampling has low variance and small bias for
  % equal weights
  gi=resampdet(ones(nmc,1),nsamp,1);
  sampft = zeros(size(xt,1),nsamp);
  if nargout>=2
    sampyt = zeros(size(xt,1),nsamp);
  end
  for i1=1:nmc
    sampind = find(gi==i1);
    nsampi = length(sampind);
    if nsampi>0
      Gp = take_nth(gp,i1);
      if isfield(Gp,'latent_method') && isequal(Gp.latent_method,'MCMC')
        Gp = rmfield(Gp,'latent_method');
      end
      if isfield(gp, 'latentValues') && ~isempty(gp.latentValues)
        % Non-Gaussian likelihood. The latent variables should be used in
        % place of observations
        for ni=1:nsampi
          if ni==1
            % use stored latent values
            f = gp.latentValues(i1,:);
          else
            % need to resample latent values
            opt=scaled_mh();
            f=scaled_mh(f, opt, Gp, x, y, z);
          end
          if nargout<2
            sampft(:,sampind(ni)) = gp_rnd(Gp, x, f', xt, 'nsamp', 1, ...
                             'z', z, 'zt', zt, 'predcf', predcf, ...
                             'tstind', tstind);
          else
            [tsampft, tsampyt] = gp_rnd(Gp, x, f', xt, 'nsamp', ...
                                        1, 'z', z, 'zt', zt, ...
                                        'predcf', predcf, 'tstind', tstind);

            sampft(:,sampind(ni)) = tsampft;
            sampyt(:,sampind(ni)) = tsampyt;
          end

        end
      else
        % Gaussian likelihood
        if nargout<2
          sampft(:,sampind) = gp_rnd(Gp, x, y, xt, 'nsamp', nsampi, ...
                           'z', z, 'zt', zt, 'predcf', predcf, ...
                           'tstind', tstind);
        else
          [tsampft, tsampyt] = gp_rnd(Gp, x, y, xt, 'nsamp', ...
                                      nsampi, 'z', z, 'zt', zt, ...
                                      'predcf', predcf, 'tstind', tstind);
          sampft(:,sampind) = tsampft;
          sampyt(:,sampind) = tsampyt;
        end
      end
    end
  end
elseif iscell(gp)
  % gp_ia
  ngp=length(gp);
  if isfield(gp{1},'ia_weight')
    gw = zeros(ngp,1);
    for i1=1:ngp
      gw(i1)=gp{i1}.ia_weight;
    end
  else
    gw=ones(ngp,1);
  end
  % resample nsamp cases from nmc samples
  % stratified resampling has has higher variance than deterministic
  % resampling, but has a smaller bias, and thus it should be more
  % suitable for unequal weights
  gi=resampstr(gw,nsamp,1);
  sampft = zeros(size(xt,1),nsamp);
  if nargout>=2
    sampyt = zeros(size(xt,1),nsamp);
  end
  for i1 = 1:ngp
    nsampi=sum(gi==i1);
    if nsampi>0
      if nargout<2
        sampft(:,gi==i1) = gp_rnd(gp{i1}, x, y, xt, 'nsamp', nsampi, ...
                         'z', z, 'zt', zt, 'predcf', predcf, ...
                         'tstind', tstind, 'fcorr', fcorr);
      else
        [tsampft, tsampyt] = gp_rnd(gp{i1}, x, y, xt, 'nsamp', nsampi, ...
                                    'z', z, 'zt', zt, 'predcf', predcf, ...
                                    'tstind', tstind, 'fcorr', fcorr);
        sampft(:,gi==i1) = tsampft;
        sampyt(:,gi==i1) = tsampyt;
      end
    end
  end
end


% EASY6  computes  a baseline vector. With given  code  and carrier
%            phase observations we estimate the ambiguities using integer
%	         least-squares (ILS) or Goad algorithms and estimate the baseline
%            components by a Kalman filter. The code does not handle
%	        outliers.

% RINEX version3.03

%Kai Borre 27-07-2002
%Copyright (c) by Kai Borre
%$Revision: 1.1 $  $Date: 2005/01/20  $
%Total revision 2.0, February 15, 2016

% The following code is general, but with the following limitations

% 1. We assume the observational columns are identical and ordered in the
% same sequence in master and rover files. The present observations just
% by chance contain identical satellites which are ordered in the same
% sequence. So sats1 and sats2 are the same. Alternatively, we need to
% establish a matching between sats1 and sats2 and likewise for Obs1
% and Obs2

% 2. There are no satellites with elevation angle less than 10 degrees.
% Minimum is 12 degrees. There are no rising or setting satellites during
% the observation period

% 3. The satellite having highest elevation angle at the start becomes the
% second highest about the middle of the observation file.

% 4. We do not apply tropospheric correction as it is very small in case
% master and rover stations only are about 1 meter apart

% 5. the actual baseline lenght was measured as 0.645 meter.

% Initial setting and computation of constants
v_light = 299792458;	     % vacuum speed of light m/s
f1 = 154*10.23E6;		     % L1 frequency Hz
f2 = 120*10.23E6;			 % L2 frequency Hz
lambda1 = v_light/f1;	     % wavelength on L1:  .19029367  m
lambda2 = v_light/f2;	     % wavelength on L2:  .244210213 m

% Read RINEX ephemerides file and convert to internal Matlab format
rinexe('log_24h.15n','eph.dat');
Eph = get_eph('eph.dat');

% Selection of observation type
%  ss = 'C1W L1W C2W L2W' %; % pseudorange and carrier phase
ss = 'C1C L1C C2X L2X'; % just to test other obsrervation types
fprintf('\n %s', ss)

% Open the master observation file
ofile1 = 'log_24h.15o';
fid1 = fopen(ofile1,'rt');

% Open the rover observation file
ofile2 = 'log_r.15o';
fid2 = fopen(ofile2,'rt');

coo = [];
ant_delta = [];
linjer = 0;
ij = [];
OBS1 = [];
OBS2 = [];
x = zeros(3,1);

% Gobbling the master file header, and collecting useful information
while 1
    linjer = linjer +1;
    line = fgetl(fid1);
    answer = strfind(line,'END OF HEADER');
    if  ~isempty(answer), break; end;
    if (line == -1), eof = 1; break; end;

    answer = strfind(line,'REC #');
    if ~isempty(answer)
        var = textscan(fid1,'%s %d14','Delimiter','\n');
        var1 = var{1}{1};
        cox = str2double(var1(1:14));
        coy = str2double(var1(15:28));
        coz = str2double(var1(29:42));
    end

    answer = strfind(line,'ANT #');
    if ~isempty(answer)
        var = textscan(fid1,'%s %d14','Delimiter','\n');
        var1 = var{1}{1};
        ax = str2double(var1(1:14));
        ay = str2double(var1(15:28));
        az = str2double(var1(29:42));
    end

    answer = strfind(line,'SYS / # / OBS TYPES');
    if ~isempty(answer)
        tline1 = strsplit(line);
        line = fgetl(fid1);
        tline2 = strsplit(line);
        tt1 = horzcat(tline1,tline2);
        for qq = 1:4
            if qq == 1, i = strcmp(tt1,ss(1:3)); end
            if qq == 2, i = strcmp(tt1,ss(5:7)) ; end
            if qq == 3, i = strcmp(tt1,ss(9:11)); end
            if qq == 4, i = strcmp(tt1,ss(13:15)); end
            ii = find(i == 1) ;
            ii = ii-2;
            if ii > 15,  ii = ii-7; end;
            ij = [ij ii];
        end
        % the cell array of strings tline1 includes two strings in the start
        % which describe the system and number of observation types.
        % Both tline1 and tline2 termintes with six additional strings.
        % An extra string appears at  the start of tline2; it originates
        % from concatenation of  the two lines. The indexing does not
        % change even if you empty the cells. They remain as empty
        % cells and keep a place
        obs_col = ij;
    end;
    answer = strfind(line,'INTERVAL');
    if ~isempty(answer)
        interval = strtok(line);
        int = str2double(interval);
    end;
    answer = strfind(line,'# OF SATELLITES');
    if ~isempty(answer)
        NumberSV = strtok(line);
        numSV = str2double(NumberSV) ;
    end
end % end reading master header

% the string arrays for the tline1 and tline2 contain an integer after the
% carrier phase observation. We must account for this by the following
% correctional table.

% the following code may be done more elegantly
for qq = 1:4
    if qq == 1, a0 = 6; b0 = 7; end
    if qq == 2, a0 = 6; b0 = 7; end
    if qq == 3, a0 = 14; b0 =15; end
    if qq == 4, a0 =14; b0 =15; end
    if strcmp(ss(a0:b0), '1C'), obs_col(1,qq) = obs_col(1,qq)+1;
    elseif  strcmp(ss(a0:b0), '1W'), obs_col(1,qq) = obs_col(1,qq)+2;
    elseif strcmp(ss(a0:b0),  '2X'), obs_col(1,qq) = obs_col(1,qq) +3;
    elseif strcmp(ss(a0:b0),  '2W'), obs_col(1,qq) = obs_col(1,qq) +4;
    else  strcmp(ss(a0:b0),  '5X'), obs_col(1,qq) = obs_col(1,qq) +5;
    end;
end;

Pos = [];
Omc = [];
base = [];
x_acc = [];
innov = [];

[time,post] = textscan(fid1,'%s %d8','Delimiter','\n');
tid = time{1}{1};
year = str2double(tid(3:6));
month = str2double(tid(8:9));
day = str2double(tid(11:12));
hour = str2double(tid(14:15));
minute = str2double(tid(17:18));
second = str2double(tid(20:29));
% static = str2double(tid(31:32));
NoSvs1 = str2double(tid(34:36));
dt1  = str2double(tid(38:56));
h = hour+minute/60+second/3600;
jd = julday(year, month, day, h);
[~, sec_of_week] = gps_time(jd);
time1 = sec_of_week; % sow1

Obs1 = zeros(NoSvs1,length(obs_col));
for i = 1:NoSvs1
    [obs1,~] = textscan(fid1,'%s %d8','Delimiter','\n');
    obs1x = obs1{1}{1} ;
    obs1y = strsplit(obs1x);
    sat1 = obs1y{1} ;
    sats1(i) = str2double(sat1(2:3));
    Obs1(i,:) = str2double(obs1y(obs_col));
end

% Gobbling the rover file header, and collecting useful information
% the whole story repated for the rover with index 2
% we start by gobbeling the header
for qq = 1:linjer +3 % 1 plus 2 from applying textscan twice
    aa = fgetl(fid2);
end

% Reading header of observation record in rover file
[time,pos] = textscan(fid2,'%s %d8','Delimiter','\n');
tid = time{1}{1};
year = str2double(tid(3:6));
month = str2double(tid(8:9));
day = str2double(tid(11:12));
hour = str2double(tid(14:15));
minute = str2double(tid(17:18));
second = str2double(tid(20:29));
static = str2double(tid(31:32));
NoSvs2 = str2double(tid(34:36));
dt2  = str2double(tid(38:56));
h = hour+minute/60+second/3600;
jd = julday(year, month, day, h);
[~, sec_of_week] = gps_time(jd);
time2 = sec_of_week; % sow2

Obs2 = zeros(NoSvs2,length(obs_col));
for i = 1:NoSvs2
    [obs2,pos] = textscan(fid2,'%s %d8','Delimiter','\n');
    obs2y = obs2{1}{1};
    obs2 = strsplit(obs2y);
    sat2 = obs2{1};
    sats2(i,:) = str2double(sat2(2:3));
    Obs2(i,:) = str2double(obs2(obs_col));
end

% Establish synchronous reading of fid1 and fid2
% The epoch interval is int seconds
if  time1 > time2
    for j = 1:round(time1-time2)
        [time,pos] = textscan(fid2,'%s %d8','Delimiter','\n');
        tid = time{1}{1};
        year = str2double(tid(3:6));
        month = str2double(tid(8:9));
        day = str2double(tid(11:12));
        hour = str2double(tid(14:15));
        minute = str2double(tid(17:18));
        second = str2double(tid(20:29));
        static = str2double(tid(31:32));
        NoSvs2 = str2double(tid(34:36));
        dt2  = str2double(tid(38:56));
        h = hour+minute/60+second/3600;
        jd = julday(year, month, day, h);
        [~, sec_of_week] = gps_time(jd);
        time2 = sec_of_week; % sow2

        Obs2 = zeros(NoSvs2,length(obs_col));
        for i = 1:NoSvs2
            [obs2,pos] = textscan(fid2,'%s %d8','Delimiter','\n');
            obs2y = obs2{1}{1};
            obs2 = strsplit(obs2y);
            sat2 = obs2{1};
            sats2(i,:) = str2double(sat2(2:3));
            Obs2(i,:) = str2double(obs2(obs_col));
        end
    end
end

if  time1 < time2
    for j = 1:round(time2-time1)
    [time,pos] = textscan(fid1,'%s %d8','Delimiter','\n');
    tid = time{1}{1};
    year = str2double(tid(3:6));
    month = str2double(tid(8:9));
    day = str2double(tid(11:12));
    hour = str2double(tid(14:15));
    minute = str2double(tid(17:18));
    second = str2double(tid(20:29));
    static = str2double(tid(31:32));
    NoSvs1 = str2double(tid(34:36));
    dt1  = str2double(tid(38:56));
    h = hour+minute/60+second/3600;
    jd = julday(year, month, day, h);
    [~, sec_of_week] = gps_time(jd);
    time1 = sec_of_week; % sow1
    Obs1 = zeros(NoSvs1,length(obs_col));
    for i = 1:NoSvs1
        [obs1,pos] = textscan(fid1,'%s %d8','Delimiter','\n');
        obs1x = obs1{1}{1};
        obs1y = strsplit(obs1x);
        sat1 = obs1y{1};
        sats1(i) = str2double(sat1(2:3));
        Obs1(i,:) = str2double(obs1y(obs_col));
    end
    end
end % synchronization

fprintf('\nFirst Common Epoch Time1 :  %8.0f', time1)
fprintf('\nFirst Common Epoch Time2 :  %8.0f\n', time2)

b_acc = [];
innov_acc = [];
bk_acc = [];
epoch = 0;

disp(' ')
disp(' ... Now wait  a few minutes for further results and a plot!')

while 1
    epoch = epoch +1;
    % Reading header of observation block at Master
    [time,post] = textscan(fid1,'%s %d8','Delimiter','\n');
    tid = time{1}{1};
    year = str2double(tid(3:6));
    month = str2double(tid(8:9));
    day = str2double(tid(11:12));
    hour = str2double(tid(14:15));
    minute = str2double(tid(17:18));
    second = str2double(tid(20:29));
    % static = str2double(tid(31:32));
    NoSvs1 = str2double(tid(34:36));
    dt1  = str2double(tid(38:56));
    h = hour+minute/60+second/3600;
    jd = julday(year, month, day, h);
    [~, sec_of_week] = gps_time(jd);
    time1 = sec_of_week; % sow1

    % Reading observation block at Master
    Obs1 = zeros(NoSvs1,length(obs_col));
    for i = 1:NoSvs1
        obs1 = textscan(fid1,'%s %d8','Delimiter','\n');
        obs1x = obs1{1}{1} ;
        obs1y = strsplit(obs1x);
        sat1 = obs1y{1} ;
        sats1(i) = str2double(sat1(2:3));
        Obs1(i,:) = str2double(obs1y(obs_col));
    end

    % Reading header of observation record in rover file
    [time,pos] = textscan(fid2,'%s %d8','Delimiter','\n');
    tid = time{1}{1};
    year = str2double(tid(3:6));
    month = str2double(tid(8:9));
    day = str2double(tid(11:12));
    hour = str2double(tid(14:15));
    minute = str2double(tid(17:18));
    second = str2double(tid(20:29));
    static = str2double(tid(31:32));
    NoSvs2 = str2double(tid(34:36));
    dt2  = str2double(tid(38:56));
    h = hour+minute/60+second/3600;
    jd = julday(year, month, day, h);
    [~, sec_of_week] = gps_time(jd);
    time2 = sec_of_week; % sow2

    Obs2 = zeros(NoSvs2,length(obs_col));
    for i = 1:NoSvs2
        [obs2,pos] = textscan(fid2,'%s %d8','Delimiter','\n');
        obs2y = obs2{1}{1};
        obs2 = strsplit(obs2y);
        sat2 = obs2{1};
        sats2(i,:) = str2double(sat2(2:3));
        Obs2(i,:) = str2double(obs2(obs_col));
    end

    % Next we test if all observed SVs have an ephemeris.
    % sats1 contains the SVs as read in the observation lines
    % The intersect command delivers Sats in sorted order!
    % Therefore we must be careful in the follwing manipulations
    Sats = intersect(sats1,Eph(1,:));

    % The command ismember does not change the sequence of entries in sats1
    lia = ismember(sats1,Sats);
    % A 0 (zero) in lia indicates that a SV has been deleted. We delete the
    % corresponding row in the observations
    sats1(lia==0) = [];
    Obs1(lia==0,:) = []; % modfied because of two columns
    NoSV1 = length(sats1);

    % Next we test if all observed sats have an ephemeris.
    % sats contains the SVs as read in the observation lines.
    % The intersect command delivers Sats in sorted order!
    % Therefore we must be careful in the following manipulations
    Sats = intersect(sats2,Eph(1,:));

    % The command ismember does not change the sequence of entries in sats
    lia = ismember(sats2,Sats);
    % A 0 (zero) in lia indicates that a SV has been deleted. We delete the
    % corresponding row in the observations
    sats2(lia==0) = [];
    Obs2(lia==0, :) = [];
    NoSV2 = length(sats2);
    % end reading first record in rover file

    % By chance sats1 and sats 2 are the same in the sample data. Otherwise
    % we need to establish  a matching between sats1 and sats2 and
    % likewise for Obs1 and Obs2

    % We compute elevation angles for all sats1 and sats2
    [X_i, el] = recpo_ls(Obs1(:,3), sats1, time1, Eph);
    [X_j0,~] = recpo_ls(Obs2(:,3), sats2, time1, Eph);
    if epoch == 1
        X_j = X_j0(1:3,1);
    end
    % if epoch == 1
    % forcing the choice of reference satellite to be the second highest
    % at the start
    %  sats1
    % refSV = 5
    %  sats1(refSV) = [];
    % end

    if epoch == 1
        % The SV with largest elevation is taken as reference SV
        [y,refSV] = max(el) ;
        old_refSV = refSV;
        fprintf('\n Reference satellite is chosen as  PRN %3.0f\n', sats1(refSV))
        if refSV ~= old_refSV
            fprintf('\n New reference satellite is chosen as  PRN  %3.0f\n\n', sats1(refSV))
            sats1 = union(sats1, old_refSV);
            % no  PRN 12 must be inserted at the entry from where it was deleted!!!
        end
    end
    %The refSV is PRN 12 has index 6 in sats1
    % Finding columns in Eph for each SV
    m = NoSV1;
    col_Eph = zeros(1,m);
    for t = 1:m
        col_Eph(t) = find_eph(Eph,sats1(t),time1+dt1);
    end

    m1 = m-1; % m1 is all sv.s minus refSV
    X_a = [];
    X = zeros(3+2*m1,1);

    % creating a row of indices for all Svs minus the refSV
    t = 1: NoSV1;
    t1 = setdiff(t,refSV);
    % Computation of covariance matrix Sigma for double differenced observations
    D = [ones(m1,1) -eye(m1) -ones(m1,1) eye(m1)];
    Sigma = D*D';

    % A block for estimation of AMBIGUITIES
    % we accumulate the normals for the first 10 epochs
    switch(epoch)

        case {1, 2, 3, 4, 5, 6, 7, 8, 9,10}

            N = zeros(3+2*m1,3+2*m1);	  % initialization of normals
            rs = zeros(3+2*m1,1);	            % initialization of right side

            % Computing rho for refSV
            [~,rhok_j,~] = get_rho(time1, Obs2(refSV,1), Eph(:,col_Eph(refSV)), X_j);
            [~,rhok_i,Xk_ECF] = get_rho(time1, Obs1(refSV,1), Eph(:,col_Eph(refSV)), X_i);

            tt = 0;
            ts = length(t1);
            b = zeros(ts,1);
            bk = zeros(ts,1);
            A1 = [];
            for t = t1
                tt = tt+1;
                [~,rhol_j, ~] = get_rho(time1,Obs2(t,1), Eph(:,col_Eph(t)), X_j);
                [~,rhol_i, Xl_ECF] = get_rho(time1,Obs1(t,1), Eph(:,col_Eph(t)), X_i);
                A0 = [(Xk_ECF(1)-X_j(1))/rhok_j - (Xl_ECF(1)-X_j(1))/rhol_j  ...
                    (Xk_ECF(2)-X_j(2))/rhok_j - (Xl_ECF(2)-X_j(2))/rhol_j ...
                    (Xk_ECF(3)-X_j(3))/rhok_j - (Xl_ECF(3)-X_j(3))/rhol_j];
                A1 = [A1; A0];
                Phi1 = (Obs1(refSV,2)-Obs1(t,2)-Obs2(refSV,2)+Obs2(t,2))*lambda1;
                Phi2 = (Obs1(refSV,4)-Obs1(t,4)-Obs2(refSV,4)+Obs2(t,4))*lambda2;
                b(tt,:) =                   Phi1-lambda1*X(3+tt,1);
                b(length(t1)+tt,:) = Phi2-lambda2*X(3+length(t1)+tt,1);
                bk(tt,:) =                    rhok_i-rhok_j-rhol_i+rhol_j;
                bk(length(t1)+tt,:) =  rhok_i-rhok_j-rhol_i+rhol_j;
            end;
            A_modi = eye(m1);	    	          % modified coefficient matrix
            A_modi(:,refSV) = -ones(m1,1);
            A_aug = [A1 lambda1*A_modi 0*eye(m1); A1 0*eye(m1) lambda2*A_modi];
            N = N+A_aug'*kron(eye(2),Sigma)*A_aug;
            rs = rs+A_aug'*kron(eye(2),Sigma)*(b-bk);

        case 11

            PP = pinv(N);
            % X contains the three baseline components and next the float ambiguities
            X = PP*rs;
            % The next step is to convert the real ambiguities in X (all entries below the
            % fourth component and to the end) into integers. We indicate two algorithms:

            % the Integer Least-Squares (ILS) algorithm
            PP = 0.5*(PP+PP');  %to make PP symmetric
            %[a, sqnorm] = ILS( X(4:4+2*m1-1,1), PP(4:4+2*m1-1,4:4+2*m1-1),2);
            % Correcting to baseline vector as consequence of changing float ambiguities
            % to fixed ones
            % X(1:3,1) = X(1:3,1)-PP(1:3,4:4+2*m1-1)*inv(PP(4:4+2*m1-1,4:4+2*m1-1))*...
            %     (X(4:4+2*m1-1,1)-a(:,1)); %select SECOND  first set of candidates
            % X(4:4+2*m1-1,1) = a(:,1);
            %a = a(:,2); % forces the second set of N values in use

            % the Goad algorithm
            for j = 1:tt
                K1 = round(X(3+tt)-X(3+m1+tt));
                K2 = round(60*X(3+tt)-77*X(3+m1+tt));
                trueN2 = round((60*K1-K2)/17);   % (15.10)
                trueN1 = round(trueN2+K1);         % (15.11)
                amb(j,1:2) = [trueN1 trueN2];
            end
            a =  [amb(:,1);  amb(:,2) ];

            fprintf('\n N1 for PRN %3.0f: %3.0f',[sats1(t1); a(1:m1,1)'])
            fprintf('\n')
            fprintf('\n N2 for PRN %3.0f: %3.0f',[sats1(t1); a(m1+1:2*m1,1)'])
            fprintf('\n')

            % Setting covariances for the Kalman filter; the state vector contains (x,y,z)
            P =  10^8*eye(3);	                       		 % covariances of state vector
            Q = 0.1*eye(3);			                          % covariances of system
            R = 0.1*kron(eye(2),inv(Sigma));	    % covariances of observations
            [phi_j,lambda_j,h_j] = togeod(63781137, 298.257223563, X_j(1), X_j(2), X_j(3));
            h_i = h_j;

        otherwise

            % estimate of baseline components
            tt = 0;  % a counter
            A = zeros(m1,3); %zeros(2*m1,3);
            % Computing rho for refsv
            [torr2,rhok_j,~] = get_rho(time2, Obs2(refSV,3), Eph(:,col_Eph(refSV)), X_j);
            [torr1,rhok_i,Xk_ECF] = get_rho(time1, Obs1(refSV,3), Eph(:,col_Eph(refSV)), X_i);
            for t = t1
                tt = tt+1;
                [torr4,rhol_j, ~] = get_rho(time2,Obs2(t,3), Eph(:,col_Eph(t)), X_j);
                [torr3,rhol_i,Xl_ECF] = get_rho(time1,Obs1(t,3), Eph(:,col_Eph(t)), X_i);
                A0 = [(Xk_ECF(1)-X_j(1))/rhok_j - (Xl_ECF(1)-X_j(1))/rhol_j  ...
                    (Xk_ECF(2)-X_j(2))/rhok_j - (Xl_ECF(2)-X_j(2))/rhol_j ...
                    (Xk_ECF(3)-X_j(3))/rhok_j - (Xl_ECF(3)-X_j(3))/rhol_j];
                A(tt,:) = A0;
                A(m1+tt,:) = A0;
                %time_corr = (torr1-torr2-torr3-torr4);
                % Composing the right side

                % Tropospheric correction using standard meteorological parameters
                %[ ~,el_ki, ~] = topocent(X_i(1:3),Xk_ECF-X_i(1:3));
                %[ ~,el_li,~] = topocent(X_i(1:3),Xl_ECF-X_i(1:3));
                %[ ~,el_kj, ~] = topocent(X_j(1:3),Xk_ECF-X_j(1:3));
                %[az,el_lj,d] = topocent(X_j(1:3),Xl_ECF-X_j(1:3));
                %%el_ki,    el_li,    el_kj,    el_lj
                %t_corr = tropo(sin(el_lj*pi/180),...
                %    h_j*1.e-3,1013,293,50,0,0,0)...
                %    -tropo(sin(el_li*pi/180),....
                %    h_i*1.e-3,1013,293,50,0,0,0)...
                %   -tropo(sin(el_kj*pi/180),...
                %   h_j*1.e-3,1013,293,50,0,0,0)...
                %   +tropo(sin(el_ki*pi/180),...
                %   h_i*1.e-3,1013,293,50,0,0,0);
                t_corr = 0;
                b(tt,:) = (Obs1(refSV,2)-Obs1(t,2)-Obs2(refSV,2)+Obs2(t,2)  ...
                    -a(tt,1))*lambda1-t_corr;
                b(m1+tt,:)  = (Obs1(refSV,4)-Obs1(t,4)-Obs2(refSV,4)+Obs2(t,4) ...
                    -a(m1+tt,1))*lambda2-t_corr;
                bk(tt,:) =         ( rhok_i-rhok_j-rhol_i+rhol_j);
                bk(m1+tt,:) =  (rhok_i-rhok_j-rhol_i+rhol_j);
            end; % t

            % Kalman filter, see Table 8.2 on page 223 in Borre & Strang (2012):
            % Algorithms for Global Positioning, Wellesley-Cambridge Press
            P = P+Q;
            K = P*A'*inv(A*P*A'+R);
            x = x+ K*(b-A*x);
            P = (eye(3)-K*A)*P;
            b_old = b;
            b_acc = [b_acc b];
            x_acc = [x_acc x];
            X_j = X_i(1:3,:)+x;
    end % switch
    if feof(fid1) == 1, break, end;
    if feof(fid2) == 1, break; end;
end % while

% Transformation of geocentric baseline coordinates into topocentric coordinates
e = zeros(1,epoch-11);
n = zeros(1,epoch-11);
u = zeros(1,epoch-11);
for i = 1:epoch-11
    [e(i),n(i),u(i)] = xyz2enu(phi_j,lambda_j,x_acc(1,i),x_acc(2,i),x_acc(3,i));
end

fprintf('\n\nBaseline Components\n')
fprintf('\nX: %8.3f m,  Y: %8.3f m,  Z: %8.3f m\n', ...
    x_acc(1,end), x_acc(2,end), x_acc(3,end))
fprintf('\nE: %8.3f m,  N: %8.3f m,  U: %8.3f m\n',mean(e,2),mean(n,2),mean(u,2))

figure(1);
% we subtract e-, n-, and u-values for epoch 15 to start close to zero!
plot([(e-e(15))' (n-n(15))' (u-u(15))' ]*1000,'linewidth', .1)
title('Baseline Components from Kalman Filter')
ylabel('Baseline Relative to Initial Epoch [mm]','fontsize',14)
xlabel('Epochs [1 s interval]','fontsize',14)
legend('East','North','Up')
set(gca,'fontsize',14)
legend

print -dpdf easy61

figure(2);
plot(x_acc'*1000)
title('Baseline components  {\it(X}, {\itY}, {\itZ})')

ylabel('Absolute components [mm]','fontsize',14)

print -dpdf easy62

%%%%%%%%%% end easy6.m  %%%%%%%


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


function [massImbalance, imBalancedMass, imBalancedCharge, imBalancedRxnBool, elements, missingFormulaeBool, balancedMetBool] = checkMassChargeBalance(model, printLevel, modelName)
% Tests for a list of reactions if these reactions are
% mass-balanced by adding all elements on left hand side and comparing them
% with the sums of elements on the right hand side of the reaction.
% A reaction is considered elementally imbalanced if any of the molecular
% species involved is missing a chemical formula.
%
% USAGE:
%
%    [massImbalance, imBalancedMass, imBalancedCharge, imBalancedRxnBool, elements, missingFormulaeBool, balancedMetBool] = checkMassChargeBalance(model, printLevel, modelName)
%
% INPUT:
%    model:                  COBRA model structure:
%
%                              * .S - `m` x `n` stoichiometric matrix
%                              * .metForumlas - `m` x 1 cell array of metabolite formulas
%                              * .metCharges - `m` x 1 double array of charges
%                              * model.SIntRxnBool - Boolean of reactions heuristically though to be mass balanced. (optional)
%
% OPTIONAL INPUTS:
%    printLevel:             {-1, (0), 1} where:
%
%                            -1 = print out diagnostics on problem reactions to a file,
%                            0 = silent,
%                            1 = print elements as they are checked (display progress),
%                            2 = also print out diagnostics on problem reactions to screen
%    modelName:               name of the file
%
% OUTPUTS:
%    massImbalance:          `nRxn` x `nElement` matrix with mass imbalance
%                            for each element checked. 0 if balanced.
%    imBalancedMass:         `nRxn` x 1 cell with mass imbalance
%                            e.g. -3 H means three hydrogens disappear in the reaction.
%    imBalancedCharge:       `nRxn` x 1 vector with charge imbalance, empty if no imbalanced reactions
%    imBalancedRxnBool:      boolean vector indicating imbalanced reactions (including exchange reactions!)
%    elements:               `nElement` x 1 cell array of element abbreviations checked
%    missingFormulaeBool:    `nMet` x 1 boolean vector indicating metabolites without formulae
%    balancedMetBool:        boolean vector indicating metabolites exclusively involved in balanced reactions
%
%
% OPTIONAL OUTPUT FILES:
% modelName_mass_imbalanced_reactions.txt  provides a human readable summary
% for each mass imbalanced reaction.
%
% For each reaction it contains:
% j             the index of the column of the stoichiometric matrix corresponding to the reaction
% rxns{j}       the abbreviation of the reaction
% imbalance     the number of each element that is unbalanced
% equation      the reaction equation
%
% For each metabolite involved in a reaction it contains
% i               the index of the row of the stoichiometric matrix corresponding to the metabolite
% mets{i}         the abbreviation of the reaction
% S(i,j)          the stoichiometric coefficient
% metFormulas{i}  the chemical formula of the metabolite
%
% Example
% #Rxn;rxnAbbr;imbalance;equation
% 32;2AMACSULT;3 O, 1 S;2amac[c] + nadph[c] + paps[c]  -> nadp[c] + Lcyst[c] + pap[c]
%    58	       2amac[c]	-1	C3H5NO2
%    60	       nadph[c]	-1	C21H26N7O17P3
%    61	        nadp[c]	1	C21H25N7O17P3
%    62	        paps[c]	-1	C10H11N5O10P2
%    63	       Lcyst[c]	1	C3H6NO5S
%    64	         pap[c]	1	C10H11N5O10P2
%
% .. Authors:
%       - Ines Thiele 12/09
%       - IT, 06/10, Corrected some bugs and improved speed.
%       - RF, 09/09/10, Support for very large models and printing to file.
%       - RF, 18/12/14, Default is now to check balancing of all reactions.

[nMet, nRxn]=size(model.S);

if ~exist('printLevel', 'var')
    printLevel=0;
end

if ~isfield(model,'SIntRxnBool')
    model.SIntRxnBool=true(nRxn,1);%assume all reactions are supposed to be internal if no other info provided
end

if ~exist('modelName','var')
    modelName='';
end
if ~isfield(model,'rxns')
    for i=1:nRxn
        model.rxns{i,1}=['rxn' int2str(i)];
    end
end
% List of elements
elements = {'H','C', 'O', 'P', 'S', 'N', 'Mg','X','Fe','Zn','Co','Ca','Y','I','Na','Cl','K','R','FULLR'};

E=sparse(nMet, length(elements));
massImbalance=zeros(nRxn, length(elements));
missingFormulaeBool=cellfun(@isempty, model.metFormulas);
for j = 1 : length(elements)
    if j==1
        [dE, E_el, missingFormulaeBool]=checkBalance(model, elements{j}, printLevel,[],missingFormulaeBool);
        massImbalance(:, j)=dE;
        E(:, j)=E_el;
        if printLevel>0
            fprintf('%s\n', ['Checked element ' elements{j}]);
        end
    else
        %no need to print out for each element which metabolites have no
        %formula
        [massImbalance(:, j), E(:, j)]=checkBalance(model, elements{j}, 0);
        if printLevel>0
            fprintf('%s\n', ['Checking element ' elements{j}]);
        end
    end
end

%ignore mass imbalance of exchange reactions if the internal reactions have
%been identified at the input
if any(~model.SIntRxnBool)
    massImbalance(~model.SIntRxnBool,:)=1;
end

E=full(E);

% A reaction is considered elementally imbalanced if any of the molecular
% species involved is missing a chemical formula.
imBalancedRxnBool=any(massImbalance, 2) | any(model.S(missingFormulaeBool, :),1)';

imBalancedMass=cell(nRxn, 1);
for i = 1 : nRxn
    imBalancedMass{i, 1}='';
    if imBalancedRxnBool(i)
        for j = 1 : length(elements)
            if massImbalance(i, j) ~= 0
                if ~strcmp(imBalancedMass{i, 1}, '')
                    imBalancedMass{i, 1} = [imBalancedMass{i, 1} ', ' int2str(massImbalance(i, j)) ' ' elements{j}];
                else
                    imBalancedMass{i, 1} = [int2str(massImbalance(i, j)) ' ' elements{j}];
                end
            end
        end
        if strfind(imBalancedMass{i, 1}, 'NaN')
            imBalancedMass{i, 1}='NaN';
        end
    end
    if mod(i, 1000)==0
        fprintf('%n\t%s\n', i, ['reactions checked for ' elements{j} ' balance']);
    end
end
if printLevel==-1
    firstMissing=0;
    for p=1:nRxn
        %only print out for reactions supposed to be mass balanced
        if model.SIntRxnBool(p) && ~strcmp(imBalancedMass{p,1},'')
            %at the moment, ignore reactions with a metabolite that have
            %no formula
            if ~strcmp(imBalancedMass{p, 1}, 'NaN')
                if ~firstMissing
                    fid=fopen([modelName 'mass_imbalanced_reactions.txt'],'w');
                    fprintf(fid, '%s;%s;%s;%s\n', 'j', 'rxns{j}', 'imbalance', 'equation');
                    fprintf(fid, '%s;%s;%s;%s\n\n', 'i', 'mets{i}', 'S(i,j)', 'metFormulas{i}');
                    fprintf('%s\n',['There are mass imbalanced reactions, see ' modelName 'mass_imbalanced_reactions.txt'])
                    firstMissing=1;
                end
                equation=printRxnFormula(model, model.rxns(p), 0);
                fprintf(fid, '%s;%s;%s;%s\n', int2str(p), model.rxns{p}, imBalancedMass{p, 1}, equation{1});
                for m=1:size(model.S, 1)
                    if model.S(m, p) ~= 0
                        fprintf(fid,'%5u\t%15s\t%g\t%s\n',m,model.mets{m},full(model.S(m,p)),model.metFormulas{m});
                    end
                end
            end
        end
    end
    if firstMissing
        fclose(fid);
    end
end
if printLevel==2
    for p=1:nRxn
        %only print out for reactions supposed to be mass balanced
        if model.SIntRxnBool(p) && ~strcmp(imBalancedMass{p,1},'')
            %at the moment, ignore reactions with a metabolite that have
            %no formula
            if ~strcmp(imBalancedMass{p, 1}, 'NaN')
                equation=printRxnFormula(model, model.rxns(p), 0);
                fprintf('%6s\t%30s\t%10s\t%s\n', int2str(p), model.rxns{p}, imBalancedMass{p, 1}, equation{1});
                % for m=1:size(model.S, 1)
                %     if model.S(m, p) ~= 0
                %         fprintf('%5u\t%15s\t%g\t%s\n',m,model.mets{m},full(model.S(m,p)),model.metFormulas{m});
                %     end
                % end

            end
        end
    end
end

%
if nnz(strcmp('', imBalancedMass))==nRxn
    imBalancedMass=[];
end

% Check for charge balance (initialize with NaN, if the fields are not set
% this will make it clear.
imBalancedCharge = NaN * ones(nRxn, 1);
firstMissing=0;
if isfield(model, 'metCharges')
    for m=1:nMet
        if isnan(model.metCharges(m)) && ~isempty(model.metFormulas{m})
            if printLevel==2
                fprintf('%s\t%s\n', int2str(m), [model.mets{m} ' has no charge but has formula.'])
                if ~firstMissing
                    warning('model structure must contain model.metCharges field for each metabolite');
                end
                firstMissing=1;
            end
            if printLevel==-1
                if ~firstMissing
                    fid=fopen([modelName 'metabolites_without_charge.txt'],'w');
                end
                firstMissing=1;
                fprintf(fid, '%s\t%s\n', int2str(m), model.mets{m});
            end
        end
    end

    imBalancedCharge = model.S' * double(model.metCharges); % Matlab does not support this operation on two int values - one needs to be converted to double. The smaller matrix is selected.
end

if printLevel==-1 && isfield(model,'SIntRxnBool')
    firstMissing=0;
    if any(imBalancedCharge)
        for q=1:nRxn
            if model.SIntRxnBool(q) && strcmp(imBalancedMass{q, 1}, '') && imBalancedCharge(q) ~= 0
                if ~firstMissing
                    fid=fopen([modelName 'mass_balanced_charge_imbalanced_reactions.txt'],'w');
                    fprintf('%s\n',['There are mass balanced, but charge imbalanced reactions, see ' modelName 'charge_imbalanced_reactions.txt'])
                    firstMissing=1;
                end
                equation=printRxnFormula(model, model.rxns(q), 0);
                fprintf(fid, '%s\t%s\t%s\n', int2str(q), model.rxns{q}, equation{1});
                % for m=1:size(model.S, 1)
                %     if model.S(m, q) ~= 0
                %         fprintf(fid,'%5u\t%15s\t%g\t%g\t%s\n',m,model.mets{m},full(model.S(m,q)),model.metCharges(m),model.metFormulas{m});
                %     end
                % end
            end
        end
        if firstMissing
            fclose(fid);
        end
    end
end

if printLevel==2 && isfield(model,'SIntRxnBool')
    if any(imBalancedCharge)
        fprintf('%s\n', 'Mass balanced, but charged imbalanced reactions:')
        for q=1:nRxn
            if model.SIntRxnBool(q) && strcmp(imBalancedMass{p, 1}, '') && imBalancedCharge(q) ~= 0
                equation=printRxnFormula(model, model.rxns(q), 0);
                fprintf('%s\t%s\t%s\n', int2str(q), model.rxns{q}, equation{1});
                for m=1:size(model.S, 1)
                    if model.S(m, q) ~= 0
                        fprintf('%5u\t%15s\t%g\t%g\t%s\n',m,model.mets{m},full(model.S(m,q)),model.metCharges(m),model.metFormulas{m});
                    end
                end
            end
        end
    end
end

%If the field is set we should assume, that all values are defined.
if isfield(model, 'metCharges')
    imBalancedRxnBool = imBalancedRxnBool | imBalancedCharge ~= 0;
end

if isfield(model,'SIntRxnBool')
    imBalancedRxnBool = imBalancedRxnBool | ~model.SIntRxnBool;
end

%nonzero rows corresponding to completely mass balanced reactions
balancedMetBool = getCorrespondingRows(model.S,true(nMet,1),~imBalancedRxnBool,'exclusive');
massImbalance = sparse(massImbalance);


function DVH_out = DVHwithShifts(planC,structNum,doseNum)
%function DVH_out = DVHwithShifts(planC,structNum,doseNum)
%
%APA, 11/09/2010
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

%Number of Fractions and Trials
numFractions = 30;
numTrials    = 100;

% Systematic Shifts in cm (Inter-treatment)
XdispSys_Std = 0;
YdispSys_Std = 0;
ZdispSys_Std = 0;

% Random Shifts in cm (inter-fraction)
XdispRnd_Std = 1;
YdispRnd_Std = 1;
ZdispRnd_Std = 1;

indexS = planC{end};

%Distances are in cm
XpdfMean = 0;

YpdfMean = 0;

ZpdfMean = 0;

%Get scan associated with doseNum
scanNum = getAssociatedScan(planC{indexS.dose}(doseNum).assocScanUID, planC);

if isempty(scanNum) %Assume dose is associated with this scan
    scanNum = getStructureAssociatedScan(structNum,planC);
end

%Get reference transformation matrix for doseNum
if ~isempty(scanNum) && isempty(planC{indexS.dose}(doseNum).transM)
    referenceTransM = planC{indexS.scan}(scanNum).transM;
else
    referenceTransM = planC{indexS.dose}(doseNum).transM;
end

%Store the reference doseArray
referenceDoseArray = planC{indexS.dose}(doseNum).doseArray;

%Try and get a binWidth from stateS.  If it doesnt exist, get it from
%the CERROptions file (allows this function to be called outside CERR)
global stateS;
if ~isempty(stateS) && isfield(stateS, 'optS') && isfield(stateS.optS, 'DVHBinWidth') && ~isempty(stateS.optS.DVHBinWidth)
    binWidth = stateS.optS.DVHBinWidth;
else
    optS = CERROptions;
    binWidth = optS.DVHBinWidth;
end

%Compute DVH at given dose
[dosesCurrentV, volsV] = getDVH(structNum, doseNum, planC);
[doseBinsCurrentV, volsHistCurrentV] = doseHist(dosesCurrentV, volsV, binWidth);

%Divide doseArray into fractions
planC{indexS.dose}(doseNum).doseArray = planC{indexS.dose}(doseNum).doseArray / numFractions;

%Get systematic errors
deltaX_systematic = randn(1, numTrials) * XdispSys_Std + XpdfMean;
deltaY_systematic = randn(1, numTrials) * YdispSys_Std + YpdfMean;
deltaZ_systematic = randn(1, numTrials) * ZdispSys_Std + ZpdfMean;

hWait = waitbar(0,'Computing plan robustness...');

if isempty(referenceTransM)
    referenceTransM_matrix = eye(4);
else
    referenceTransM_matrix = referenceTransM;
end

%Obtain DVH calculation points in blocks
optS    = planC{indexS.CERROptions};

ROIImageSize = [planC{indexS.scan}(scanNum).scanInfo(1).sizeOfDimension1  planC{indexS.scan}(scanNum).scanInfo(1).sizeOfDimension2];

deltaY = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;

%Get raster segments for structure.
[segmentsM, planC, isError] = getRasterSegments(structNum, planC);

if isempty(segmentsM)
    isError = 1;
end
numSegs = size(segmentsM,1);

%Relative sampling of ROI voxels in this place, compared to CT spacing.
%Set when rasterSegments are generated (usually on import).
sampleRate = optS.ROISampleRate;

%Sample the rows
indFullV =  1 : numSegs;
if sampleRate ~= 1
    rV = 1 : length(indFullV);
    rV([rem(rV+sampleRate-1,sampleRate)~=0]) = [];
    indFullV = rV;
end

%Block process to avoid swamping on large structures
if isfield(optS, 'DVHBlockSize') & ~isempty(optS.DVHBlockSize)
    DVHBlockSize = optS.DVHBlockSize;
else
    DVHBlockSize = 5000;
end

blocks = ceil(length(indFullV)/DVHBlockSize);

start = 1;

for b = 1 : blocks

  %Build the interpolation points matrix

  dummy = zeros(1,DVHBlockSize * ROIImageSize(1));
  x1V = dummy;
  y1V = dummy;
  z1V = dummy;
  volsSectionV =  dummy;

  if start+DVHBlockSize > length(indFullV)
    stop = length(indFullV);
  else
    stop = start + DVHBlockSize - 1;
  end

  indV = indFullV(start:stop);

  mark = 1;
  for i = indV

    tmpV = segmentsM(i,1:10);
    delta = tmpV(5) * sampleRate;
    xV = tmpV(3): delta : tmpV(4);
    len = length(xV);
    rangeV = ones(1,len);
    yV = tmpV(2) * rangeV;
    zV = tmpV(1) * rangeV;
    sliceThickness = tmpV(10);
    %v = delta^2 * sliceThickness;
    v = delta * (deltaY*sampleRate) * sliceThickness;
    x1V(mark : mark + len - 1) = xV;
    y1V(mark : mark + len - 1) = yV;
    z1V(mark : mark + len - 1) = zV;
    volsSectionV(mark : mark + len - 1) = v;
    mark = mark + len;

  end

  %cut unused matrix elements
  x1V = x1V(1:mark-1);
  y1V = y1V(1:mark-1);
  z1V = z1V(1:mark-1);
  volsSectionV = volsSectionV(1:mark-1);

  %Get transformation matrices for both dose and structure.
  transMDose    = getTransM('dose', doseNum, planC);
  transMStruct  = getTransM('struct', structNum, planC);

  %Forward transform the structure's coordinates.
  if ~isempty(transMStruct)
      [x1V, y1V, z1V] = applyTransM(transMStruct, x1V, y1V, z1V);
  end

  dvhCalcPtsC{b} = [x1V', y1V', z1V'];

  %Interpolate.
%   [dosesSectionV] = getDoseAt(doseNum, x1V, y1V, z1V, planC);

%   dosesV = [dosesV, dosesSectionV];
%   volsV  = [volsV, volsSectionV];

  start = stop + 1;

end

%Loop over to generate DVH
for iTrial = 1:numTrials

    deltaX = deltaX_systematic(iTrial) + randn(1, numFractions) * XdispRnd_Std + XpdfMean;
    deltaY = deltaY_systematic(iTrial) + randn(1, numFractions) * YdispRnd_Std + YpdfMean;
    deltaZ = deltaZ_systematic(iTrial) + randn(1, numFractions) * ZdispRnd_Std + ZpdfMean;

    tmpDoseV = zeros(1,length(dosesCurrentV));

    for iFraction = 1:numFractions

        waitbar(((iTrial-1)*numFractions + iFraction)/(numTrials*numFractions),hWait)

        transM = referenceTransM_matrix;
        transM(1:3,4) = transM(1:3,4) + [deltaX(iFraction); deltaY(iFraction); deltaZ(iFraction)];

        %Apply the new transM to dose
        planC{indexS.dose}(doseNum).transM = transM;

        %Get doses and volumes of points in structure.
        %[dosesV, volsV] = getDVH(structNum, doseNum, planC);

        volsV  = [];
        dosesV = [];

        for b = 1 : blocks
            x1V = dvhCalcPtsC{b}(:,1);
            y1V = dvhCalcPtsC{b}(:,2);
            z1V = dvhCalcPtsC{b}(:,3);

            %Back transform the coordinates into the doses' coordinate system.
            [x1V, y1V, z1V] = applyTransM(inv(transM), x1V, y1V, z1V);

            %Interpolate.
            [dosesSectionV] = getDoseAt(doseNum, x1V, y1V, z1V, planC);

            dosesV = [dosesV, dosesSectionV];
            volsV  = [volsV, volsSectionV];
        end

        tmpDoseV = tmpDoseV + dosesV;

    end

    DVHm(:,iTrial) = single(tmpDoseV(:));

    %Compute Mean and Std for binned dose
    %for iTrial=1:numTrials
        doseTrialV = DVHm(:,iTrial);
        [doseBinsV, volsHistV] = doseHist(doseTrialV, volsV, binWidth);
        doseBinsTmpV = doseBinsV;
        volsHistTmpV = volsHistV;
        if iTrial > 1
            AddToCurrent = ~ismember(doseBins{1},doseBinsV);
            AddToPrevious = ~ismember(doseBinsV,doseBins{1});
            doseBinsV = [doseBinsV doseBins{1}(AddToCurrent)];
            volsHistV = [volsHistV volsHist{1}(AddToCurrent)];
            for jTrial = 1:iTrial-1
                doseBins{jTrial} = [doseBins{jTrial} doseBinsTmpV(AddToPrevious)];
                volsHist{jTrial} = [volsHist{jTrial} volsHistTmpV(AddToPrevious)];
                [doseBins{jTrial},indSort] = sort(doseBins{jTrial});
                volsHist{jTrial} = volsHist{jTrial}(indSort);
            end
        end
        doseBins{iTrial} = doseBinsV;
        volsHist{iTrial} = volsHistV;
        [doseBins{iTrial},indSort] = sort(doseBins{iTrial});
        volsHist{iTrial} = volsHist{iTrial}(indSort);
    %end

end

close(hWait)

%Compute Std of binned volumes
blockSize = 100;
for iBlock = 1:ceil(length(volsHist{1})/blockSize)
    if iBlock == ceil(length(volsHist{1})/blockSize)
        indicesV = (iBlock-1)*blockSize+1:length(volsHist{1});
    else
        indicesV = (iBlock-1)*blockSize+1:iBlock*blockSize;
    end
    volsHistStdM = [];
    for iTrial = 1:length(volsHist)
        volsHistStdM(:,iTrial) = volsHist{iTrial}(indicesV);
    end
    volsHistStdV(indicesV) = std(volsHistStdM,0,2)';
end

%Reassign reference transM and doseArray to doseNum
planC{indexS.dose}(doseNum).transM = referenceTransM;
planC{indexS.dose}(doseNum).doseArray = referenceDoseArray;

%Compute Mean and Std Dev in blocks
numVoxels = length(volsV);
DVHBlockSize = 5000;
blocks = ceil(numVoxels/DVHBlockSize);
start = 1;

for b = 1 : blocks

  if start+DVHBlockSize > numVoxels
    stop = numVoxels;
  else
    stop = start + DVHBlockSize - 1;
  end

  DVH_block = DVHm(start:stop,:);

  meanDoseV(start:stop) = mean(DVH_block,2);
  stdDoseV(start:stop)  = std(DVH_block,0,2);

  start = stop + 1;

end

DVH_out = [meanDoseV(:)'; volsV(:)'];


function isodoseClosedContour = getBeamCrossSection()
%isodoseClosedContour50 = getBeamCrossSection()
%
%Output: isodoseClosedContour - representation of beam contour
%dimension (3xN). 1st, 2nd and 3rd row represent x, y and z-coords of the
%contour respectively. It is assumed that the ray starts at (0,0,0) and
%moves along +(ve) x-axis. If collimatorType does not match the list, an
%empty isodoseClosedContour50 is returned.
%
%APA 01/21/09
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

% Get a 10x10 section at 100cm distance. In future, these points should
% come from Leaf-sequences or beamlet-fluence map.
isodoseClosedContour(3,:) = [-10 10 10 -10 -10]/2;
isodoseClosedContour(2,:) = [-10 -10 10 10 -10]/2;
isodoseClosedContour(1,:) = [100 100 100 100 100];
return


function rtk=udclk_ppp(rtk)

global glc
opt=rtk.opt; VAR_CLK=60^2; CLIGHT=glc.CLIGHT; tt=abs(rtk.tt);
navsys=opt.navsys; mask=rtk.mask; prn=0;

% for single-system except GPS
if strcmp(navsys,'R')
    dtr=rtk.sol.dtr(2);
    if abs(dtr)<=1e-16,dtr=1e-16;end
    rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+2);

    % for GLONASS icb
    if opt.gloicb==glc.GLOICB_LNF
        if rtk.x(rtk.iicb+1)==0
            rtk=initx(rtk,0.1,VAR_CLK,rtk.iicb+1);
        end
    elseif opt.gloicb==glc.GLOICB_QUAD
        if rtk.x(rtk.iicb+1)==0
            rtk=initx(rtk,0.1,VAR_CLK,rtk.iicb+1);
        end
        if rtk.x(rtk.iicb+2)==0
            rtk=initx(rtk,0.1,VAR_CLK,rtk.iicb+2);
        end
    end
    return
elseif strcmp(navsys,'E')
    dtr=rtk.sol.dtr(3);
    if abs(dtr)<=1e-16,dtr=1e-16;end
    rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+3);
    return
elseif strcmp(navsys,'C')
    dtr=rtk.sol.dtr(4);
    if abs(dtr)<=1e-16,dtr=1e-16;end
    rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+4);
    return
elseif strcmp(navsys,'J')
    dtr=rtk.sol.dtr(5);
    if abs(dtr)<=1e-16,dtr=1e-16;end
    rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+5);
    return
end

% for GPS or multi-system
dtr=rtk.sol.dtr(1);
if abs(dtr)<=1e-16,dtr=1e-16;end
rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+1);
for i=2:glc.NSYS
    if mask(i)==0,continue;end
    if i==glc.SYS_GLO
        dtr=rtk.sol.dtr(2);
        if rtk.x(rtk.ic+2)==0
            if abs(dtr)<=1e-16,dtr=1e-16;end
            rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+2);
        else
            rtk.P(rtk.ic+i,rtk.ic+i)=rtk.P(rtk.ic+i,rtk.ic+i)+prn^2*tt;
        end
        % for GLONASS icb
        if opt.gloicb==glc.GLOICB_LNF
            if rtk.x(rtk.iicb+1)==0
                rtk=initx(rtk,0.1,VAR_CLK,rtk.iicb+1);
            end
        elseif opt.gloicb==glc.GLOICB_QUAD
            if rtk.x(rtk.iicb+1)==0
                rtk=initx(rtk,0.1,VAR_CLK,rtk.iicb+1);
            end
            if rtk.x(rtk.iicb+2)==0
                rtk=initx(rtk,0.1,VAR_CLK,rtk.iicb+2);
            end
        end
    else
        dtr=rtk.sol.dtr(i);
        if rtk.x(rtk.ic+i)==0
            if abs(dtr)<=1e-16,dtr=1e-16;end
            rtk=initx(rtk,CLIGHT*dtr,VAR_CLK,rtk.ic+i);
        else
            rtk.P(rtk.ic+i,rtk.ic+i)=rtk.P(rtk.ic+i,rtk.ic+i)+prn^2*tt;
        end
    end
end

return


%%*******************************************************************
%% NTrhsfun: compute the right-hand side vector of the
%%           Schur complement equation for the NT direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [rhs,EinvRc,hRd] = NTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu,hRd,dX,dZ)

spdensity = par.spdensity;
m = length(rp);
if (nargin > 8)
    corrector = 1;
else
    corrector = 0;
    hRd = zeros(m,1);
end
hEinvRc = zeros(m,1);
EinvRc  = cell(size(blk,1),1);
rhsfree = [];
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});  numblk = length(pblk{2});
    if strcmp(pblk{1},'l')
        if iscell(sigmu)
            EinvRc{p} = sigmu{p}./Z{p} -X{p};
        else
            EinvRc{p} = sigmu./Z{p} -X{p};
        end
        Rq = sparse(n,1);
        if (corrector) && (norm(par.parbarrier{p})==0)
            Rq = dX{p}.*dZ{p}./Z{p};
        else
            tmp  = par.dd{p}.*Rd{p};
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'q')
        if iscell(sigmu)
            EinvRc{p} = qops(pblk,-sigmu{p}./(par.gamz{p}.*par.gamz{p}),Z{p},4) -X{p};
        else
            EinvRc{p} = qops(pblk,-sigmu./(par.gamz{p}.*par.gamz{p}),Z{p},4) -X{p};
        end
        Rq = sparse(n,1);
        if (corrector) && (norm(par.parbarrier{p})==0)
            w = sqrt(par.gamz{p}./par.gamx{p});
            hdx = qops(pblk,w,par.ff{p},5,dX{p});
            hdz = qops(pblk,w,par.ff{p},6,dZ{p});
            hdxdz = Arrow(pblk,hdx,hdz);
            vv = qops(pblk,w,par.ff{p},5,X{p});
            Vihdxdz = Arrow(pblk,vv,hdxdz,1);
            Rq = qops(pblk,w,par.ff{p},6,Vihdxdz);
        else
            tmp  = par.dd{p}.*Rd{p} + qops(pblk,qops(pblk,Rd{p},par.ee{p},1),par.ee{p},3);
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'s')
        n2 = pblk{2}.*(pblk{2}+1)/2;
        if iscell(sigmu)
            %%ss = [0,cumsum(pblk{2})];
            %%sigmuvec = zeros(n,1);
            %%for k = 1:length(pblk{2});
            %%   sigmuvec(ss(k)+1:ss(k+1)) = sigmu{p}(k)*ones(pblk{2}(k),1);
            %%end
            sigmuvec = mexexpand(pblk{2},sigmu{p});
            tmp = spdiags(sigmuvec./par.sv{p} -par.sv{p},0,n,n);
        else
            tmp = spdiags(sigmu./par.sv{p} -par.sv{p},0,n,n);
        end
        EinvRc{p} = Prod3(pblk,par.G{p}',tmp,par.G{p},1);
        Rq = sparse(n,n);
        if (corrector) && (norm(par.parbarrier{p})==0)
            hdZ = Prod3(pblk,par.G{p},dZ{p},par.G{p}',1);
            hdX = spdiags(qops(pblk,par.parbarrier{p}',1./par.sv{p},3)-par.sv{p},0,n,n)-hdZ;
            tmp = Prod2(pblk,hdX,hdZ,0);
            tmp = 0.5*(tmp+tmp');
            if (numblk == 1)
                d = par.sv{p};
                e = ones(pblk{2},1);
                Rq = 2*tmp./(d*e'+e*d');
                if (nnz(Rq) <= spdensity*n2); Rq = sparse(Rq); end
            else
                Rq = sparse(n,n);
                ss = [0, cumsum(pblk{2})];
                for i = 1:numblk
                    pos = ss(i)+1 : ss(i+1);
                    d = par.sv{p}(pos); e = ones(length(pos),1);
                    Rq(pos,pos) = 2*tmp(pos,pos)./(d*e' + e*d'); %#ok
                end
            end
            Rq = Prod3(pblk,par.G{p}',Rq,par.G{p},1);
        else
            tmp = Prod3(pblk,par.W{p},Rd{p},par.W{p},1,par.nzlistAy{p});
            tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),{tmp});
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'u')
        rhsfree = [rhsfree; Rd{p}]; %#ok
    end
end
%%
rhs = rp + hRd - hEinvRc;
rhs = full([rhs; rhsfree]);
%%*******************************************************************