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


clear
% close all
clc;

addpath(genpath('../libs'))
addpath('./myFunc')
% path_to_matconvnet = '../matconvnet';
path_to_matconvnet = '../matconvnet_std';
path_to_matconvnet = '/home/skong2/scratch/matconvnet-1.0-beta23_modifiedDagnn';
path_to_model = '../models/';

run(fullfile(path_to_matconvnet, 'matlab', 'vl_setupnn'));
addpath(genpath(fullfile('dependencies', 'matconvnet','examples')));
%% read matconvnet model
load('imdb_toydata_v3_from_mnist.mat');
% imdb.path_to_dataset = '/run/shm/toydata';

% set GPU
gpuId = 2; %[1, 2];
gpuDevice(gpuId);
flagSaveFig = true; % {true false} whether to store the result

saveFolder = 'main007_instSeg_v1_absEucMM';
modelName = 'softmax_net-epoch-83.mat';

saveFolder = 'main007_instSeg_v2_absEucMM_invAlpha';
modelName = 'softmax_net-epoch-83.mat';

%saveFolder = 'main007_instSeg_v3_absEucMM_adaptMM';
%modelName = 'softmax_net-epoch-92.mat';

%% modify network for testing
netMat = load( fullfile('./exp', saveFolder, modelName) );
netMat = netMat.net;
netMat = dagnn.DagNN.loadobj(netMat);

% RFinfo = netMat.getVarReceptiveFields('input');

rmLayerName = 'obj_instSeg_MM';
if ~isnan(netMat.getLayerIndex(rmLayerName))
    netMat.removeLayer(rmLayerName); % remove layer
end
rmLayerName = 'obj_instSeg';
if ~isnan(netMat.getLayerIndex(rmLayerName))
    netMat.removeLayer(rmLayerName); % remove layer
end
rmLayerName = 'obj_instSeg_reg';
if ~isnan(netMat.getLayerIndex(rmLayerName))
    netMat.removeLayer(rmLayerName); % remove layer
end
netMat.vars(netMat.layers(netMat.getLayerIndex('res7_l2norm')).outputIndexes).precious = 1;
%% configure training environment
saveFolder = [strrep(saveFolder, '/', '') '_visualization'];
if ~isdir(saveFolder) && flagSaveFig
    mkdir(saveFolder);
end
netMat.move('gpu');
% netMat.mode = 'test';
netMat.mode = 'normal';
testList = find(imdb.set==2);
rawCountsAll = {};

imgMat = [];
semanticMaskMat = [];
instanceMaskMat = [];
predInstanceMaskMat = [];
st = 0;
for i = 1:5%length(testList)
    curBatch = fullfile(imdb.path_to_dataset, imdb.imgList{testList(i)});
    datamat = load(curBatch);

%     datamat.imgMat = reshape(datamat.imgMat, [size(datamat.imgMat,1), size(datamat.imgMat,2), 1, size(datamat.imgMat,3)]);
    datamat.semanticMaskMat = reshape(datamat.semanticMaskMat, [size(datamat.semanticMaskMat,1), size(datamat.semanticMaskMat,2), 1, size(datamat.semanticMaskMat,3)]);
    datamat.instanceMaskMat = reshape(datamat.instanceMaskMat, [size(datamat.instanceMaskMat,1), size(datamat.instanceMaskMat,2), 1, size(datamat.instanceMaskMat,3)]);

    imFeed = bsxfun(@minus, datamat.imgMat, imdb.meta.meanvalue);
    inputs = {'input', gpuArray(imFeed)};
    netMat.eval(inputs) ;
    res7_l2norm = gather(netMat.vars(netMat.layers(netMat.getLayerIndex('res7_l2norm')).outputIndexes).value);

    imgMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.imgMat;
    semanticMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.semanticMaskMat;
    instanceMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.instanceMaskMat;
    predInstanceMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = res7_l2norm;
    fprintf('\t%d/%d\n', i, length(testList));

    st = st + size(datamat.imgMat,4);
end
%% visualize
num = 25;
M = 5;
N = 5;
h = 28;
w = 28;
panelSZ = round(64/2)*2;
stepSize = (panelSZ-h)/2;

showcaseSamples_R = imgMat(:,:,1,1:num);
showcaseSamples_R = reshape(showcaseSamples_R, [numel(showcaseSamples_R(:,:,1,1)), num]);
showcaseSamples_R = showStitch(showcaseSamples_R, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);
showcaseSamples_G = imgMat(:,:,2,1:num);
showcaseSamples_G = reshape(showcaseSamples_G, [numel(showcaseSamples_G(:,:,1,1)), num]);
showcaseSamples_G = showStitch(showcaseSamples_G, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);
showcaseSamples_B = imgMat(:,:,3,1:num);
showcaseSamples_B = reshape(showcaseSamples_B, [numel(showcaseSamples_B(:,:,1,1)), num]);
showcaseSamples_B = showStitch(showcaseSamples_B, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);
showcaseSamples = cat(3, showcaseSamples_R, showcaseSamples_G, showcaseSamples_B);

showcaseSemanticMask= semanticMaskMat(:,:,:,1:num);
showcaseSemanticMask = reshape(showcaseSemanticMask, [numel(showcaseSemanticMask(:,:,1,1)), num]);
showcaseSemanticMask = showStitch(showcaseSemanticMask, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);

showcaseInstanceMask= instanceMaskMat(:,:,:,1:num);
showcaseInstanceMask = reshape(showcaseInstanceMask, [numel(showcaseInstanceMask(:,:,1,1)), num]);
showcaseInstanceMask = showStitch(showcaseInstanceMask, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);

predInstanceMask_R = predInstanceMaskMat(:,:,1,1:num);
predInstanceMask_R = reshape(predInstanceMask_R, [numel(predInstanceMask_R(:,:,1,1)), num]);
predInstanceMask_R = showStitch(predInstanceMask_R, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);
predInstanceMask_G = predInstanceMaskMat(:,:,2,1:num);
predInstanceMask_G = reshape(predInstanceMask_G, [numel(predInstanceMask_G(:,:,1,1)), num]);
predInstanceMask_G = showStitch(predInstanceMask_G, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);
predInstanceMask_B = predInstanceMaskMat(:,:,3,1:num);
predInstanceMask_B = reshape(predInstanceMask_B, [numel(predInstanceMask_B(:,:,1,1)), num]);
predInstanceMask_B = showStitch(predInstanceMask_B, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);
predInstanceMask = cat(3, predInstanceMask_R, predInstanceMask_G, predInstanceMask_B);

predInstanceMask = predInstanceMask - min(predInstanceMask(:));
predInstanceMask = predInstanceMask ./ max(predInstanceMask(:));

imgFig2 = figure(2);
subWindowH = 2;
subWindowW = 2;
set(imgFig2, 'Position', [100 100 1400 900]) % [1 1 width height]
windowID = 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseSamples); axis off image; title('random samples'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseSemanticMask); axis off image; title('showcase SemanticMask'); caxis([0, 11]); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseInstanceMask); axis off image; title('showcase instanceMask'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMask); axis off image; title('predInstanceMask'); colorbar; windowID = windowID + 1;

if flagSaveFig
    showcaseSemanticMask = uint8(showcaseSemanticMask * 255/11);
    showcaseInstanceMask = showcaseInstanceMask*255/5;

    imwrite(showcaseSamples, fullfile(saveFolder, sprintf('%04d_showcaseSamples.bmp',i)));
    imwrite(showcaseSemanticMask, fullfile(saveFolder, sprintf('%04d_showcaseSemanticMask.bmp',i)));
    imwrite(showcaseInstanceMask, fullfile(saveFolder, sprintf('%04d_showcaseInstanceMask.bmp',i)));
    imwrite(predInstanceMask, fullfile(saveFolder, sprintf('%04d_predInstanceMask.bmp',i)));

    export_fig( sprintf('%s/%04d_visualization.jpg', saveFolder, i) );
    save(sprintf('%s/results.mat', saveFolder), 'imgMat', 'instanceMaskMat', 'semanticMaskMat', 'predInstanceMaskMat');
end

%% leaving blank


function [ Pos ] = getEEFCartesianOrientation( t )
%% This function is used to get the endeffector cartizian orientation of the KUKA iiwa 7 R 800.

%% Syntax:
% [ Ori ] = getEEFCartisianOrientation( t )

%% About:
% This function is used to get the endeffector cartizian orientation of the KUKA iiwa 7 R 800.
% The orientation is of the media flange of the robot relative to the base
% frame of the robot.

%% Arreguments:
% t: is the TCP/IP connection
% Ori: is 1x3 cell array,  alpha,beta and gama angles of the endeffector,
% the angles are measured in radians.

% Copyright, Mohammad SAFEEA, 3rd of May 2017

theCommand='Eef_pos';
fprintf(t, theCommand);
message=fgets(t);
%disp(message)
[P1,N]=getDoubleFromString(message);
ttt=0;
for i=4:6
    ttt=ttt+1;
    Pos{ttt}=P1{i};
end

end

function [jPos,j]=getDoubleFromString(message)
n=max(max(size(message)));
j=0;
numString=[];
for i=1:n
    if message(i)=='_'
        j=j+1;
        jPos{j}=str2num(numString);
        numString=[];
    else
        numString=[numString,message(i)];
    end
end
end


% eegthresh() -  reject trials with out-of-bounds channel values within a
%                specified epoch time range.
%
% Usage:
%   >> [Iin, Iout, newsignal, elec] = eegthresh( signal, frames, ...
%                elecs, negthresh, posthresh, timerange, starttime,endtime);
%
% Required inputs:
%   signal     - 2-D data matrix [channels, frames*sweeps],
%                or 3-D data matrix [channels, frames, sweeps]
%   frames     - number of points per epoch
%   elecs      - [int vector] electrode indices to reject on
%   negthresh  - minimum rejection threshold(s) in uV. This can be an array
%                of values for individual electrodes. If fewer values than
%                electrodes, the last value is used for the remaining
%                electrodes.
%   posthresh  - maximum rejection threshold(s) in uV (same syntax as for
%                negthresh)
%   timerange  - [mintime maxtime] time range limits of the signal
%   starttime  - Starting limit (in seconds or Hz) of range to perform
%                rejection (same syntax  as negthresh)
%   endtime    - Ending limit (in seconds or Hz) of range to perform
%                rejection (same syntax  as negthresh)
%
% Outputs:
%   Iin        - Indexes of epochs accepted
%   Iout       - Indexes of epochs rejected
%   newsignal  - input data after epoch rejection
%   elec       - electrode that triggered the rejections (array of 0s
%                and 1s with the same number of columns as Iout
%                and number of rows = number of electrodes).
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2001
%
% See also: pop_eegthresh(), eeglab()

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [Iin, Iout, newsignal, elec] = eegthresh( signal, pnts, electrodes, negthresh, posthresh, timerange, starttime, endtime);

if nargin < 7
	help eegthresh;
	return;
end;

if starttime < timerange(1)
	disp('eegthresh: starting point out of range, adjusted');
    starttime = timerange(1);
end;
if endtime > timerange(2)
	disp('eegthresh: ending point out of range, adjusted');
    endtime = timerange(2);
end;

% complete thresholds values if necessary
%----------------------------------------
if size(posthresh,2) < size(electrodes,2)
	posthresh = [ posthresh posthresh(end)*ones(1,size(electrodes,2)-size(posthresh,2))];
end;
if size(negthresh,2) < size(electrodes,2)
	negthresh = [ negthresh negthresh(end)*ones(1,size(electrodes,2)-size(negthresh,2))];
end;

% complete timeselect values if necessary
%----------------------------------------
if size(starttime,2) < size(electrodes,2)
	starttime = [ starttime starttime(end)*ones(1,size(electrodes,2)-size(starttime,2))];
end;
if size(endtime,2) < size(electrodes,2)
	endtime = [ endtime endtime(end)*ones(1,size(electrodes,2)-size(endtime,2))];
end;

% find the maximum for each trial
%--------------------------------
sweeps = size(signal(1,:),2)/pnts;
signal = reshape(signal(electrodes,:), size(electrodes(:),1), pnts, sweeps);

% reject the selected trials
%---------------------------
elec = zeros(size(electrodes(:),1), sweeps);
allelec = zeros(1, sweeps);
for indexe = 1:size(electrodes(:),1)
	% transform the time range
	% ------------------------
	framelowlimit  = max(1,floor((starttime(indexe)-timerange(1))/(timerange(2)-timerange(1))*(pnts-1))+1);
	framehighlimit = floor((endtime(indexe)  -timerange(1))/(timerange(2)-timerange(1))*(pnts-1))+1;

	% remove outliers
	% ---------------
	sigtmp = squeeze(signal(indexe,framelowlimit:framehighlimit,:));
    if size(signal,3) == 1, sigtmp = sigtmp'; end;
	sigmax = max(sigtmp, [], 1);
	sigmin = min(sigtmp, [], 1);
	elec(indexe,:) = ( sigmin < negthresh(indexe) ) | ( sigmax > posthresh(indexe) );
	allelec = allelec | elec(indexe,:);
end;
Iout = find( allelec == 1 );
Iin  = find( allelec == 0 );
elec = elec(:,Iout);

% reject the selected trials
%---------------------------
newsignal = reshape(signal, size(signal,1), pnts, sweeps);
if ~isempty(Iin)
	newsignal = newsignal(:,:,Iin);
	if ndims(signal) == 2
		newsignal = newsignal(:,:);
	end;
else
	newsignal = [];
end;
return;


function ChannelMat = in_channel_cartool_els(ChannelFile)
% IN_CHANNEL_ELS: Reads a .els file : cartesian coordinates of a set of electrodes, divided in various clusters
%
% USAGE:  ChannelMat = in_channel_els(ChannelFile)
%
% INPUT:
%    - ChannelFile : name of file to open, WITH .ELS EXTENSION
% OUTPUT:
%    - ChannelMat  : Brainstorm channel structure
%
% FORMAT: (.ELS)
%     ASCII file :
%     Format : "ES01<RETURN>"
%              "<nb_electrodes> <RETURN>"
%              "<nb_clusters> <RETURN>"
%              Repeat for each cluster :
%                  "<Cluster_name> <RETURN>"
%                  "<Cluster_number_of_electrodes> <RETURN>"
%                  "<Cluster_type> <RETURN>"
%              End of repeat
%              Repeat for each electrode :
%                  "<X1> <Y1> <Z1> ?<electrode_label>? ?<optional_flag>?<RETURN>"
%              End.
%     Notes :
%     - Cluster_type : could be seen as the dimensionality of the cluster :
%           - 0 for separated electrodes (like auxiliaries), or "points"
%           - 1 for a strip of electrodes, or "line"
%           - 2 for a grid of electrodes, or "array"
%           - 3 for a 3D set of electrodes, usually on the scalp
%     - optional_flag : ignored in this program

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2006-2015

% Initialize output variable
ChannelMat = [];

%% ===== READ ELS FILE =====
% Open file
[fid, message] = fopen(ChannelFile, 'r');
if (fid == -1)
    disp(['BST> Error: Could not open file: ' message]);
    return;
end
% Read magic number
readData = fgetl(fid);
if ~strcmp(readData, 'ES01')
    error('Invalid ELS file.');
end
% Read number of electrodes
Ne = str2num(fgetl(fid));
% Read number of clusters
Nclust = str2num(fgetl(fid));
% Read all clusters
clusterNames  = cell(1,Nclust);
allLoc      = zeros(0,3);
allLabels   = {};
allClusters = {};
allTypes    = {};
for iClust = 1:Nclust
    clusterNames{iClust} = fgetl(fid);
    clusterNchan = str2num(fgetl(fid));
    clusterType  = str2num(fgetl(fid));
    % Skip electrode positions and move to next cluster
    for iChan = 1:clusterNchan
        chanLine = fgetl(fid);
        readData = textscan(chanLine, '%n %n %n %s', 1);
        allLoc(end+1,:) = [readData{1:3}];
        allClusters{end+1} = clusterNames{iClust};
        if isempty(readData{4}) || isempty(readData{4}{1})
            allLabels{end+1} = sprintf('%s%02d', clusterNames{iClust}, iChan);
        else
            allLabels{end+1} = readData{4}{1};
        end
        % Channel type
        switch clusterType
            case 0,    allTypes{end+1} = 'Misc';
            case 1,    allTypes{end+1} = 'SEEG';
            case 2,    allTypes{end+1} = 'ECOG';
            case 3,    allTypes{end+1} = 'EEG';
            otherwise, allTypes{end+1} = 'EEG';
        end
    end
end
% Close file
fclose(fid);

% Verify the expected number of electrodes
% If too many electrodes were read, only keep the first 'Ne' electrodes
readNe = size(allLoc,1);
if (readNe < Ne)
    disp('BST> Warning: Incorrect number of electrodes in file.');
elseif (readNe > Ne)
    allLoc(Ne+1:readNe, :) = [];
    allLabels(Ne+1:readNe, :) = [];
    allClusters(Ne+1:readNe, :) = [];
    allTypes(Ne+1:readNe, :) = [];
end

%% ===== SELECT CLUSTERS =====
% If there are multiple clusters
if (Nclust > 1)
    % Ask user to select clusters
    isSelected = java_dialog('checkbox', 'Select the clusters to import:', 'Import ELS file', [], clusterNames, ones(size(clusterNames)));
    if isempty(isSelected) || ~any(isSelected)
        return;
    end
    % Select only the required sensors
    iChannels = find(ismember(allClusters, clusterNames(isSelected==1)));
else
    iChannels = 1:readNe;
end

%% ===== CONVERT IN BRAINSTORM FORMAT =====
ChannelMat = db_template('channelmat');
ChannelMat.Comment = 'Cartool ELS';
ChannelMat.Channel = repmat(db_template('channeldesc'), 1, length(iChannels));
for i = 1:length(iChannels)
    ChannelMat.Channel(i).Name    = allLabels{iChannels(i)};
    ChannelMat.Channel(i).Comment = '';
    ChannelMat.Channel(i).Type    = allTypes{iChannels(i)};
    ChannelMat.Channel(i).Group   = allClusters{iChannels(i)};
    ChannelMat.Channel(i).Loc     = allLoc(iChannels(i),:)';
    ChannelMat.Channel(i).Orient  = [];
    ChannelMat.Channel(i).Weight  = 1;
end


% Defines constants
%
% Author: Jonathan Karr
% Affilitation: Covert Lab, Department of Bioengineering, Stanford University
% Last updated: 5/7/2009
classdef ConstantUtil
    properties (Constant=true)
        nAvogadro=6.0221417930e23;
        elements=struct(...
            'H',1.0079,...
            'He',4.0026,...
            'Li',6.941,...
            'Be',9.0122,...
            'B',10.811,...
            'C',12.0107,...
            'N',14.0067,...
            'O',15.9994,...
            'F',18.9984,...
            'Ne',20.1797,...
            'Na',22.9897,...
            'Mg',24.305,...
            'Al',26.9815,...
            'Si',28.0855,...
            'P',30.9738,...
            'S',32.065,...
            'Cl',35.453,...
            'Ar',39.948,...
            'K',39.0983,...
            'Ca',40.078,...
            'Sc',44.9559,...
            'Ti',47.867,...
            'V',50.9415,...
            'Cr',51.9961,...
            'Mn',54.938,...
            'Fe',55.845,...
            'Co',58.9332,...
            'Ni',58.6934,...
            'Cu',63.546,...
            'Zn',65.39,...
            'Ga',69.723,...
            'Ge',72.64,...
            'As',74.9216,...
            'Se',78.96,...
            'Br',79.904,...
            'Kr',83.8,...
            'Rb',85.4678,...
            'Sr',87.62,...
            'Y',88.9059,...
            'Zr',91.224,...
            'Nb',92.9064,...
            'Mo',95.94,...
            'Tc',98,...
            'Ru',101.07,...
            'Rh',102.9055,...
            'Pd',106.42,...
            'Ag',107.8682,...
            'Cd',112.411,...
            'In',114.818,...
            'Sn',118.71,...
            'Sb',121.76,...
            'Te',127.6,...
            'I',126.9045,...
            'Xe',131.293,...
            'Cs',132.9055,...
            'Ba',137.327,...
            'La',138.9055,...
            'Ce',140.116,...
            'Pr',140.9077,...
            'Nd',144.24,...
            'Pm',145,...
            'Sm',150.36,...
            'Eu',151.964,...
            'Gd',157.25,...
            'Tb',158.9253,...
            'Dy',162.5,...
            'Ho',164.9303,...
            'Er',167.259,...
            'Tm',168.9342,...
            'Yb',173.04,...
            'Lu',174.967,...
            'Hf',178.49,...
            'Ta',180.9479,...
            'W',183.84,...
            'Re',186.207,...
            'Os',190.23,...
            'Ir',192.217,...
            'Pt',195.078,...
            'Au',196.9665,...
            'Hg',200.59,...
            'Tl',204.3833,...
            'Pb',207.2,...
            'Bi',208.9804,...
            'Po',209,...
            'At',210,...
            'Rn',222,...
            'Fr',223,...
            'Ra',226,...
            'Ac',227,...
            'Th',232.0381,...
            'Pa',231.0359,...
            'U',238.0289,...
            'Np',237,...
            'Pu',244,...
            'Am',243,...
            'Cm',247,...
            'Bk',247,...
            'Cf',251,...
            'Es',252,...
            'Fm',257,...
            'Md',258,...
            'No',259,...
            'Lr',262,...
            'Rf',261,...
            'Db',262,...
            'Sg',266,...
            'Bh',264,...
            'Hs',277,...
            'Mt',268);
        elementNumbers=struct(...
            'H', 1,...
            'He',4,...
            'Li',7,...
            'Be',9,...
            'B', 11,...
            'C', 12,...
            'N', 14,...
            'O', 16,...
            'F', 19,...
            'Ne',20,...
            'Na',23,...
            'Mg',24,...
            'Al',27,...
            'Si',28,...
            'P', 31,...
            'S', 32,...
            'Cl',35,...
            'Ar',40,...
            'K', 39,...
            'Ca',40,...
            'Sc',45,...
            'Ti',48,...
            'V', 51,...
            'Cr',52,...
            'Mn',55,...
            'Fe',56,...
            'Co',59,...
            'Ni',59,...
            'Cu',64,...
            'Zn',65,...
            'Ga',70,...
            'Ge',73,...
            'As',75,...
            'Se',79,...
            'Br',80,...
            'Kr',84,...
            'Rb',85,...
            'Sr',87,...
            'Y', 89,...
            'Zr',91,...
            'Nb',93,...
            'Mo',96,...
            'Tc',98,...
            'Ru',101,...
            'Rh',103,...
            'Pd',107,...
            'Ag',108,...
            'Cd',112,...
            'In',115,...
            'Sn',119,...
            'Sb',122,...
            'Te',128,...
            'I', 127,...
            'Xe',131,...
            'Cs',133,...
            'Ba',137,...
            'La',139,...
            'Ce',140,...
            'Pr',141,...
            'Nd',144,...
            'Pm',145,...
            'Sm',150,...
            'Eu',152,...
            'Gd',157,...
            'Tb',159,...
            'Dy',163,...
            'Ho',165,...
            'Er',167,...
            'Tm',169,...
            'Yb',173,...
            'Lu',175,...
            'Hf',178,...
            'Ta',181,...
            'W', 184,...
            'Re',186,...
            'Os',190,...
            'Ir',192,...
            'Pt',195,...
            'Au',197,...
            'Hg',201,...
            'Tl',204,...
            'Pb',207,...
            'Bi',209,...
            'Po',209,...
            'At',210,...
            'Rn',222,...
            'Fr',223,...
            'Ra',226,...
            'Ac',227,...
            'Th',232,...
            'Pa',231,...
            'U', 238,...
            'Np',237,...
            'Pu',244,...
            'Am',243,...
            'Cm',247,...
            'Bk',247,...
            'Cf',251,...
            'Es',252,...
            'Fm',257,...
            'Md',258,...
            'No',259,...
            'Lr',262,...
            'Rf',261,...
            'Db',262,...
            'Sg',266,...
            'Bh',264,...
            'Hs',277,...
            'Mt',268);
        secondsPerMinute = 60;
        secondsPerHour=3600;
        minutesPerHour=60;

        realmax = 1e10;
    end
end


function [outColor] = nflcolor(teamName, colorType)
%NFLCOLOR Return NFL RGB color values by team or city name
%   COLOR = NFLCOLOR(TEAMNAME,COLORTYPE) returns the 3-element RGB
%   color values of an NFL team specified using the team's city name
%   (Buffalo for example) or by the team name (Bills for example).  The
%   type of color returned is determined by COLORTYPE, COLORTYPE must
%   either be 'team' for the primary team color or 'name for the color used
%   on jersy names.  If COLORTYPE is omitted, the primary team color is
%   output.  Inputs are not case sensitive.  Partial matches are
%   supported.  In cases where more than one match is found, the first
%   match is provided and a warning is generated.
%
%   Examples:
%
%   bengalsBlack = nflcolor('bengals', 'team');
%   bengalsOrange = nflcolor('bengals','name');
%   bengalsOrange = nflcolor('cinci','name');
%   dolphinAqua = nflcolor('miami');
%
%  Color values are from the following web site...
%  http://en.wikipedia.org/wiki/Template:Infobox_NFL_player#Team_colors
%
%  Note:  Giants fans may wish to edit this function and claim New York for
%  themselves. As written, the fuction gives Jets colors when New York is
%  provided as the team name.
%

% Define nfl colors, column 3 is team color, column 4 is name color
nflData = ...
    {'buffalo',        'bills',     '0f4589', 'ffffff';
    'miami',          'dolphins',  '005e6a', 'ffffff';
    'new england',    'patriots',  '0d254c', 'd6d6d6';
    'new york',       'jets',      '313f36', 'ffffff';
    'baltimore',      'ravens',    '2a0365', 'd0b239';
    'cincinnati',     'bengals',   '000000', 'f03a16';
    'cleveland',      'browns',    'de6108', '312821';
    'pittsburgh',     'steelers',  '000000', 'f1c500';
    'houston',        'texans',    '001831', 'ffffff';
    'indianapolis',   'colts',     '003b7b', 'ffffff';
    'jacksonville',   'jaguars',   '00576e', 'd0b239';
    'tennessee',      'titans',    '648fcc', '0d254c';
    'denver',         'broncos',   '10274c', 'df6109';
    'kansas city',    'chiefs',    'b20032', 'f2c800';
    'oakland',        'raiders',   '000000', 'c4c8cb';
    'san diego',      'chargers',  '08214a', 'eec607';
    'dallas',         'cowboys',   'c5ced6', '000080';
    'new york',       'giants',    '192f6b', 'ffffff';
    'philadelphia',   'eagles',    '003b48', 'c4c8cb';
    'washington',     'redskins',  '7d0008', 'ffbe26';
    'chicago',        'bears',     '03182f', 'df6108';
    'detroit',        'lions',     '005da6', 'c4c8cb';
    'green bay',      'packers',   '213d30', 'ffcc00';
    'minnesota',      'vikings',   '3b0160', 'f0bf00';
    'atlanta',        'falcons',   'bd0d18', '000000';
    'carolina',       'panthers',  '000000', '0088d4';
    'new orleans',    'saints',    '000000', 'c9b074';
    'tampa bay',      'buccaneers','b20032', 'ffffff';
    'arizona',        'cardinals', '870619', 'ffffff';
    'st louis',       'rams',      '0d254c', 'c9b074';
    'saint louis',    'rams',      '0d254c', 'c9b074';
    'st. louis',      'rams',      '0d254c', 'c9b074';
    'san francisco',  '49ers',     '840026', 'c9b074';
    'seattle',        'seahawks',  '03182f', '4eae17'};

idx=strmatch(lower(teamName),nflData(:,1:2));

if isempty(idx)
    error('Team name not found');
end

nEntries = size(nflData,1);
if idx(1)>nEntries
    idx(1) = idx(1)-nEntries;
end

if length(idx)>1
    warning('nflcolor:multipleMatches',['Found multiple matches for %s, ' ...
        'using %s'], teamName, nflData{idx(1),2});
end

if nargin<2
    colorType = 'team';
end

switch lower(colorType)
    case 'team'
        colorHex = nflData{idx(1),3};
    case 'name'
        colorHex = nflData{idx(1),4};
    otherwise
        error('Invalid colorType specified');
end

outColor = [hex2dec(colorHex(1:2)),...
    hex2dec(colorHex(3:4)),...
    hex2dec(colorHex(5:6))]/255;


function model=convertToCobraV2(model)
% Generate a model structure with metabolite and reaction subfields instead of vectors.
%
% create a COBRA v2 model structure with model.met(m) for metabolites and
% model.rxn(n) for reactions. This keeps all the information about a
% metabolite or reaction in one place. The primary keys model.mets and
% model.rxns are kept as is, and these can be used to access groups of
% model.met or model.rxn quickly.
%
% Replaces (*) with [*] to denote compartment in metabolite abbreviations
%
%
%INPUT
% model.S                   Stoichiometric matrix
% model.rxns                Abbreviation for each reaction (primary key)
% model.rxnNames            Full name for each reaction
% model.mets                Abbreviation for each metabolite (primary key)
% model.metNames            Full name for each metabolite
% model.biomassRxnAbbr      abbreviation of biomass reaction
%
%OPTIONAL INPUT
% model.metFormulas         m x 1 cell array of strings with chemical
%                           formula for metabolite
% model.metCharges          m x 1 numeric charge for each metabolite
%                           species
%
%OUTPUT
% model.S                       Sparse Stoichiometric matrix +/- with
%                               adjustment of reactions involving C02
% model.SIntRxnBool             Boolean of internal reactions, i.e.
%                               non-mass balanced reactions
% model.met(m).abbreviation     metabolite abbreviation (primary key)
% model.met(m).officialName     metabolite name
% model.rxn(n).abbreviation     reaction abbreviation (primary key)
% model.rxn(n).officialName     reaction name
% model.rxn(n).equation         reaction equation
% model.rxn(n).directionality   qualitative directionality
% model.rxn(n).regulationStatus {('On'),'Off'} Off if lb=ub=0
%
%OPTIONAL OUTPUT
% model.met(m).formula          metabolite elemental formula
%
% Ronan M. T. Fleming

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

%make it sparse
model.S=sparse(model.S);

numChar=1;
[allMetCompartments,uniqueCompartments]=getCompartment(model.mets,numChar);

if ~exist('thermoAdjustmentToS','var')
    thermoAdjustmentToS=1;
end

for m=1:nMet
    %find the rows with columns that are all zeros
    if nnz(model.S(m,:))==0
        fprintf('%s\n',['metabolite ' model.mets{m} ' without a reaction???']);
%         error(['metabolite ' model.mets{m} ' without a reaction.']);
    end

    %replace (*) with [*] to denote compartment
    model.mets{m}(end-2)='[';
    model.mets{m}(end)=']';
end

% start the conversion to a cobra v2 style model
fprintf('%s\n','Converting to cobra toolbox v2 style model.')
[nMet,nRxn]=size(model.S);

for n=1:nRxn
    model.rxn(n).abbreviation=model.rxns{n};
    model.rxn(n).officialName=model.rxnNames{n};
    %equation
    equation=printRxnFormula(model,model.rxns(n),0);
    model.rxn(n).equation=equation{1};
    %directionality
    if model.lb(n)<0 && model.ub(n)>0
        model.rxn(n).directionality='reversible';
    end
    if model.lb(n)<0 && model.ub(n)<=0
        model.rxn(n).directionality='reverse';
    end
    if model.lb(n)>=0 && model.ub(n)>0
        model.rxn(n).directionality='forward';
    end
    if model.lb(n)==0 && model.ub(n)==0
        model.rxn(n).directionality='off';
    end
end

%new metabolite structure
for m=1:nMet
    model.met(m).abbreviation=model.mets{m};
    model.met(m).officialName=model.metNames{m};
%    model=rmfield(model,'metNames');
end

if isfield(model,'metFormulas')
    for m=1:nMet
         model.met(m).formula=model.metFormulas{m};
    end
    model=rmfield(model,'metFormulas');
end

if isfield(model,'metCharges')
    for m=1:nMet
         model.met(m).charge=model.metCharges(m);
    end
    model=rmfield(model,'metCharges');
end

% %replace any dashes in metabolite names with underscores
% for m=1:nMet
%     x = strfind(model.met(m).abbreviation,'-');
%     if x~=0
%         %cobra v2 mets have underscores
%         model.met(m).abbreviation(x)='_';
%         %make cobra v1 mets have all underscores also
%         abbr=model.mets{m};
%         abbr(x)='_';
%         model.mets{m}=abbr;
%     end
% end

%finds the reactions in the model which export/import from the model
%boundary i.e. mass unbalanced reactions
%e.g. Exchange reactions
%     Demand reactions
%     Sink reactions
model=findSExRxnInd(model);

fprintf('%s\n\n','...finished converting.')


classdef CarlaEnvironment < matlab.System & matlab.system.mixin.Propagates
    % Untitled Add summary here
    %
    % This template includes the minimum set of functions required
    % to define a System object with discrete state.

    % Public, tunable properties
    properties

    end

    properties(DiscreteState)

    end

    % Pre-computed constants
    properties(Access = private)
        % Semantic segmentation resolution
        width = 960;
        height = 540;

        % RGB resolution
        rgb_width = 1920;
        rgb_height = 1080;

        tesla;
        last_position;

        ss_rgb;
        module_ss_rgb;

        rgb;
        module_rgb;

        last_update_time;
        history_array = zeros(2, 8);
        steer_spline;

        steer_window_avg = 0;
        push_back_time = 0;
        frames_since_last_update = 0;

        bias = -0.033;
    end

    methods(Access = protected)
        function setupImpl(obj)
            % Perform one-time calculations, such as computing constants
            port = int16(2000);
            client = py.carla.Client('localhost', port);
            client.set_timeout(10.0);
            world = client.get_world();

            % Spawn Vehicle
            blueprint_library = world.get_blueprint_library();
            car_list = py.list(blueprint_library.filter("model3"));
            car_bp = car_list{1};
            spawn_point = py.random.choice(world.get_map().get_spawn_points());
%             spawn_point.location.x = -88.8;
%             spawn_point.location.y = 149.0;
%             spawn_point.rotation.yaw = 90;

            obj.tesla = world.spawn_actor(car_bp, spawn_point);
            obj.tesla.set_autopilot(false);

            control = obj.tesla.get_control();
            control.throttle = 0.6;
            control.steer = 0;
            obj.tesla.apply_control(control);

            % Semantic Segmentation
            blueprint = world.get_blueprint_library().find('sensor.camera.semantic_segmentation');
            blueprint.set_attribute('image_size_x', num2str(obj.width))
            blueprint.set_attribute('image_size_y', num2str(obj.height))

            transform = py.carla.Transform(py.carla.Location(pyargs('x',0.8, 'z',1.7)));
            obj.ss_rgb = world.spawn_actor(blueprint, transform, pyargs('attach_to',obj.tesla));

            obj.module_ss_rgb = sensorBind(obj.ss_rgb, "ss_rgb", "semantic_segmentation_rgb", "array");

            % RGB
            blueprint = world.get_blueprint_library().find('sensor.camera.rgb');
            blueprint.set_attribute('image_size_x', num2str(obj.rgb_width))
            blueprint.set_attribute('image_size_y', num2str(obj.rgb_height))

            transform = py.carla.Transform(py.carla.Location(pyargs('x',-5.0, 'z',2.5)));
            obj.rgb = world.spawn_actor(blueprint, transform, pyargs('attach_to',obj.tesla));

            obj.module_rgb = sensorBind(obj.rgb, "rgb", "rgb", "array");

            obj.last_position = obj.tesla.get_location();

            obj.last_update_time = cputime;
        end

        function [SEMANTIC_SEGMENTATION_RGB, RGB] = stepImpl(obj)

            % Semantic Segmentation
            SEMANTIC_SEGMENTATION_RGB = uint8(py.getattr(obj.module_ss_rgb, 'array'));

            %% Lane Detection
            control = obj.tesla.get_control();

            % Crop the image to region of interest
            horizontal_crop = 0.0;
            hor_start_pos = ceil(obj.width/2);
            hor_end_pos = floor(obj.width * (1 - horizontal_crop / 2));

            vertical_crop = .5;
            vert_start_pos = 1 + ceil(obj.height * vertical_crop);

            filtered_lanes = SEMANTIC_SEGMENTATION_RGB(vert_start_pos:obj.height,hor_start_pos:hor_end_pos,1) == 157;

            % Apply hough transformations to get the lines from the sensor
            % image
            BW = filtered_lanes;
            [H,T,R] = hough(BW);
            P  = houghpeaks(H,5,'threshold',ceil(0.0*max(H(:))));
            lines = houghlines(BW,T,R,P,'FillGap',50,'MinLength',20);

            % Calculate the angles of the lines based on the x-y
            % coordinates
            angles = zeros(1, length(lines));
            for k = 1:length(lines)
                angles(k) = rad2deg(atan((lines(k).point1(2) - lines(k).point2(2))/(lines(k).point1(1) - lines(k).point2(1))));
            end

            % Normally from the camera position, the angle between the road
            % and the car if the car is moving straight
            ref_angle = 57;
            max_angle = max(angles);

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Loop to draw the nearest right lanes detected
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            imshow(filtered_lanes), hold on

            for k = 1:length(lines)
               angle = rad2deg(atan((lines(k).point1(2) - lines(k).point2(2))/(lines(k).point1(1) - lines(k).point2(1))));
               if ~(angle == max_angle)
                  continue
               end

               xy = [lines(k).point1; lines(k).point2];
               plot(xy(:,1),xy(:,2),'LineWidth',2,'Color','green');

               % Plot beginnings and ends of lines
               plot(xy(1,1),xy(1,2),'x','LineWidth',2,'Color','yellow');
               plot(xy(2,1),xy(2,2),'x','LineWidth',2,'Color','red');

            end

            set(gcf, 'Visible', 'on');
            hold off;
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            % If the vehicle has just spawned and not started moving, dont
            % update steer history
            started = false;
            current_location = obj.tesla.get_location();
            thres_loc = 0.001;

            if abs(current_location.x - obj.last_position.x) > thres_loc...
                    && abs(current_location.y - obj.last_position.y) > thres_loc
                started = true;
            end

            % Calculate the time it took to process one frame. Really
            % necessary to make the calculations independent of the frame
            % rate. Otherwise, running simulation on a pc that can run it
            % on 20 FPS will steer 20 times vs 2 FPS steers 2 times, dt
            % scales that
            dt = cputime - obj.last_update_time;
            fac = 0.5 * tanh(0.1 * abs(ref_angle - max_angle)) * dt;

            obj.last_update_time = cputime;

            % How much effect the last steering has on the future steer
            mem = 0.0025;

            % Filter out lines that are almost horizontal
            if ~isempty(fac) && ((max_angle < 85) && (max_angle > 25)) && started
                if max_angle < ref_angle
                    steer = mem * control.steer + fac;
                else
                    steer = mem * control.steer - fac;
                end
            else
                steer = obj.bias;
            end

            % Update the spline history ~ every .2 seconds. A very high FPS
            % would polute the spline fitting
            if cputime - obj.push_back_time > .2

                % Delete the oldest element in the array
                obj.history_array(:,1) = [];

                % Most recent history weight
                weight = 1.25;

                % Average of the steer since the last update
                steer = (obj.steer_window_avg + steer)/(obj.frames_since_last_update + 1);

                % Add to the histroy
                obj.history_array = [obj.history_array, [cputime;steer]];

                % Pre-processing for the curve fitting
                [timeData, steerData] = prepareCurveData( obj.history_array(1,:),...
                                                          obj.history_array(2,:));

                obj.history_array(end) = obj.history_array(end) * weight;

                % Fit a smoothing spline to the data
                obj.steer_spline = fit( timeData, steerData, 'smoothingspline', ...
                                        'Normalize', 'on', 'SmoothingParam',0.95);

                obj.history_array(end) = obj.history_array(end) / weight;

                obj.steer_window_avg = 0;
                obj.frames_since_last_update = 0;

                obj.push_back_time = cputime;

            else
                % If not yet time to update, keep track of the steer values
                % in this time slot
                obj.steer_window_avg = obj.steer_window_avg + steer;
                obj.frames_since_last_update = obj.frames_since_last_update + 1;
            end

            % Predict and steer the car
            control.steer = obj.steer_spline(cputime + dt/2);

            obj.tesla.apply_control(control);
            obj.last_position = current_location;

            %% RGB sensor
            RGB = uint8(py.getattr(obj.module_rgb, 'array'));
        end

        function [SEMANTIC_SEGMENTATION_RGB, RGB] = isOutputComplexImpl(~)
            SEMANTIC_SEGMENTATION_RGB = false;
            RGB = false;
        end

        function [SEMANTIC_SEGMENTATION_RGB, RGB] = getOutputSizeImpl(obj)
            SEMANTIC_SEGMENTATION_RGB = [obj.height obj.width 3];
            RGB = [obj.rgb_height obj.rgb_width 3];
        end

        function [SEMANTIC_SEGMENTATION_RGB, RGB] = getOutputDataTypeImpl(~)
            SEMANTIC_SEGMENTATION_RGB = 'uint8';
            RGB = 'uint8';
        end

        function [SEMANTIC_SEGMENTATION_RGB, RGB] = isOutputFixedSizeImpl(~)
            SEMANTIC_SEGMENTATION_RGB = true;
            RGB = true;
        end

        function resetImpl(~)
            % Initialize / reset discrete-state properties
        end
    end

    methods(Access= public)
        function delete(obj)
            % Delete the car from the Carla world
            if ~isempty(obj.tesla)
                obj.tesla.destroy();
            end

            % Semantic segmentation sensor
            if ~isempty(obj.ss_rgb)
                obj.ss_rgb.destroy();
            end

            % RGB camera
            if ~isempty(obj.rgb)
                obj.rgb.destroy();
            end
        end
    end
end


% GET THE GLOBAL TRAJECTORY DATA FOR AN INDIVIDUAL OBJECT
function [currentFigure,figureSet] = GetFigure_objectTrajectory(SIM,DATA,currentFigure)

figureSet = [];
for n = 1:DATA.totalObjects
    [currentFigure,figureSet(n)] = BuildObjectTrajectoryFigure(SIM,DATA,currentFigure,n);
end
% ASSEMBLE TABBED FIGURE
windowHandle = GetTabbedFigure(figureSet,'OpenMAS Trajectory Overview');
set(windowHandle,'Position',DATA.figureProperties.windowSettings);          % Maximise the figure in the tab
savefig(windowHandle,[SIM.outputPath,'global-trajectory-overview']);        % Save the output figure
end

% Generate the trajectory figure for the object
function [currentFigure,figureHandle] = BuildObjectTrajectoryFigure(SIM,DATA,currentFigure,objectNum)

% Declare title string for figure
titlestr = sprintf('Global trajectory data for %s over a period of %ss',SIM.OBJECTS(objectNum).name,num2str(SIM.TIME.endTime));

objectLabel = sprintf('[ID-%s] %s',num2str(SIM.OBJECTS(objectNum).objectID),SIM.OBJECTS(objectNum).name);

% CONFIGURE THE PLOT ATTRIBUTES
figurePath  = strcat(SIM.outputPath,sprintf('global-trajectories-%s',sprintf('id-%s-%s',num2str(SIM.OBJECTS(objectNum).objectID),SIM.OBJECTS(objectNum).name)));
figureHandle = figure('Name',objectLabel);
set(figureHandle,'Position', DATA.figureProperties.windowSettings);        % [x y width height]
set(figureHandle,'Color',DATA.figureProperties.figureColor);               % Background colour
set(figureHandle,'Visible','off');
plotCellWidth = 4; plotCellA = 1;                                          % The width of each figure, the start of the plot
setappdata(figureHandle, 'SubplotDefaultAxesLocation', [0.1, 0.1, 0.85, 0.80]);

% EXTRACT TIME-STATE DATA FROM THE TRAJECTORY MATRIX
%[objectStates] = OMAS_getTrajectoryData_mex(DATA.globalTrajectories,SIM.globalIDvector,SIM.OBJECTS(objectNum).objectID,inf);
[objectStates] = OMAS_getTrajectoryData(DATA.globalTrajectories,SIM.globalIDvector,SIM.OBJECTS(objectNum).objectID,inf);

% STATE NAME VECTOR
stateTags = {{'$x$','$(m)$'},{'$y$','$(m)$'},{'$z$','$(m)$'},...
             {'$u$','$(m/s)$'},{'$v$','$(m/s)$'},{'$w$','$(m/s)$'},...
             {'$\phi$','$(rad)$'},{'$\theta$','$(rad)$'},{'$\psi$','$(rad)$'}};
% CONVERT QUATERNION STATE TO EULER ANGLE STATES
eulerStates = zeros(9,size(objectStates,2));
for i = 1:size(objectStates,2)
    eulerStates(1:6,i) = objectStates(1:6,i);
    eulerStates(7:9,i) = OMAS_geometry.quaternionToEulers(objectStates(7:10,i));
end

clear objectStates;

% FOR EACH OBJECTS PERSPECTIVE
stateVectorLength = size(eulerStates,1);
for n = 1:stateVectorLength
    % CALCULATE THE ABSOLUTE SEPERATION FROM ALL OTHER OBJECTS OVER TIME
    plotCellB = n*plotCellWidth;                                    % The end of the plot
    plotLocation = subplot(stateVectorLength,plotCellWidth,[plotCellA plotCellB]);

    % PLOT THE SEPERATION DATA ON CURRENT FIGURE
    plot(plotLocation,DATA.timeVector(1:SIM.TIME.endStep),...
         eulerStates(n,1:SIM.TIME.endStep),...
         'LineStyle','-',...
         'LineWidth',DATA.figureProperties.lineWidth,...
         'Color','b');
    % Y-axes
    ylabel(plotLocation,stateTags{n},...
        'Interpreter',DATA.figureProperties.interpreter,...
        'fontname',DATA.figureProperties.fontName,...
        'Fontweight',DATA.figureProperties.fontWeight,...
        'FontSize',DATA.figureProperties.axisFontSize,...
        'FontSmoothing','on');
    % Axes
    set(plotLocation,...
        'TickLabelInterpreter',DATA.figureProperties.interpreter,...
        'fontname',DATA.figureProperties.fontName,...
        'Fontweight',DATA.figureProperties.fontWeight,...
        'FontSize',DATA.figureProperties.axisFontSize,...
        'FontSmoothing','on',...
        'Color',DATA.figureProperties.axesColor);
    grid on; box on; grid minor;
    plotLocation.XAxis.MinorTickValues = plotLocation.XAxis.Limits(1):SIM.TIME.dt:plotLocation.XAxis.Limits(2);
    % ADD FIGURE TITLE TO FIRST PLOT HEADER
    if n == 1
        % Append title to first subplot
        title(plotLocation,titlestr,...
            'Interpreter',DATA.figureProperties.interpreter,...
            'fontname',DATA.figureProperties.fontName,...
            'Fontweight',DATA.figureProperties.fontWeight,...
            'Fontsize',DATA.figureProperties.titleFontSize,...
            'FontSmoothing','on');

    end
    % Prevent overlap of x-label
    if n == stateVectorLength
        % X-axis
        xlabel(plotLocation,'t (s)',...
            'Interpreter',DATA.figureProperties.interpreter,...
            'fontname',DATA.figureProperties.fontName,...
            'Fontweight',DATA.figureProperties.fontWeight,...
            'FontSize',DATA.figureProperties.axisFontSize,...
            'FontSmoothing','on');
    else
        set(plotLocation,'XTickLabel',[]);
    end
    % Define the x-limits
    xlim([SIM.TIME.startTime,SIM.TIME.endTime]);                           % Ensures plot alignment/sizing
    % Move to next subplot location
    plotCellA = plotCellA + plotCellWidth;
end
hold off;

% SAVE THE OUTPUT FIGURE
set(figureHandle,'Visible','on');                % Make it visable for saving
savefig(figureHandle,figurePath);                % As matlab figure
% Publish as .pdf if requested
if DATA.figureProperties.publish
	GetFigurePDF(figureHandle,figurePath);
end
set(figureHandle,'Visible','off');
currentFigure = currentFigure + 1;  % Iterate the plot count
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 plot_path(paths, mapClass, SFCs)
% PLOT_PATH Visualize a path and sfc's ellipses
%   PLOT_PATH(path mapClass, SFCs) creates a figure showing a path through
%   the environment.
%   SFCs provide functions for drawing ellipses: drawEllipsoids();

persistent map decomps init;
if isempty(init)
    map = mapClass;
    decomps = SFCs;
    init = 1;
end

figure('Name','Animation');

hold on;
for i = 1:size(map.blocks,1)
    block = map.blocks(i, :);
    drawBlock(block);
end

path = paths{1};

% draw Inflated obstacle
for i = 1:size(map.blocks,1)
    margin = map.margin;
    block = map.blocks(i, :);
    block(1,1:3) = block(1,1:3) - margin;
    block(1,4:6) = block(1,4:6) + margin;
    block(7) = max(150, block(7));  block(8) = max(150, block(8));   block(9) = max(150, block(9));     %% set color
    drawBlock(block);
end

if size(path,1) > 0
    % pcshow(path, [0,1,0],'MarkerSize', 0.1);
    % The lower left corner position, width and height. 260 here is exactly 7cm
%     set(gcf,'position', [500 500 260 220]);
%     set(gcf,'position', [500 500 400 400]);   % big pic
%     set(gcf,'position', [500 500 300 300]);   % small pic
    set(gcf,'color','w');
    set(gca,'color','w');

    %% Display the unit of x y z
%     xlabel('x length(m)');
%     ylabel('y length(m)');
%     zlabel('z length(m)');

end

% Set the axis range
axis_add = 2*map.res(1);
axis([map.boundary.ld(1)-axis_add, map.boundary.ru(1)+axis_add,   ...
      map.boundary.ld(2)-axis_add, map.boundary.ru(2)+axis_add,   ...
      map.boundary.ld(3)-axis_add, map.boundary.ru(3)+axis_add])

hold on
for j = 1 : size(paths, 2)
    if j == 1
        % JPS's result
%         plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3);
    elseif j == 2
        % JPS() + simple path() result:
%         plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','#A2142F','LineWidth',3);
        plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3);
    end
end
hold off

set(gca,'DataAspectRatio',[0.1 0.1 0.1]);

% drawEllipsoid = true;
if (exist('drawEllipsoid') && (drawEllipsoid == true))
    decomps{1}.drawEllipsoids();
%     decomps{2}.drawEllipsoids();
end

grid on

view(30, 10);
hold off;

end

function drawBlock(block)

    x = [ones(4,1) * block(1); ones(4,1) * block(4)];
    y = [ones(2,1) * block(5); ones(2,1) * block(2); ones(2,1) * block(5); ones(2,1) * block(2)];
    z = [block(3);block(6);block(3);block(6);block(3);block(6);block(3);block(6)];

    vert = [x, y, z];
    fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];
    c = block(7:9)/255;
    patch('Vertices',vert,'Faces',fac,...
          'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2);

    x = [ones(4,1) * block(1); ones(4,1) * block(4)];
    y = [block(2);block(5);block(2);block(5);block(2);block(5);block(2);block(5)];
    z = [ones(2,1) * block(3); ones(2,1) * block(6); ones(2,1) * block(3); ones(2,1) * block(6)];

    vert = [x, y, z];
    fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];
    c = block(7:9)/255;
    patch('Vertices',vert,'Faces',fac,...
          'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2);
end


%Simulation test case
%
% Author: Jonathan Karr, jkarr@stanford.edu
% Author: Jared Jacobs, jmjacobs@stanford.edu
% Affiliation: Covert Lab, Department of Bioengineering, Stanford University
% Last updated: 7/13/2010
classdef Simulation_KnowledgeBase_Test < TestCase
    methods
        function this = Simulation_KnowledgeBase_Test(name)
            this = this@TestCase(name);
        end

        function testInitializationFromKnowledgeBase(~)
            dbConnectionParameters = getConfig();
            database = edu.stanford.covert.db.MySQLDatabase(dbConnectionParameters);
            kbWID = edu.stanford.covert.cell.kb.KnowledgeBaseUtil.selectLatestKnowledgeBase(database);
            kb = edu.stanford.covert.cell.kb.KnowledgeBase(database, kbWID);

            sim = edu.stanford.covert.cell.sim.Simulation(kb.states, kb.processes);
            sim.initializeConstants(kb);
            sim.applyOptions('lengthSec', 10, 'verbosity', 0, 'seed', 1);
            fitter = edu.stanford.covert.cell.sim.util.FitConstants(sim, struct('method', 'heuristic', 'verbosity', 0));
            fitter.run();
            sim.run();

            assertElementsAlmostEqual(...
                1 / (9.0 * 3600) * log(2), sim.state('MetabolicReaction').growth(:,:,1),...
                'relative', 35e-2);

            database.close();
        end
    end
end


function [a, b] = isintent(this,intent)
% Correspondance between fieldnames and NIfTI intent codes
% FORMAT ind = isintent(this,intent)
% this    -  GIfTI object
% intent  -  fieldnames
% a       -  indices of found intent(s)
% b       -  indices of dataarrays of found intent(s)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: isintent.m 6345 2015-02-20 12:25:50Z guillaume $

a = [];
b = [];
if ischar(intent), intent = cellstr(intent); end
for i=1:length(this(1).data)
    switch this(1).data{i}.attributes.Intent(14:end)
        case 'POINTSET'
            [tf, loc] = ismember('vertices',intent);
            if tf
                a(end+1) = loc;
                b(end+1) = i;
            end
            [tf, loc] = ismember('mat',intent);
            if tf
                a(end+1) = loc;
                b(end+1) = i;
            end
        case 'TRIANGLE'
            [tf, loc] = ismember('faces',intent);
            if tf
                a(end+1) = loc;
                b(end+1) = i;
            end
        case 'VECTOR'
            [tf, loc] = ismember('normals',intent);
            if tf
                a(end+1) = loc;
                b(end+1) = i;
            end
        case 'NODE_INDEX'
            [tf, loc] = ismember('indices',intent);
            if tf
                a(end+1) = loc;
                b(end+1) = i;
            end
        case cdata
            [tf, loc] = ismember('cdata',intent);
            if tf
                a(end+1) = loc;
                b(end+1) = i;
            end
            if strcmp(this(1).data{i}.attributes.Intent(14:end),'LABEL')
                [tf, loc] = ismember('labels',intent);
                if tf
                    a(end+1) = loc;
                    b(end+1) = i;
                end
            end
        otherwise
            fprintf('Intent %s is ignored.\n',this.data{i}.attributes.Intent);
    end
end
%[d,i] = unique(a);
%if length(d) < length(a)
%    warning('Several fields match intent type. Using first.');
%    a = a(i);
%    b = b(i);
%end

function c = cdata

c = {
'NONE'
'CORREL'
'TTEST'
'FTEST'
'ZSCORE'
'CHISQ'
'BETA'
'BINOM'
'GAMMA'
'POISSON'
'NORMAL'
'FTEST_NONC'
'CHISQ_NONC'
'LOGISTIC'
'LAPLACE'
'UNIFORM'
'TTEST_NONC'
'WEIBULL'
'CHI'
'INVGAUSS'
'EXTVAL'
'PVAL'
'LOGPVAL'
'LOG10PVAL'
'ESTIMATE'
'LABEL'
'NEURONAMES'
'GENMATRIX'
'SYMMATRIX'
'DISPVECT'
'QUATERNION'
'DIMLESS'
'TIME_SERIES'
'RGB_VECTOR'
'RGBA_VECTOR'
'SHAPE'
'CONNECTIVITY_DENSE'
'CONNECTIVITY_DENSE_TIME'
'CONNECTIVITY_PARCELLATED'
'CONNECTIVITY_PARCELLATED_TIME'
'CONNECTIVITY_CONNECTIVITY_TRAJECTORY'
};