%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function self = despike(self, spiketype, maxRatio)
    % rsam.despike Despike a rsam object by comparing ratios of
    % concurrent samples. Checks for spikes lasting 1 or 2 samples.
    %   rsamdespiked = rsamobject.despike(spiketype, maxRatio)

    %   Example 1: Remove spikes which are at least 10 times
    %   adjacent samples. Store these in s.spikes.
    %        s = s.despike('spikes', 10)
    %
    %   Example 2: Remove spikes which are at least 3 times
    %   adjacent samples. Store these in s.events.
    %        s = s.despike('events', 3)
    %
    %   Inputs:
    %       maxRatio - Maximum ratio that defines "normal" data
    %                       compared to surrounding samples
    %   Outputs:
    %       s = rsam object with spikes removed

    % NOTE: THIS METHOD REQUIRES EXTRA PROPERTIES FOR rsam CLASS
        %spikes = []; % a vector of rsam objects that describe large spikes
        % in the data. Populated after running 'despike' method. These are
        % removed simultaneously from the data vector.
        %transientEvents = []; % a vector of rsam objects that describe
        % transient events in the data that might correspond to vt, rf, lp
        % etc. Populated after running 'despike' method with the
        % 'transientEvents' argument. These are not removed from the data
        % vector, but are instead returned in the continuousData vector.

    % find spikes lasting 1 sample only
    y= self.data;
    spikeNumber = 0;
    for i=2:length(self.data)-1
        if self.data(i)>maxRatio*self.data(i-1)
            if self.data(i)>maxRatio*self.data(i+1)
                %sample i is an outlier
                y(i) = mean([self.data(i-1) self.data(i+1)]);
                spikeNumber = spikeNumber + 1;
                %spikes(spikeNumber) = spike(self.dnum(i), self.data(i) - y(i), y(i), '');
                spikes(spikeNumber) = rsam('dnum', self.dnum(i), 'data', self.data(i) - y(i),  'sta', self.sta, 'chan', self.chan, 'seismogram_type', self.seismogram_type, 'units', self.units, 'measure', self.measure);
                disp(sprintf('%s: sample %d, time %s, before %f, this %f, after %f. Replacing with %f',upper(spiketype),i, datestr(self.dnum(i)), self.data(i-1), self.data(i), self.data(i+1), y(i)));
            end
        end
    end

    % find spikes lasting 2 samples
    for i=2:length(self.data)-2
        if self.data(i)>maxRatio*self.data(i-1) & self.data(i+1)>maxRatio*self.data(i-1)
            if self.data(i)>maxRatio*self.data(i+2) & self.data(i+1)>maxRatio*self.data(i+2)
                %samples i & i+1 are outliers
                y(i:i+1) = mean([self.data(i-1) self.data(i+2)]);
                spikeNumber = spikeNumber + 1;
                % spikes(spikeNumber) = spike( ...
                %     self.dnum(i:i+1), ...
                %     self.data(i:i+1) - y(i:i+1), ...
                %     y(i:i+1), '' );
                spikes(spikeNumber) = rsam('dnum', self.dnum(i:i+1), 'data', self.data(i:i+1) - y(i:i+1),  'sta', self.sta, 'chan', self.chan, 'seismogram_type', self.seismogram_type, 'units', self.units, 'measure', self.measure);
                disp(sprintf('%s: sample %d, time %s, before %f, these %f %f, after %f. Replacing with %f',upper(spiketype), i, datestr(self.dnum(i)), self.data(i-1), self.data(i), self.data(i+1), self.data(i+2), y(i)));
            end
        end
    end

    % find spikes lasting 3 samples - could be a short as 62
    % seconds
    if exist('spikes', 'var')
        if ~strcmp(spiketype, 'spikes') % only makes sense for events - an actual telemetry spike will be 1 or 2 samples long only (a few seconds)
            for i=2:length(self.data)-3
                if self.data(i)>maxRatio*self.data(i-1) & self.data(i+1)>maxRatio*self.data(i-1) & self.data(i+2)>maxRatio*self.data(i-1)
                    if self.data(i)>maxRatio*self.data(i+3) & self.data(i+1)>maxRatio*self.data(i+3) & self.data(i+2)>maxRatio*self.data(i+3)
                        %samples i & i+1 are outliers
                        y(i:i+2) = mean([self.data(i-1) self.data(i+3)]);
                        spikeNumber = spikeNumber + 1;
                        % spikes(spikeNumber) = spike( ...
                        %     self.dnum(i:i+1), ...
                        %     self.data(i:i+1) - y(i:i+1), ...
                        %     y(i:i+1), '' );
                        spikes(spikeNumber) = rsam('dnum', self.dnum(i:i+2), 'data', self.data(i:i+2) - y(i:i+2), 'sta', self.sta, 'chan', self.chan, 'seismogram_type', self.seismogram_type, 'units', self.units, 'measure', self.measure);
                        disp(sprintf('%s: sample %d, time %s, before %f, these %f %f %f, after %f. Replacing with %f',upper(spiketype), i, datestr(self.dnum(i)), self.data(i-1), self.data(i), self.data(i+1), self.data(i+2), self.data(i+3), y(i)));
                    end
                end
            end
        end
    end

    self.data = y;
    if exist('spikes', 'var')
        if strcmp(spiketype, 'spikes')
            self.spikes = spikes;
        else
            self.transientEvents = spikes;
        end
    end
end


function [t, x, u] = snd_mpc(snd, output_data, varargin)
    warning off all;
%%
    snd.net_load = snd.load *2 - snd.PV/3 - snd.wind; %Power Balance

    %   Solve the optimal control problem
    t_Start = tic;
    [snd.u0, V_current, exitflag, output] = snd_solveOptimalControlProblem( snd );
    t_Elapsed = toc( t_Start );
    %V_current

    if ( snd.iprint >= 1 )
        printSolution(snd.printClosedloopData, snd.mpciter, snd.xmeasure, snd.u0, snd.iprint, exitflag, t_Elapsed, output_data);
    end

    %Save current information:
    x_estimated = snd_computeOpenloopSolution( snd , snd.u0 );

    t = output;
    x = x_estimated;
    u = snd.u0;

end


function varargout = plot(sF,varargin)
%
% Syntax
%
%   plot(sF)
%
% Input
%  sF - @S2FunHarmonicSym
%
% See also
% S2Fun/plot

% create a new figure if needed
%[mtexFig,isNew] = newMtexFigure('datacursormode',@tooltip,varargin{:});

% get plotting region
if sF.antipodal, varargin = [varargin,'antipodal']; end
sR = sF.s.fundamentalSector(varargin{:});

% set axes alignment
if isa(sF.s,'crystalSymmetry')
  varargin = [sF.s.plotOptions,varargin];
end

% perform plotting
[varargout{1:nargout}] = sF.plot@S2Fun(sR,sF.s,varargin{:});

function txt = tooltip(varargin)

[h_local,~,value] = getDataCursorPos(mtexFig);

h_local = Miller(h_local,sF.s,'uvw');
h_local = round(h_local,'tolerance',3*degree);
txt = [xnum2str(value) ' at ' char(h_local)];

end

end


function [Ef, Varf, xtnn, xt1, xt2] = gp_cpred(gp,x,y,xt,ind,varargin)
%GP_CPRED Conditional predictions using specific covariates
%
%  Description
%    GP_CPRED(GP,X,Y,XT,IND,OPTIONS) does predictions using only
%    covariates specified in vector IND. Other covariates are fixed to
%    either mean, median or values chosen by user. Returns predictions for
%    latent values, variance and corresponding inputs. If IND=0, only time
%    is used as a covariate for Cox-PH model.
%
%   OPTIONS is optional parameter-value pair
%      method - which value to fix the not used covariates, 'median'
%               (default), 'mean' or 'mode'
%      var    - vector specifying optional values for not used covariates,
%               elements corresponding to mean/median values should
%               be set to NaN.
%      plot   - option for plotting, 'off' (default) or 'on'
%      normdata - a structure with fields xmean, xstd, ymean, and ystd
%               to allow plotting in the original data scale (see
%               functions normdata and denormdata)
%      target - option for choosing what is computed 'mu' (default),
%               'f' or 'cdf'
%      tr     - Euclidean distance treshold for not using grid points when
%               doing predictions with 2 covariates, default 0.25
%      predcf - an index vector telling which covariance functions are
%               used for prediction. Default is all (1:gpcfn).
%               See additional information below.
%      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.
%      zt     - optional observed quantity in triplet (xt_i,yt_i,zt_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, the expected
%               value for the ith case.

ip=inputParser;
ip.FunctionName = 'GP_CPRED';
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.addRequired('xt',  @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('yt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addRequired('ind', @(x) ~isempty(x) && isvector(x))
ip.addParamValue('var',  [], @(x) isreal(x))
ip.addParamValue('z', [], @(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('method', 'median', @(x)  ismember(x, {'median', 'mean' 'mode'}))
ip.addParamValue('plot', 'off', @(x)  ismember(x, {'on', 'off'}))
ip.addParamValue('tr', 0.25, @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('target', 'mu', @(x) ismember(x,{'f','mu','cdf'}))
ip.addParamValue('prct', [5 50 95], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('normdata', struct(), @(x) isempty(x) || isstruct(x))
ip.addParamValue('xlabels', [], @(x) isempty(x) || iscell(x));
ip.parse(gp, x, y, xt, ind, varargin{:});
zt=ip.Results.zt;
options=struct();
options.predcf=ip.Results.predcf;
options.prct=ip.Results.prct;
options.tstind=ip.Results.tstind;
method = ip.Results.method;
vars = ip.Results.var;
plot_results = ip.Results.plot;
tr = ip.Results.tr;
target = ip.Results.target;
if strcmp(target,'f')
    options = rmfield(options,'prct');
end
yt=ip.Results.yt;
if ~isempty(yt)
  options.yt=yt;
end
z=ip.Results.z;
if ~isempty(z)
  options.z=z;
end
if ~isempty(zt)
  options.zt=zt;
end
if isempty(zt)
  options.zt=z;
end
% normdata
nd=ip.Results.normdata;
ipnd=inputParser;
ipnd.FunctionName = 'normdata';
ipnd.addParamValue('xmean',zeros(1,size(x,2)),@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('xstd',ones(1,size(x,2)),@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('xlog',zeros(1,size(x,2)),@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('ymean',0,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('ystd',1,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.addParamValue('ylog',0,@(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))));
ipnd.parse(nd);
nd=ipnd.Results;

[tmp, nin] = size(x);

if iscell(gp)
  liktype=gp{1}.lik.type;
else
  liktype=gp.lik.type;
end

if isequal(liktype, 'Coxph') && isequal(target,'mu')
    target='f';
    warning('GP_CPRED: Target ''mu'' not applicable for a Cox-PH model. Switching to target ''f''')
end

if ~isempty(vars) && (~isvector(vars) || length(vars) ~= nin)
  error('Vector defining fixed variable values must be same length as number of covariates')
end

xto=xt; [n,tmp]=size(xto);

if length(ind)==1

  if ind~=0
    [xtnn, iu] = unique(xt(:,ind));
  else
    xtnn = xt(1,:);
    iu = 1;
  end
  if ~isempty(z)
    options.zt = options.zt(iu);
  end
  if ~isempty(yt)
    options.yt = options.yt(iu);
  end
  xt = repmat(feval(method,xt), size(xtnn,1), 1);
  if ~isempty(vars)
    xt(:,~isnan(vars)) = repmat(vars(~isnan(vars)), length(xtnn), 1);
  end

  if ind>0
    xt(:,ind) = xtnn;
  end
  if ~strcmp(liktype, 'Coxph')
    switch target
      case 'f'
        [Ef, Varf] = gp_pred(gp, x, y, xt, options);
      case 'mu'
        prctmu = denormdata(gp_predprctmu(gp, x, y, xt, options),nd.ymean,nd.ystd);
        Ef = prctmu; Varf = [];
      case 'cdf'
        cdf = gp_predcdf(gp, x, y, xt, options);
        Ef = cdf; Varf = [];
    end
  else
    [Ef1,Ef2,Covf] = pred_coxph(gp,x,y,xt,'z',options.z,'zt',options.zt);
    nt=size(Ef1,1);
    if ind>0
      % conditional posterior given Ef1=E[Ef1]
      Ef = Ef2;
      Varf = diag(Covf(nt+1:end,nt+1:end)-Covf(nt+1:end,1:nt)*(Covf(1:nt,1:nt)\Covf(1:nt,nt+1:end)));
    else
      % conditional posterior given Ef2=E[Ef2]
      Ef = Ef1;
      Varf = diag(Covf(1:nt, 1:nt)-Covf(1:nt,nt+1:end)*(Covf(nt+1:end,nt+1:end)\Covf(nt+1:end,1:nt)));
      xtnn = gp.lik.xtime;
    end
  end
  if isequal(plot_results, 'on')
    if ind>0
      if ind>=1&numel(nd.xmean)>=ind
        xtnn=denormdata(xtnn,nd.xmean(ind),nd.xstd(ind));
      end
      deltadist=gp_finddeltadist(gp);
      if ~ismember(ind,deltadist)
        switch target
          case 'f'
            plot(xtnn, Ef, 'ob', xtnn, Ef, '-k', xtnn, Ef-1.64*sqrt(Varf), '--b', xtnn, Ef+1.64*sqrt(Varf), '--b')
          case 'mu'
            plot(xtnn, prctmu(:,2), 'ob', xtnn, prctmu(:,2), '-k', xtnn, prctmu(:,1), '--b', xtnn, prctmu(:,3), '--b')
          case 'cdf'
            plot(xtnn, Ef, 'o-b')
        end
      else
        switch target
          case 'f'
            plot(xtnn, Ef, 'ob', [xtnn xtnn]',[Ef-1.64*sqrt(Varf) Ef+1.64*sqrt(Varf)]', '-b')
            xlim([1.5*xtnn(1)-0.5*xtnn(2) 1.5*xtnn(end)-.5*xtnn(end-1)])
            set(gca,'xtick',xtnn)
          case 'mu'
            plot(xtnn, prctmu(:,2), 'ob', [xtnn xtnn]',[prctmu(:,1) prctmu(:,3)]', '-b')
            xlim([1.5*xtnn(1)-0.5*xtnn(2) 1.5*xtnn(end)-.5*xtnn(end-1)])
            set(gca,'xtick',xtnn)
          case 'cdf'
            plot(xtnn, Ef, 'ob')
        end
      end
    else
      % use stairs for piecewise constant baseline hazard
      xtnn = gp.lik.stime;
      [xx,yy]=stairs(xtnn, [Ef;Ef(end)]);
      [xx,yyl]=stairs(xtnn, [Ef-1.64*sqrt(Varf);Ef(end)-1.64*sqrt(Varf(end))]);
      [xx,yyu]=stairs(xtnn, [Ef+1.64*sqrt(Varf);Ef(end)+1.64*sqrt(Varf(end))]);
      plot(xx, yy, '-k', xx, yyl, '--b', xx, yyu, '--b')
    end
  end

elseif length(ind)==2

  uu1=unique(xt(:,ind(1)));
  uu2=unique(xt(:,ind(2)));
  nu1=numel(uu1);
  nu2=numel(uu2);
  if nu1==2 || nu2==2
    % First or second covariate binary

    if nu1>2 && nu2==2
      % switch indeces, so that binary covariate is first
      tmp=ind(1);ind(1)=ind(2);ind(2)=tmp;
      tmp=uu1;uu1=uu2;uu2=tmp;
    end

    xt1=xt(xt(:,ind(1))==uu1(1),:);
    xt2=xt(xt(:,ind(1))==uu1(2),:);
    [xtnn1, iu1] = unique(xt1(:,ind(2)));
    [xtnn2, iu2] = unique(xt2(:,ind(2)));

    options1=options;
    options2=options;
    if ~isempty(z)
      options1.zt = options.zt(iu1);
      options2.zt = options.zt(iu2);
    end

    xt1 = repmat(feval(method,xt1), length(xtnn1), 1);
    xt2 = repmat(feval(method,xt2), length(xtnn2), 1);
    if ~isempty(vars)
      xt1(:,~isnan(vars)) = repmat(vars(~isnan(vars)), length(xtnn1), 1);
      xt2(:,~isnan(vars)) = repmat(vars(~isnan(vars)), length(xtnn2), 1);
    end
    xt1(:,ind(1)) = uu1(1); xt1(:,ind(2)) = xtnn1;
    xt2(:,ind(1)) = uu1(2); xt2(:,ind(2)) = xtnn2;

    if ~strcmp(liktype, 'Coxph')
      switch target
        case 'f'
          [Ef1, Varf1] = gp_pred(gp, x, y, xt1);
          [Ef2, Varf2] = gp_pred(gp, x, y, xt2);
        case 'mu'
          prctmu1 = denormdata(gp_predprctmu(gp, x, y, xt1, options1),nd.ymean,nd.ystd);
          prctmu2 = denormdata(gp_predprctmu(gp, x, y, xt2, options2),nd.ymean,nd.ystd);
      end
    else
      [Ef11,Ef12,Covf] = pred_coxph(gp,x,y,xt1, 'z', z);
      Ef1 = Ef12; Varf1 = diag(Covf(size(Ef11,1)+1:end,size(Ef11,1)+1:end));
      [Ef21,Ef22,Covf] = pred_coxph(gp,x,y,xt2, 'z', z);
      Ef2 = Ef22; Varf2 = diag(Covf(size(Ef21,1)+1:end,size(Ef21,1)+1:end));
    end

    if isequal(plot_results, 'on')
      xtnn1=denormdata(xtnn1,nd.xmean(ind(2)),nd.xstd(ind(2)));
      xtnn2=denormdata(xtnn2,nd.xmean(ind(2)),nd.xstd(ind(2)));
      if nu1>2 && nu2==2
        lstyle10='or';lstyle11='-r';lstyle12='--r';
        lstyle20='ob';lstyle21='-b';lstyle22='--b';
      else
        lstyle10='ob';lstyle11='-b';lstyle12='--b';
        lstyle20='or';lstyle21='-r';lstyle22='--r';
      end
      deltadist=gp_finddeltadist(gp);
      if ~ismember(ind(2),deltadist)
        switch target
          case 'f'
            plot(xtnn1, Ef1, lstyle10, xtnn1, Ef1, lstyle11, xtnn1, Ef1-1.64*sqrt(Varf1), lstyle12, xtnn1, Ef1+1.64*sqrt(Varf1), lstyle12); hold on;
            plot(xtnn2, Ef2, lstyle20, xtnn2, Ef2, lstyle21, xtnn2, Ef2-1.64*sqrt(Varf2), lstyle22, xtnn2, Ef2+1.64*sqrt(Varf2), lstyle22); hold off;
          case 'mu'
            plot(xtnn1, prctmu1(:,2), lstyle10, xtnn1, prctmu1(:,2), lstyle11, xtnn1, prctmu1(:,1), lstyle12, xtnn1, prctmu1(:,3), lstyle12); hold on;
            plot(xtnn2, prctmu2(:,2), lstyle20, xtnn2, prctmu2(:,2), lstyle21, xtnn2, prctmu2(:,1), lstyle22, xtnn2, prctmu2(:,3), lstyle22); hold off;
        end
      else
        delta=(diff(xtnn1(1:2))/10);
        switch target
          case 'f'
            plot(xtnn1-delta, Ef1, lstyle10, [xtnn1 xtnn1]'-delta, [Ef1-1.64*sqrt(Varf1) Ef1+1.64*sqrt(Varf1)]', lstyle11); hold on;
            plot(xtnn2+delta, Ef2, lstyle20, [xtnn2 xtnn2]'+delta, [Ef2-1.64*sqrt(Varf2) Ef2+1.64*sqrt(Varf2)]', lstyle21); hold off;
            xlim([1.5*xtnn1(1)-0.5*xtnn1(2) 1.5*xtnn1(end)-.5*xtnn1(end-1)])
          case 'mu'
            plot(xtnn1-delta, prctmu1(:,2), lstyle10, [xtnn1 xtnn1]'-delta, [prctmu1(:,1) prctmu1(:,3)]', lstyle11); hold on;
            plot(xtnn2+delta, prctmu2(:,2), lstyle20, [xtnn2 xtnn2]'+delta, [prctmu2(:,1) prctmu2(:,3)]', lstyle21); hold off;
            xlim([1.5*xtnn1(1)-0.5*xtnn1(2) 1.5*xtnn1(end)-.5*xtnn1(end-1)])
        end
      end
    end
    switch target
      case 'f'
        Ef = {Ef1  Ef2}; Varf = {Varf1 Varf2}; xtnn={xtnn1 xtnn2};
      case 'mu'
        Ef = {prctmu1 prctmu2}; Varf = {[] []}; xtnn={xtnn1 xtnn2};
    end

  else
    % both the first and the second covariate are non-binary
    deltadist=gp_finddeltadist(gp);
    if ~ismember(ind(1),deltadist)
      xtnn1 = linspace(min(xt(:,ind(1))), max(xt(:,ind(1))), 20);
    else
      xtnn1 = unique(xt(:,ind(1)));
    end
    if ~ismember(ind(2),deltadist)
      xtnn2 = linspace(min(xt(:,ind(2))), max(xt(:,ind(2))), 20);
    else
      xtnn2 = unique(xt(:,ind(2)));
    end
    [XT1, XT2] = meshgrid(xtnn1, xtnn2); XT1=XT1(:); XT2=XT2(:);
    xtnn1=denormdata(xtnn1,nd.xmean(ind(1)),nd.xstd(ind(1)));
    xtnn2=denormdata(xtnn2,nd.xmean(ind(2)),nd.xstd(ind(2)));
    if ~isempty(z)
      options.zt = repmat(options.zt(1), size(XT1));
    end
    xt = repmat(feval(method,xt), length(XT1), 1);
    if ~isempty(vars)
      xt(:,~isnan(vars)) = repmat(vars(~isnan(vars)), length(XT1), 1);
    end
    xt(:,ind) = [XT1 XT2];
    if ~strcmp(liktype, 'Coxph')
      switch target
        case 'f'
          [Ef, Varf] = gp_pred(gp, x, y, xt);
        case 'mu'
          prctmu = gp_predprctmu(gp, x, y, xt, options, 'prct', 50);
          Ef = prctmu; Varf = [];
        case 'cdf'
          cdf = gp_predcdf(gp, x, y, xt, options);
          Ef = cdf; Varf = [];
      end
    else
      [Ef1,Ef2,Covf] = pred_coxph(gp,x,y,xt, options);
      Ef = Ef2; Varf = diag(Covf(size(Ef1,1)+1:end,size(Ef1,1)+1:end));
    end

    indd = zeros(size(Ef));

    for i2=1:n
      for i3=1:numel(XT1)
        if sqrt(sum((xto(i2,ind)-xt(i3,ind)).^2)) < tr
          indd(i3) = 1;
        end
      end
    end

    XT1(indd==0) = NaN; XT2(indd==0) = NaN; Ef(indd==0) = NaN; Varf(indd==0) = NaN;

    if isequal(plot_results, 'on')
      xtnn1=denormdata(xtnn1,nd.xmean(ind(1)),nd.xstd(ind(1)));
      xtnn2=denormdata(xtnn2,nd.xmean(ind(2)),nd.xstd(ind(2)));
      surf(reshape(XT1,numel(xtnn2),numel(xtnn1)), reshape(XT2,numel(xtnn2),numel(xtnn1)), reshape(Ef,numel(xtnn2),numel(xtnn1)))
      view(2)
      axis tight
      shading flat
      colormap(mapcolor(Ef,repmat(nanmedian(Ef(:)),[1 2])))
      colorbar('EastOutside')
    end

    %xtnn = [XT1(indd==1), XT2(indd==1)]; Ef = Ef(indd==1); Varf = Varf(indd==1);
    xtnn = [XT1, XT2];
  end

else
  error('Only 1 or 2 covariates can be defined for predicting')
end

end


function varsigma = fgplvmPosteriorVar(model, X);

% FGPLVMPOSTERIORVAR Variances of the posterior at points given by X.
% FORMAT
% DESC returns the posterior mean and variance for a given set of
% points.
% ARG model : the model for which the posterior will be computed.
% ARG x : the input positions for which the posterior will be
% computed.
% RETURN sigma : the variances of the posterior distributions.
%
% SEEALSO : gpPosteriorVar, fgplvmCreate
%
% COPYRIGHT : Neil D. Lawrence, 2009

% FGPLVM

varsigma = gpPosteriorVar(model, X);


function [F0,Fz,Fx,Fxz] = getFzxfromSET(F,z,x)

F = sdpvar(F);
F0 = -getbasematrix(F,0);
zvars = getvariables(z);
xvars = getvariables(x);

for i = 1:length(xvars)
    Fx{i} = -getbasematrix(F,xvars(i));
end

for i = 1:length(zvars)
    Fz{i} = -getbasematrix(F,zvars(i));
end

for i = 1:length(xvars)
    for j = 1:length(zvars)
        Fxz{i,j} = -getbasematrix(F,getvariables(x(i)*z(j)));
    end
end


%% Main function to generate tests
function tests = testSuite

tests = functiontests(localfunctions);

end

%% Test Functions
function wgs2gcjTest(testCase)
% test wgs to gcj

TESTS =  [
    31.1774276, 121.5272106, 31.17530398364597, 121.531541859215; %shanghai
    22.543847, 113.912316, 22.540796131694766, 113.9171764808363; %shenzhen
    39.911954, 116.377817, 39.91334545536069, 116.38404722455657; %beijing
    39.739200,-104.990300,39.739200,-104.990300; %Denver edge case
    -27.469800,153.025100, -27.469800,153.025100; % Brisbane edge case
    -22.906800,-43.172900, -22.906800,-43.172900; % Rio de Janerio edge case
    ];

[actLat actLong] = wgs2gcj(TESTS(:,1),TESTS(:,2));
expLat = TESTS(:,3);
expLong = TESTS(:,4);

verifyEqual(testCase,actLat,expLat, 'AbsTol' , .000001);
verifyEqual(testCase,actLong,expLong, 'AbsTol' , .000001);

end

function gcj2wgsTest(testCase)
% test wgs to gcj

TESTS =  [
    31.1774276, 121.5272106, 31.17530398364597, 121.531541859215; %shanghai
    22.543847, 113.912316, 22.540796131694766, 113.9171764808363; %shenzhen
    39.911954, 116.377817, 39.91334545536069, 116.38404722455657; %beijing
    39.739200,-104.990300,39.739200,-104.990300; %Denver edge case
    -27.469800,153.025100, -27.469800,153.025100; % Brisbane edge case
    -22.906800,-43.172900, -22.906800,-43.172900; % Rio de Janerio edge case
    ];

[actLat actLong] = gcj2wgs(TESTS(:,3),TESTS(:,4));
expLat = TESTS(:,1);
expLong = TESTS(:,2);

measurementDistance = distance(expLat, expLong, actLat,actLong);
actualDistance = zeros(length(measurementDistance),1);
verifyEqual(testCase,actualDistance,measurementDistance, 'AbsTol' , 5);

end

function gcj2wgsExactTest(testCase)
% test wgs to gcj

TESTS =  [
    31.1774276, 121.5272106, 31.17530398364597, 121.531541859215; %shanghai
    22.543847, 113.912316, 22.540796131694766, 113.9171764808363; %shenzhen
    39.911954, 116.377817, 39.91334545536069, 116.38404722455657; %beijing
    39.739200,-104.990300,39.739200,-104.990300; %Denver edge case
    -27.469800,153.025100, -27.469800,153.025100; % Brisbane edge case
    -22.906800,-43.172900, -22.906800,-43.172900; % Rio de Janerio edge case
    ];

[actLat actLong] = gcj2wgs_exact(TESTS(:,3),TESTS(:,4));
expLat = TESTS(:,1);
expLong = TESTS(:,2);

measurementDistance = distance(expLat, expLong, actLat,actLong);
actualDistance = zeros(length(measurementDistance),1);
verifyEqual(testCase,actualDistance,measurementDistance, 'AbsTol' , .5);

end

function mars2wgsTest(testCase)
% test wgs to gcj should be very exact and very fast....

TESTS =  [
    31.1774276, 121.5272106, 31.17530398364597, 121.531541859215; %shanghai
    22.543847, 113.912316, 22.540796131694766, 113.9171764808363; %shenzhen
    39.911954, 116.377817, 39.91334545536069, 116.38404722455657; %beijing
    39.739200,-104.990300,39.739200,-104.990300; %Denver edge case
    -27.469800,153.025100, -27.469800,153.025100; % Brisbane edge case
    -22.906800,-43.172900, -22.906800,-43.172900; % Rio de Janerio edge case
    ];

[actLat actLong] = mars2wgs(TESTS(:,3),TESTS(:,4));
expLat = TESTS(:,1);
expLong = TESTS(:,2);

measurementDistance = distance(expLat, expLong, actLat,actLong);
actualDistance = zeros(length(measurementDistance),1);
verifyEqual(testCase,actualDistance,measurementDistance, 'AbsTol' , .01);

end

function wgs2bdTest(testCase)
% test wgs to gcj should be very exact and very fast....

 TESTS_BD = [
    29.199786, 120.019809, 29.196131605295484, 120.00877901149691;
    29.210504, 120.036455, 29.206795749156136, 120.0253853970846
    ];

[actLat actLong] = wgs2bd(TESTS_BD(:,3),TESTS_BD(:,4));
expLat = TESTS_BD(:,1);
expLong = TESTS_BD(:,2);

verifyEqual(testCase,actLat,expLat, 'AbsTol' , .00005);
verifyEqual(testCase,actLong,expLong, 'AbsTol' , .00005);

end

function bd2wgsTest(testCase)
% test wgs to gcj should be very exact and very fast....

 TESTS_BD = [
    29.199786, 120.019809, 29.196131605295484, 120.00877901149691;
    29.210504, 120.036455, 29.206795749156136, 120.0253853970846
    ];

[actLat actLong] = bd2wgs(TESTS_BD(:,1),TESTS_BD(:,2));
expLat = TESTS_BD(:,3);
expLong = TESTS_BD(:,4);

verifyEqual(testCase,actLat,expLat, 'AbsTol' , .00005);
verifyEqual(testCase,actLong,expLong, 'AbsTol' , .00005);

end

function gcj2bdTest(testCase)
% test wgs to gcj should be very exact and very fast....
% bd lat, bd long, gcj lat, gcj long
 TESTS_BD = [39.90851,116.43351,39.90245,116.42703];

[actLat actLong] = gcj2bd(TESTS_BD(:,3),TESTS_BD(:,4));
expLat = TESTS_BD(:,1);
expLong = TESTS_BD(:,2);

verifyEqual(testCase,actLat,expLat, 'AbsTol' , .00005);
verifyEqual(testCase,actLong,expLong, 'AbsTol' , .00005);

end

function bd2gcjTest(testCase)
% test wgs to gcj should be very exact and very fast....
% bd lat, bd long, gcj lat, gcj long
 TESTS_BD = [39.90851,116.43351,39.90245,116.42703];

[actLat actLong] = bd2gcj(TESTS_BD(:,1),TESTS_BD(:,2));
expLat = TESTS_BD(:,3);
expLong = TESTS_BD(:,4);

verifyEqual(testCase,actLat,expLat, 'AbsTol' , .00005);
verifyEqual(testCase,actLong,expLong, 'AbsTol' , .00005);

end

function deg2utmTest(testCase)
% test wgs to utm should be very exact and very fast....
% LAT, LONG, X, Y

TestUTMDEG = [36.903784,-104.545898,540455.18,4084295.17];

[actUTMx, actUTMy] = deg2utm(TestUTMDEG(:,1),TestUTMDEG(:,2));
expUTMx = TestUTMDEG(:,3);
expUTMy = TestUTMDEG(:,4);

verifyEqual(testCase,actUTMx,expUTMx, 'AbsTol' , .1);
verifyEqual(testCase,actUTMy,expUTMy, 'AbsTol' , .1);

end

function utm2degTest(testCase)
% test wgs to gcj should be very exact and very fast....
% LAT, LONG, X, Y

TestUTMDEG = [36.903784,-104.545898,540455.18,4084295.17];

[actLat, actLong] = utm2deg(TestUTMDEG(:,3),TestUTMDEG(:,4),'13N');
expLat = TestUTMDEG(:,1);
expLong = TestUTMDEG(:,2);

verifyEqual(testCase,actLat,expLat, 'AbsTol' , .00001);
verifyEqual(testCase,actLong,expLong, 'AbsTol' , .00001);

end

function speedTest(testCase)
% test wgs to gcj
n = 50000;
TESTS =  [
    31.1774276, 121.5272106, 31.17530398364597, 121.531541859215; %shanghai
    22.543847, 113.912316, 22.540796131694766, 113.9171764808363; %shenzhen
    39.911954, 116.377817, 39.91334545536069, 116.38404722455657; %beijing
    39.739200,-104.990300,39.739200,-104.990300; %Denver edge case
    -27.469800,153.025100, -27.469800,153.025100; % Brisbane edge case
    -22.906800,-43.172900, -22.906800,-43.172900; % Rio de Janerio edge case
    ];

tic;
for i=1:n
    [actLat actLong] = wgs2gcj(TESTS(:,1),TESTS(:,2));
end
times.wgs2gcj = toc;

tic;
for i=1:n
    [actLat actLong] = mars2wgs(TESTS(:,1),TESTS(:,2));
end
times.mars2gcj = toc;

tic;
for i=1:n
    [actLat actLong] = gcj2wgs(TESTS(:,1),TESTS(:,2));
end
times.gcj2wgs = toc;

tic;
for i=1:n
    [actLat actLong] = gcj2wgs_exact(TESTS(:,1),TESTS(:,2));
end
times.gcj2wgs_exact = toc;

times

end

 function setupOnce(testCase)  % do not change function name

    testsBD = [
    29.199786, 120.019809, 29.196131605295484, 120.00877901149691;
    29.210504, 120.036455, 29.206795749156136, 120.0253853970846
    ];

 end

 function teardownOnce(testCase)  % do not change function name
% change back to original path, for example
end

function setup(testCase)  % do not change function name
% open a figure, for example
end

function teardown(testCase)  % do not change function name
% close figure, for example
end


function h = image_histogram(P,varargin)
% :Usage:
% ::
%
%    h = image_histogram(P,[method(string)],[range])
%
% eliminates 0, NaN voxels from either image
% red then blue
%
% :Inputs:
%
%   **P:**
%        is one or two images in string array
%
%   **Methods:**
%        'def'
%
%   **range:**
%        optional 3rd input, range of values to include in histogram
%
% :Example:
% ::
%
%    h = image_histogram('p_Omnibus.img','def',[0 1-eps]);
%
% ..
%    tor wager
% ..

V = spm_vol(P); %v = spm_read_vols(V);

v = iimg_read_vols(V);

if length(varargin) > 0, meth = varargin{1};  else, meth = 'def'; end
if length(varargin) > 1, range = varargin{2};  else, range = [-Inf Inf]; end

for f = 1:size(P,1), [dum,ff{f}] = fileparts(P(f,:)); end

%p2 = which('/Users/tor/Documents/tor_scripts/3DheadUtility/canonical_brains/scalped_avg152T1_graymatter.img')
%[p2,p2new] = reslice_imgs(P(1,:),p2,1);
%maskV = spm_read_vols(spm_vol(p2new)); maskV = maskV(:);

switch meth
    case 'def'

        disp('Plotting histograms')

        v1 = squeeze(v(:,:,:,1));
        if size(v,4) > 1
            v2 = squeeze(v(:,:,:,2));
        end

        v1 = v1(:);
        if size(v,4) > 1, v2 = v2(:); ,end

        if size(v,4) > 1
            wh = isnan(v1) | isnan(v2) | v1 == 0 | v2 == 0 ...
                | v1 < range(1) | v1 > range(2) | v2 < range(1) | v2 > range(2); %| ~maskV;
        else
            wh = isnan(v1) | v1 == 0 | v1 < range(1) | v1 > range(2);
        end

        v1(wh) = [];
        if size(v,4) > 1
            v2(wh) = [];
        else
            v2 = [];
        end

        [h,xx] = hist([v1;v2],max(10,length(v1)./300));
        h1 = hist(v1,xx);

        if size(v,4) > 1, h2 = hist(v2,xx); end

    otherwise
        error('unknown action string')
end

figure('Color','w');

if size(v,4) > 1, plot(xx,h1,'r',xx,h2,'b');
else
        plot(xx,h1,'r');
end

set(gca,'FontSize',16)

legend(ff)

return


function [f,J,Q] = spm_fx_cmm(x,u,P,M)
% state equations for canonical neural-mass and mean-field models
% FORMAT [f,J,Q] = spm_fx_cmm(x,u,P,M)
%
% x - states and covariances
%
% x(i,j,k)        - k-th state of j-th population of i-th source
%                   i.e., running over sources, pop. and states
%
%   population: 1 - excitatory spiny stellate cells (input cells)
%               2 - superficial pyramidal cells     (forward output cells)
%               3 - inhibitory interneurons         (intrisic interneuons)
%               4 - deep pyramidal cells            (backward output cells)
%
%        state: 1 V  - voltage
%               2 gE - conductance (excitatory)
%               3 gI - conductance (inhibitory)
%
%--------------------------------------------------------------------------
% refs:
%
% Marreiros et al (2008) Population dynamics under the Laplace assumption
%
% See also:
%
% Friston KJ.
% The labile brain. I. Neuronal transients and nonlinear coupling. Philos
% Trans R Soc Lond B Biol Sci. 2000 Feb 29;355(1394):215-36.
%
% McCormick DA, Connors BW, Lighthall JW, Prince DA.
% Comparative electrophysiology of pyramidal and sparsely spiny stellate
% neurons of the neocortex. J Neurophysiol. 1985 Oct;54(4):782-806.
%
% Brunel N, Wang XJ.
% What determines the frequency of fast network oscillations with irregular
% neural discharges? I. Synaptic dynamics and excitation-inhibition
% balance. J Neurophysiol. 2003 Jul;90(1):415-30.
%
% Brunel N, Wang XJ.
% Effects of neuromodulation in a cortical network model of object working
% memory dominated by recurrent inhibition. J Comput Neurosci. 2001
% Jul-Aug;11(1):63-85.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_cmm.m 5369 2013-03-28 20:09:27Z karl $

% get dimensions and configure state variables
%--------------------------------------------------------------------------
ns   = size(M.x,1);                      % number of sources
np   = size(M.x,2);                      % number of populations per source
nk   = size(M.x,3);                      % number of states per population
x    = reshape(x,ns,np,nk);              % hidden states

% extrinsic connection strengths
%==========================================================================

% exponential transform to ensure positivity constraints
%--------------------------------------------------------------------------
A{1} = exp(P.A{1});                      % forward
A{2} = exp(P.A{2});                      % backward
C    = exp(P.C);                         % subcortical

% detect and reduce the strength of reciprocal (lateral) connections
%--------------------------------------------------------------------------
for i = 1:length(A)
    L    = (A{i} > exp(-8)) & (A{i}' > exp(-8));
    A{i} = A{i}./(1 + 8*L);
end

% intrinsic connection strengths
%==========================================================================

% condition specific effects
%--------------------------------------------------------------------------
G    = full(P.H);
if any(P.G)
    G(2,2,:) = squeeze(G(2,2,:)) + P.G;
end
G(2,2,:) = 0;
G    = exp(G);

% connectivity switches
%==========================================================================
% 1 - excitatory spiny stellate cells (granular input cells)
% 2 - superficial pyramidal cells     (forward  output cells)
% 3 - inhibitory interneurons         (intrisic interneuons)
% 4 - deep pyramidal cells            (backward output cells)

% extrinsic connections (F B) - from superficial and deep pyramidal cells
%--------------------------------------------------------------------------
SA   = [1   0 ;
        0   1 ;
        0   2 ;
        0   0]/8;

% intrinsic connections (np x np) - excitatory
%--------------------------------------------------------------------------
GE   = [ 0    0    0    0
         4    0    0    0
         4    0    0    2
         0    4    0    0];

% intrinsic connections (np x np) - inhibitory
%--------------------------------------------------------------------------
GI   = [ 8    2    2    0
         0    8    2    0
         0    0   32    0
         0    0    8  128];

% rate constants (ns x np) (excitatory 4ms, inhibitory 16ms)
%--------------------------------------------------------------------------
KE   = 1000/4;                             % excitatory rate constants
KI   = exp(-P.T)*[1/64 1/32 1/16 1/16]*1000; % inhibitory rate constants

% Voltages
%--------------------------------------------------------------------------
VL   = -70;                               % reversal  potential leak (K)
VE   =  60;                               % reversal  potential excite (Na)
VI   = -90;                               % reversal  potential inhib (Cl)
VR   = -40;                               % threshold potential

CV   = exp(P.CV).*[16 16 32 16]/1000;     % membrane capacitance
GL   = 1;                                 % leak conductance

% mean-field effects:
%==========================================================================

% neural-mass approximation to covariance of states
%----------------------------------------------------------------------
Vx   = exp(P.S)*32;

% mean population firing and afferent extrinsic input
%--------------------------------------------------------------------------
m      = spm_Ncdf_jdw(x(:,:,1),VR,Vx);    % mean firing rate
a(:,1) = A{1}*m(:,2);                     % forward afference
a(:,2) = A{2}*m(:,4);                     % backward afference

% Averge background activity and exogenous input
%==========================================================================
BE     = exp(P.E)/2;

% input
%--------------------------------------------------------------------------
if isfield(M,'u')

    % endogenous input
    %----------------------------------------------------------------------
    U = u(:)*8/1000;

else

    % exogenous input
    %----------------------------------------------------------------------
    U = C*u(:)/1000;

end

% flow over every (ns x np) subpopulation
%==========================================================================
f     = x;
for i = 1:ns

        % intrinsic coupling
        %------------------------------------------------------------------
        E = (G(:,:,i).*GE)*m(i,:)';
        I = (G(:,:,i).*GI)*m(i,:)';

        % extrinsic coupling (excitatory only) and background activity
        %------------------------------------------------------------------
        E = E + BE + SA*a(i,:)';

        % and exogenous input(U)
        %------------------------------------------------------------------
        E(1) = E(1) + U(i);

        % Voltage
        %==================================================================
        f(i,:,1) =          (GL*(VL - x(i,:,1)) + ...
                      x(i,:,2).*(VE - x(i,:,1)) + ...
                      x(i,:,3).*(VI - x(i,:,1)) )./CV;

        % Conductance
        %==================================================================
        f(i,:,2) = (E' - x(i,:,2)).*KE;
        f(i,:,3) = (I' - x(i,:,3)).*KI(i,:);

end

% vectorise equations of motion
%==========================================================================
f = spm_vec(f);

if nargout < 2, return, end

% Jacobian
%==========================================================================
J = spm_cat(spm_diff(M.f,x,u,P,M,1));

if nargout < 3, return, end

% Delays
%==========================================================================
% Delay differential equations can be integrated efficiently (but
% approximately) by absorbing the delay operator into the Jacobian
%
%    dx(t)/dt     = f(x(t - d))
%                 = Q(d)f(x(t))
%
%    J(d)         = Q(d)df/dx
%--------------------------------------------------------------------------
% [specified] fixed parameters
%--------------------------------------------------------------------------
D  = [2 16];

d  = -D.*exp(P.D)/1000;
Sp = kron(ones(nk,nk),kron( eye(np,np),eye(ns,ns)));  % states: same pop.
Ss = kron(ones(nk,nk),kron(ones(np,np),eye(ns,ns)));  % states: same source

Dp = ~Ss;                            % states: different sources
Ds = ~Sp & Ss;                       % states: same source different pop.
D  = d(2)*Dp + d(1)*Ds;

% Implement: dx(t)/dt = f(x(t - d)) = inv(1 - D.*dfdx)*f(x(t))
%                     = Q*f = Q*J*x(t)
%--------------------------------------------------------------------------
Q  = spm_inv(speye(length(J)) - D.*J);


function varargout = squeeze(varargin)
%SQUEEZE   Squeeze a DISKFUN to one variable, if possible.
%   G = squeeze(F) returns a DISKFUN if F depends on THETA and R in
%   polar coordinates. If F depends only on the theta-variable, a row
%   CHEBFUN is returned. If it depends on just the r-variable, a
%   column CHEBFUN is returned.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = squeeze@separableApprox(varargin{:});

end


function[str_all, Irec] = icassp12(example, Img, Nloops, SHOW_IMGS);

if nargin < 4
  SHOW_IMGS = false;
  if nargin < 3
    Nloops = 1;
    if nargin < 2
      Img = lena;
      if nargin < 1;
	example = 'gauss_sp_denoise';
      end
    end
  end
end

%  SHOW_IMGS = true;

%  example = 'icassp12'
%  example = 'gauss_sp_denoise';
%  example = 'gauss_random_denoise';

%  Img = 'Clena';
%  Img = 'peppers';
%  Img = 'lena';
%  Img = 'barb';
%  Img = 'cman';
%  Img = 'Cboats';

%  Img = 'all';

%  Nloops = 1; % This value will be change to 10 if example == icassp12

%%%%%%%%%%%%%%%%%%%%%%%%
%        Setup         %
%%%%%%%%%%%%%%%%%%%%%%%%

  % Check for SSIM
  SSIM_CODE = check4ssim();

  % Setup simulation
  [Nloops, Img] = setupSim(example, Nloops, Img);

  % open string (for reports)
  str_all = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       Input images        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if( strcmp(lower(Img), 'all') )

  all = 1;
  if( strcmp(example, 'icassp12') )
    Img = {'lena', 'goldhl', 'cman512', 'barb', 'Clena', 'Cbarb', 'Cboats', 'Cgoldhl' };
  else
    Img = {'lena', 'peppers', 'goldhl.', 'cman512', 'Clena', 'Cpeppers', 'Cmandrill'};
  end

  NImgs = length(Img);

else

  all = 0;
  NImgs = 1;

end  % _END_ IF(strcmp(lower(Img))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%          loop over Img
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for t = 1:NImgs,

  if( all == 1 )
    tImg = Img{t};
  else
    tImg = lower(Img);
  end

  [Ig Color] = selectImage(tImg);

  if Color == 1
    SSIM_CODE = 0;
  end

  disp(' ');
  disp(sprintf('%s Color %d', tImg, Color));
  disp(' ');

  [Nrows Ncols Ndims] = size(Ig);

  % Regularization parameter of the L2 / L1
  [lambdaS_l2 Lambda_O Lambda_update] = setRegParameter(example, Ndims);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %  loops for average results    %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  for loops=1:Nloops

    %%%%%%%%%%%%%%%%%
    %  add noise    %
    %%%%%%%%%%%%%%%%%

    % Noise level
    sigmaGauss = [5/255, 10/255, 15/255 ];
    NGauss = length(sigmaGauss);

    spnoise = [0.3 0.5 0.7];
    Nspnoise = length(spnoise);

    randomnoise = [0.1 0.2 0.3];
    NRnoise = length(randomnoise);

    % noisy images
    clear InCell;
    [InCell noiseName vNoise nNoise] = addNoise(Ig, example, sigmaGauss, NGauss, spnoise, Nspnoise, randomnoise, NRnoise);
    nNoisyIm = length(InCell);

    % loop over noisy images
    for k=1:length(InCell)

      In = InCell{k};

%^^^^^% ^^^^^^^^^^^^^^^^^^^^^
%     %   Clock is ticking
%     % ---------------------
      tstart = tic;

      % First estimate the pixels that are corrupted
      switch lower(example)

        case{'gauss_sp_denoise'}
          S0 = adaptMedian_colfilt(In);		% estimate S&P corrupted pixels

        case{'gauss_random_denoise'}	% estimate random value (impulse noise) corrupted pixels
          S0 = estimateRValImpulse(In);

      end	% _END_ SWITCH

      % Set lambda (regularization) matrix
      lambda_l1l2 = zeros(Nrows, Ncols, 2*Ndims);

      for d=1:Ndims,
          lambda_l1l2(:,:,d)  =  Lambda_O*(S0(:,:,d) ~= 0);
          lambda_l1l2(:,:,Ndims+d) = lambdaS_l2(k)*(S0(:,:,d)==0);
      end

      % =================================================
      % ---------- Setup for IRN_L1L2  ------------------

      pars_irn_adapt = irntvInputPars('l1tv_l2tv');

      pars_irn_adapt.adapt_epsR   = 1;
      pars_irn_adapt.epsR_cutoff  = 0.01;
      pars_irn_adapt.adapt_epsF   = 1;
      pars_irn_adapt.epsF_cutoff  = 0.05;

      pars_irn_adapt.pcgtol_ini = 1e-4;

      switch lower(example)
        case{'gauss_sp_denoise'}
          pars_irn_adapt.loops      = 1;
          IRNloops = 7;
        case{'gauss_random_denoise'}
          pars_irn_adapt.loops      = 2;
          IRNloops = 4;
      end

      pars_irn_adapt.U0      = In;   % necessary the for adapt case

      % =================================================

      % 1st call
      Ig_L1L2 = irntv(In, {}, lambda_l1l2, pars_irn_adapt);

      % loop

      for m = 2:IRNloops

        for d=1:Ndims
          lambda_l1l2(:,:,d)  =  Lambda_update*lambda_l1l2(:,:,d);
        end

        pars_irn_adapt.U0      = Ig_L1L2;	% reset initial sol. to previous sol.

        Ig_L1L2 = irntv(In, {}, lambda_l1l2, pars_irn_adapt);

      end % _END_ FOR(m)

      % denoised image
      Irec{k} = Ig_L1L2;

%^^^^^% ^^^^^^^^^^^^^^^^^^^^^
%     %    Time performance
      l1l2_time(k, loops) = toc(tstart);
%     % ---------------------

      if(SHOW_IMGS)
        figure; imagesc( Normalize(Irec{k}) ); axis image; axis off; colormap gray;
      end

      % Reconstruction quality assessment
      [psnr_l1l2(k, loops), snr_l1l2(k, loops), ssim_l1l2(k, loops)] = qualityAssessment(Ig, Irec{k}, SSIM_CODE);

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

      %           Generate reports

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

      if(k==1)

        if(Color == 0) str = sprintf('Img: grayscale %s (denoising)\n', lower(tImg));
        else str = sprintf('Img: color %s (denoising)\n', lower(tImg));
        end

        str_all = [ str_all str ];

        str = sprintf(' sigma/%s \t SNR \t SSIM \t PSNR    Time\n', noiseName);
        str_all = [ str_all str ];

      end

        str = sprintf(' %1.0f/255 -- %1.1f \t %1.2f \t %1.2f \t %1.2f    %1.2f\n', ...
                   255*sigmaGauss( rem(k,NGauss) + NGauss*(rem(k,NGauss)==0) ), vNoise( floor((k-1)/nNoise)+1 ), ...
                   snr_l1l2(k, loops), ssim_l1l2(k, loops), psnr_l1l2(k, loops), l1l2_time(k, loops) );

%        if(k<=nNoisyIm)
%          str = sprintf(' %1.0f/255 -- %1.1f \t %1.2f \t %1.2f \t %1.2f    %1.2f\n', ...
%                     255*sigmaGauss( rem(k,NGauss) + NGauss*(rem(k,NGauss)==0) ), vNoise( floor((k-1)/nNoise)+1 ), ...
%                     snr_l1l2(k, loops), ssim_l1l2(k, loops), psnr_l1l2(k, loops), l1l2_time(k, loops) );
%        else
%          str = sprintf(' %1.0f/255 -- %1.1f \t %1.2f \t %1.2f \t %1.2f    %1.2f\n', ...
%                     255*sigmaGauss( rem(k,NGauss) + NGauss*(rem(k,NGauss)==0) ), vNoise( floor((k-1-nNoisyIm)/nNoise)+1 ), ...
%                     snr_l1l2(k, loops), ssim_l1l2(k, loops), psnr_l1l2(k, loops), l1l2_time(k, loops) );
%        end
      str_all = [ str_all str ];

      if(k==length(InCell))
        str = sprintf(' \n\n');
      end

    end % _END_ FOR(length(InCell))

  end % FOR(loops)

  disp(str_all)

end % FOR(t)  -- test Image

end

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

function[SSIM_CODE] = check4ssim()

  SSIM_CODE = exist('ssim_index');
  if( SSIM_CODE == 0 )
    disp('NOTE:');
    disp('  The function ssim_index (code for [SSIM]) is not in your path...');
    disp('  You may download it from:');
    disp('  http://www.ece.uwaterloo.ca/~z70wang/research/ssim/');
    disp('  ');
    disp('  Disabling SSIM reports');
    disp('  ');
    disp('  [SSIM]  Z. Wang, A. Bovik, H. Sheikh and E. Simoncelli, ');
    disp('         "Image quality assessment: From error visibility to');
    disp('         structural similarity " ');
    disp('         IEEE Transactions on Image Processing, 2004, 13:4(600-612).');

  end

end

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

function[Nloops, Img] = setupSim(example, Nloops, Img)

  tmpN = Nloops;
  tmpI = Img;

  if( strcmp(example, 'icassp12') )

    disp('  Running the "icassp11" example...');
    disp('  This script reproduces the Exprimental Results of ');
    disp('  "Mixed Gaussian-impulse Noise Image Restoration via');
    disp('    Total Variation"');
    disp(' ')
    disp('  This script takes sometime (ten-trial average)');
    disp(' ')
    tmpN = 10;

    if( ~strcmp(tmpI, 'all') )

      disp('NOTE:');
      disp('  If you run the "icassp12" example, you should set Img = "all" ...');
      disp('  setting Img = "all" ');
      tmpI = 'all';
    end

  end

  Nloops = tmpN;
  Img    = tmpI;

end

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

function[Ig Color] = selectImage(tImg)

  switch lower(tImg)

    case{'lena'}
      Ig = double( imread('gray_imgs/lena_gray_512.png') ) / 255;
      Color = 0;

    case{'peppers'}
      Ig = double( imread('gray_imgs/peppers_gray.png') ) / 255;
      Color = 0;

    case{'cman'}
      Ig = double( imread('gray_imgs/cameraman_256x256.tiff') ) / 255;
      Color = 0;

    case{'cman512'}
      Ig = double( imread('gray_imgs/cameraman_512x512.tiff') ) / 255;
      Color = 0;

    case{'goldhl'}
      Ig = double( imread('gray_imgs/goldhill_gray.png') ) / 255;
      Color = 0;

    case{'barb'}
      Ig = double( imread('gray_imgs/barbara_gray512.png') ) / 255;
      Color = 0;

    case{'clena'}
      Ig = double( imread('color_imgs/lena_color_512.png') ) / 255;
      SSIM_CODE = 0;
      Color = 1;

    case{'cbarb'}
      Ig = double( imread('color_imgs/barbara_color.png') ) / 255;
      SSIM_CODE = 0;
      Color = 1;

    case{'cboats'}
      Ig = double( imread('color_imgs/boats_color.png') ) / 255;
      SSIM_CODE = 0;
      Color = 1;

    case{'cgoldhl'}
      Ig = double( imread('color_imgs/goldhill_color.png') ) / 255;
      SSIM_CODE = 0;
      Color = 1;

    case{'none'}
      disp(' ');
      disp('Select a test image (see code for an example)...');
      disp(' ');
      disp('Exiting ICASSP12 test code...');
      return;

    otherwise
      error('Not a valid image\n');

  end  % _END_ SWITCH(Img)

end

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

function[lambdaS_l2, Lambda_O, Lambda_update] = setRegParameter(example, Ndims)

    switch lower(example)

    case{'gauss_sp_denoise'}
      if(Ndims == 1)
        lambdaS_l2 = [ 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025 ];
      else
        lambdaS_l2 = 1.5*[ 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025 ];
      end

      Lambda_O = 5;
      Lambda_update = 1.15;

    case{'gauss_random_denoise'}
      if(Ndims == 1)
        lambdaS_l2 = [ 1.25*0.0075, 0.0185, 0.025, ...
                 1.25*0.0075, 0.0185, 0.025, ...
                 1.25*0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025 ];
%                   0.0075, 0.0185, 0.025 ]/8;
      else
        lambdaS_l2 = 1.5*[ 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025, ...
                 0.0075, 0.0185, 0.025 ]/8;
      end

      Lambda_O = 5;
      Lambda_update = 1.15;

    end

end

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

function[InCell, noiseName, vNoise, nNoise] = addNoise(Ig, example, sigmaGauss, NGauss, spnoise, Nspnoise, randomnoise, NRnoise)

    % Add Gaussian noise
    Ig_05L2 = imnoise(Ig,'gaussian', 0, (sigmaGauss(1)^2) );
    Ig_10L2 = imnoise(Ig,'gaussian', 0, (sigmaGauss(2)^2) );
    Ig_15L2 = imnoise(Ig,'gaussian', 0, (sigmaGauss(3)^2) );

    % Add impulse noise (S&P or random value)
    switch lower(example)

      case{'gauss_sp_denoise'}

	noiseName = 'S&P';
	vNoise    = spnoise;
	nNoise    = Nspnoise;

	for k=1:Nspnoise,

	  InCell{NGauss*(k-1)+1} = imnoise(Ig_05L2, 'salt & pepper', spnoise(k));
	  InCell{NGauss*(k-1)+2} = imnoise(Ig_10L2, 'salt & pepper', spnoise(k));
	  InCell{NGauss*(k-1)+3} = imnoise(Ig_15L2, 'salt & pepper', spnoise(k));
	end

      case{'gauss_random_denoise'}

	noiseName = 'RandVal';
	vNoise    = randomnoise;
	nNoise    = NRnoise;

	[Nrows Ncols Ndims] = size(Ig);
	dummy = 0.5*ones(Nrows, Ncols, Ndims);

	for k=1:NRnoise,

	  mask = (imnoise(dummy, 'salt & pepper', randomnoise(k)) ~= 0.5);
	  InCell{NGauss*(k-1)+1} = Ig_05L2.*(1-mask) + mask.*rand(Nrows, Ncols, Ndims);

	  mask = (imnoise(dummy, 'salt & pepper', randomnoise(k)) ~= 0.5);
	  InCell{NGauss*(k-1)+2} = Ig_10L2.*(1-mask) + mask.*rand(Nrows, Ncols, Ndims);

	  mask = (imnoise(dummy, 'salt & pepper', randomnoise(k)) ~= 0.5);
	  InCell{NGauss*(k-1)+3} = Ig_15L2.*(1-mask) + mask.*rand(Nrows, Ncols, Ndims);
	end

    end % _END_ SWITCH

end

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

function[S0] = estimateRValImpulse(In)

          thres_diffmed= 0.2;
          window_side= 2;

          road_size= 7;
          thres_ranked= 0.05;

          [Nrows Ncols Ndims] = size(In);

          if(Ndims == 1)

            mask_diff_median = diff_median_blk( In, window_side, thres_diffmed);
            mask_ranked_mean = ranked_mean( In, window_side, road_size, thres_ranked);
            S0 = mask_ranked_mean & mask_diff_median;

          else

            for d=1:Ndims,
              mask_diff_median = diff_median_blk( In(:,:,d), window_side, thres_diffmed);
              mask_ranked_mean = ranked_mean( In(:,:,d), window_side, road_size, thres_ranked);
              S0(:,:,d) = mask_ranked_mean & mask_diff_median;
            end

          end

end

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

function[psnr_l1l2, snr_l1l2, ssim_l1l2] = qualityAssessment(Ig, Ig_L1L2, SSIM_CODE)

      psnr_l1l2 = psnr(Ig, Ig_L1L2);
      snr_l1l2  = snr(Ig, Ig_L1L2);
      if(SSIM_CODE)
        ssim_l1l2 = ssim_index(255*Normalize(Ig), 255*Normalize(Ig_L1L2));
      else
        ssim_l1l2 = NaN;
      end

end

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

function[y] = Normalize(x)

  y = (x - min(x(:)))/(max(x(:)) - min(x(:)));

end


function [mu, varsigma] = modelPosteriorMeanVar(model, X);

% MODELPOSTERIORMEANVAR Mean and variances of the posterior at points given by X.
% FORMAT
% DESC returns the posterior mean and variance for a given set of
% points.
% ARG model : the model for which the posterior will be computed.
% ARG x : the input positions for which the posterior will be
% computed.
% RETURN mu : the mean of the posterior distribution.
% RETURN sigma : the variances of the posterior distributions.
%
% SEEALSO : modelCreate
%
% COPYRIGHT : Neil D. Lawrence, 2009

% MLTOOLS

  fhandle = str2func([model.type 'PosteriorMeanVar']);
  if str2num(version('-release'))>13
    [mu, varsigma] = fhandle(model, X);
  else
    [mu, varsigma] = feval(fhandle, model, X);
  end


function odf = rotate(odf,rot,varargin)
% rotate ODF
%
% Input
%  odf - @SO3Fun
%  q   - @rotation
%
% Output
%  rotated odf - @SO3Fun

ss = odf.SS.Laue;
if length(ss)>2 && ~any(rot == ss(:))
  warning('Rotating an ODF with specimen symmetry will remove the specimen symmetry')
  odf.SS = specimenSymmetry;
end

for i = 1:length(odf.components)
  odf.components{i} = odf.components{i}.rotate(rot,varargin{:});
end


classdef NSLS < ALGORITHM
% <multi> <real/integer>
% Multiobjective optimization framework based on nondominated sorting and
% local search

%------------------------------- Reference --------------------------------
% B. Chen, W. Zeng, Y. Lin, and D. Zhang, A new local search-based
% multiobjective optimization algorithm, IEEE Transactions on Evolutionary
% Computation, 2015, 19(1): 50-73.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();

            %% Optimization
            while Algorithm.NotTerminated(Population)
                Offspring  = Operator(Problem,Population);
                Population = EnvironmentalSelection([Population,Offspring],Problem.N);
            end
        end
    end
end


classdef GridOptions < handle
    properties
        dx              double {isfinite}                   % east-west distance between grid points
        dy              double                              % north-south distance between grid points
        dz              double                              % vertical distance between grid points
        % Defines whether horizontal distances are constant, or whether they scale as the grid
        % deviates from the equator.
        followMeridians     matlab.lang.OnOffSwitchState  	= 'off'
        gridEntireArea      matlab.lang.OnOffSwitchState    = 'off'
        FixedAnchorPoint    double                          = []
        % grid cannot be used past these limits [xmin xmax ymin ymax]
        AbsoluteGridLimits  (1,4) double
        GridType            GridTypes                       = GridTypes.XY;
        RefEllipsoid        referenceEllipsoid
    end

    properties(Dependent, Hidden)
        horizUnits
        dzUnits
    end

    methods
        function obj = GridOptions(grid_type, dx_dy_dz, RefEllipsoid, varargin)
            % old usage: obj = GridOptions(dx, dy, dz_km, horiz_units, follow_meridians, gridEntireArea)
            % GRIDOPTIONS defines parameters that are used to create ZmapGrids
            %
            % obj = GRIDOPTIONS( GRID_TYPE, [dx,dy,dz], ELLPSOID ) where GRID_TYPE is a GridType,
            % and defines the spatial orientation of the grid.  [dx,dy,dz] are a doublet or triplet
            % describing the grid spacing, and ELLIPSOID defines how everything will be interpreted.
            % the ellipsoid's LengthUnit determines the units of this grid.  (See the option for
            % FollowMeridians, below). An specifying the Ellipsoid value of nonEllipsoid() will
            % declare that this is a cartesian grid instead of a geodetic one.
            %
            % obj = GRIDOPTIONS( ..., 'FollowMeridians',true) Horizontal Units are degrees and grid
            % therefore narrows toward the poles.
            %
            % obj = GRIDOPTIONS(...,'GridEntireArea',true) grids entire area.
            %
            % For a horizontal grid, the first parameter should be [dx, dy] or [dx,dy,nan]
            % For a cross-sectional grid, the first parameter should be [dS nan dz], where dS is
            % a linear distance.
            % For a 3-d grid, the first parameters should be [dx,dy,dz]
            %
            %  see also ZmapGrid, referenceEllipsoid, GridType, nonEllipsoid
            p = inputParser;
            p.addRequired('GridType');
            p.addRequired('dx_dy_dz');
            p.addRequired('RefEllipsoid');
            p.addParameter('FollowMeridians', false);
            p.addParameter('GridEntireArea', false);
            p.parse(grid_type, dx_dy_dz, RefEllipsoid,varargin{:})

            obj.GridType        = p.Results.GridType;
            obj.RefEllipsoid    = p.Results.RefEllipsoid;
            obj.followMeridians = p.Results.FollowMeridians;
            obj.gridEntireArea  = p.Results.GridEntireArea;

            if iscartesian(obj.RefEllipsoid)
                assert(p.Results.FollowMeridians == false, 'Cannot follow Meridians for a cartesian grid');
            end

            switch obj.GridType
                case 'XY'
                    obj.dx = dx_dy_dz(1);
                    obj.dy = dx_dy_dz(2);
                    obj.dz = 1;

                case 'XZ'
                    obj.dx = dx_dy_dz(1);
                    obj.dy = nan;
                    obj.dz = dx_dy_dz(end);

                case 'XYZ'
                    obj.dx = dx_dy_dz(1);
                    obj.dy = dx_dy_dz(2);
                    obj.dz = dx_dy_dz(3);
            end
        end

        function u = get.horizUnits(obj) % for backwards compatibility
            if obj.followMeridians
                u = 'degree';
            else
                u = obj.RefEllipsoid.LengthUnit;
            end
        end

        function u = get.dzUnits(obj) % for backwards compatibility
            u = obj.RefEllipsoid.LengthUnit;
        end

    end % methods section

    methods(Static)
        function [mygrid, mygridopts] = fromDialog(existing_gridopt, ellipsoid, shape)
            % FROMDIALOG shows an interactive dialog box allowing user to choose grid
            mygrid = [];
            mygridopts = [];
            if ~exist('ellipsoid', 'var')
                ellipsoid = getappdata(groot, 'ZmapDefaultReferenceEllipsoid');
            end

            if ~exist('shape', 'var')
                shape = ShapeGeneral();
            end
            if exist('existing_gridopt', 'var') && ~isempty(existing_gridopt)
                gc = grid_chooser(ellipsoid, existing_gridopt, shape);
            else
                gc = grid_chooser(ellipsoid, [], shape);
            end
            gc.ResultDump = @set_values;
            waitfor(gc)
            function set_values(g, gop)
                mygrid = g;
                mygridopts = gop;
            end
        end
    end %static methods
end


function img = mrExtractImgVol(volume,iSize,numSlices,samp, ...
                                 dataRange,badVal)
%
%  img = mrExtractImgVol(volume,iSize,numSlices,samp,dataRange,badVal)
%
%AUTHOR:  Engel
%DATE:    November, 1994
%PURPOSE:
%  Extracts and interpolates the values from a volume of data.
% The locations to be returned are listed in the parameter samp.
%
% The input volume is formatted so that each row is an image
%
% The sampling grid is a matrix whose columns are x,y,and z.
% For the image to be displayable using myShowImage, the sampling
% matrix should run down columns fastest, rows slowest.
%
% volume:     a volume of data represented as a vector
% iSize:      the row and col size of individual images in the volume
% numSlices:  the number of image slices in the volume
% samp:       the sample locations we wish to extract from the volume.
%
% OPTIONAL ARGUMENTS:
% dataRange:  the starting and ending Z-coordinates for the
%	      volume of data.  It is optional. If no dataRange is given
%	      the Z range is assumed to be 1:numSlices. This allows subsets
%             of the full volume to be passed in.
%
% badVal:  specifies the value that should be returned
%	   for values outside the data set.
%
% global interpflag:  the value of this global specifies whether or not
% 	              to interpolate the volume data.
%
% RETURNED:
%   img:     an image is returned, selected from the volume data
%	     according to the samp values.
%

% BW 10/19/95

global interpflag

% Check arguments
%
if (nargin < 6) 	% No badVal
 badVal = 1;
end
if (nargin >= 5) 	% There is a dataRange argument
 samp(:,3) = samp(:,3) - dataRange(1) + 1;
end
if (nargin < 4)
 error('mrExtractImgVol:  Requires 4 arguments')
end

% If the global interpolation flag is set, then interpolate the data.
% to make sure there are values at the sample values.
%
if (interpflag)

% myCinterp3 uses trilinear interpolation to create a value
% at each samp location.  It refuses to extrapolate, so if
% a sample point is outside the volume size, badVal is returned
%
 img = myCinterp3(volume,iSize,numSlices,samp,badVal)';

else

% Check the samp range to make sure we are not out of bounds.
% Points that are out of bounds will be assigned badVal
%
 bad = (samp(:,1) > iSize(2)) | (samp(:,2) > iSize(1)) ...
 | (samp(:,3) > (numSlices+.5)) | (samp(:,1) < .5)  ...
 | (samp(:,2) < 1) | (samp(:,3) < 1);

% Convert the 3d coordinates to the volume coord.
%
  volcoords = mrVcoord(round(samp),iSize);
% volcoords = mr3d21d(samp,iSize,[1,numSlices]);

%Take care of out of bounds points
%
 bad = bad | (volcoords < 1 )| (volcoords > length(volume));

%Extract data.  Notice that badVal is used here, too.
%
 img = zeros(size(volcoords));
 img(~bad) = volume(volcoords(~bad));
 img(bad) = badVal*ones(1,sum(bad));

% Replaced this line.  Does this make trouble for someone? -- BW
%
% img(isnan(img)) = badVal*ones(1,sum(isnan(img)));
end


function newDepth = SiftFu(sequenceName, frameIDs)

%{
Please cite the following paper if you use this code

Citation:
J. Xiao, A. Owens and A. Torralba
SUN3D Database: Semantic RGB-D Bundle Adjustment with Human in the Loop
Proceedings of 14th IEEE International Conference on Computer Vision (ICCV2013)
%}

addpath(genpath('SIFTransac'))
vl_setup;

global toVisualize;
toVisualize = true;

%% IO
if ~exist('sequenceName','var')
    % load demo sequence
    % look for all sequence name list at http://sun3d.csail.mit.edu/player/list.html
    %sequenceName = 'hotel_mr/scan1';
    %sequenceName = 'hotel_umd/maryland_hotel3';
    %sequenceName = 'brown_bm_1/brown_bm_1';
    sequenceName = 'mit_32_d428/bs4j179mmv';
end

% the root path of SUN3D
% change it to local if you downloaded the data
%SUN3Dpath = '/data/vision/torralba/sun3d/record/scene_final';
SUN3Dpath = 'http://sun3d.csail.mit.edu/data/';

% read intrinsic
global K;
K = reshape(readValuesFromTxt(fullfile(SUN3Dpath,sequenceName,'intrinsics.txt')),3,3)';

% file list
imageFiles = dirSmart(fullfile(SUN3Dpath,sequenceName,'image/'),'jpg');
depthFiles = dirSmart(fullfile(SUN3Dpath,sequenceName,'depth/'),'png');

% read time stamp
imageFrameID = zeros(1,length(imageFiles));
imageTimestamp = zeros(1,length(imageFiles));
for i=1:length(imageFiles)
    id_time = sscanf(imageFiles(i).name, '%d-%d.jpg');
    imageFrameID(i) = id_time(1);
    imageTimestamp(i) = id_time(2);
end
depthFrameID = zeros(1,length(depthFiles));
depthTimestamp = zeros(1,length(depthFiles));
for i=1:length(depthFiles)
    id_time = sscanf(depthFiles(i).name, '%d-%d.png');
    depthFrameID(i) = id_time(1);
    depthTimestamp(i) = id_time(2);
end

% synchronize: find a depth for each image
frameCount = length(imageFiles);
IDimage2depth = zeros(1,frameCount);
for i=1:frameCount
    [~, IDimage2depth(i)]=min(abs(double(depthTimestamp)-double(imageTimestamp(i))));
end

%plot(double(imageTimestamp)-double(depthTimestamp(IDimage2depth)))

if ~exist('frameIDs','var')
    frameIDs = 1:frameCount;
else
    frameIDs = frameIDs(frameIDs>=1 & frameIDs<=frameCount);
end

imageFiles = imageFiles(frameIDs);
depthFiles = depthFiles(IDimage2depth(frameIDs));

%% kinect fusion

% for optimization

global VMap;
global NMap;
global CMap;

global XYZcam;
global Ncam;
global IMGcam;

%% tsdf

global voxel;
global tsdf_value;
global tsdf_weight;
global tsdf_color;    tsdf_color = [];

voxel.unit = 0.01; % Kevin: 4mm = 0.004 meter. Kinect cannot go better than 3mm
voxel.mu_grid = 10; % used to be 4
voxel.size_grid = [512; 512; 1024]; % [512; 512; 512];

voxel.range(1,1) = - voxel.size_grid(1) * voxel.unit / 2;
voxel.range(1,2) = voxel.range(1,1) + (voxel.size_grid(1)-1) * voxel.unit;

voxel.range(2,1) = - voxel.size_grid(2) * voxel.unit / 2;
voxel.range(2,2) = voxel.range(2,1) + (voxel.size_grid(2)-1) * voxel.unit;

voxel.range(3,1) = -0.5; % - voxel.size_grid(3) * voxel.unit / 2;
voxel.range(3,2) = voxel.range(3,1) + (voxel.size_grid(3)-1) * voxel.unit;

voxel.mu = voxel.mu_grid * voxel.unit;

fprintf('memory = %f GB\n',  prod(voxel.size_grid) * 4 / (1024*1024*1024));
fprintf('space = %.2f m x %.2f m x %.2f m ', voxel.size_grid(1) * voxel.unit, voxel.size_grid(2) * voxel.unit, voxel.size_grid(3) * voxel.unit);
fprintf('= [%.2f,%.2f] x [%.2f,%.2f] x [%.2f,%.2f]\n',voxel.range(1,1),voxel.range(1,2),voxel.range(2,1),voxel.range(2,2),voxel.range(3,1),voxel.range(3,2));

%tsdf_value  = -ones([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)],'single');
tsdf_value  =  ones([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)],'single');
tsdf_weight = zeros([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)],'single');

% comment out this line to avoid using color
% tsdf_color  = zeros([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)], 'uint32');

f = K(1,1);

global ViewFrustumC;
ViewFrustumC = [...
    0 -320 -320  320  320;
    0 -240  240  240 -240;
    0    f    f    f    f];
ViewFrustumC = ViewFrustumC/f * 8; % 8 meter is the furthest depth of kinect

% precompute
global raycastingDirectionC;
[pX,pY]=meshgrid(1:640,1:480);

raycastingDirectionC = [pX(:)'-K(1,3); pY(:)'-K(2,3); f*ones(1,640*480)]; % clipping at 8 meter is the furthest depth of kinect
raycastingDirectionC = raycastingDirectionC ./ repmat(sqrt(sum(raycastingDirectionC.^2,1)),3,1);

%%
useSIFT = false;
if useSIFT
    MatchPairs = cell(1,length(imageFiles)-1);
end

cameraRtC2W = repmat([eye(3) zeros(3,1)], [1,1,length(imageFiles)]);

dispclim = [0 5];

for frameID = 1:length(imageFiles)

    fprintf('================================ Frame %d ================================\n',frameID);

    IMGcam = imageRead(fullfile(fullfile(SUN3Dpath,sequenceName,'image',imageFiles(frameID).name)));

    %subplot(3,4,4)
    %imagesc(IMGcam);  axis equal; axis tight;
    %drawnow;
    %title('input color');

    if frameID==1
        IMGcam1 = IMGcam;
    end

    % read the frame

    depth = depthRead(fullfile(fullfile(SUN3Dpath,sequenceName,'depth',depthFiles(frameID).name)));
    XYZcam = depth2XYZcamera(K, depth);

    if frameID==1
        XYZcam1 = XYZcam;
    end

    Ncam = vertex2normal(XYZcam);

    if toVisualize
        if frameID==1
            subplot(3,4,9)
        else
            subplot(3,4,1)
        end
        imagesc(XYZcam(:,:,3),dispclim); axis equal; axis tight
        title(sprintf('Frame %d: Input Depth',frameID));
        axis off

        if frameID==1
            subplot(3,4,10)
        else
            subplot(3,4,2)
        end
        imagesc((Ncam+1)/2); axis equal; axis tight
        title(sprintf('Frame %d: Input Normal',frameID));
        axis off

        if frameID==1
            subplot(3,4,11)
        else
            subplot(3,4,3)
        end
        raycastingDirectionW = transformRTdir(raycastingDirectionC,[eye(3) zeros(3,1)]);
        DotMap = reshape(max(0,sum(-reshape(Ncam,[480*640 3])' .* raycastingDirectionW,1)),[480 640]);
        imagesc(DotMap);
        colormap('gray'); axis equal; axis tight
        title(sprintf('Frame %d: Input Phong',frameID));
        axis off
    end

    if frameID==1
        camRtC2W = [eye(3) [0;0;0]];
    else
        MatchPairs{frameID-1} =  align2view(1,IMGcam1,XYZcam1,frameID,IMGcam,XYZcam);
        camRtC2W = MatchPairs{frameID-1}.Rt;

        if size(MatchPairs{frameID-1}.matches,2)<5
            disp('SIFT matching failed, ignoring this frame');
            continue;
        end
    end

    cameraRtC2W(:,:,frameID) = camRtC2W;

    %% update TSDF
    disp('update TSDF...');
    %tic
    updateTSDF(camRtC2W);
    %toc

    %{
    %% visualizing the voxel
    subplot(3,4,10)
    for i=1:voxel.size_grid(3)
        if min(min(min(tsdf_value(:,:,i)))) ~= max(max(max(tsdf_value(:,:,i))))
            imagesc(tsdf_weight(:,:,i)'); axis equal; axis tight; xlabel('x'); ylabel('y');
            title(['frame ' num2str(i) ' = depth ' num2str((i-1)*voxel.unit+voxel.range(3,1)) ' meter']);
            pause(0.05);
        end
    end

    subplot(3,4,10)
    for i=1:voxel.size_grid(3)
        if min(min(min(tsdf_value(:,:,i)))) ~= max(max(max(tsdf_value(:,:,i))))
            imagesc(tsdf_value(:,:,i)'); axis equal; axis tight; xlabel('x'); ylabel('y');
            title(['frame ' num2str(i) ' = depth ' num2str((i-1)*voxel.unit+voxel.range(3,1)) ' meter']);
            pause(0.05);
        end
    end
    %}

    %% ray casting for the result
    disp('ray casting');
    tic
    % ray casting result is in the world coordinate
    [VMap,NMap,tMap,CMap] = raycastingTSDFvectorized([eye(3) [0;0;0]], [0.4 8]);
    %[VMap,NMap,tMap] = raycastingTSDFdump(camRtC2WrayCasting, [0.4 8]);
    toc

    newDepth = reshape(VMap(3,:),480,640);

    if toVisualize
        subplot(3,4,5)
        imagesc(newDepth,dispclim); axis equal; axis tight; axis off
        title(sprintf('Fused Depth',frameID));
    end

    VMap = reshape(double(VMap'),480,640,3);
    VMap(:,:,4) = ~isnan(VMap(:,:,1));
    NMap = vertex2normal(VMap);
    % normalize normal map
    %NMap = NMap ./ repmat(sqrt(sum(NMap.^2,1)),3,1);
    if toVisualize
        subplot(3,4,6)
        imagesc((NMap+1)/2); axis equal; axis tight; axis off
        title(sprintf('Fused Noraml',frameID));
    end

    if toVisualize
        subplot(3,4,7)
        raycastingDirectionW = transformRTdir(raycastingDirectionC,[eye(3) zeros(3,1)]);
        DotMap = reshape(max(0,sum(-reshape(NMap,[480*640 3])' .* raycastingDirectionW,1)),[480 640]);
        imagesc(DotMap);
        colormap('gray'); axis equal; axis tight; axis off
        title(sprintf('Fused Phong',frameID));
    end

    if toVisualize
        subplot(3,4,12)
        imagesc(abs(newDepth - XYZcam1(:,:,3)),dispclim); axis equal; axis tight; axis off
        title(sprintf('Difference of Depth',frameID));

        drawnow;
    end

    %{

    subplot(3,4,6)
    imagesc(min(1,max(0,(reshape(NMap',[480 640 3])+1)/2))); axis equal; axis tight
    title(sprintf('Frame %d: rayCasting Normal Map',frameID));
    drawnow;

    %figure
    %visNMap = reshape(NMap',[480 640 3]);
    %out = visualizeNormals(-visNMap);
    %imagesc(out)
    %title('rayCasting Normal Map');

    subplot(3,4,7)
    raycastingDirectionW = transformRTdir(raycastingDirectionC,camRtC2W);
    DotMap = reshape(max(0,sum(-NMap .* raycastingDirectionW,1)),[480 640]);
    imagesc(DotMap);
    colormap('gray'); axis equal; axis tight
    title(sprintf('Frame %d: phong shading',frameID));
    drawnow;

    if ~isempty(tsdf_color)
        subplot(3,4,8)
        imagesc(reshape(CMap',[480,640,3])/255);  axis equal; axis tight;
        drawnow;
        title(sprintf('Frame %d: ray casting color',frameID));
    end

    %}

    %subplot(3,4,9)
    %imagesc(reshape(tMap,480,640)); axis equal; axis tight
    %title(sprintf('Frame %d: rayCasting tMap',frameID));
    %drawnow;

end

end

%% IO functions

function values = readValuesFromTxt(filename)
try
    values = textscan(urlread(filename),'%f');
catch
    fid = fopen(filename,'r');
    values = textscan(fid,'%f');
    fclose(fid);
end
values = values{1};
end

function XYZcamera = depth2XYZcamera(K, depth)
[x,y] = meshgrid(1:640, 1:480);
XYZcamera(:,:,1) = (x-K(1,3)).*depth/K(1,1);
XYZcamera(:,:,2) = (y-K(2,3)).*depth/K(2,2);
XYZcamera(:,:,3) = depth;
XYZcamera(:,:,4) = depth~=0;
end

function depth = depthRead(filename)
depth = imread(filename);
depth = bitor(bitshift(depth,-3), bitshift(depth,16-3));
depth = single(depth)/1000;
end
%{
    % test to make sure it is correct
    for i=0:65535
        depth = uint16(i);
        code =bitor(bitshift(depth,3),bitshift(depth,3-16));
        recoverDepth = bitor(bitshift(code,-3), bitshift(code,16-3));
        if (depth~=recoverDepth)
            fprintf('error + %d\n',i);
        end
    end
%}

function image = imageRead(filename)
image = imread(filename);
end

function files = dirSmart(page, tag)
[files, status] = urldir(page, tag);
if status == 0
    files = dir(fullfile(page, ['*.' tag]));
end
end

function [files, status] = urldir(page, tag)
if nargin == 1
    tag = '/';
else
    tag = lower(tag);
    if strcmp(tag, 'dir')
        tag = '/';
    end
    if strcmp(tag, 'img')
        tag = 'jpg';
    end
end
nl = length(tag);
nfiles = 0;
files = [];

% Read page
page = strrep(page, '\', '/');
[webpage, status] = urlread(page);

if status
    % Parse page
    j1 = findstr(lower(webpage), '<a href="');
    j2 = findstr(lower(webpage), '');
    Nelements = length(j1);
    if Nelements>0
        for f = 1:Nelements
            % get HREF element
            chain = webpage(j1(f):j2(f));
            jc = findstr(lower(chain), '">');
            chain = deblank(chain(10:jc(1)-1));

            % check if it is the right type
            if length(chain)>length(tag)-1
                if strcmp(chain(end-nl+1:end), tag)
                    nfiles = nfiles+1;
                    chain = strrep(chain, '%20', ' '); % replace space character
                    files(nfiles).name = chain;
                    files(nfiles).bytes = 1;
                end
            end
        end
    end
end
end


%IKINE_SYM  Symbolic inverse kinematics
%
% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic
% solutions of the SerialLink object ROBOT.  The cells of Q represent the
% different possible configurations.  Each cell of Q is a vector (Nx1), and
% element J is the symbolic expressions for the J'th joint angle.  The
% solution is in terms of the desired end-point pose of the robot which is
% represented by the symbolic matrix (3x4) with elements
%      nx ox ax tx
%      ny oy ay ty
%      nz oz az tz
% where the first three columns specify orientation and the last column
% specifies translation.
%
% K <= N can have only specific values:
%  - 2 solve for translation tx and ty
%  - 3 solve for translation tx, ty and tz
%  - 6 solve for translation and orientation
%
% Options::
%
% 'file',F    Write the solution to an m-file named F
%
% Example::
%
%         mdl_planar2
%         sol = p2.ikine_sym(2);
%         length(sol)
%         ans =
%               2       % there are 2 solutions
%         s1 = sol{1}  % is one solution
%         q1 = s1(1);      % the expression for q1
%         q2 = s1(2);      % the expression for q2
%
% Notes::
% - Requires the Symbolic Toolbox for MATLAB.
% - This code is experimental and has a lot of diagnostic prints.
% - Based on the classical approach using Pieper's method.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function out = ikine_sym(srobot, N, varargin)

    %
    % Given a robot model the following steps are performed:
    % 1. Convert model to symbolic form
    % 2. Find relevant trig equations and solve them for joint angles
    % 3. Write an M-file to implement the solution
    %      xikine(T)
    %      xikine(T, S) where S is a 3 vector with elements 1 or 2 to select
    %       the first or second solution for the corresponding joint.
    %
    % TODO:
    %  - handle the wrist joints, only first 3 joints so far
    %  - handle base and tool transforms

    opt.file = [];
    opt = tb_optparse(opt, varargin);

    % make a symbolic representation of the passed robot
    srobot = sym(srobot);
    q = srobot.gencoords();

    % test N DOF has an allowable value
    switch N
        case 2
        case 3
        case 6
        otherwise
            error('RTB:ikine_sym:badarg', 'Can only solve for 2,3,6 DOF');
    end

    % define symbolic elements of the homogeneous transform
    syms nx ox ax tx
    syms ny oy ay ty
    syms nz oz az tz
    syms d3

    % inits
    Q = {};
    trigsubOld = [];
    trigsubNew = [];

    % loop over each joint variable
    for j=1:N
        fprintf('----- solving for joint %d\n', j);

        % create some equations to sift through
        [left,right] = pieper(srobot, j, 'left');

        % decide which equations to look at
        if j <= 3
            % for first three joints only focus on translational part
            left = left(1:3, 4); left = left(:);
            right = right(1:3, 4); right = right(:);
        else
            % for last three joints only focus on rotational part
            left = left(1:3, 1:3); left = left(:);
            right = right(1:3, 1:3); right = right(:);
        end

        % substitute sin/cos for preceding joint as S/C, essentially removes
        % the joint variables from the equations and treats them as constants.
        if ~isempty(trigsubOld)
            left = subs(left, trigsubOld, trigsubNew);
            right = subs(right, trigsubOld, trigsubNew);
        end

        % then simplify the LHS
        %   do it after the substitution to prevent sum of angle terms being introduced
        left = simplify(left);

        % search for a solveable equation:
        %    function of current joint variable on the LHS
        %    constant element on the RHS
        k = NaN;
        for i=1:length(left)
            if hasonly(left(i), j) && isconstant(right(i))
                k = i;
                break;
            end
        end

        eq = [];

        if ~isnan(k)
            % create the equation to solve: LHS-RHS == 0
            eq = left(k) - right(k);
        else
            % ok, we weren't lucky, try another strategy

            % find all equations:
            %    function of current joint variable on the LHS

            k = [];
            for i=1:length(left)
                % has qj on the left and constant on the right
                if hasonly(left(i), j)
                    k = [k i];
                end
            end

            % hopefully we found two of them
            if length(k) < 2
                continue;
            end

            % we did, lets see if the sum square RHS is constant
            rhs = simplify(right(k(1))^2 + right(k(2))^2); % was simple
            if isconstant( rhs )
                % it is, let's sum and square the LHS
                fprintf('lets square and add %d %d\n', k);

                eq = simplify( expand( left(k(1))^2 + left(k(2))^2 ) ) - rhs; % was simple
            end
        end

        % expand the list of joint variable subsitutions
        fprintf('subs sin/cos q%d for S/C\n', j);
        trigsubOld = [trigsubOld mvar('sin(q%d)', j) mvar('cos(q%d)', j)];
        trigsubNew = [trigsubNew mvar('S%d', j) mvar('C%d', j)];

        if isempty(eq)
            fprintf('cant solve this equation');
            k
            left(k)==right(k)
            error('cant solve');
        end
        % now solve the equation
        if srobot.links(j).isrevolute()
            % for revolute joint it will be a trig equation, do we know how to solve it?
            Q{j} = solve_joint(eq, j );
            if isempty(Q)
                warning('cant solve this kind of equation');
            end
        else
            fprintf('prismatic case\n')
            q = sym( sprintf('q%d', j) );
            Q{j} = solve( eq == 0, q);
        end
    end

    % final simplification
    %  get rid of C^2+S^2 and C^4, S^4 terms
    fprintf('**final simplification pass\n')

    % create a list of simplifications
    %  substitute S^2 = 1-C^2, S^4=(1-C^2)^2
    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2 mvar('S%d', j)^4];
        tsubNew = [tsubNew 1-mvar('C%d', j)^2 (1-mvar('C%d', j)^2)^2];
    end

    for j=1:N
        for k=1:5
            % seem to need to iterate this, not quite sure why
            Q{j} = simplify( expand( subs(Q{j}, tsubOld, tsubNew) ) );
        end
    end

    % Q is a cell array of equations for joint variables
    if nargout > 0
        out = Q;
    end

    if ~isempty(opt.file)
        fprintf('**generate MATLAB code\n')
        gencode(Q);
    end
end

%PIEPER Return a set of equations using Pieper's method
%
% [L,R] = pieper(robot, n, which)
%
% If robot has link matrix A1 A2 A3 A4 then returns 12 equations from equating the coefficients of
%
%  A1' T = A2 A3 A4     n=1, which='left'
%  A2' A1' T = A3 A4    n=2, which='left'
%  A3' A2' A1' T = A4   n=3, which='left'
%
%  T A4' = A1 A2 A3     n=1, which='right'
%  T A4' A3' = A1 A2    n=2, which='right'
%  T A4' A3' A2' = A1   n=3, which='right'
%
% Judicious choice of the equations can lead to joint solutions

function [L,R] = pieper(robot, n, which)

    if nargin < 3
        which = 'left';
    end

    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real

    T = [nx ox ax tx
        ny oy ay ty
        nx oz az tz
        0  0  0  1 ];

    T = inv(robot.base) * T * inv(robot.tool);

    q = robot.gencoords();

    % Create the symbolic A matrices
    for j=1:robot.n
        A{j} = robot.links(j).A(q(j));
    end

    switch which
        case 'left'
            left = T;
            for j=1:n
                left = inv(A{j}) * left ;
            end

            right = eye(4,4);
            for j=n+1:robot.n
                right = right * A{j};
            end

        case 'right'
            left = T;
            for j=1:n
                left = left * inv(A{robot.n-j+1});
            end

            right = eye(4,4);
            for j=1:(robot.n-n)
                right = right * A{j};
            end
    end

    %     left = simple(left);
    %     right = simple(right);

    if nargout == 0
        left == right
    elseif nargout == 1
        L = left;
    elseif nargout == 2
        L = left;
        R = right;
    end
end

%SOLVE_JOINT Solve a trigonometric equation
%
% S = SOLVE_JOINT(EQ, J) solves the equation EQ=0 for the joint variable qJ.
% The result is a cell array of solutions.
%
% The equations must be of the form:
%  A cos(qJ) + B sin(qJ) = 0
%  A cos(qJ) + B sin(qJ) = C
%
% where A, B, C are arbitrarily complex expressions.  qJ can be the only
% joint variable in the expression.

function s = solve_joint(eq, j)

    sinj = mvar('sin(q%d)', j);
    cosj = mvar('cos(q%d)', j);

    A = getcoef(eq, cosj);
    B = getcoef(eq, sinj);

    if isempty(A) || isempty(B)
        warning('don''t know how to solve this kind of equation');
    end

    C = -simplify(eq - A*cosj - B*sinj);  % was simple

    if C == 0
        % A cos(q) + B sin(q) = 0
        s(2) = atan2(A, -B);
        s(1) = atan2(-A, B);
    else
        % A cos(q) + B sin(q) = C
        r = sqrt(A^2 + B^2 - C^2);
        phi = atan2(A, B);

        s(2) = atan2(C, r) - phi;
        s(1) = atan2(C, -r) - phi;
    end
    if nargout == 0
        try
            eval(s)
        catch
            s
        end
    end
end

%MVAR Create a symbolic variable
%
% V = MVAR(FMT, ARGS) is a symbolic variable created using SPRINTF
%
% eg. mvar('q%d', j)
%
% The symbolic is explicitly declared to be real.

function v = mvar(fmt, varargin)

    if isempty(strfind(fmt, '('))
        % not a function
        v = sym( sprintf(fmt, varargin{:}), 'real' );
    else
        v = sym( sprintf(fmt, varargin{:}) );

    end
end

%HASONLY Determine if an expression contains only certain joint variables
%
% S = HASONLY(E L) is true if the joint variables (q1, q2 etc.) in the expression E
% are listed in the vector L.
%
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1 2 3]) -> true
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1]) -> false

function s = hasonly(eq, j)

    q = findq(eq);
    if isempty(q)
        s = false;
    else
        s = all(ismember(j, findq(eq)));
    end
end

%ISCONSTANT Determine if an expression is free of joint variables
%
% S = ISCONSTANT(E) is true if the expression E contains no joint variables such
% q1, q2 etc.

function s = isconstant(eq)
    s = isempty(findq(eq));
end

%FINDQ Find the joint variables in expression
%
% Q = FINDQ(E) returns a list of integers indicating the joint variables found
% in the expression E.  For instance an instance of 'q1' would cause a 1 to be
% returned and so on.
%
% Eg. findq('sin(q1)*cos(q2)+S3') -> [1 2]

function q = findq(s)

    q = [];

    for var=symvar(s)
        if isempty(var)
            break
        end
        varname = char(var);
        if varname(1) == 'q'
            q = [q str2num(varname(2:end))];
        end
    end
end

function coef = getcoef(eq, trig)
    z = children( collect(eq, trig) );
    z = children( z(1) );
    coef = z(1);
end

% Output a joint expression to a file

function s = gencode(Q, filename)

    function s = G(s, fmt, varargin)
        s = strvcat(s, sprintf(fmt, varargin{:}));
    end

    s = 'function q = xikine(T, sol)';
    s = G(s, '  if nargin < 2; sol = ones(1, %d); end', length(Q));
    s = G(s, '  px = T(1,4); py = T(2,4); pz = T(3,4);');

    for j=1:3
        Qj = Q{j};   % cast it to subclass
        if length(Qj) == 1
            s = G(s, '  q(%d) = %s', j, matgen2(Qj));
        elseif length(Qj) == 2
            s = G(s, '  if sol(%d) == 1', j);
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(1)));
            s = G(s, '  else');
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(2)));
            s = G(s, '  end');

        end

        s = G(s, '  S%d = sin(q(%d));', j, j);
        s = G(s, '  C%d = cos(q(%d));', j, j);
        s = G(s, ' ');

    end
    s = G(s, 'end');

    fp = fopen(filename, 'w');
    for i=1:numrows(s)
        fprintf(fp, '%s\n', deblank(s(i,:)));
    end
    fclose(fp);

end

% Generate MATLAB code from an expression
%
% Requires a bit of a hack, a subclass of sym (sym2) to do this

function s = matgen2(e)

    s = matgen(sym2(e));

    k = strfind(s, '=');
    s = deblank( s(k+2:end) );
end


function [KB] = EB2KB(EB)
% Convert computery things from exabytes to kilobytes.
% Chad A. Greene 2012
KB = EB*1125899906843000;


function Y = coth(X)
    % Symbolic hyperbolic cotangent.

    % Convert inputs to SymExpression
    % X = SymExpression(X);

    % construct the operation string
    sstr = ['Coth[' X.s ']'];

    % create a new object with the evaluated string
    Y = SymExpression(sstr);
end


filename='ImprovedBridge_hexahedra';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance','perimeter'};
weights = [1 0.1];
constraint = {'volume'};
optimizer = 'HAMILTON-JACOBI'; incrementFactor = 1;
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'STANDARD';
showBC = true;

HJiter0 = 1;
e2 = 1;
N_holes = [12 5 5];
R_holes = 0.2;
phase_holes = [0 0 0];
nsteps = 10;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 0.7;
optimality_initial = 5e-2;
constr_initial = 5e-2;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function mtrPDB2TrackVis(volFile, trkExFile, pdbFile, trkFile)
% TrackVis file is in mm coordinates, but without center shift
% PDB file is loaded in AcPc coordinates.

% Load vol file to get AcPc xform
vol = niftiRead(volFile);
xformFrom = vol.qto_ijk;
xformTo = abs(vol.qto_xyz);
xformTo(1:3,4)=0;
xformFromAcPc = xformTo*xformFrom;

% Get mm dimensions of image
dim_mm = vol.dim(:).*abs(diag(vol.qto_xyz(1:3,1:3)));

% Load trks example header file
trks.header = read_trk_hdr(trkExFile);

% Load PDB file
fg = mtrImportFibers(pdbFile);

% Get the fiber coords into mm (without AcPc shift)
fg = dtiXformFiberCoords(fg,xformFromAcPc);

% Get into trk coordinates
trks.fiber = {};
for ll=1:length(fg.fibers)
    trks.fiber{ll}.num_points = size(fg.fibers{ll},2);
    trks.fiber{ll}.points = fg.fibers{ll}';
end

trks.header.n_count = length(fg.fibers);
write_trk(trks,trkFile);

return;


% Representation-Free Model Predictive Control for Dynamic Quadruped Panther
% Author: Yanran Ding
% Last modified: 2020/12/21
%
% Code accompanying the paper:
% Yanran Ding, Abhishek Pandala, Chuanzheng Li, Young-Ha Shin, Hae-Won Park
% "Representation-Free Model Predictive Control for Dynamic Motions in Quadrupeds"
% Transactions on Robotics
%
% preprint available at: https://arxiv.org/abs/2012.10002
% video available at: https://www.youtube.com/watch?v=iMacEwQisoQ&t=101s

%% initialization
clear all;close all;clc
addpath fcns fcns_MPC

%% --- parameters ---
% ---- gait ----
% 0-trot; 1-bound; 2-pacing 3-gallop; 4-trot run; 5-crawl
gait = 1;
p = get_params(gait);
p.playSpeed = 1;
p.flag_movie = 1;       % 1 - make movie
use_qpSWIFT = 1;        % 0 - quadprog, 1 - qpSWIFT (external)

dt_sim = p.simTimeStep;
SimTimeDuration = 0.5;  % [sec]
MAX_ITER = floor(SimTimeDuration/p.simTimeStep);

% desired trajectory
p.acc_d = 1;
p.vel_d = [0.5;0];
p.yaw_d = 0;

%% Model Predictive Control
% --- initial condition ---
% Xt = [pc dpc vR wb pf]': [30,1]
if gait == 1
    [p,Xt,Ut] = fcn_bound_ref_traj(p);
else
    [Xt,Ut] = fcn_gen_XdUd(0,[],[1;1;1;1],p);
end

% --- logging ---
tstart = 0;
tend = dt_sim;

[tout,Xout,Uout,Xdout,Udout,Uext,FSMout] = deal([]);

% --- simulation ----
h_waitbar = waitbar(0,'Calculating...');
tic
for ii = 1:MAX_ITER
    % --- time vector ---
    t_ = dt_sim * (ii-1) + p.Tmpc * (0:p.predHorizon-1);

    % --- FSM ---
    if gait == 1
        [FSM,Xd,Ud,Xt] = fcn_FSM_bound(t_,Xt,p);
    else
        [FSM,Xd,Ud,Xt] = fcn_FSM(t_,Xt,p);
    end

    % --- MPC ----
    % form QP
    [H,g,Aineq,bineq,Aeq,beq] = fcn_get_QP_form_eta(Xt,Ut,Xd,Ud,p);

    if ~use_qpSWIFT
        % solve QP using quadprog
        [zval] = quadprog(H,g,Aineq,bineq,Aeq,beq,[],[]);
    else
        % interface with the QP solver qpSWIFT
        [zval,basic_info] = qpSWIFT(sparse(H),g,sparse(Aeq),beq,sparse(Aineq),bineq);
    end

    Ut = Ut + zval(1:12);

    % --- external disturbance ---
    [u_ext,p_ext] = fcn_get_disturbance(tstart,p);
    p.p_ext = p_ext;        % position of external force
    u_ext = 0*u_ext;

    % --- simulate ---
    [t,X] = ode45(@(t,X)dynamics_SRB(t,X,Ut,Xd,0*u_ext,p),[tstart,tend],Xt);

    % --- update ---
    Xt = X(end,:)';
    tstart = tend;
    tend = tstart + dt_sim;

    % --- log ---
    lent = length(t(2:end));
    tout = [tout;t(2:end)];
    Xout = [Xout;X(2:end,:)];
    Uout = [Uout;repmat(Ut',[lent,1])];
    Xdout = [Xdout;repmat(Xd(:,1)',[lent,1])];
    Udout = [Udout;repmat(Ud(:,1)',[lent,1])];
    Uext = [Uext;repmat(u_ext',[lent,1])];
    FSMout = [FSMout;repmat(FSM',[lent,1])];

    waitbar(ii/MAX_ITER,h_waitbar,'Calculating...');
end
close(h_waitbar)
fprintf('Calculation Complete!\n')
toc

%% Animation
[t,EA,EAd] = fig_animate(tout,Xout,Uout,Xdout,Udout,Uext,p);


function [ output_args ] = CudaConvolution( input_args )
% CUDACONVOLUTION Download description
% Please download the
%
% 64 Bit CUDA 4.2 MEX files here.
%
% and download the
%
% 32 Bit CUDA 4.2 MEX files here.
%
% and download the
%
% NVIDIA driver version 190.38 for your OS from here.
%
realKernel = double(1:5);
imagKernel = double(1:5);
realKernel(:)= 0.127;
imagKernel(:)= 0;

% Construct the filter kernel
Filter_Kernel = complex(realKernel,imagKernel);

imagSignal = double(1:2^15);

time = 0:(1/2^15):(1-1/2^15);
idealSignal = sin(2*pi*5*time);

realSignal = idealSignal + randn(size(time))/2;
imagSignal(:) = 0;

% Construct the signal
Signal_1D_Complex = complex(realSignal,imagSignal);

% Convolution of kernel and signal on the CUDA device
filteredSignal = cuFilter(Signal_1D_Complex,Filter_Kernel);

plot(realSignal,'b');hold;
plot(real(filteredSignal),'m');
plot(idealSignal,'g');


% mattocell() - convert matrix to cell array
%
% Usage: >> C = mattocell( M );
%
% Author: Arnaud Delorme, CNL / Salk Institute, Jan 25 2002
%
% Note: this function overload the nnet toolbox function mattocell,
% but does not have all its capacities. You can delete the current
% function if you have the toolbox.

% Copyright (C) Jan 25 2002 Arnaud Delorme, CNL / Salk Institute
%
% 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 C = mattocell( M, varargin );

if nargin < 1
	help mattocell;
	return;
end;
if isempty(M)
	C = [];
	return;
end;
C = cell(size(M));
for i=1:size(M,1)
    for j=1:size(M,2)
        C{i,j} = M(i,j);
    end;
end;


function DI = detectChange(obj, t1, t2)
% Use logarithm for ease of the following thresholding
DI = abs(log(double(t1)+eps) - log((double(t2) + eps)));
end


function f = dot( F, G )
%DOT   Vector dot product.
%   DOT(F, G) returns the dot product of the DISKFUNV objects F and G.
%   DOT(F, G) is the same as F'*G.
%
% See also CROSS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check:
if ( isempty( F ) || isempty( G ) )
    f = diskfun();
    return
end

% Extract components:
Fc = F.components;
Gc = G.components;

% Calculate dot-product:
f = times(Fc{1}, Gc{1})+times(Fc{2}, Gc{2});

end


classdef TestPhase
    %TestPhase

    methods (Static)
        function test_radians
            x = randn(5,6);
            y = randn(5,6);
            ang = cv.phase(x, y);
            validateattributes(ang, {class(x)}, {'size',size(x)});

            x = single(x);
            y = single(y);
            ang = cv.phase(x, y);
            validateattributes(ang, {class(x)}, {'size',size(x)});
        end

        function test_degrees
            x = randn(5,6);
            y = randn(5,6);
            ang = cv.phase(x, y, 'Degrees',true);
            validateattributes(ang, {class(x)}, {'size',size(x)});
        end

        function test_error_argnum
            try
                cv.phase();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function F = atand(F, varargin)
%ATAND   Inverse tangent of a CHEBFUN, result in degrees.
%   ATAND(F) computes the inverse tangent (in degrees) of the CHEBFUN F.
%
%   ATAND(F, PREF) does the same but uses the CHEBFUNPREF object PREF when
%   computing the composition.
%
% See also TAND, ATAN2D, ATAN.

% Copyright 2017 by The University of Oxford and The Chebfun Developers. See
% http://www.chebfun.org/ for Chebfun information.

% Call the compose method:
F = compose(F, @atand, varargin{:});

end