function [x,resL2,qualMeasOut]= LSMR(proj,geo,angles,niter,varargin)

% LSMR solves the CBCT problem using LSMR.
%
%  LSMR(PROJ,GEO,ANGLES,NITER) solves the reconstruction problem
%   using the projection data PROJ taken over ALPHA angles, corresponding
%   to the geometry descrived in GEO, using NITER iterations.
%
%  LSMR(PROJ,GEO,ANGLES,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%  'lambda'  Value of parameter lambda, default 0.
%  'Init'    Describes diferent initialization techniques.
%             * 'none'     : Initializes the image to zeros (default)
%             * 'FDK'      : intializes image to FDK reconstrucition
%             * 'multigrid': Initializes image by solving the problem in
%                            small scale and increasing it when relative
%                            convergence is reached.
%             * 'image'    : Initialization using a user specified
%                            image. Not recomended unless you really
%                            know what you are doing.
%  'InitImg'    an image for the 'image' initialization. Avoid.
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%  'restart'  true or false. By default the algorithm will restart when
%             loss of ortogonality is found.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Malena Sabate Landman, Ander Biguri
%--------------------------------------------------------------------------
%%

[verbose,x,QualMeasOpts,gpuids,lambda,gt,restart]=parse_inputs(proj,geo,angles,varargin);

measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    x0=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);
resL2 = zeros(1,niter);

% David Chin-Lung Fong and Michael Saunders //doi.org/10.1137/10079687X

% Enumeration as given in the paper for 'Algorithm LSMR'
iter=0;
remember=0;
while iter<niter
    % (1) Initialize
    u=proj-Ax(x,geo,angles,'Siddon','gpuids',gpuids);
    normr = norm(u(:),2);
    beta = normr;
    u = u/beta;

    v=Atb(u,geo,angles,'matched','gpuids',gpuids);
    alpha = norm(v(:),2); % msl: do we want to check if it is 0?
    v = v/alpha;

    alphabar = alpha;
    zetabar = alpha * beta;
    rho = 1;
    rhobar = 1;
    cbar = 1;
    sbar = 0;
    h = v;
    hbar = 0;

    % Compute the residual norm ||r_k||
    betadd = beta;
    betad = 0;
    rhod = 1;
    tautilda = 0;
    thetatilda = 0;
    zeta = 0;
    d = 0;

    % (2) Start iterations
    for ii=iter:niter
        iter=iter+1;
        if measurequality && ~strcmp(QualMeasOpts,'error_norm')
            x0 = x; % only store if necesary
        end
        if (iter==1 && verbose);tic;end

        % (3) Continue the bidiagonalization
        u = Ax(v,geo,angles,'Siddon','gpuids',gpuids) - alpha*u;
        beta = norm(u(:),2);
        u = u / beta;

        v = Atb(u,geo,angles,'matched','gpuids',gpuids) - beta*v;

        alpha = norm(v(:),2);
        v = v / alpha;

        % (4) Construct and apply rotation \hat{P}_k
        alphahat = sqrt(alphabar^2 + lambda^2);
        chat = alphabar/alphahat;
        shat = lambda/alphahat;

        % (5) Construct and apply rotation P_k
        rhopre = rho;
        rho = sqrt(alphahat^2 + beta^2);
        c = alphahat / rho;
        s = beta / rho;
        theta = s * alpha;
        alphabar = c * alpha;

        % (6) Construct and apply rotation \bar{P}_k
        thetabar = sbar * rho;
        rhobarpre = rhobar;
        rhobar = sqrt((cbar*rho)^2 + theta^2);
        cbar = cbar * rho / rhobar;
        sbar = theta / rhobar;
        zetapre = zeta;
        zeta = cbar * zetabar;
        zetabar = -sbar * zetabar;

        % (7) Update \bar{h}, x, h
        hbar = h - (thetabar*rho/(rhopre*rhobarpre))*hbar;
        x = x + (zeta / (rho*rhobar)) * hbar;
        h = v - (theta / rho) * h;

        % (8) Apply rotation \hat{P}_k, P_k
        betaacute = chat* betadd;
        betacheck = - shat* betadd;

        % Computing ||r_k||

        betahat = c * betaacute;
        betadd = -s * betaacute;

        % Update estimated quantities of interest.
        %  (9) If k >= 2, construct and apply \tilde{P} to compute ||r_k||
        rhotilda = sqrt(rhod^2 + thetabar^2);
        ctilda = rhod / rhotilda;
        stilda = thetabar / rhotilda;
        thetatildapre = thetatilda;
        thetatilda = stilda * rhobar;
        rhod = ctilda * rhobar;
        % betatilda = ctilda * betad + stilda * betahat; % msl: in the orinal paper, but not used
        betad = -stilda * betad + ctilda * betahat;

        % (10) Update \tilde{t}_k by forward substitution
        tautilda = (zetapre - thetatildapre*tautilda) / rhotilda;
        taud = (zeta - thetatilda*tautilda) / rhod;

        % (11) Compute ||r_k||
        d = d + betacheck^2;
        gamma_var = d + (betad - taud)^2 + betadd^2;
        aux = sqrt(gamma_var); % this is the residual teh algorithm follows, but we lose ortogonality, so we compute it explicitly

        % ||A^T r_k || is just |zetabar|

        % (6) Test for convergence.
        % msl: I still need to implement this.
        % msl: There are suggestions on the original paper. Let's talk about it!

        if measurequality
            qualMeasOut(:,iter)=Measure_Quality(x0,x,QualMeasOpts);
        end
        % The following should never happen, but the reallity is that if we use
        % the residual from the algorithm, it starts diverging from this explicit residual value.
        % This is an interesting fact that I believe may be caused either by
        % the mismatch of the backprojection w.r.t the real adjoint, or
        % numerical issues related to doing several order of magnitude
        % difference operations on single precission numbers.
        aux=proj-Ax(x,geo,angles,'Siddon','gpuids',gpuids);
        resL2(iter)=im3Dnorm(aux,'L2');
        if iter>1 && resL2(iter)>resL2(iter-1)
            % we lost orthogonality, lets restart the algorithm unless the
            % user asked us not to.

            % undo bad step.
            x=x-(zeta / (rho*rhobar)) * hbar;
            % if the restart didn't work.
            if remember==iter || ~restart
                disp(['Algorithm stoped in iteration ', num2str(iter),' due to loss of ortogonality.'])
                return;
            end
            remember=iter;
            iter=iter-1;
            if verbose
                disp(['Orthogonality lost, restarting at iteration ', num2str(iter) ])
            end
            break
        end

        if (iter==1 && verbose)
            expected_time=toc*niter;
            disp('LSMR');
            disp(['Expected duration   :    ',secs2hms(expected_time)]);
            disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
            disp('');
        end
    end
end
end

%% parse inputs'
function [verbose,x,QualMeasOpts,gpuids, lambda,gt,restart]=parse_inputs(proj,geo,angles,argin)
opts=     {'init','initimg','verbose','qualmeas','gpuids','lambda','groundtruth','restart'};
defaults=ones(length(opts),1);

% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:LSMR:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:LSMR:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extranc value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:LSMR:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end

    switch opt
        case 'init'
            x=[];
            if default || strcmp(val,'none')
                x=zeros(geo.nVoxel','single');
                continue;
            end
            if strcmp(val,'FDK')
                x=FDK(proj,geo,angles);
                continue;
            end
            if strcmp(val,'multigrid')
                x=init_multigrid(proj,geo,angles);
                continue;
            end
            if strcmp(val,'image')
                initwithimage=1;
                continue;
            end
            if isempty(x)
                error('TIGRE:LSMR:InvalidInput','Invalid Init option')
            end
            % % % % % % % ERROR
        case 'initimg'
            if default
                continue;
            end
            if exist('initwithimage','var')
                if isequal(size(val),geo.nVoxel')
                    x=single(val);
                else
                    error('TIGRE:LSMR:InvalidInput','Invalid image for initialization');
                end
            end
            %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:LSMR:InvalidInput','Invalid quality measurement parameters');
                end
            end
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE:Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'lambda'
            if default
                lambda = 0;
            else
                lambda = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        case 'restart'
            if default
                restart=true;
            else
                restart=val;
            end
        otherwise
            error('TIGRE:LSMR:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in LSMR()']);
    end
end

end


function [set_ar, varxh, klifit] = ARShat_misd(ng,xg,L,lag_max,set_ar_start);

%ARSHAT_MISD AR models of increasing order from measurements with missing data
%   [set_ar, varxh, klifit] = ARShat_misd(ng,xg,[Lmin Lmax],lag_max) estimates
%   autoregressive models from measurements containing missing data for
%   orders Lmin, ..., Lmax.
%   lag_max contains the maximum lag for the ARFIL algorithm for all orders
%   that are considered.
%
%   NOTE: The mean of the signal is NOT subtracted.
%
%   [set_ar, varxh, klifit] = ARShat_misd(ng,xg,[Lmin Lmax],set_ar_start)
%   The cell array set_ar_start contains starting models for all model
%   orders considered.
%
%   Output:
%   set_ar is a cell array containing all estimated models;
%   varxh  is the estimated signal variance;
%   klifit is the fit for each model in terms of the Kullback-Leibler Index
%          (kli) for missing data.
%
%   See also: ARHAT_MISD, SIG2AR_MISD.

%S. de Waele, August 2001.

showwb = 0; %1 = Show waitbar

nseg = length(ng);
%-------------------------------------------------------------
%Reading input arguments
%-------------------------------------------------------------
mstart = 0;
if exist('set_ar_start'),
    mstart = 1;
else
    mstart = 0;
end

Lmin = L(1);
Lmax = L(2);
nk = Lmax-Lmin+1;

%-------------------------------------------------------------
%Estimation
%-------------------------------------------------------------
sumvars = 0;
for seg = 1:nseg
   sumvars = sumvars + std(xg{seg})^2;
end
varxh = sumvars/nseg

nobsi = length(xg);
set_ar = cell(nk,1);
klifit  = zeros(nk,1);

if showwb,
	hwb = waitbar(0/nk,[int2str(Lmin) ' to ' int2str(Lmax) ' AR models - ARSirr-tan']);
end

rch_prev_calc = 0; %Currently, no previous model has been calculated
for ik = 1:nk,
   k = Lmin+ik-1;
   if k == 0,
      %White noise estimate
      klifit(ik) = ARMLfit([],ng,xg,0,lag_max(ik));
      rch = [];
   else	%if k == 0
      %Determination starting point.
      %Option 1:	white noise estimate
      rc_00 = zeros(1,k);
      % klifit_wn = fit_cL_irr(rc_start,ng,xg,fs);
      klifit_wn = ARMLfit([],ng,xg,rc_00,lag_max(ik));
      rc_start = rc_00;
      klifit_start(1) = klifit_wn;
      %Option 2:	Append zero to previous model
      rc_old0 = [rch_prev 0];
      klifit_old0 = ARMLfit([],ng,xg,rc_old0,lag_max(ik));
      rc_start(2,:) = rc_old0;
      klifit_start(2) = klifit_old0;
      if mstart,
         %Option 3:	User-provided Starting Model.
         [dummy rc_sm] = ar2arset(set_ar_start{ik});
         rc_sm = rc_sm(2:end);
         klifit_sm = ARMLfit([],ng,xg,rc_sm,lag_max(ik));
         rc_start(3,:) = rc_sm;
         klifit_start(3) = klifit_sm;
      end
      %Selection of inition conditions with best fit.
      [klifit_start, i_min] = min(klifit_start);
      rc_start = rc_start(i_min,:)
      methods = {'allzero','previous+0','User-provided'};
      disp(['Selected starting point: ' methods{i_min}])
      %Optimization starting in selected point
      [rch,fit_temp] = ARhat_misd(ng,xg,rc_start,[],lag_max(ik));
      klifit(ik) = fit_temp;
      if klifit(ik) > klifit_start
         warning(['Optimization did not yield better model for k = ' int2str(k) '.'])
         rch = rc_start;
         klifit(ik) = klifit_start;
      end
   end %if k == 0
   set_ar{ik} = rc2arset([1 rch]);
   rch_prev = rch; %For the next round
   rch_prev_calc = 1;
   if showwb,
      waitbar(ik/nk)
   end
end
if showwb,
	close(hwb)
end


function res = ne(a,b)
%NE           Implements  a ~= b  for slopes, compares only a.r and b.r
%

% written  12/06/98     S.M. Rump
% modified 09/28/01     S.M. Rump  matrices and multi-dimensional arrays
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 05/21/09     S.M. Rump  typo
%

  if ~isa(a,'slope')
    asize = size(a);
    bsize = b.size;
    res = ( a(:)~=b.r(:,1) );
  elseif ~isa(b,'slope')
    asize = a.size;
    bsize = size(b);
    res = ( a.r(:,1)~=b(:) );
  else
    asize = a.size;
    bsize = b.size;
    res = ( a.r(:,1)~=b.r(:,1) );
  end

  if ~isequal(asize,bsize) & ( prod(asize)~=1 ) & ( prod(bsize)~=1 )
    error('incompatible size for slope ~= ')
  end

  if prod(asize)==1
    res = reshape(res,bsize);
  else
    res = reshape(res,asize);
  end


function [timeson, timesoff, fbin]=ontimestat(bm, threshold)
% [timeson, timesoff, fbin]=ontimestat(bm, thershold)
% Computes ON and OFF times of the time serires bm. ON (OFF) time is how long is
% the time-serires bm above (below) the threshold.
% bm=f';
% threshold=500;

fbin=(bm>threshold);
timeson=computetimes(fbin);
timesoff=computetimes(1-fbin);
end
function times=computetimes(fbin)
zv = zeros(1,size(fbin,2));
fbin2=[zv; fbin; zv];
pos=find(diff(fbin2)>0);
neg=find(diff(fbin2)<0);
times=neg-pos;
end


function [Grad,t]=StdRect(tStart,tEnd,gAmp,step)

%tStart start time
%tEnd end time
%step increment steps
%gAmp amplitude

t=linspace(tStart,tEnd,step);
Grad=gAmp*ones(size(t)); % rectangle

end


function p = prior_invt(varargin)
%PRIOR_INVT  Student-t prior structure for the inverse of the parameter
%
%  Description
%    P = PRIOR_INVT('PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    creates for the inverse of the parameter Student's
%    t-distribution prior structure in which the named parameters
%    have the specified values. Any unspecified parameters are set
%    to default values.
%
%    P = PRIOR_INVT(P, 'PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    modify a prior structure with the named parameters altered
%    with the specified values.
%
%    Parameterisation is done as in Bayesian Data Analysis,
%    second edition, Gelman et.al 2004.
%
%    Parameters for Student-t prior [default]
%      mu       - location [0]
%      s2       - scale [1]
%      nu       - degrees of freedom [4]
%      mu_prior - prior for mu [prior_fixed]
%      s2_prior - prior for s2 [prior_fixed]
%      nu_prior - prior for nu [prior_fixed]
%
%  See also
%    PRIOR_T, PRIOR_*

% Copyright (c) 2000-2001,2010,2012 Aki Vehtari
% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010 Jaakko Riihim�ki

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

  ip=inputParser;
  ip.FunctionName = 'PRIOR_INVT';
  ip.addOptional('p', [], @isstruct);
  ip.addParamValue('mu',0, @(x) isscalar(x));
  ip.addParamValue('mu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('s2',1, @(x) isscalar(x) && x>0);
  ip.addParamValue('s2_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('nu',4, @(x) isscalar(x) && x>0);
  ip.addParamValue('nu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'Inv-t';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'Inv-t')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('mu',ip.UsingDefaults)
    p.mu = ip.Results.mu;
  end
  if init || ~ismember('s2',ip.UsingDefaults)
    p.s2 = ip.Results.s2;
  end
  if init || ~ismember('nu',ip.UsingDefaults)
    p.nu = ip.Results.nu;
  end
  % Initialize prior structure
  if init
    p.p=[];
  end
  if init || ~ismember('mu_prior',ip.UsingDefaults)
    p.p.mu=ip.Results.mu_prior;
  end
  if init || ~ismember('s2_prior',ip.UsingDefaults)
    p.p.s2=ip.Results.s2_prior;
  end
  if init || ~ismember('nu_prior',ip.UsingDefaults)
    p.p.nu=ip.Results.nu_prior;
  end

  if init
    % set functions
    p.fh.pak = @prior_invt_pak;
    p.fh.unpak = @prior_invt_unpak;
    p.fh.lp = @prior_invt_lp;
    p.fh.lpg = @prior_invt_lpg;
    p.fh.recappend = @prior_invt_recappend;
  end

end

function [w, s] = prior_invt_pak(p)

  w=[];
  s={};
  if ~isempty(p.p.mu)
    w = p.mu;
    s=[s; 'Inv-t.mu'];
  end
  if ~isempty(p.p.s2)
    w = [w log(p.s2)];
    s=[s; 'log(Inv-t.s2)'];
  end
  if ~isempty(p.p.nu)
    w = [w log(p.nu)];
    s=[s; 'log(Inv-t.nu)'];
  end
end

function [p, w] = prior_invt_unpak(p, w)

  if ~isempty(p.p.mu)
    i1=1;
    p.mu = w(i1);
    w = w(i1+1:end);
  end
  if ~isempty(p.p.s2)
    i1=1;
    p.s2 = exp(w(i1));
    w = w(i1+1:end);
  end
  if ~isempty(p.p.nu)
    i1=1;
    p.nu = exp(w(i1));
    w = w(i1+1:end);
  end
end

function lp = prior_invt_lp(x, p)

  lJ = -log(x)*2;   % log(1/x^2) log(|J|) of transformation
  xt  = 1./x;       % transformation
  lp = sum(gammaln((p.nu+1)./2) -gammaln(p.nu./2) -0.5*log(p.nu.*pi.*p.s2) -(p.nu+1)./2.*log(1+(xt-p.mu).^2./p.nu./p.s2) + lJ);

  if ~isempty(p.p.mu)
    lp = lp + p.p.mu.fh.lp(p.mu, p.p.mu);
  end
  if ~isempty(p.p.s2)
    lp = lp + p.p.s2.fh.lp(p.s2, p.p.s2) +log(p.s2);
  end
  if ~isempty(p.p.nu)
    lp = lp + p.p.nu.fh.lp(p.nu, p.p.nu) +log(p.nu);
  end
end

function lpg = prior_invt_lpg(x, p)

  lJg = -2./x;      % gradient of log(|J|) of transformation
  xt  = 1./x;       % transformation
  xtg = -1./x.^2;   % derivative of transformation
  lpg = xtg.*(-(p.nu+1).* (xt-p.mu) ./ (p.nu.*p.s2 + (xt-p.mu).^2)) + lJg;

  if ~isempty(p.p.mu)
    lpgmu = sum( (p.nu+1).* (xt-p.mu) ./ (p.nu.*p.s2 + (xt-p.mu).^2) ) + p.p.mu.fh.lpg(p.mu, p.p.mu);
    lpg = [lpg lpgmu];
  end
  if ~isempty(p.p.s2)
    lpgs2 = (sum( -1./(2.*p.s2) +((p.nu + 1)*(p.mu - xt)^2)./(2*p.s2*((p.mu-xt)^2 + p.nu*p.s2))) + p.p.s2.fh.lpg(p.s2, p.p.s2)).*p.s2 + 1;
    lpg = [lpg lpgs2];
  end
  if ~isempty(p.p.nu)
    lpgnu = (0.5*sum( digamma1((p.nu+1)./2)-digamma1(p.nu./2)-1./p.nu-log(1+(xt-p.mu).^2./p.nu./p.s2)+(p.nu+1)./(1+(xt-p.mu).^2./p.nu./p.s2).*(xt-p.mu).^2./p.s2./p.nu.^2) + p.p.nu.fh.lpg(p.nu, p.p.nu)).*p.nu + 1;
    lpg = [lpg lpgnu];
  end
end

function rec = prior_invt_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
  if ~isempty(p.p.mu)
    rec.mu(ri,:) = p.mu;
  end
  if ~isempty(p.p.s2)
    rec.s2(ri,:) = p.s2;
  end
  if ~isempty(p.p.nu)
    rec.nu(ri,:) = p.nu;
  end
end


function dim = imSize(img, varargin)
%IMSIZE  Compute the size of an image in [x y z] order
%
%   SIZ = imSize(IMG)
%   Returns the size of the image, in x-y-z order, for a grayscale or a
%   color image.
%   In the case of a 3D color image, returns only the 3 physical dimensions
%   of the image.
%
%   S = imSize(IMG, DIR)
%   Return the size of the image in the specified direction.
%   DIR=1 corresponds to X axis (second direction of matlab array)
%   DIR=2 corresponds to Y axis (first direction of matlab array)
%   DIR=3 corresponds to Z axis (third or fourth direction of matlab array)
%
%   S = imSize(IMG, RESOL)
%   Return the size of the image in physical units. RESOL is a 1-by-3 row
%   vector containing resolution in x, y and z directions respectively.
%
%
%   Example
%   % Compute the size of a color image
%     img = imread('peppers.png');
%     imSize(img)
%     ans =
%        512   384
%     (returns the width, then the height)
%
%   % compute size of a portion of MRI image
%     img = analyze75read(analyze75info('brainMRI.hdr'));
%     img2 = img(1:50, :, :);   % keep 50 rows, in the y-direction
%     imSize(img2)
%     ans =
%        128    50    27
%   % (the number of voxels in y-direction is given as second parameter)
%
%   % return only the number of rows, i.e. the number of voxels in the
%   % Y-direction
%     ny = imSize(img2, 2)
%     ny =
%         50
%
%   See also
%     imPhysicalExtent, size, ndims
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-01,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% size of matlab array
dim = size(img);

% remove color dimension if necessary
if isColorImage(img)
    dim(3) = [];
end

% swap x-y dimensions
nd = length(dim);
dim = dim([2 1 3:nd]);

if ~isempty(varargin)
    var = varargin{1};

    if isscalar(var)
        % in case a single direction is asked, select it
        dim = dim(varargin{1});

    elseif length(var) == length(dim)
        % if resolution is specified, compute size in physical units
        dim = dim .* var;
    end
end


function gamma = omp(varargin)
%OMP Sparsity-constrained Orthogonal Matching Pursuit.
%  GAMMA = OMP(D,X,G,T) solves the optimization problem
%
%       min  |X - D*GAMMA|_2     s.t.  |GAMMA|_0 <= T
%      gamma
%
%  for each of the signals in X, using Batch Orthogonal Matching Pursuit.
%  Here, D is a dictionary with normalized columns, X is a matrix
%  containing column signals, T is the # of non-zeros in each signal
%  representation, and G is the Gramm matrix D'*D. The output GAMMA is a
%  matrix containing the sparse representations as its columns.
%
%  GAMMA = OMP(D,X,[],T) performs the same operation, but without the
%  matrix G, using OMP-Cholesky. This call produces the same output as
%  Batch-OMP, but is significantly slower. Using this syntax is only
%  recommended when available memory is too small to store G.
%
%  GAMMA = OMP(DtX,G,T) is the fastest implementation of OMP, but also
%  requires the most memory. Here, DtX stores the projections D'*X. In this
%  case Batch-OMP is used, but without having to compute D'*X in advance,
%  which slightly improves runtime. Note that in general, the call
%
%    GAMMA = OMP(D'*X,G,T);
%
%  will be faster than the call
%
%    GAMMA = OMP(D,X,G,T);
%
%  due to optimized matrix multiplications in Matlab. However, when the
%  entire matrix D'*X cannot be stored in memory, one of the other two
%  versions can be used. Both compute D'*X for just one signal at a time,
%  and thus require much less memory.
%
%  GAMMA = OMP(...,PARAM1,VAL1,PARAM2,VAL2,...) specifies additional
%  parameters for OMP. Available parameters are:
%
%    'gammamode' - Specifies the representation mode for GAMMA. Can be
%                  either 'full' or 'sparse', corresponding to a full or
%                  sparse matrix, respectively. By default, GAMMA is
%                  returned as a sparse matrix.
%    'messages'  - Specifies whether progress messages should be displayed.
%                  When positive, this is the number of seconds between
%                  status prints. When negative, indicates that no messages
%                  should be displayed (this is the default).
%    'checkdict' - Specifies whether dictionary normalization should be
%                  verified. When set to 'on' (default) the dictionary
%                  atoms are verified to be of unit L2-norm. Setting this
%                  parameter to 'off' disables verification and accelerates
%                  function performance. Note that an unnormalized
%                  dictionary will produce invalid results.
%    'profile'   - Can be either 'on' or 'off'. When 'on', profiling
%                  information is displayed at the end of the funciton
%                  execution.
%
%
%  Summary of OMP versions:
%
%    version      |   speed     |   memory
%  --------------------------------------------------
%   OMP(DtX,G,T)  |  very fast  |  very large
%   OMP(D,X,G,T)  |  fast       |  moderate
%   OMP(D,X,[],T) |  very slow  |  small
%  --------------------------------------------------
%
%
%  References:
%  [1] M. Elad, R. Rubinstein, and M. Zibulevsky, "Efficient Implementation
%      of the K-SVD Algorithm using Batch Orthogonal Matching Pursuit",
%      Technical Report - CS, Technion, April 2008.
%
%  See also OMP2.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  April 2009

% default options

sparse_gamma = 1;
msgdelta = -1;
checkdict = 1;
profile = 0;

% determine number of parameters

paramnum = 1;
while (paramnum<=nargin && ~ischar(varargin{paramnum}))
  paramnum = paramnum+1;
end
paramnum = paramnum-1;

% parse options

for i = paramnum+1:2:length(varargin)
  paramname = varargin{i};
  paramval = varargin{i+1};

  switch lower(paramname)

    case 'gammamode'
      if (strcmpi(paramval,'sparse'))
        sparse_gamma = 1;
      elseif (strcmpi(paramval,'full'))
        sparse_gamma = 0;
      else
        error('Invalid GAMMA mode');
      end

    case 'messages'
      msgdelta = paramval;

    case 'checkdict'
      if (strcmpi(paramval,'on'))
        checkdict = 1;
      elseif (strcmpi(paramval,'off'))
        checkdict = 0;
      else
        error('Invalid checkdict option');
      end

    case 'profile'
      if (strcmpi(paramval,'on'))
        profile = 1;
      elseif (strcmpi(paramval,'off'))
        profile = 0;
      else
        error('Invalid profile mode');
      end

    otherwise
      error(['Unknown option: ' paramname]);
  end

end

% determine call type

if (paramnum==3)
  DtX = varargin{1};
  G = varargin{2};
  T = varargin{3};
  D = [];
  X = [];
elseif (paramnum==4)
  D = varargin{1};
  X = varargin{2};
  G = varargin{3};
  T = varargin{4};
  DtX = [];
else
  error('Invalid number of parameters');
end

% verify dictionary normalization

if (checkdict)
  if (isempty(G))
    atomnorms = sum(D.*D);
  else
    atomnorms = diag(G);
  end
  if (any(abs(atomnorms-1) > 1e-2))
    error('Dictionary columns must be normalized to unit length');
  end
end

% omp

gamma = ompmex(D,X,DtX,G,T,sparse_gamma,msgdelta,profile);


% Copyright 2013 The MathWorks, Inc
%% Beamforming Example
% This example shows how to model antenna and target motion, and monitor
% array sidelobes while beamforming.
%% Model
% A 4-panel, ground based antenna array is mounted on a vehicle moving on a
% circular path at constant speed.
fc    = 3e9;
sAnt  = lowProfileArray('FrequencyRange',[2/3*fc 4/3*fc],'ViewArray',false);
[sSAnt, sSAntPlat]= splitSubArrays(sAnt);
for i=1:4,
    sCol{i}  = phased.Collector('Sensor',sSAnt{i},'OperatingFrequency',fc);
    sBF{i}   = phased.PhaseShiftBeamformer('OperatingFrequency',fc,...
                        'SensorArray',sSAnt{i},'DirectionSource','Input port',...
                        'WeightsOutputPort',true); %#ok<*SAGROW>
    sResp{i} = phased.ArrayResponse('SensorArray',sSAnt{i},'WeightsInputPort',true);
end
% A target is flying over the array.
sTgtMotion = phased.Platform('InitialPosition',[10e4; 0; 5e4],...
                             'Velocity',[-400; -200; -40]);
s=sin(2*pi*5*(0:.01:1))'; % Target Echo

%% Simulate
N = 500;
scanAz = -180:180;
for ii=1:N
    [AzEl,face,antpos,tgtpos,range] = updatePos(sSAntPlat,sTgtMotion,1/N,ii);
    viewTrajectories                               % Visualize trajectories
    y = []; resp = []; chan = 0;
    for k=find(face),
        x = step(sCol{k},s,AzEl);                  % Collect signal
        noise = 0.01*(randn(size(x)));             % Add noise
        chan = chan + 1;
        [y(:,chan),w] = step(sBF{k},x+noise,AzEl); % Beamform
        ang = [scanAz;ones(size(scanAz))*AzEl(2)];
        resp(:,chan) = step(sResp{k},fc,ang,w);    % Array response
    end
    y = mean(y,2);                                 % Combine active array outputs
    resp = mean(resp,2);
    plotBeam                                       % Visualize array response
end


clear all;
addpath(genpath('../CoreModules'));
n_epoch=20; %training epochs
dataset_name='mnist'; %dataset name
network_name='mlp'; %network name
use_gpu=(gpuDeviceCount>0) %use gpu or not
if use_gpu
    %Requires Neural Network Toolbox to use it.
    opts.use_nntoolbox=license('test','neural_network_toolbox')
end
%function handle to prepare your data
PrepareDataFunc=@PrepareData_MNIST_MLP;
%function handle to initialize the network
NetInit=@net_init_mlp_mnist_dropout;

%automatically select learning rates
use_selective_sgd=1;
%select a new learning rate every n epochs
ssgd_search_freq=10;
learning_method=@sgd; %training method: @sgd,@rmsprop,@adagrad,@adam
opts.parameters.mom=0.9;
opts.parameters.clip=1e1;
%sgd parameter
%(unnecessary if selective-sgd is used)
sgd_lr=5e-2;

%opts.parameters.clip=1e-2;
opts.parameters.weightDecay=0;
opts.parameters.batch_size=500;

Main_Template(); %call training template


%%*****************************************************************************
%% checkdepconst: compute AAt to determine if the
%%             constraint matrices Ak are linearly independent.
%%
%% [At,b,y,idxB,neardepconstr,feasible,AAt] = checkdepconstr(blk,At,b,y,rmdepconstr);
%%
%% rmdepconstr = 1, if want to remove dependent columns in At
%%             = 0, otherwise.
%%
%% idxB = indices of linearly independent columns of original At.
%% neardepconstr = 1 if there is nearly dependent columns in At
%%            = 0, otherwise.
%% Note: the definition of "nearly dependent" is dependent on the
%%       threshold used to determine the small diagonal elements in
%%       the LDLt factorization of A*At.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************************

   function  [At,b,y,idxB,neardepconstr,feasible,AAt] = ...
              checkdepconstr(blk,At,b,y,rmdepconstr);

   global existlowrank printlevel
   global nnzmatold
%%
%% compute AAt
%%
   m = length(b);
   AAt = sparse(m,m);  numdencol = 0; UU = [];
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      if strcmp(pblk{1},'s');
         m1 = size(At{p,1},2); m2 = m - m1;
         if (m2 > 0)
            if (m2 > 0.3*m); AAt = full(AAt); end
            dd = At{p,3};
            lenn = size(dd,1);
            idxD = [0; find(diff(dd(:,1))); lenn];
            ss = [0, cumsum(pblk{3})];
            if (existlowrank)
               AAt(1:m1,1:m1) = AAt(1:m1,1:m1) + At{p,1}'*At{p,1};
               for k = 1:m2
                  idx = [ss(k)+1 : ss(k+1)];
                  idx2 = [idxD(k)+1: idxD(k+1)];
                  ii = dd(idx2,2)-ss(k); %% undo cumulative indexing
                  jj = dd(idx2,3)-ss(k);
                  len = pblk{3}(k);
                  Dk = spconvert([ii,jj,dd(idx2,4); len,len,0]);
                  tmp = svec(pblk,At{p,2}(:,idx)*Dk*At{p,2}(:,idx)');
                  tmp2 = AAt(1:m1,m1+k) + (tmp'*At{p,1})';
                  AAt(1:m1,m1+k) = tmp2;
                  AAt(m1+k,1:m1) = tmp2';
               end
            end
            DD = spconvert([dd(:,2:4); sum(pblk{3}),sum(pblk{3}),0]);
            VtVD = (At{p,2}'*At{p,2})*DD;
            VtVD2 = VtVD'.*VtVD;
            for k = 1:m2
               idx0 = [ss(k)+1 : ss(k+1)];
               %%tmp = VtVD(idx0,:)' .* VtVD(:,idx0);
               tmp = VtVD2(:,idx0);
               tmp = tmp*ones(length(idx0),1);
               tmp3 = AAt(m1+[1:m2],m1+k) + mexqops(pblk{3},tmp,ones(length(tmp),1),1);
               AAt(m1+[1:m2],m1+k) = tmp3;
            end
         else
            AAt = AAt + abs(At{p,1})'*abs(At{p,1});
         end
      else
         decolidx = checkdense(At{p,1}');
         %% checkdense punts if the matrix has too many dense columns
         %% in this case we make the whole matrix dense
         if ~isempty(decolidx);
            n2 = size(At{p,1},1);
            dd = ones(n2,1);
            len= length(decolidx);
            dd(decolidx) = zeros(len,1);
            AAt = AAt + (abs(At{p,1})' *spdiags(dd,0,n2,n2)) *abs(At{p,1});
            tmp = At{p,1}(decolidx,:)';
            UU = [UU, tmp];
            numdencol = numdencol + len;
         else
            if (nnz(At{p,1})>0.2*numel(At{p,1}))
               Atp=full(abs(At{p,1}));
            else
               Atp=abs(At{p,1});
            end
            AAt = AAt + Atp'*Atp;
         end
      end
   end
   if (numdencol > 0) & (printlevel)
      fprintf('\n number of dense column in A = %d',numdencol);
   end
   numdencol = size(UU,2);
%%
%%
%%
   feasible = 1; neardepconstr = 0;
   if ~issparse(AAt); AAt = sparse(AAt); end
   nnzmatold = mexnnz(AAt);
   rho = 1e-15;
   diagAAt = diag(AAt);
   mexschurfun(AAt,rho*max(diagAAt,1));
   [L.R,indef,L.perm] = chol(AAt,'vector');
   L.d = full(diag(L.R)).^2;
   if (indef)
      msg = 'AAt is not pos. def.';
      idxB = [1:m];
      neardepconstr = 1;
      if (printlevel); fprintf('\n checkdepconstr: %s',msg); end
      return;
   end
%%
%% find independent rows of A
%%
   dd = zeros(m,1);
   idxB = [1:m]';
   dd(L.perm) = abs(L.d);
   idxN = find(dd < 1e-13*mean(L.d));
   ddB = dd(setdiff([1:m],idxN));
   ddN = dd(idxN);
   if ~isempty(ddN) & ~isempty(ddB) & (min(ddB)/max(ddN) < 10)
      %% no clear separation of elements in dd
      %% do not label constraints as dependent
      idxN = [];
   end
   if ~isempty(idxN)
      neardepconstr = 1;
      if (printlevel)
         fprintf('\n number of nearly dependent constraints = %1.0d',length(idxN));
      end
      if (numdencol==0)
         if (rmdepconstr)
            idxB = setdiff([1:m]',idxN);
            if (printlevel)
               fprintf('\n checkdepconstr: removing dependent constraints...');
            end
            [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);
   	    tol = 1e-8;
            if (resnorm > sqrt(tol))
               idxB = [1:m]';
               neardepconstr = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: basis rows cannot be reliably identified,');
                  fprintf(' abort removing nearly dependent constraints');
               end
               return;
            end
            tmp = W'*b(idxB) - b(idxN);
            nnorm = norm(tmp)/max(1,norm(b));
            if (nnorm > tol)
               feasible = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: inconsistent constraints exist,');
                  fprintf(' problem is infeasible.');
               end
            else
               feasible = 1;
               for p = 1:size(blk,1)
                  At{p,1} = At{p,1}(:,idxB);
               end
               b = b(idxB);
               y = y(idxB);
               AAt = AAt(idxB,idxB);
            end
	 else
            if (printlevel)
               fprintf('\n To remove these constraints,');
               fprintf(' re-run sqlp.m with OPTIONS.rmdepconstr = 1.');
            end
         end
      else
         if (printlevel)
            fprintf('\n warning: the sparse part of AAt may be nearly singular.');
         end
      end
   end
%%*****************************************************************************
%% findcoeffsub:
%%
%% [W,resnorm] = findcoeffsub(blk,At,idXB,idXN);
%%
%% idXB = indices of independent columns of At.
%% idxN = indices of   dependent columns of At.
%%
%% AB = At(:,idxB); AN = At(:,idxN) = AB*W
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*****************************************************************************

   function [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);

   AB = []; AN = [];
   for p = 1:size(blk,1)
      AB = [AB; At{p,1}(:,idxB)];
      AN = [AN; At{p,1}(:,idxN)];
   end
   [m,n] = size(AB);
%%
%%-----------------------------------------
%% find W so that AN = AB*W
%%-----------------------------------------
%%
   [L,U,P,Q] = lu(sparse(AB));
   rhs  = P*AN;
   Lhat = L(1:n,:);
   W = Q*( U \ (Lhat \ rhs(1:n,:)));
   resnorm = norm(AN-AB*W,'fro')/max(1,norm(AN,'fro'));
%%*****************************************************************************


function subset_sum_serial_test ( )

%*****************************************************************************80
%
%% SUBSET_SUM_SERIAL_TEST tests the SUBSET_SUM_SERIAL library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2011
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUBSET_SUM_SERIAL_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the SUBSET_SUM_SERIAL library.\n' );

  subset_sum_serial_test01 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUBSET_SUM_SERIAL_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function imgCompleteFinal = sc_complete(imgFileName)
% SC_COMPLETE: image completion with user-defined mask
%
% Input:
%     - imgFileName: image file name
% Output:
%     - imgCompletion: completed result
%
% Example:
%   imgFileName   = '002_input_hole.png';
%   imgCompletion = sc_complete(imgFileName);
%
% Disclaimer:
%   This is a Matlab re-implementation of the paper:
%
%   Jia-Bin Huang, Sing Bing Kang, Narendra Ahuja, and Johannes Kopf,
%   Image Completion using Planar Structure Guidance,
%   ACM Transactions on Graphics (Proceedings of SIGGRAPH 2014), 33(4), 2014
%
%   It is provided for educational/researrch purpose only. If you find the
%   software useful, please consider cite our paper.
%
% Contact:
%   Jia-Bin Huang
%   University of Illinois, Urbana-Champaign
%   www.jiabinhuang.com
<<<<<<< HEAD
%   jbhuang0604@gmail.com

% Set up required path
startup;

% Load image
imgID = 9;
imgFileName = [num2str(imgID, '%03d'), '_input_hole.png'];
=======
%   jbhuang0604@gmail.com

% % Load image
% imgID = 3;
% imgFileName = [num2str(imgID, '%03d'), '_input_hole.png'];
>>>>>>> 80fe71bd858652004119dc8946439915385cc282

imgFileName = 'test.png';

% Option parameters
[optA, optS] = sc_init_opt;

% =========================================================================
% Planar structure extraction
% =========================================================================
fprintf('- Extract planar structures \n');
tic;
[img, mask, maskD, modelPlane, modelReg] = sc_extract_planar_structure(imgFileName, optA);
tAnalysis = toc;
fprintf('Done in %6.3f seconds.\n\n', tAnalysis);

% =========================================================================
% Guided image completion
% =========================================================================
% Construct image pyramid for coarse-to-fine image completion
fprintf('- Construct image pyramid: \n');
tic;
% Create image pyramid
[imgPyr, maskPyr, scaleImgPyr] = sc_create_pyramid(img, maskD, optS);
% Structure constraints
[modelPlane, modelReg] = sc_planar_structure_pyramid(scaleImgPyr, modelPlane, modelReg);
tImgPyramid = toc;
fprintf('Done in %6.3f seconds.\n\n', tImgPyramid);

% Completion by synthesis
fprintf('- Image completion using planar structure guidance \n');
tic;
imgPyr = sc_synthesis(imgPyr, maskPyr, modelPlane, modelReg, optS);
tSynthesis = toc;
fprintf('Synthesis took %6.3f seconds.\n', tSynthesis);

% Return the top level
imgSyn = imgPyr{optS.topLevel};

% =========================================================================
% Poisson blending
% =========================================================================
tic;
imgCompleteFinal = sc_poisson_blend(img, imgSyn, mask);
tBlend = toc;
fprintf('Blending took %6.3f seconds.\n', tBlend);

imwrite(imgCompleteFinal, fullfile('result', [imgFileName(1:end-4), '_completion.png']));

% [To-Do] Visualizing completion process
% sc_vis_synthesis_process(imgCompletePyr, imgPyrNNF, imgPyrCost);

end


%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('show_camera'),
    show_camera = 1;
end;

if ~exist('n_ima')|~exist('fc'),
    fprintf(1,'No calibration data available.\n');
    return;
end;

check_active_images;

if ~exist(['omc_' num2str(ind_active(1))]),
    fprintf(1,'No calibration data available.\n');
    return;
end;

%if ~exist('no_grid'),
no_grid = 0;
%end;

if ~exist(['n_sq_x_' num2str(ind_active(1))]),
    no_grid = 1;
end;

if ~exist('alpha_c'),
    alpha_c = 0;
end;

if 0,

    err_std = std(ex');

    fprintf(1,'\n\nCalibration results without principal point estimation:\n\n');
    fprintf(1,'Focal Length:     fc = [ %3.5f   %3.5f]\n',fc);
    fprintf(1,'Principal point:  cc = [ %3.5f   %3.5f]\n',cc);
    fprintf(1,'Distortion:       kc = [ %3.5f   %3.5f   %3.5f   %3.5f]\n',kc);
    fprintf(1,'Pixel error:      err = [ %3.5f   %3.5f]\n\n',err_std);

end;

% Color code for each image:

colors = 'brgkcm';

%%% Show the extrinsic parameters

if ~exist('dX'),
    eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']);
    dY = dX;
end;

IP = 2*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1];
BASE = 2*(.9)*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]);
IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15);
POS = [[6*dX;0;0] [0;6*dX;0] [-dX;0;5*dX] [-dX;-dX;-dX] [0;0;-dX]];

if ishandle(4),
    figure(4);
    [a,b] = view;
else
    figure(4);
    a = 50;
    b = 20;
end;

figure(4);
clf;
hold on;

for kk = 1:n_ima,

    if active_images(kk);

        if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]),

            eval(['XX_kk = X_' num2str(kk) ';']);

            if ~isnan(XX_kk(1,1))

                eval(['omc_kk = omc_' num2str(kk) ';']);
                eval(['Tc_kk = Tc_' num2str(kk) ';']);
                N_kk = size(XX_kk,2);

                if ~exist(['n_sq_x_' num2str(kk)]),
                    no_grid = 1;
                else
                    eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
                    if isnan(n_sq_x(1)),
                        no_grid = 1;
                    end;
                end;

                if ~no_grid,
                    eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
                    eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
                    if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))),
                        no_grid = 1;
                    end;
                end;

                if ~isnan(omc_kk(1,1)),

                    R_kk = rodrigues(omc_kk);

                    BASEk = R_kk'*(BASE - Tc_kk * ones(1,6));
                    IPk = R_kk'*(IP - Tc_kk * ones(1,15));
                    POSk = R_kk'*(POS - Tc_kk * ones(1,5));

                    YY_kk = XX_kk;

                    if ~no_grid,

                        YYx = zeros(n_sq_x+1,n_sq_y+1);
                        YYy = zeros(n_sq_x+1,n_sq_y+1);
                        YYz = zeros(n_sq_x+1,n_sq_y+1);

                        YYx(:) = YY_kk(1,:);
                        YYy(:) = YY_kk(2,:);
                        YYz(:) = YY_kk(3,:);

                        figure(4);

                        if show_camera,

                        p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]);
                        h1 = patch(p1);
                        set(h1,'facecolor',[52 217 160]/255,'EdgeColor', 'r');
                        p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]);
                        h2 = patch(p2);
                        %set(h2,'facecolor',[236 171 76]/255,'EdgeColor', 'none');
                        set(h2,'facecolor',[247 239 7]/255,'EdgeColor', 'none');

                        plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1');
                        plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1);
                        text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold');

                        end;

                        hhh= mesh(YYx,YYy,YYz);
                        set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none');

                    else

                        figure(4);

                        if show_camera,

                        p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]);
                        h1 = patch(p1);
                        set(h1,'facecolor',[52 217 160]/255,'EdgeColor', 'r');
                        p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]);
                        h2 = patch(p2);
                        %set(h2,'facecolor',[236 171 76]/255,'EdgeColor', 'none');
                        set(h2,'facecolor',[247 239 7]/255,'EdgeColor', 'none');

                        plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1');
                        plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1);
                        hww = text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold');

                        end;

                        plot3(YY_kk(1,:),YY_kk(2,:),YY_kk(3,:),['.' colors(rem(kk-1,6)+1)]);

                    end;

                end;

            end;

        end;

    end;

end;

figure(4);rotate3d on;
axis('equal');
title('Extrinsic parameters (world-centered)');
%view(60,30);
xlabel('X_{world}')
ylabel('Y_{world}')
zlabel('Z_{world}')
view(a,b);
axis vis3d;
axis tight;
grid on;
plot3(3*dX*[1 0 0 0 0],3*dX*[0 0 1 0 0],3*dX*[0 0 0 0 1],'r-','linewidth',3);
hold off;
set(4,'color',[1 1 1]);

set(4,'Name','3D','NumberTitle','off');

%hh = axis;
%hh(5) = 0;
%axis(hh);

%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n');

if exist('h_switch2')==1,
    if ishandle(h_switch2),
        delete(h_switch2);
    end;
end;

if n_ima ~= 0,
    if show_camera,
        h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=0;ext_calib2;', 'Position',[1-.30 0.04  .30  .04],'String','Remove camera reference frames','fontsize',8,'fontname','clean','Tag','Pushbutton1');
    else
        h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=1;ext_calib2;', 'Position',[1-.30 0.04  .30  .04],'String','Add camera reference frames','fontsize',8,'fontname','clean','Tag','Pushbutton1');
    end;
end;

if exist('h_switch')==1,
    if ishandle(h_switch),
        delete(h_switch);
    end;
end;

h_switch = uicontrol('Parent',4,'Units','normalized', 'Callback','ext_calib', 'Position',[1-.30 0  .30  .04],'String','Switch to camera-centered view','fontsize',8,'fontname','clean','Tag','Pushbutton1');

figure(4);
rotate3d on;


function out = CO_TranslateShape(y,shape,d,howToMove)
% CO_TranslateShape  Statistics on datapoints inside geometric shapes across the time series.
%
% Inputs specify a shape and its size, and a method for moving this shape
% through the time domain.
%
% This is usually more informative in an embedding space (CO_Embed2_...), but
% here we do it just in the temporal domain (_t_).
%
% In the future, could perform a similar analysis with a soft boundary, some
% decaying force function V(r), or perhaps truncated...?
%
%---INPUTS:
% y, the input time series
% shape, the shape to move about the time-domain (e.g., 'circle')
% d, a parameter specifying the size of the shape (e.g., d = 2)
% howToMove, a method specifying how to move the shape about, e.g., 'pts'
%               places the shape on each point in the time series.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% 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, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Check inputs:
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(shape)
    shape = 'circle';
end
if nargin < 3 || isempty(d)
    d = 2; % a default distance d = 2
end
if nargin < 4 || isempty(howToMove)
    howToMove = 'pts'; % by default, places shapes on each timepoint
end

% ------------------------------------------------------------------------------
% Preliminaries:
% ------------------------------------------------------------------------------
N = length(y); % the length of the time series

% y must be a column vector, transpose it if it's a row vector
if size(y,2) > size(y,1)
    y = y';
end

% Add a time index
ty = [(1:N)', y]; % has increasing integers as time in the first column

%-------------------------------------------------------------------------------
% Generate the statistics on the number of points inside the shape as it is
% translated across the time series
%-------------------------------------------------------------------------------
switch howToMove
    case 'pts' % Place shapes on each timepoint (excluding a range at start and end)
        switch shape
            case 'circle' % uses a circle of radius 'd'
                r = d;
                w = floor(r); % only consider a window radius w (these are the
                            %    only points that could possibly be inside)
                rnge = 1+w:N-w;
                NN = length(rnge); % number of admissible points
                np = zeros(NN,1); % number of points
                for i = 1:NN
                    win = ty(rnge(i)-w:rnge(i)+w,:); % create window
                    difwin = win - ones(2*w+1,1)*ty(rnge(i),:);
                    np(i) = sum(sum(difwin.^2,2) <= r^2); % number of points enclosed in shape
                end
            case 'rectangle'
                % uses a rectangle of half-width d (integer), height as double the current time
                % series value (on either side of origin)
                w = d;
                rnge = 1+d:N-d;
                NN = length(rnge); % number of admissible points
                np = zeros(NN,1); % number of points
                for i = 1:NN
                    np(i) = sum(abs(y(rnge(i)-d:rnge(i)+d)) <= abs(y(i)));
                end
        otherwise
            error('Unknown shape ''%s''',shape)
        end
    otherwise
        error('Unknown setting for ''howToMove'' input: ''%s''',howToMove)
end

%-------------------------------------------------------------------------------
%% Output statistics
%-------------------------------------------------------------------------------

% Statistics on number of hits inside the shape:
out.max = max(np); % max hits
out.std = std(np); % std of hits
out.mean = mean(np); % mean number of hits

% Count the hits:
histnp = arrayfun(@(x)sum(np==x),unique(np));

% Compute mode of the histogram:
[out.npatmode,out.mode] = max(histnp);
out.npatmode = out.npatmode/NN;

% Output all stats:
if 2*w + 1 >= 1; out.ones = mean(np==1); end
if 2*w + 1 >= 2; out.twos = mean(np==2); end
if 2*w + 1 >= 3; out.threes = mean(np==3); end
if 2*w + 1 >= 4; out.fours = mean(np==4); end
if 2*w + 1 >= 5; out.fives = mean(np==5); end
if 2*w + 1 >= 6; out.sixes = mean(np==6); end
if 2*w + 1 >= 7; out.sevens = mean(np==7); end
if 2*w + 1 >= 8; out.eights = mean(np==8); end
if 2*w + 1 >= 9; out.nines = mean(np==9); end
if 2*w + 1 >= 10; out.tens = mean(np==10); end
if 2*w + 1 >= 11; out.elevens = mean(np==11); end

% -----
% Stationarity of the statistics in 2,3, & 4 segments of the time series:

out.statav2_m = SY_SlidingWindow(np,'mean','std',2,1);
out.statav2_s = SY_SlidingWindow(np,'std','std',2,1);
out.statav3_m = SY_SlidingWindow(np,'mean','std',3,1);
out.statav3_s = SY_SlidingWindow(np,'std','std',3,1);
out.statav4_m = SY_SlidingWindow(np,'mean','std',4,1);
out.statav4_s = SY_SlidingWindow(np,'std','std',4,1);

end


function eloo = gpla_looe(w, gp, x, y, varargin)
%GPLA_LOOE  Evaluate the mean negative log leave-one-out predictive
%           density with Laplace approximation.
%
%  Description
%    LOOE = GPLA_LOOE(W, GP, X, Y, OPTIONS) takes a parameter
%    vector W, Gaussian process structure GP, a matrix X of input
%    vectors and a matrix Y of targets, and evaluates the mean
%    negative log leave-one-out predictive density
%      LOOE = - 1/n sum log p(Y_i | X, Y_{\i}, th)
%    where th represents the parameters (lengthScale,
%    magnSigma2...), X is inputs and Y is observations.
%
%    Laplace leave-one-out is approximated by using analytic
%    leave-one-out equations for the approximated latent
%    distribution.
%
%    OPTIONS is optional parameter-value pair
%      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.
%
%  See also
%    GP_LOOE, GPLA_LOOPRED, GPLA_E
%
% Copyright (c) 2011 Aki Vehtari

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

ip=inputParser;
ip.FunctionName = 'GPLA_LOOE';
ip.addRequired('w', @(x) isempty(x) || ...
               isvector(x) && isreal(x) && all(isfinite(x)));
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.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.parse(w, gp, x, y, varargin{:});
z=ip.Results.z;

gp=gp_unpak(gp, w);
[tmp, tmp, lpyt] = gpla_loopred(gp, x, y, 'z', z);
eloo=-sum(lpyt);


function f=comp_idgtreal(coef,g,a,M,lt,phasetype)
%COMP_IDGTREAL  Compute IDGTREAL
%   Usage:  f=comp_idgtreal(c,g,a,M,lt,phasetype);
%
%   Input parameters:
%         c     : Array of coefficients.
%         g     : Window function.
%         a     : Length of time shift.
%         M     : Number of modulations.
%         lt    : lattice type
%   Output parameters:
%         f     : Signal.
%

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DGT
%   REFERENCE: OK

N=size(coef,2);
L=N*a;

if lt(2)==1
    f = comp_isepdgtreal(coef,g,L,a,M,phasetype);
else
    % Quinqux lattice
    f=comp_inonsepdgtreal_quinqux(coef,g,a,M);
end;


% Do the example from Satnam Alag's PhD thesis, UCB ME dept 1996 p46

% Make the following polytree, where all arcs point down

% 1   2
%  \ /
%   3
%  / \
% 4   5

N = 5;
dag = zeros(N,N);
dag(1,3) = 1;
dag(2,3) = 1;
dag(3, [4 5]) = 1;

ns = [2 1 2 1 2];

bnet = mk_bnet(dag, ns, 'discrete', []);

bnet.CPD{1} = gaussian_CPD(bnet, 1, 'mean', [1 0]', 'cov', [4 1; 1 4]);
bnet.CPD{2} = gaussian_CPD(bnet, 2, 'mean', 1, 'cov', 1);
B1 = [1 2; 1 0]; B2 = [2 1]';
bnet.CPD{3} = gaussian_CPD(bnet, 3, 'mean', [0 0]', 'cov', [2 1; 1 1], ...
			   'weights', [B1 B2]);
H1 = [1 1];
bnet.CPD{4} = gaussian_CPD(bnet, 4, 'mean', 0, 'cov', 1, 'weights', H1);
H2 = [1 0; 1 1];
bnet.CPD{5} = gaussian_CPD(bnet, 5, 'mean', [0 0]', 'cov', eye(2), 'weights', H2);

engine = {};
engine{end+1} = jtree_inf_engine(bnet);
engine{end+1} = pearl_inf_engine(bnet, 'protocol', 'tree');
engine{end+1} = pearl_inf_engine(bnet, 'protocol', 'parallel');
E = length(engine);

if 1
% no evidence
evidence = cell(1,N);
ll = zeros(1,E);
for e=1:E
  [engine{e}, ll(e)] = enter_evidence(engine{e}, evidence);
  add_ev = 1;
  m = marginal_nodes(engine{e}, 3, add_ev);
  assert(approxeq(m.mu, [3 2]'))
  assert(approxeq(m.Sigma, [30 9; 9 6]))

  m = marginal_nodes(engine{e}, 4, add_ev);
  assert(approxeq(m.mu, 5))
  assert(approxeq(m.Sigma, 55))

  m = marginal_nodes(engine{e}, 5, add_ev);
  assert(approxeq(m.mu, [3 5]'))
  assert(approxeq(m.Sigma, [31 39; 39 55]))
end
end

if 1
% evidence on leaf 5
evidence = cell(1,N);
evidence{5} = [5 5]';
for e=1:E
  [engine{e}, ll(e)] = enter_evidence(engine{e}, evidence);
  add_ev = 1;
  m = marginal_nodes(engine{e}, 3, add_ev);
  assert(approxeq(m.mu, [4.4022 1.0217]'))
  assert(approxeq(m.Sigma, [0.7011 -0.4891; -0.4891 1.1087]))

  m = marginal_nodes(engine{e}, 4, add_ev);
  assert(approxeq(m.mu, 5.4239))
  assert(approxeq(m.Sigma, 1.8315))

  m = marginal_nodes(engine{e}, 1, add_ev);
  assert(approxeq(m.mu, [0.3478 1.1413]'))
  assert(approxeq(m.Sigma, [1.8261 -0.1957; -0.1957 1.0924]))

  m = marginal_nodes(engine{e}, 2, add_ev);
  assert(approxeq(m.mu, 0.9239))
  assert(approxeq(m.Sigma, 0.8315))

  m = marginal_nodes(engine{e}, 5, add_ev);
  assert(approxeq(m.mu, evidence{5}))
  assert(approxeq(m.Sigma, zeros(2)))
end
end

if 1
% evidence on leaf 4 (non-info-state version is uninvertible)
evidence = cell(1,N);
evidence{4} = 10;
for e=1:E
  [engine{e}, ll(e)] = enter_evidence(engine{e}, evidence);
  add_ev = 1;
  m = marginal_nodes(engine{e}, 3, add_ev);
  assert(approxeq(m.mu, [6.5455 3.3636]'))
  assert(approxeq(m.Sigma, [2.3455 -1.6364; -1.6364 1.9091]))

  m = marginal_nodes(engine{e}, 5, add_ev);
  assert(approxeq(m.mu, [6.5455 9.9091]'))
  assert(approxeq(m.Sigma, [3.3455 0.7091; 0.7091 1.9818]))

  m = marginal_nodes(engine{e}, 1, add_ev);
  assert(approxeq(m.mu, [1.9091 0.9091]'))
  assert(approxeq(m.Sigma, [2.1818 -0.8182; -0.8182 2.1818]))

  m = marginal_nodes(engine{e}, 2, add_ev);
  assert(approxeq(m.mu, 1.2727))
  assert(approxeq(m.Sigma, 0.8364))
end
end

if 1
% evidence on leaves 4,5 and root 2
evidence = cell(1,N);
evidence{2} = 0;
evidence{4} = 10;
evidence{5} = [5 5]';
for e=1:E
  [engine{e}, ll(e)] = enter_evidence(engine{e}, evidence);
  add_ev = 1;
  m = marginal_nodes(engine{e}, 3, add_ev);
  assert(approxeq(m.mu, [4.9964 2.4444]'));
  assert(approxeq(m.Sigma, [0.6738 -0.5556; -0.5556 0.8889]));

  m = marginal_nodes(engine{e}, 1, add_ev);
  assert(approxeq(m.mu, [2.2043 1.2151]'));
  assert(approxeq(m.Sigma, [1.2903 -0.4839; -0.4839 0.8065]));
end
end

if 1
  [time, engine] = cmp_inference_static(bnet, engine, 'maximize', 0, 'check_ll', 0, ...
				     'singletons_only', 0, 'observed', [1 3 5]);
end


function [N,rho,P]=atmosParam4GasTemp(gasTable,T,rhow)
%%ATMOSPARAM4GASTEMP  Get basic parameters for atmospheric refractivity,
%                     density, and pressure given a table of constitudent
%                     gasses in the atmosphere, the temperature and the
%                     absolute humidity. The model is best suited for
%                     altitudes below 90km as ionozed parameters, such as
%                     anomolous oxygen, are not used.
%
%INPUTS: gasTable An NX2 cell array where gasTable{i,1} is a string
%                 describing the ith constituent atmospheric element and
%                 gasTable{i,2} is the number density of the element in
%                 particles per cubic meter. For a list of constituent
%                 elements that can be handled, see the documentation for
%                 the Constants.gasProp method. Unknown constituent
%                 elements that are passed will be ignored.
%               T The temperature in degrees Kelvin.
%            rhow An optional parameter specifying the mass density of
%                 water vapor at the point in question in units of
%                 kilograms per cubic meter. If omitted, the air is assumed
%                 to be dry (rhow=0). The total density of the air is
%                 assumed to be the sum of the dry air density and rhow.
%                 Alternatively, this parameter can be omitted and 'H2O'
%                 can be given as one of the constituent elements in
%                 gasTable.
%
%OUTPUTS: N The refractivity of the atmosphere. In this model, N is always
%           real. N=10^6*(n-1) where n is the index of refraction. This is
%           generally valid for frequencies from L-band (1GHz) to 10 GHz
%           (the middle of X-band).
%       rho The atmospheric density at the point in question in units of
%           kilograms per cubic meter.
%         P The atmospheric pressure at the point in question in units of
%           Newtons per square meter (Pascals). It assumes that the gasses
%           can be treated as ideal gasses.
%
%The refractive index is then found using the dry and wet air densities
%using the formula of [1], which should be valid for frequencies between
%1GHz and 10GHz. It ignores the lossiness of the atmosphere.
%
%REFERENCES:
%[1] J. M. Aparicio and S. Laroche, "An evaluation of the expression of
%    the atmospheric refractivity for GPS signals," Journal of Geophysical
%    Research, vol. 116, no. D11, 16 Jun. 2011.
%
%September 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3)
   rhow=0;
end

numGasses=size(gasTable,1);
numberDensities=[gasTable{:,2}];
totalGasNumberDensity=sum(numberDensities);

%rhod will be the total mass density of dry air at the point in kg/m^3.
rhod=0;
for curGas=1:numGasses
    AMU=Constants.gasProp(gasTable{curGas,1});
    %If an unknown gas is given, then ignore it.
    if(~isempty(AMU))
        %The number density has units of particles per cubic meter.
        %Multiplied by the atomic mass of the gas, we have atomic mass
        %units (AMU) per cubic meter. Multiplied by the value of the atomic
        %mass unit in kilograms, we get kilograms per cubic meter.
        rhod=rhod+Constants.atomicMassUnit*numberDensities(curGas)*AMU;
    else
        %This is so that the number density of the unknown constitutent is
        %also ignored when computing the pressure, so that the results are
        %consistent with the computation of the density.
        numberDensities(curGas)=0;
    end
end

rho=rhod+rhow;

%To use the ideal gas law to find the air pressure, the number of water
%molecules per cubic meter of the atmosphere is needed. This is obtained
%using the molar mass of water (H2O) and Avogadro's constant
Av=Constants.AvogadroConstant;
%The molar mass of water (H2O) in atomic mass units (grams per mole).
HAMU=Constants.elementAMU(1);
OAMU=Constants.elementAMU(8);
MMWater=HAMU*2+OAMU;

%The number of atoms of water per cubic meter. The 1000 transforms grams to
%kilograms.
NH2O=rhow/(1000*Av*MMWater);

%The total number density of the gasses in the atmosphere. That is, the
%number of atoms per cubic meter.
NTotal=totalGasNumberDensity+NH2O;
kB=Constants.BoltzmannConstant;
P=NTotal*kB*T;

%T is the temperature in Kelvin.
tau=273.15/T-1;
N0=(222.682+0.069*tau)*rhod+(6701.605+6385.886*tau)*rhow;
N=N0*(1+10^(-6)*N0/6);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [LU_or_x,info,c] = klu (A,operation,b,opts)                        %#ok
%KLU sparse left-looking LU factorization, using a block triangular form.
%
%   Example:
%   LU = klu (A)            factorizes R\A(p,q) into L*U+F, returning a struct
%   x = klu (A,'\',b)       x = A\b, using KLU
%   x = klu (b,'/',A)       x = b/A, using KLU
%   x = klu (LU,'\',b)      x = A\b, where LU = klu(A)
%   x = klu (b,'/',LU)      x = b/A, where LU = klu(A)
%
%   KLU(A) factorizes a square sparse matrix, L*U+F = R\A(p,q), where L and U
%   are the factors of the diagonal blocks of the block, F are the entries
%   above the diagonal blocks.  r corresponds to the 3rd output of dmperm; it
%   specifies where the block boundaries are.  The kth block consists of
%   rows/columns r(k) to r(k+1)-1 of A(p,q).
%
%   Note that the use of the scale factor R differs between KLU and UMFPACK
%   (and the LU function, which is based on UMFPACK).  In LU, the factorization
%   is L*U = P*(R1\A)*Q; in KLU it is L*U+F = R2\(P*A*Q).  R1 and R2 are related
%   via R2 = P*R1*P', or equivalently R2 = R1(p,p).
%
%   The LU output is a struct containing members L, U, p, q, R, F, and r.
%
%   opts is an optional input struct which appears as the last input argument.
%   Entries not present are set to their defaults:
%
%                       default
%       opts.tol        0.001   partial pivoting tolerance; valid range 0 to 1.
%       opts.btf        1       use block triangular form (BTF) if nonzero
%       opts.ordering   0       how each block is ordered:
%                               0: AMD, 1: COLAMD, 2: natural,
%                               3: CHOLMOD's ordering of (A'*A),
%                               4: CHOLMOD's ordering of (A+A')
%       opts.scale      2       1: R = diag(sum(abs(A)')), row-sum
%                               2: R = diag(max(abs(A)')), max in each row
%                               otherwise: none (R=I)
%       opts.maxwork    0       if > 0, limit work in BTF ordering to
%                               opts.maxwork*nnz(A); no limit if <= 0.
%
%       The CHOLMOD ordering is to try AMD (for A+A') or COLAMD (for A'*A)
%       first.  If the fill-in with AMD or COLAMD is high, METIS is tried (on
%       A+A' or A'*A), and the best ordering found is selected.  CHOLMOD, METIS,
%       CAMD, and CCOLAMD are required.  If not available, only ordering options
%       0, 1, and 2 may be used (AMD and COLAMD are always required by KLU).
%
%   Two optional outputs, [LU,info,c] = klu (A) or [x,info,c] = klu (A,'\',b)
%   provide statistics about the factorization:
%
%       info.noffdiag   number of off-diagonal pivots chosen (after preordering)
%       info.nrealloc   number of memory reallocations of L and U
%       info.rcond      a very cheap estimate of 1/(condition number)
%       info.rgrowth    reciprocal pivot growth
%       info.flops      flop count
%       info.nblocks    # of blocks in BTF form (1 if not computed)
%       info.ordering   AMD, COLAMD, natural, cholmod(AA'), cholmod(A+A')
%       info.scale      scaling (<=0: none, 1: sum, 2: max)
%       info.lnz        nnz(L), including diagonal
%       info.unz        nnz(U), including diagonal
%       info.offnz      nnz(F)
%       info.tol        pivot tolerance used
%       info.memory     peak memory usage in bytes
%       c               the same as MATLAB's condest
%
%   info and c are relevant only if the matrix is factorized (LU = klu (A),
%   x = klu (A,'/',b), or x = klu (b,'/',A) usages).
%
%   See also BTF, LU, DMPERM, CONDEST, CHOLMOD, AMD, COLAMD, CAMD, CCOLAMD.

% Copyright 2004-2009 Timothy A. Davis, Univ. of Florida
% http://www.cise.ufl.edu/research/sparse

error ('klu mexFunction not found') ;


function T = conj(T)
% conjugate of a tensor
%
% Input
%  T - @tensor
%
% Output
%  T - @tensor
%

switch T.rank

  case 1

  case 2

    T.M = conj(T.M);

  case 3

  case 4

    % check for symmetry

    % convert to a matrix
    M = tensor42(T.M);

    % invert the matrix
    M = conj(M);

    % make some corrections
    w = 1./(1+((1:6)>3));
    w = w.' * w;
    M = M .* w;

    % convert to back a 4 rank tensor
    T.M = tensor24(M);
end


%% Anaylsis_SSVEP
clear all; clc; close all;
%% initialization
DATADIR = 'WHERE\IS\DATA';
%% SSVEP
SSVEPDATA = 'EEG_SSVEP.mat';
STRUCTINFO = {'EEG_SSVEP_train', 'EEG_SSVEP_test'};
SESSIONS = {'session1', 'session2'};
TOTAL_SUBJECTS = 54;

%% INITIALIZATION
FS = 100;

%init
params = {'time', 4;...
'freq' , 60./[5, 7, 9, 11];...
'fs' , FS;...
'band' ,[0.5 40];...
'channel_index', [23:32]; ...
'time_interval' ,[0 4000]; ...
'marker',  {'1','up';'2', 'left';'3', 'right';'4', 'down'}; ...
};

%% validation
for sessNum = 1:length(SESSIONS)
    session = SESSIONS{sessNum};
    fprintf('\n%s validation\n',session);
    for subNum = 1:TOTAL_SUBJECTS
        subject = sprintf('s%d',subNum);
        fprintf('LOAD %s ...\n',subject);

        data = importdata(fullfile(DATADIR,session,subject,SSVEPDATA));

        CNT{1} = prep_resample(data.(STRUCTINFO{1}), FS,{'Nr', 0});
        CNT{2} = prep_resample(data.(STRUCTINFO{2}), FS,{'Nr', 0});
        ACC.SSVEP(subNum,sessNum) = SSVEP_performance(CNT, params);
        fprintf('%d = %f\n',subNum, ACC.SSVEP(subNum,sessNum));
        clear CNT
    end
end


function [MB] = B2MB(B)
% Convert computery things from bytes to megabytes.
% Chad A. Greene 2012
MB = B*2^-20 ;


%compute velmag_centralhead
function [data,units]=compute_velmagcentralhead(trx,n)
larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velmagcentralhead=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    velmagcentralhead{1,i}=bsxfun(@hypot,trx(larva).dxcentralhead_mm,trx(larva).dycentralhead_mm);
end

units=parseunits('rad');
data=velmagcentralhead;


function varargout = vscale(varargin)
%VSCALE   Vertical scale of a SPHEREFUN.
%
% VSCL = VSCALE(F) returns the vertial scale of a SPHEREFUN as determined
% by evaluating on a coarse tensor-product grid.

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

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

end


function PowerOverTime(nlp, sys)

    domains = sys.Gamma.Nodes.Domain;
    T  = SymVariable('t',[2, 1]);
    for i=1:numel(domains)
        domain = domains{i};

        u = domain.Inputs.Control.u;
        dq = domain.States.dx;
        B = domain.Gmap.Control.u;

        P = tovector(norm(dq.*(B*u)).^2)./(T(2)-T(1));
        power_fun = SymFunction(['powerOverTime_' sys.Gamma.Nodes.Name{i}],P,{T,u,dq});
        rs_phase = getPhaseIndex(nlp,sys.Gamma.Nodes.Name{i});
        addRunningCost(nlp.Phase(rs_phase),power_fun,{'T','u','dx'});

    end
    nlp.update;
end


function bnet = mk_hhmm3(varargin)
% MK_HHMM3 Make a 3 level Hierarchical HMM
% bnet = mk_hhmm3(...)
%
% 3-layer hierarchical HMM where level 1 only connects to level 2, not 3 or obs.
% This enforces sub-models (which differ only in their Q1 index) to be shared.
% Also, we enforce the fact that each model always starts in its initial state
% and only finishes in its final state. However, the prob. of finishing (as opposed to
% self-transitioning to the final state) can be learned.
% The fact that we always finish from the same state means we do not need to condition
% F(i) on Q(i-1), since finishing prob is indep of calling context.
%
% The DBN is the same as Fig 10 in my tech report.
%
%   Q1 ---------->  Q1
%   |              / |
%   |             /  |
%   |  F2 -------    |
%   |  ^         \   |
%   | /|          \  |
%   v  |           v v
%   Q2-| -------->   Q2
%  /|  |             ^
% / |  |            /|
% | |  F3 ---------/ |
% | |  ^           \ |
% | v /              v
% | Q3 ----------->  Q3
% |  |
% \  |
%  v v
%   O
%
%
% Optional arguments in name/value format [default]
%
% Qsizes      - sizes at each level [ none ]
% Osize       - size of O node [ none ]
% discrete_obs - 1 means O is tabular_CPD, 0 means O is gaussian_CPD [0]
% Oargs       - cell array of args to pass to the O CPD  [ {} ]
% transprob1  - transprob1(i,j) = P(Q1(t)=j|Q1(t-1)=i)  ['ergodic']
% startprob1  - startprob1(j) = P(Q1(t)=j)  ['leftstart']
% transprob2  - transprob2(i,k,j) = P(Q2(t)=j|Q2(t-1)=i,Q1(t)=k)  ['leftright']
% startprob2  - startprob2(k,j) = P(Q2(t)=j|Q1(t)=k)  ['leftstart']
% termprob2   - termprob2(j,f) = P(F2(t)=f|Q2(t)=j)  ['rightstop']
% transprob3  - transprob3(i,k,j) = P(Q3(t)=j|Q3(t-1)=i,Q2(t)=k)  ['leftright']
% startprob3  - startprob3(k,j) = P(Q3(t)=j|Q2(t)=k)  ['leftstart']
% termprob3   - termprob3(j,f) = P(F3(t)=f|Q3(t)=j)  ['rightstop']
%
% leftstart means the model always starts in state 1.
% rightstop means the model always finished in its last state (Qsize(d)).
%
% Q1:Q3 in slice 1 are of type tabular_CPD
% Q1:Q3 in slice 2 are of type hhmmQ_CPD.
% F2 is of type hhmmF_CPD, F3 is of type tabular_CPD.

ss = 6; D = 3;
Q1 = 1; Q2 = 2; Q3 = 3; F3 = 4; F2 = 5; obs = 6;
Qnodes = [Q1 Q2 Q3]; Fnodes = [F2 F3];
names = {'Q1', 'Q2', 'Q3', 'F3', 'F2', 'obs'};

intra = zeros(ss);
intra(Q1, Q2) = 1;
intra(Q2, [F2 Q3 obs]) = 1;
intra(Q3, [F3 obs]) = 1;
intra(F3, F2) = 1;

inter = zeros(ss);
inter(Q1,Q1) = 1;
inter(Q2,Q2) = 1;
inter(Q3,Q3) = 1;
inter(F2,[Q1 Q2]) = 1;
inter(F3,[Q2 Q3]) = 1;

% get sizes of nodes
args = varargin;
nargs = length(args);
Qsizes = [];
Osize = 0;
for i=1:2:nargs
  switch args{i},
   case 'Qsizes', Qsizes = args{i+1};
   case 'Osize', Osize = args{i+1};
  end
end
if isempty(Qsizes), error('must specify Qsizes'); end
if Osize==0, error('must specify Osize'); end

% set default params
discrete_obs = 0;
Oargs = {};
startprob1 = 'ergodic';
startprob2 = 'leftstart';
startprob3 = 'leftstart';
transprob1 = 'ergodic';
transprob2 = 'leftright';
transprob3 = 'leftright';
termprob2 = 'rightstop';
termprob3 = 'rightstop';

for i=1:2:nargs
  switch args{i},
   case 'discrete_obs', discrete_obs = args{i+1};
   case 'Oargs',        Oargs = args{i+1};
   case 'Q1args',       Q1args = args{i+1};
   case 'Q2args',       Q2args = args{i+1};
   case 'Q3args',       Q3args = args{i+1};
   case 'F2args',       F2args = args{i+1};
   case 'F3args',       F3args = args{i+1};
  end
end

ns = zeros(1,ss);
ns(Qnodes) = Qsizes;
ns(obs) = Osize;
ns(Fnodes) = 2;

dnodes = [Qnodes Fnodes];
if discrete_obs
  dnodes = [dnodes obs];
end
onodes = [obs];

bnet = mk_dbn(intra, inter, ns, 'observed', onodes, 'discrete', dnodes, 'names', names);
eclass = bnet.equiv_class;

if strcmp(startprob1, 'ergodic')
  startprob1 = normalise(ones(1,ns(Q1)));
end
if strcmp(startprob2, 'leftstart')
  startprob2 = zeros(ns(Q1), ns(Q2));
  starpbrob2(:, 1) = 1.0;
end
if strcmp(startprob3, 'leftstart')
  startprob3 = zeros(ns(Q2), ns(Q3));
  starpbrob3(:, 1) = 1.0;
end

if strcmp(termprob2, 'rightstop')
  p = 0.9;
  termprob2 = zeros(Qsize(2),2);
  termprob2(:, 2) = p;
  termprob2(:, 1) = 1-p;
  termprob2(1:(Qsize(2)-1), 1) = 1;
end
if strcmp(termprob3, 'rightstop')
  p = 0.9;
  termprob3 = zeros(Qsize(3),2);
  termprob3(:, 2) = p;
  termprob3(:, 1) = 1-p;
  termprob3(1:(Qsize(3)-1), 1) = 1;
end

% SLICE 1

% We clamp untied nodes in the first slice, since their params can't be estimated
% from just one sequence

bnet.CPD{eclass(Q1,1)} = tabular_CPD(bnet, Q1, 'CPT', startprob1, 'adjustable', 0);
bnet.CPD{eclass(Q2,1)} = tabular_CPD(bnet, Q2, 'CPT', startprob2, 'adjustable', 0);
bnet.CPD{eclass(Q3,1)} = tabular_CPD(bnet, Q3, 'CPT', startprob3, 'adjustable', 0);

bnet.CPD{eclass(F2,1)}  = hhmmF_CPD(bnet, F2, Qnodes, 2, D, 'termprob', termprob2);
bnet.CPD{eclass(F3,1)}  = tabular_CPD(bnet, F3, 'CPT', termprob3);

if discrete_obs
  bnet.CPD{eclass(obs,1)} = tabular_CPD(bnet, obs, Oargs{:});
else
  bnet.CPD{eclass(obs,1)} = gaussian_CPD(bnet, obs, Oargs{:});
end

% SLICE 2

bnet.CPD{eclass(Q1,2)} = hhmmQ_CPD(bnet, Q1+ss, Qnodes, 1, D, 'transprob', transprob1, 'startprob', startprob1);
bnet.CPD{eclass(Q2,2)} = hhmmQ_CPD(bnet, Q2+ss, Qnodes, 2, D, 'transprob', transprob2, 'startprob', startprob2);
bnet.CPD{eclass(Q3,2)} = hhmmQ_CPD(bnet, Q3+ss, Qnodes, 3, D, 'transprob', transprob3, 'startprob', startprob3);


function complex_image = bpBasic(data)
%BPBASIC This function performs a basic backprojection operation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The following fields need to be populated:                           %
% data.deltaF:  Vector containing the step size of frequency data (Hz) %
% data.minF:  Vector containing the start frequency of each pulse (Hz) %
% data.x_mat:  The x-position of each pixel (m)                        %
% data.y_mat:  The y-position of each pixel (m)                        %
% data.z_mat:  The z-position of each pixel (m)                        %
% data.Tx.X:  The transmit x-position of the sensor at each pulse (m)  %
% data.Tx.Y:  The transmit y-position of the sensor at each pulse (m)  %
% data.Tx.Z:  The transmit z-position of the sensor at each pulse (m)  %
% data.R0:  The (bistatic) range to motion-comp point (m)              %
% data.phdata:  Phase history data (frequency domain)                  %
%               Fast time in rows, slow time in columns                %
%                                                                      %
% The following fields are optional:                                   %
% data.Rcv.X:  The receive x-position of the sensor at each pulse (m)  %
% data.Rcv.Y:  The receive y-position of the sensor at each pulse (m)  %
% data.Rcv.Z:  The receive z-position of the sensor at each pulse (m)  %
%              If no Rcv field is given, the monostataic case          %
%              (Rcv = Tx) is assumed.                                  %
% data.Nfft:  Size of the FFT to form the range profile                %
% data.hide_waitbar: Whether to suppress display of waitbar or not.    %
%                    If you call bpBasic from within a loop, you will  %
%                    probably want to use this.  Default is false.     %
%                                                                      %
% The output is:                                                       %
% complex_image:  The complex image value at each pixel                %
%                                                                      %
% History:                                                             %
%    Original code                                                     %
%       LeRoy Gorham, Air Force Research Laboratory, WPAFB, OH         %
%       leroy.gorham@wpafb.af.mil                                      %
%       Original Date Released:  8 Apr 2010                            %
%    Modified by Wade Schwartzkopf, NGA/IDT                            %
%       Wade.C.Schwartzkopf.ctr@nga.mil                                %
%       Structural changes to handle CPHD                              %
%    Modified by Daniel Andre, Dstl, Porton Down, UK                   %
%       dandre@dstl.gov.uk                                             %
%       Extension of the code to the bistatic case (1 Sep 2011)        %
%                                                                      %
% Gorham, L.A. and Moore, L.J., "SAR image formation toolbox for       %
%   MATLAB,"  Algorithms for Synthetic Aperture Radar Imagery XVII     %
%   7669, SPIE (2010).                                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Setup input parameters
% Determine the size of the phase history data for easier to read code
num_pulses = size(data.phdata,2);

% Maximum scene extent in range (m)
c = 299792458; % Speed of light (m/s)
max_extent_range=c/mean(data.deltaF); % Two-way range (transmitter, target, receiver)

% Setup reasonable default value for Nfft if none passed in
if ~isfield(data,'Nfft')
    data.Nfft = 2^(3+nextpow2(size(data.phdata,1))); % Use power-of-2 FFT
    % The linear interolation in the interp1 below causes cross-range
    % artifacts unless we sinc interpolate first with a much larger FFT size
    % than necessary.  Thus the "3+" in the line above.
end

% Assume monostatic case if bistatic info is not given
if ~isfield(data,'Rcv')
    data.Rcv = data.Tx;
end

show_waitbar = ~(isfield(data,'hide_waitbar')&&data.hide_waitbar);

%% Do actual backprojection computation
% Calculate the range to every bin in the range profile (m)
range_vector = linspace(-data.Nfft/2,data.Nfft/2-1,data.Nfft)*max_extent_range/data.Nfft;

% Initialize the image with all zero values
complex_image = zeros(size(data.x_mat));

if show_waitbar
    wb = waitbar(0);
    tic;
end
% Loop through every pulse
for ii = 1:num_pulses
    % Form the range profile with zero padding added
    rc = fftshift(ifft(data.phdata(:,ii),data.Nfft));

    % Calculate differential (bistatic) range for each pixel in the image (m)
    dR = sqrt((data.Tx.X(ii)-data.x_mat).^2 + ... % Range from transmit to pixel
              (data.Tx.Y(ii)-data.y_mat).^2 + ...
              (data.Tx.Z(ii)-data.z_mat).^2) + ...
         sqrt((data.Rcv.X(ii)-data.x_mat).^2 + ... % Range from receive to pixel
              (data.Rcv.Y(ii)-data.y_mat).^2 + ...
              (data.Rcv.Z(ii)-data.z_mat).^2) - ...
         (2*data.R0(ii)); % Range from transmit to motion-comp point to receive

    % Calculate phase correction for image
    phCorr = exp(1i*2*pi*data.minF(ii)/c*dR);

    % Determine which pixels fall within the range swath
    I = find(and(dR > min(range_vector), dR < max(range_vector)));

    % Update the image using linear interpolation
    complex_image(I) = complex_image(I) + interp1(range_vector,rc,dR(I),'linear') .* phCorr(I);

    if show_waitbar
        % Determine remaining execution time and display
        t_sofar = toc;
        t_est = (t_sofar*num_pulses/ii)-t_sofar;
        wb_message=sprintf('Pulse %d of %d, Time remaining: %s',ii,...
            num_pulses,datestr(datenum(0,0,0,0,0,t_est),13));
        waitbar(ii/num_pulses,wb,wb_message);
    end
end
if show_waitbar
    close(wb);
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  battistn@tcnj.edu
% Date Modified: April 2021
% Current Institution: TCNJ
%
% IB2d Date Created: May 27th, 2015
% Institution Created: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (torsional springs or non-invariant beams)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (battistn@tcnj.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the target point positions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function targets = update_Target_Point_Positions(dt,current_time,targets)

%IDs = targets(:,1);                 % Stores Lag-Pt IDs in col vector
%xPts= targets(:,2);                % Original x-Values of x-Target Pts.
%yPts= targets(:,3);                 % Original y-Values of y-Target Pts.
%kStiffs = targets(:,4);             % Stores Target Stiffnesses

%-----------------------------------------------------
% Geometric Translations to Quadrant 1
%    since stored .mat geo data centered at (0,0)
%-----------------------------------------------------
xoffset = 1;   % To put geometry into QUADRANT-1
yoffset = 0.5; % To put geometry into QUADRANT-1

%-----------------------------------------------
% Note: (1) ds for stored data is 0.6/(2*1024)
%       (2) 'ratio' is comparing 1024:desired resolution
%-----------------------------------------------
Lx=2;          % Horizontal Eulerian grid length
Nx=64;        % Eulerian grid resolution
dx=Lx/Nx;      % Eulerian grid spacing
ds=dx/2;       % Lagrangian point spacing

%-----------------------------------------------
% Load prescribed position data for tentacles
%-----------------------------------------------
load('total_coeffs.mat')
load('coral_coeff_30.mat')

%-----------------------------------------------
% Arclength
%-----------------------------------------------
s=(0:ds:total_meanL)/total_meanL;

%---------------------------------------------------------
% Get index correponding to current time in
%   simulation to determine how interpolation occurs
%---------------------------------------------------------
indx=ceil(current_time/dt)+1;

%-----------------------------------------------
% Load geometry state data
%-----------------------------------------------
load('cval.mat')

%-------------------------------------------------
% Get interpolation polynomial coefficients and
%   then determine geometry of LEFT tentacle
%-------------------------------------------------
C1=c1vals(indx,:);
C2=c2vals(indx,:);

XbL_1=(C1(1)*s.^3+C1(2)*s.^2+C1(3)*s+C1(4));
YbL_1=(C2(1)*s.^3+C2(2)*s.^2+C2(3)*s+C2(4));
L_ten=sum(sqrt((XbL_1(2:end)-XbL_1(1:end-1)).^2 +(YbL_1(2:end)-YbL_1(1:end-1)).^2 ));

XbL=(C1(1)*s.^3+C1(2)*s.^2+C1(3)*s+C1(4))*total_meanL/L_ten+total_offset;
YbL=(C2(1)*s.^3+C2(2)*s.^2+C2(3)*s+C2(4))*total_meanL/L_ten;

%-------------------------------------------------------
% Set up symmetric RIGHT tentacle interpolation states
%-------------------------------------------------------
XbR=-XbL;
YbR=YbL;

%-------------------------------------------------------
% Get coral polyp STEM geometry
%-------------------------------------------------------
YbStem=(YbL(1))+0*XbStem;

%-------------------------------------------------------
% Combine geometry into one vector and translate
%-------------------------------------------------------
x=[flip(XbR) XbStem XbL];
y=[flip(YbR) YbStem YbL];
%
x = x+xoffset;
y = y+yoffset;

%-------------------------------------------------------
% Update target point positions!
%-------------------------------------------------------
targets(:,2) = x;   % Store new xVals
targets(:,3) = y;   % Store new yVals


 function jf_slicer(data, varargin)
%function jf_slicer(data, [options])
%|
%| slice 3d data interactively (along 3rd dimension).
%| Uses scroll wheel to sweep through all slices.
%|
%| todo: hsv!
%|
%| in
%|	data	[nx ny nz]
%|
%| options
%|	clim	[1 2]		clim arg to im()
%|	iz	[1]		initial slice (default: nz/2+1)
%|
%| Jeff Fessler, University of Michigan

if ~nargin, ir_usage, end
if streq(data, 'test'), jf_slicer_test, return, end

arg.clim = [];
arg.iz = [];
arg = vararg_pair(arg, varargin);
if ~isreal(data)
	printm 'warning: taking abs of complex data'
	data = abs(data);
end

if isempty(arg.clim)
	arg.clim = double(minmax(data)');
end

nz = size(data, 3);

if isempty(arg.iz)
	iz = ceil((nz+1)/2);
else
	iz = arg.iz;
end

clamp = @(iz) max(min(iz, nz), 1);

stuff.data = data;
stuff.arg = arg;

%jf_slicer_call(iz, stuff)
%drawnow
jf_slicer_show

set(gcf, 'WindowScrollWheelFcn', @jf_slicer_scroll)
set(gcf, 'WindowKeyPressFcn', @jf_slicer_key)

%h = jf_add_slider('callback', @jf_slicer_call, 'data', {stuff}, ...
%	'min', 1, 'max', nz, 'sliderstep', [1 1]/(nz-1), 'value', iz);

function jf_slicer_scroll(src, event)
	iz = iz + event.VerticalScrollCount;
	iz = clamp(iz);
	jf_slicer_show
end % jf_slicer_scroll

function jf_slicer_key(src, event)
	key = get(gcf, 'CurrentCharacter');
	key = 0 + key;
	switch key
	case {28, 31, 45} % left arrow, down arrow, -
		iz = iz - 1;
	case {29, 30, 43} % right arrow, up arrow, +
		iz = iz + 1;
	otherwise
		if 49 <= key && key <= 57
			iz = key - 48; % 1-9
		end
	end
	iz = clamp(iz);
	pr iz
	jf_slicer_show
end % jf_slicer_key

function jf_slicer_show
	im(data(:,:,iz), arg.clim), cbar
	xlabelf('%d / %d', iz, nz)
end % jf_slicer_show

end % jf_slicer

function jf_slicer_call(iz, stuff)
persistent iz_save
if isempty(iz_save)
	iz_save = -1;
end
iz = round(iz);
if iz ~= iz_save
	iz_save = iz;
	arg = stuff.arg;
	im(stuff.data(:,:,iz), arg.clim), cbar
end

end % jf_slicer_call

function jf_slicer_test
data = reshape(1:7*8*9, 7, 8, 9);
im plc 1 2
im subplot 2
if im
	jf_slicer(data, 'clim', [0 500])
end
end % jf_slicer_test


function r8btree_node_add_binary_test ( )

%*****************************************************************************80
%
%% R8BTREE_NODE_ADD_RANDOM_TEST adds random numbers to a BTREE.
%
%  Discussion:
%
%    Because the data is generated randomly, the tree will not be guaranteeed
%    to be well balanced.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 November 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8BTREE_NODE_ADD_RANDOM_TEST\n' );
  fprintf ( 1, '  Use R8BTEE_NODE_ADD to build a BTREE,\n' );
  fprintf ( 1, '  by adding 33 random numbers, one at a time.\n' );
%
%  Initialize the BTREE.
%
  node_num = 0;
  tree = [];
  data_num = 2;
  tree_data = [];
%
%  Generate 33 nodes in [0,1];
%
  for i = 1 : 33

    x = rand ( );
    fx = x * x;
    node_data = [ x, fx ];

    [ node_num, tree, tree_data ] = r8btree_node_add ( node_num, tree, ...
      data_num, tree_data, node_data );

  end
%
%  Print the BTREE.
%
  r8btree_print ( node_num, tree, data_num, tree_data );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8BTREE_NODE_ADD_RANDOM_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


function [B, L, N, A] = imBoundaryContours(bin, varargin)
%IMBOUNDARYCONTOURS Extract polygonal contours of a binary image.
%
%   CNTS = imBoundaryContours(BIN)
%   Computes a set of contours corresponding to the structures in binary
%   image BIN. The result CNTS is a cell array of polygons, each polygon
%   being a N-by-2 array containing the vertex coordinates.
%
%   This function is mainly a wrapper to the 'bwboundaries' function, that
%   returns the resulting set of contours in a different format.
%
%
%   Example
%     % draw the polygon corresponding to a single region
%     img = imread('circles.png');
%     polys = imContours(img);
%     figure; imshow(img); hold on;
%     drawPolygon(polys, 'r')
%
%   See also
%     bwboundaries, imcontour, imOtsuThreshold, imContourLines
%     contourMatrixToPolylines (MatGeom toolbox)
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2012-07-27,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

% check input image is binary
if ~isempty(varargin)
    error('Requires input image to be binary.');
end

% call the native bwboundaries function
[B, L, N, A] = bwboundaries(bin);

% object boundaries
for i = 1:N
    bnd = B{i};
    B{i} = bnd(:, [2 1]);
end

% hole boundaries
for i = N+1:length(B)
    bnd = B{i};
    B{i} = bnd([1 end:-1:2], [2 1]);
end


function [classifier] = saveclassifier(name,svs,alpha,xtrain,b,options,sec,t,lsiters,stats)
% {saveclassifier} generates structure with details of a kernel classifier.
%
%      classifier = saveclassifier(name,svs,alpha,xtrain,b,options)
%
%      classifier = saveclassifier(name,svs,alpha,xtrain,b,options,
%                                  sec,t,lsiters,stats)
%
%      name: string identifier of the classifier
%      svs: indices of the training points that are present on the
%           classifier function
%      alpha: coefficient vector (with |svs| elements)
%      xtrain: corresponding training examples
%      b: bias
%      options: the options data structure used to train the classifier,
%               that contains the regularization parameters, kernel
%	            function, kernel parameters etc...
%
%      [optional]
%      sec: training time
%      t: number of iteration performed while training
%      lsi: vector with the number of line searches for each iteration
%      stats: matrix of misc statistics
%
%      classifier: is a data structure with the following fields
%                  classifier.name: name
%                  classifier.svs: svs
%                  classifier.alpha: alpha
%                  classifier.xtrain: xtrain
%                  classifier.b: b
%                  classifier.options: options
%
%                  [if optional arguments are present]
%                  classifier.iters: t
%                  classifier.lsiters: lsiters
%                  classifier.traintime: sec
%                  classifier.stats: stats
%
% Author: Stefano Melacci (2009)
%         mela@dii.unisi.it
%         * based on the code of Vikas Sindhwani, vikas.sindhwani@gmail.com

classifier.name=name;
classifier.svs=svs;
classifier.alpha=alpha;
classifier.b=b;
classifier.xtrain=xtrain;
classifier.options=options;
if exist('t','var') && ~isempty(t), classifier.iters=t; end
if exist('lsiters','var') && ~isempty(lsiters), classifier.lsiters=lsiters; end
if exist('sec','var') && ~isempty(sec), classifier.traintime=sec; end
if exist('stats','var') && ~isempty(stats), classifier.stats=stats; end


function minimise( varargin )

%MINIMISE Specifiies a convex (or affine) objective to be maximized.

cvx_problem = evalin( 'caller', 'cvx_problem', '[]' );
if isempty( cvx_problem ) || ~isa( cvx_problem, 'cvxprob' ),
    error( 'A cvx problem does not exist in this scope.' );
end
if ~isempty( cvx_problem.direction ),
    if isequal( cvx_problem.direction, 'find' ),
        error( 'Objective functions cannot be added to sets.' );
    else
        error( 'An objective function has already been supplied.' );
    end
end

if nargin < 1,
    error( 'Objective expression missing.' );
elseif iscellstr( varargin ),
    arg = evalin( 'caller', sprintf( '%s ', varargin{:} ) );
elseif nargin > 1,
    error( 'Too many input arguments.' );
else
    arg = varargin{1};
end

if ~isa( arg, 'cvx' ) && ~isa( arg, 'double' ) && ~isa( arg, 'sparse' ),
    error( 'Cannot accept an objective of type ''%s''.', class( arg ) );
end
persistent remap
if isempty( remap ),
    remap = cvx_remap( 'convex', 'log-convex' );
end
vx = remap( cvx_classify( arg ) );
if ~all( vx ),
    error( 'Disciplined convex programming error:\n   Cannot minimise a(n) %s expression.', cvx_class(arg(vx==0),false,true) );
end

newobj( cvx_problem, 'minimize', arg );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.