%%*****************************************************************************
%% HSDsqlp: solve an semidefinite-quadratic-linear program
%%   by infeasible path-following method on the homogeneous self-dual model.
%%
%%  [obj,X,y,Z,info,runhist] =
%%      HSDsqlp(blk,At,C,b,OPTIONS,X0,y0,Z0,kap0,tau0,theta0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%     OPTIONS: a structure that specifies parameters required in HSDsqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%  (kap0,tau0,theta0): initial parameters (if not given, the default is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  ...
%%      (all have default values set in sqlparameters.m).
%%
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

  function [obj,X,y,Z,info,runhist] = ...
            HSDsqlpmain(blk,At,C,b,par,X0,y0,Z0,kap0,tau0,theta0);

%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%
   global spdensity  solve_ok  printlevel
   global schurfun   schurfun_par
%%
   randstate = rand('state');  randnstate = randn('state');
   rand('state',0);   randn('state',0);
%%
   matlabversion = par.matlabversion;
   vers          = par.vers;
   predcorr      = par.predcorr;
   gam           = par.gam;
   expon         = par.expon;
   gaptol        = par.gaptol;
   inftol        = par.inftol;
   steptol       = par.steptol;
   maxit         = par.maxit;
   printlevel    = par.printlevel;
   stoplevel     = par.stoplevel;
   scale_data    = par.scale_data;
   spdensity     = par.spdensity;
   rmdepconstr   = par.rmdepconstr;
   cachesize     = par.cachesize;
   smallblkdim   = par.smallblkdim;
   schurfun      = par.schurfun;
   schurfun_par  = par.schurfun_par;
   ublksize      = par.ublksize;
%%
   tstart = cputime;
   X = X0; y = y0; Z = Z0;
   for p = 1:size(blk,1)
      if strcmp(blk{p,1},'u'); Z{p} = zeros(blk{p,2},1); end
   end
%%
%%-----------------------------------------
%% convert unrestricted blk to linear blk.
%%-----------------------------------------
%%
   ublkidx = zeros(size(blk,1),1);
   Cpert = zeros(size(blk,1),1); Cnew = C;
   perturb_C = 1;
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      n = sum(pblk{2});
      tmp = max(1,norm(C{p},'fro'))/sqrt(n);
      if strcmp(pblk{1},'u')
         if (printlevel); fprintf(' *** convert ublk to linear blk'); end
         ublkidx(p) = 1;
         n = 2*pblk{2};
         blk{p,1} = 'l'; blk{p,2} = n;
         if (perturb_C); Cpert(p) = 1e-2*tmp; end
         C{p}  = [C{p}; -C{p}];
         At{p} = [At{p}; -At{p}];
         Cnew{p} = C{p} + Cpert(p)*ones(n,1);
         X{p}  = 1+rand(n,1); %% do not add a factor of n
         Z{p}  = 1+rand(n,1); %%
      elseif strcmp(pblk{1},'s')
         if (perturb_C); Cpert(p) = 1e-3*tmp; end
         Cnew{p} = C{p} + Cpert(p)*speye(n);
      else
         if (perturb_C); Cpert(p) = 1e-3*tmp; end
         Cnew{p} = C{p} + Cpert(p)*ones(n,1);
      end
   end
%%
%%-----------------------------------------
%% check if the matrices Ak are
%% linearly independent.
%%-----------------------------------------
%%
   m0 = length(b);
   [At,b,y,indeprows,depconstr,feasible,AAt] = ...
   checkdepconstr(blk,At,b,y,rmdepconstr);
   if (~feasible)
      msg = 'SQLP is not feasible';
      if (printlevel); fprintf('\n %s',msg); end
      return;
   end
   par.depconstr = depconstr;
%%
   normC = zeros(length(C),1);
   for p = 1:length(C); normC(p) = max(max(abs(C{p}))); end
   normC = 1+max(normC);
   normb = 1+max(abs(b));
   nn = ops(C,'getM');
   m = length(b);
   if (nargin <= 8) | (isempty(kap0) | isempty(tau0) | isempty(theta0))
      if (max([ops(At,'norm'),ops(C,'norm'),norm(b)]) > 1e6)
         kap0 = 10*blktrace(blk,X,Z);
      else
         kap0 = blktrace(blk,X,Z);
      end
      tau0 = 1; theta0 = 1;
   end
   kap = kap0; tau = tau0; theta = theta0;
%%
   normX0 = ops(X0,'norm')/tau; normZ0 = ops(Z0,'norm')/tau;
   bbar  = (tau*b-AXfun(blk,At,[],X))/theta;
   ZpATy = ops(Z,'+',Atyfun(blk,At,[],[],y));
   Cbar = ops(ops(ops(tau,'*',C),'-',ZpATy),'/',theta);
   gbar = (blktrace(blk,C,X)-b'*y+kap)/theta;
   abar = (blktrace(blk,X,Z)+tau*kap)/theta;
   for p = 1:size(blk,1);
      pblk = blk(p,:);
      if strcmp(pblk{1},'s')
         At{p} = [At{p}, -svec(pblk,Cnew{p},1), svec(pblk,Cbar{p},1)];
      else
         At{p} = [At{p}, -Cnew{p}, Cbar{p}];
      end
   end
   Bmat = [sparse(m,m), -b, bbar; b', 0, gbar; -bbar', -gbar, 0];
   em1 = zeros(m+2,1); em1(m+1) = 1;
   em2 = zeros(m+2,1); em2(m+2) = 1;
   par.Umat = [[b;0;0], [bbar;gbar;0], em1, em2];
   par.m = m;
   par.diagAAt = [full(diag(AAt)); 1; 1];
%%
%%-----------------------------------------
%% find the combined list of non-zero
%% elements of Aj, j = 1:k, for each k.
%%-----------------------------------------
%%
   par.numcolAt = length(b)+2;
   [At,C,Cnew,X,Z,par.permA,par.invpermA,par.permZ] = ...
    HSDsortA(blk,At,C,Cnew,[b;0;0],X,Z);
   [par.isspA,par.nzlistA,par.nzlistAsum,par.isspAy,par.nzlistAy] = ...
    nzlist(blk,At,par);
%%
%%-----------------------------------------
%% initialization
%%-----------------------------------------
%%
   y2 = [y; tau; theta];
   AX = AXfun(blk,At,par.permA,X);
   rp = [zeros(m,1); kap; -abar] - AX - Bmat*y2;
   Rd = ops(Atyfun(blk,At,par.permA,par.isspAy,-y2),'-',Z);
   trXZ = blktrace(blk,X,Z);
   mu  = (trXZ+kap*tau)/(nn+1);
   obj = [blktrace(blk,C,X), b'*y]/tau;
   gap = trXZ/tau^2;
   relgap = gap/(1+mean(abs(obj)));
   ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,[y;0;0]));
   ZpATynorm = ops(ZpATy,'norm');
   prim_infeas = norm(b - AX(1:m)/tau)/normb;
   dual_infeas = ops(ops(C,'-',ops(ZpATy,'/',tau)),'norm')/normC;
   infeas = max(prim_infeas,dual_infeas);
   pstep = 0; dstep = 0; pred_convg_rate = 1; corr_convg_rate = 1;
   prim_infeas_bad = 0;
   termcode  = -6;
   msg = [];
   runhist.pobj    = obj(1);
   runhist.dobj    = obj(2);
   runhist.gap     = gap;
   runhist.relgap  = relgap;
   runhist.pinfeas = prim_infeas;
   runhist.dinfeas = dual_infeas;
   runhist.infeas  = infeas;
   runhist.cputime = cputime-tstart;
   runhist.step    = 0;
   ttime.preproc   = runhist.cputime;
   ttime.pred = 0; ttime.pred_pstep = 0; ttime.pred_dstep = 0;
   ttime.corr = 0; ttime.corr_pstep = 0; ttime.corr_dstep = 0;
   ttime.pchol = 0; ttime.dchol = 0; ttime.misc = 0;
%%
%%-----------------------------------------
%% display parameters, and initial info
%%-----------------------------------------
%%
   if (printlevel >= 2)
      fprintf('\n********************************************');
      fprintf('***********************\n');
      fprintf('   SDPT3: homogeneous self-dual path-following algorithms');
      fprintf('\n********************************************');
      fprintf('***********************\n');
      [hh,mm,ss] = mytime(ttime.preproc);
      if (printlevel>=3)
         fprintf(' version  predcorr  gam  expon\n');
         if (vers == 1); fprintf('   HKM '); elseif (vers == 2); fprintf('    NT '); end
         fprintf('     %1.0f      %4.3f   %1.0f\n',predcorr,gam,expon);
         fprintf('it  pstep dstep p_infeas d_infeas  gap')
         fprintf('     mean(obj)    cputime\n');
         fprintf('------------------------------------------------');
         fprintf('-------------------\n');
         fprintf('%2.0f  %4.3f %4.3f %2.1e %2.1e',0,0,0,prim_infeas,dual_infeas);
         fprintf('  %2.1e %- 7.6e  %s:%s:%s',gap,mean(obj),hh,mm,ss);
         fprintf(' %2.1e %2.1e %2.1e',kap,tau,theta);
      end
   end
%%
%%---------------------------------------------------------------
%% start main loop
%%---------------------------------------------------------------
%%
   EE = ops(blk,'identity');
   normE = ops(EE,'norm'); Zpertold = 1;
   [Xchol,indef(1)] = blkcholfun(blk,X);
   [Zchol,indef(2)] = blkcholfun(blk,Z);
   if any(indef)
      msg = 'Stop: X, Z are not both positive definite';
      if (printlevel); fprintf('\n  %s\n',msg); end
      info.termcode = -3;
      info.msg1 = msg;
      return;
   end
%%
   breakyes = 0; dy = zeros(length(b),1); dtau = 0; dtheta = 0;
   for iter = 1:maxit;

       update_iter = 0; pred_slow = 0; corr_slow = 0; step_short = 0;
       tstart = cputime;
       time = zeros(1,11);
       time(1) = cputime;
       par.tau = tau;
       par.kap = kap;
       par.theta = theta;
       par.mu   = mu;
       par.iter = iter;
       par.y    = y;
       par.dy2  = [dy; dtau; dtheta];
       par.rp   = rp;
       par.ZpATynorm = ZpATynorm;
%%
%%--------------------------------------------------
%% perturb C associated with ublk
%%--------------------------------------------------
%%
       if (perturb_C)
          Cpertold = Cpert;
          for p = 1:size(blk,1)
             pblk = blk(p,:);
             n = sum(pblk{2});
             tmp = max(1,norm(C{p},'fro'))/sqrt(n);
             if (max(relgap,infeas) < 1e-6)
                if (norm(X{p},'fro') < 1e2); const=0.2; else; const=0.3; end
                Cpert(p) = max(const*Cpert(p),1e-10*tmp);
             elseif (max(relgap,infeas) < 1e-2)
                if (norm(X{p},'fro') < 1e2); const=0.4; else; const=0.5; end
                Cpert(p) = max(const*Cpert(p),1e-8*tmp);
   	     else
	        Cpert(p) = max(0.9*Cpert(p),1e-6*tmp);
             end
             Cpert = min(Cpert,Cpertold);
             if (prim_infeas < min([0.1*dual_infeas, 1e-7*runhist.pinfeas(1)])) ...
      	        & (iter > 1 & dual_infeas > 0.8*runhist.dinfeas(iter-1) & relgap < 1e-4)
                Cpert(p) = 0.5*Cpert(p);
             end
             if (dual_infeas < min([0.1*prim_infeas, 1e-7*runhist.dinfeas(1)])) ...
   	        & (iter > 1 & prim_infeas > 0.8*runhist.pinfeas(iter-1) & relgap < 1e-4)
                Cpert(p) = 0.5*Cpert(p);
             end
	     if (max(relgap,1e-2*infeas) < 1e-6 & relgap < 0.1*infeas)
                Cpert(p) = 0.5*Cpert(p);
             end
             if (prim_infeas < min([1e-4*dual_infeas,1e-7]) & theta < 1e-6) ...
	        | (prim_infeas < 1e-4 & theta < 1e-10)
                Cpert(p) = 0.1*Cpert(p); 0;
             end
             if (dual_infeas < min([1e-4*prim_infeas,1e-7]) & theta < 1e-6) ...
	        | (dual_infeas < 1e-4 & theta < 1e-10)
                Cpert(p) = 0.1*Cpert(p); 0;
             end
             if strcmp(pblk{1},'s')
                Cnew{p} = C{p} + Cpert(p)*speye(n);
                At{p}(:,par.invpermA(p,end-1)) = -svec(pblk,Cnew{p},1);
             else
                Cnew{p} = C{p} + Cpert(p)*ones(n,1);
                At{p}(:,par.invpermA(p,end-1)) = -Cnew{p};
             end
          end
          maxCpert(iter) = max(Cpert);
          fprintf(' %2.1e',max(Cpert));
          if (iter > 10 & norm(diff(maxCpert([iter-3,iter]))) < 1e-13)
             Cpert = 0.5*Cpert;
             maxCpert(iter) = max(Cpert);
          end
          AX = AXfun(blk,At,par.permA,X);
          rp = [zeros(m,1); kap; -abar] - AX - Bmat*y2;
          Rd = ops(Atyfun(blk,At,par.permA,par.isspAy,-y2),'-',Z);
       end
%%---------------------------------------------------------------
%% predictor step.
%%---------------------------------------------------------------
%%
       if (predcorr)
          sigma = 0;
       else
          sigma = 1-0.9*min(pstep,dstep);
          if (iter == 1); sigma = 0.5; end;
       end
       sigmu = sigma*mu;

       invXchol = cell(size(blk,1),1);
       invZchol = ops(Zchol,'inv');
       if (vers == 1);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           HSDHKMpred(blk,At,par,rp,Rd,sigmu,X,Z,invZchol);
       elseif (vers == 2);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           HSDNTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);
       end
       if (solve_ok <= 0)
          msg = 'Stop: difficulty in computing predictor directions';
          if (printlevel); fprintf('\n  %s',msg); end
          runhist.cputime(iter+1) = cputime-tstart;
          termcode = -4;
          break;
       end
       time(2) = cputime;
       ttime.pred = ttime.pred + time(2)-time(1);
%%
%%-----------------------------------------
%% step-lengths for predictor step
%%-----------------------------------------
%%
      if (gam == 0)
         gamused = 0.9 + 0.09*min(pstep,dstep);
      else
         gamused = gam;
      end
      kapstep = max( (par.dkap<0)*(-kap/(par.dkap-eps)), (par.dkap>=0)*1e6 );
      taustep = max( (par.dtau<0)*(-tau/(par.dtau-eps)), (par.dtau>=0)*1e6 );
      [Xstep,invXchol] = steplength(blk,X,dX,Xchol,invXchol);
      time(3) = cputime;
      Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
      time(4) = cputime;
      pstep = min(1,gamused*min([Xstep,Zstep,kapstep,taustep]));
      dstep = pstep;
      kappred = kap + pstep*par.dkap;
      taupred = tau + pstep*par.dtau;
      trXZpred = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ) ...
                 + pstep*dstep*blktrace(blk,dX,dZ);
      mupred  = (trXZpred + kappred*taupred)/(nn+1);
      mupredhist(iter) = mupred;
      ttime.pred_pstep = ttime.pred_pstep + time(3)-time(2);
      ttime.pred_dstep = ttime.pred_dstep + time(4)-time(3);
%%
%%-----------------------------------------
%%  stopping criteria for predictor step.
%%-----------------------------------------
%%
      if (min(pstep,dstep) < steptol) & (stoplevel)
         msg = 'Stop: steps in predictor too short';
         if (printlevel)
            fprintf('\n  %s',msg);
            fprintf(': pstep = %3.2e,  dstep = %3.2e',pstep,dstep);
         end
         runhist.cputime(iter+1) = cputime-tstart;
         termcode = -2;
         breakyes = 1;
      end
      if (iter >= 2)
         idx = [max(2,iter-2) : iter];
         pred_slow = all(mupredhist(idx)./mupredhist(idx-1) > 0.4);
         idx = [max(2,iter-5) : iter];
         pred_convg_rate = mean(mupredhist(idx)./mupredhist(idx-1));
         pred_slow = pred_slow + (mupred/mu > 5*pred_convg_rate);
      end
      if (~predcorr)
         if (max(mu,infeas) < 1e-6) & (pred_slow) & (stoplevel)
            msg = 'Stop: lack of progress in predictor';
            if (printlevel)
               fprintf('\n  %s',msg);
               fprintf(': mupred/mu = %3.2f, pred_convg_rate = %3.2f.',...
               mupred/mu,pred_convg_rate);
            end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -1;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%
%%---------------------------------------------------------------
%% corrector step.
%%---------------------------------------------------------------
%%
      if (predcorr) & (~breakyes)
         step_pred = min(pstep,dstep);
         if (mu > 1e-6)
            if (step_pred < 1/sqrt(3));
               expon_used = 1;
            else
               expon_used = max(expon,3*step_pred^2);
            end
         else
            expon_used = max(1,min(expon,3*step_pred^2));
         end
         sigma = min( 1, (mupred/mu)^expon_used );
         sigmu = sigma*mu;
%%
         if (vers == 1)
            [par,dX,dy,dZ] = HSDHKMcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         elseif (vers == 2)
            [par,dX,dy,dZ] = HSDNTcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         end
         if (solve_ok <= 0)
            msg = 'Stop: difficulty in computing corrector directions';
            if (printlevel); fprintf('\n  %s',msg); end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -4;
            break;
         end
         time(5) = cputime;
         ttime.corr = ttime.corr + time(5)-time(4);
%%
%%-----------------------------------
%% step-lengths for corrector step
%%-----------------------------------
%%
         if (gam == 0)
            gamused = 0.9 + 0.09*min(pstep,dstep);
         else
            gamused = gam;
         end
         kapstep = max( (par.dkap<0)*(-kap/(par.dkap-eps)), (par.dkap>=0)*1e6 );
         taustep = max( (par.dtau<0)*(-tau/(par.dtau-eps)), (par.dtau>=0)*1e6 );
         Xstep = steplength(blk,X,dX,Xchol,invXchol);
         time(6) = cputime;
         Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
         time(7) = cputime;
         pstep = min(1,gamused*min([Xstep,Zstep,kapstep,taustep]));
         dstep = pstep;
         kapcorr = kap + pstep*par.dkap;
         taucorr = tau + pstep*par.dtau;
         trXZcorr = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ)...
                    + pstep*dstep*blktrace(blk,dX,dZ);
         mucorr = (trXZcorr+kapcorr*taucorr)/(nn+1);
         ttime.corr_pstep = ttime.corr_pstep + time(6)-time(5);
         ttime.corr_dstep = ttime.corr_dstep + time(7)-time(6);
%%
%%-----------------------------------------
%%  stopping criteria for corrector step
%%-----------------------------------------
%%
         if (iter >= 2)
            idx = [max(2,iter-2) : iter];
            corr_slow = all(runhist.gap(idx)./runhist.gap(idx-1) > 0.8);
            idx = [max(2,iter-5) : iter];
            corr_convg_rate = mean(runhist.gap(idx)./runhist.gap(idx-1));
            corr_slow = corr_slow + (mucorr/mu > max(min(1,5*corr_convg_rate),0.8));
         end
	 if (max(mu,infeas) < 1e-6) & (iter > 10) & (stoplevel) ...
            & (corr_slow & mucorr/mu > 1.0)
            msg = 'Stop: lack of progress in corrector';
   	    if (printlevel)
               fprintf('\n  %s',msg);
               fprintf(': mucorr/mu = %3.2f, corr_convg_rate = %3.2f',...
               mucorr/mu,corr_convg_rate);
            end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -1;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%
%%---------------------------------------------------------------
%% udpate iterate
%%---------------------------------------------------------------
%%
      indef = [1 1];
      if (update_iter)
         for t = 1:5
            [Xchol,indef(1)] = blkcholfun(blk,ops(X,'+',dX,pstep)); time(8) = cputime;
            if (predcorr); ttime.pchol = ttime.pchol + time(8)-time(7);
            else;          ttime.pchol = ttime.pchol + time(8)-time(4);
            end
            if (indef(1)); pstep = 0.8*pstep; else; break; end
         end
	 if (t > 1); pstep = gamused*pstep; end
	 for t = 1:5
            [Zchol,indef(2)] = blkcholfun(blk,ops(Z,'+',dZ,dstep)); time(9) = cputime;
            if (predcorr); ttime.dchol = ttime.dchol + time(9)-time(8);
            else;          ttime.dchol = ttime.dchol + time(9)-time(4);
            end
            if (indef(2)); dstep = 0.8*dstep; else; break; end
         end
	 if (t > 1); dstep = gamused*dstep; end
         AdX = AXfun(blk,At,par.permA,dX);
         AXtmp = AX(1:m) + pstep*AdX(1:m); tautmp = par.tau+pstep*par.dtau;
         prim_infeasnew = norm(b-AXtmp/tautmp)/normb;
         if any(indef)
            msg = 'Stop: X, Z not both positive definite';
            if (printlevel); fprintf('\n  %s',msg); end
   	    termcode = -3;
            breakyes = 1;
         elseif (prim_infeasnew > max([1e-8,relgap,10*infeas])) ...
	    | (prim_infeasnew > max([1e-4,20*prim_infeas]) & (infeas < 1e-2)) ...
            | (prim_infeasnew > max([1e-6,3*prim_infeas,10*dual_infeas]) ...
            & max([relgap,dual_infeas]) < 1e-5)
            if (stoplevel) & (max(pstep,dstep)<=1) & (kap < 1e-3)
               msg = 'Stop: primal infeas has deteriorated too much';
               if (printlevel); fprintf('\n  %s, %2.1e',msg,prim_infeasnew); end
               termcode = -7;
               breakyes = 1;
            end
	 end
         if (~breakyes)
            X = ops(X,'+',dX,pstep);
            y = y + dstep*dy;  Z = ops(Z,'+',dZ,dstep);
            theta = max(0, theta + pstep*par.dtheta);
            kap = kap + pstep*par.dkap;
            if (tau + pstep*par.dtau > theta)
               tau = tau + pstep*par.dtau;
            end
         end
      end
%%
%%--------------------------------------------------
%% perturb Z: do this step before checking for break
%%--------------------------------------------------
      perturb_Z = 1;
      if (~breakyes) & (perturb_Z)
         trXZtmp = blktrace(blk,X,Z);
         trXE  = blktrace(blk,X,EE);
         Zpert = max(1e-12,0.2*min(relgap,prim_infeas)).*normC./normE;
         Zpert = min(Zpert,0.1*trXZtmp./trXE);
         Zpert = min([1,Zpert,1.5*Zpertold]);
         if (infeas < 1e-2)
            Z = ops(Z,'+',EE,Zpert);
            [Zchol,indef(2)] = blkcholfun(blk,Z);
            if any(indef(2))
               msg = 'HSDsqlp stop: Z not positive definite';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -3;
               breakyes = 1;
            end
         end
         Zpertold = Zpert;
      end
%%
%%---------------------------------------------------------------
%% compute rp, Rd, infeasibities, etc.
%%---------------------------------------------------------------
%%
      y2 = [y; tau; theta];
      AX = AXfun(blk,At,par.permA,X);
      rp = [zeros(m,1); kap; -abar] - AX - Bmat*y2;
      Rd = ops(Atyfun(blk,At,par.permA,par.isspAy,-y2),'-',Z);
      trXZ = blktrace(blk,X,Z);
      mu  = (trXZ+kap*tau)/(nn+1);
      obj = [blktrace(blk,C,X), b'*y]/tau;
      gap = trXZ/tau^2;
      relgap = gap/(1+mean(abs(obj)));
      ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,[y;0;0]));
      prim_infeas = norm(b-AX(1:m)/tau)/normb;
      dual_infeas = ops(ops(C,'-',ops(ZpATy,'/',tau)),'norm')/normC;
      infeas = max(prim_infeas,dual_infeas);
      runhist.pobj(iter+1)    = obj(1);
      runhist.dobj(iter+1)    = obj(2);
      runhist.gap(iter+1)     = gap;
      runhist.relgap(iter+1)  = relgap;
      runhist.pinfeas(iter+1) = prim_infeas;
      runhist.dinfeas(iter+1) = dual_infeas;
      runhist.infeas(iter+1)  = infeas;
      runhist.cputime(iter+1) = cputime-tstart;
      runhist.step(iter+1)    = min(pstep,dstep);
      time(10) = cputime;
      ttime.misc = ttime.misc + time(10)-time(9);
      [hh,mm,ss] = mytime(sum(runhist.cputime));
      if (printlevel>=3)
         fprintf('\n%2.0f  %4.3f %4.3f',iter,pstep,dstep);
         fprintf(' %2.1e %2.1e  %2.1e',prim_infeas,dual_infeas,gap);
         fprintf(' %- 7.6e  %s:%s:%s',mean(obj),hh,mm,ss);
         fprintf(' %2.1e %2.1e %2.1e',kap,tau,theta);
      end
%%
%%--------------------------------------------------
%% check convergence.
%%--------------------------------------------------
%%
      ZpATynorm = ops(ZpATy,'norm');
      if (obj(2) > 0); homRd = (ZpATynorm/tau)/obj(2);   else; homRd = inf; end
      if (obj(1) < 0); homrp = (norm(AX(1:m))/tau)/(-obj(1)); else; homrp = inf; end
      if (ops(X,'norm')/tau > 1e15*normX0 | ops(Z,'norm')/tau > 1e15*normZ0)
         termcode = 3;
         breakyes = 1;
      end
      if (homRd < min(1e-6,1e-2*sqrt(max([infeas,relgap]*inftol)))) ...
         | (homRd < 10*tau & tau < 1e-7)
         termcode = 1;
         breakyes = 1;
      end
      if (homrp < min(1e-6,1e-2*sqrt(max([infeas,relgap]*inftol)))) ...
         | (homrp < 10*tau & tau < 1e-7)
         termcode = 2;
         breakyes = 1;
      end
      if (max(relgap,infeas) < gaptol)
         msg = sprintf('Stop: max(relative gap, infeasibilities) < %3.2e',gaptol);
         if (printlevel); fprintf('\n  %s',msg); end
         termcode = 0;
         breakyes = 1;
      end
      if (stoplevel)
         min_prim_infeas = min(runhist.pinfeas(1:iter));
         prim_infeas_bad = prim_infeas_bad + (prim_infeas > ...
            max(1e-10,5*min_prim_infeas) & (min_prim_infeas < 1e-2));
         if (mu < 1e-6)
            idx = [max(1,iter-1): iter];
         elseif (mu < 1e-3);
            idx = [max(1,iter-2): iter];
         else
            idx = [max(1,iter-3): iter];
         end
         idx2 = [max(1,iter-4): iter];
         gap_ratio2 = runhist.gap(idx2+1)./runhist.gap(idx2);
         gap_slowrate = min(0.8,max(0.6,2*mean(gap_ratio2)));
         gap_ratio = runhist.gap(idx+1)./runhist.gap(idx);
         if (infeas < 1e-4 | prim_infeas_bad) & (relgap < 1e-3) ...
            & (iter > 5) & (prim_infeas > (1-pstep/2)*runhist.pinfeas(iter))
            gap_slow = all(gap_ratio > gap_slowrate) & (relgap < 1e-3);
            min_pinfeas = min(runhist.pinfeas);
            const = 0.1;
            if (relgap < const*max(prim_infeas,dual_infeas)) ...
               & ((runhist.step(iter+1) < 0.5) | ...
                  (prim_infeas > 10*min_pinfeas & min_pinfeas < 1e-6)) ...
               & (dual_infeas > 0.8*runhist.dinfeas(iter) | (dual_infeas < 1e-2*gaptol))
               msg = 'Stop: relative gap < infeasibility';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -1;
               breakyes = 1;
            elseif (gap_slow) & (infeas > 0.8*runhist.infeas(iter)) ...
               & (theta < 1e-8)
               msg = 'Stop: progress is too slow';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -5;
               breakyes = 1;
            end
         elseif (prim_infeas_bad) & (iter >50) & all(gap_ratio > gap_slowrate)
            msg = 'Stop: progress is bad';
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -5;
            breakyes = 1;
         elseif (infeas < 1e-8) & (gap > 1.2*mean(runhist.gap(idx)))
            msg = 'Stop: progress is bad*';
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -5;
            breakyes = 1;
         end
         if (max([relgap,infeas]) < 1e-3) & (iter > 10) ...
            & (runhist.pinfeas(iter+1) > 0.9*runhist.pinfeas(max(1,iter-5))) ...
            & (runhist.dinfeas(iter+1) > 0.9*runhist.dinfeas(max(1,iter-5)))
            msg = 'Stop: progress is bad**';
            if (printlevel); fprintf('\n  %s',msg); end
            termcode = -5;
            breakyes = 1;
         end
         if (min(runhist.infeas) < 1e-4 | prim_infeas_bad) ...
            & (max(runhist.infeas) > 1e-4) & (iter > 5)
            relgap2 = abs(diff(obj))/(1+mean(abs(obj)));
            if (relgap2 < 1e-3);
               step_short = all(runhist.step([iter:iter+1]) < 0.1) ;
            elseif (relgap2 < 1)
               idx = [max(1,iter-3): iter+1];
               step_short = all(runhist.step(idx) < 0.05);
            else
               step_short = 0;
            end
            if (step_short)
               msg = 'Stop: steps too short consecutively';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -5;
               breakyes = 1;
            end
         end
	 if (infeas > 100*max(1e-12,min(runhist.infeas)) & relgap < 1e-4)
            msg = 'Stop: infeas has deteriorated too much';
            if (printlevel); fprintf('\n  %s, %3.1e',msg,infeas); end
            X = ops(X,'-',dX,pstep);
            y = y - dstep*dy;  Z = ops(Z,'-',dZ,dstep);
            kap = kap - pstep*par.dkap; tau = tau - pstep*par.dtau;
            theta = theta - pstep*par.dtheta;
            prim_infeas = runhist.pinfeas(iter); dual_infeas = runhist.dinfeas(iter);
            gap = runhist.gap(iter); relgap = runhist.relgap(iter);
            obj = [runhist.pobj(iter), runhist.dobj(iter)];
            termcode = -7;
            breakyes = 1;
         end
         if (iter > 3 & iter < 20) & (max(runhist.step(max(1,iter-3):iter+1)) < 1e-3) ...
            & (infeas > 1) & (min(homrp,homRd) > 1000*inftol)
            if (stoplevel >= 2)
               msg = 'Stop: steps too short consecutively';
               if (printlevel); fprintf('\n  %s',msg); end
               termcode = -5;
               breakyes = 1;
            end
         end
      end
      if (breakyes > 0.5); break; end
   end
%%---------------------------------------------------------------
%% end of main loop
%%---------------------------------------------------------------
%%
   if (termcode == -6)
      msg = 'Stop: maximum number of iterations reached';
      if (printlevel); fprintf('\n  %s',msg); end
   end
%%
%%---------------------------------------------------------------
%% produce infeasibility certificates if appropriate
%%---------------------------------------------------------------
%%
   X = ops(X,'/',tau); y = y/tau; Z = ops(Z,'/',tau);
   if (iter >= 1)
      param.obj         = obj;
      param.relgap      = relgap;
      param.prim_infeas = prim_infeas;
      param.dual_infeas = dual_infeas;
      param.ZpATynorm   = ZpATynorm/tau;
      param.inftol      = inftol;
      param.m0          = m0;
      param.indeprows   = indeprows;
      param.termcode    = termcode;
      param.AX          = AX(1:m)/tau;
      param.normX0      = normX0;
      param.normZ0      = normZ0;
      param.printlevel  = printlevel;
      [X,y,Z,resid,reldist,param,msg2] = ...
      HSDsqlpmisc(blk,At,C,b,X,y,Z,par.permZ,param);
      termcode = param.termcode;
   end
%%
%%---------------------------------------------------------------
%% recover unrestricted blk from linear blk
%%---------------------------------------------------------------
%%
   for p = 1:size(blk,1)
      if (ublkidx(p) == 1)
         n = blk{p,2}/2;
         X{p} = X{p}(1:n)-X{p}(n+[1:n]);
         Z{p} = Z{p}(1:n);
      end
   end
%%
%%---------------------------------------------------------------
%%  print summary
%%---------------------------------------------------------------
%%
   dimacs = [prim_infeas; 0; dual_infeas; 0];
   dimacs = [dimacs; [-diff(obj); gap]/(1+sum(abs(obj)))];
   info.dimacs   = dimacs;
   info.termcode = termcode;
   info.iter     = iter;
   info.obj      = obj;
   info.gap      = gap;
   info.relgap   = relgap;
   info.pinfeas  = prim_infeas;
   info.dinfeas  = dual_infeas;
   info.cputime  = sum(runhist.cputime);
   info.resid    = resid;
   info.reldist  = reldist;
   info.normX    = ops(X,'norm');
   info.normy    = norm(y);
   info.normZ    = ops(Z,'norm');
   info.normA    = ops(At,'norm');
   info.normb    = norm(b);
   info.normC    = ops(C,'norm');
   info.msg1     = msg;
   info.msg2     = msg2;
%%
   sqlpsummary(info,ttime,[],printlevel);
   rand('state',randstate);
%%*****************************************************************************


function [declusCat,mNumDeclus] = MonteReasenberg(numSim,Cat)
    % Monte Carlo Simulation for Reasenberg-declustering parameters
    %
    % Wrapper function to do Monte Carlo simulations of the
    % input parameters into the Reasenberg Declustering algorithm
    %%

    % if ~exist('FileOut','var')
    %     disp(['You must provide the output results filename!'])
    %     return
    % end

    resFileOut = 'DeclusRes';
    parmFileOut = 'DeclusParms';
    mNumDeclus=[];

    %%
    % set Default ranges for Reasenberg input variables and find their range
    raTaumin = [.5 2.5];
    raTaumax = [3 20];
    raP = [.9 .999];
    raXk = [0.4 0.6];
    raXmeff = [2.7 2.9];
    raRfact = [5 20];
    raErr = [2 4];
    raDerr = [4 6];

    tauminDiff = (raTaumin(2) - raTaumin(1));
    taumaxDiff = (raTaumax(2) - raTaumax(1));
    pDiff = (raP(2) - raP(1));
    xkDiff = (raXk(2) - raXk(1));
    xmeffDiff = (raXmeff(2) - raXmeff(1));
    rfactDiff = (raRfact(2) - raRfact(1));

    %% add column for independence probability
    % actually will just be number of times the event has appeared in a catalogue
    % (will need to divide by simNum to get P)

    Cat(:,10) = 0;

    % set the rand number generator state
    rng('shuffle');

    % simulate parameter values and run the delcustering code

    lErr = raErr(1);
    lDerr = raDerr(1);
    rdc= ReasenbergDeclusterClass(Cat, 'err', lErr, 'derr', lDerr, ....
        'AutoShowPlots',false,'DelayProcessing',true,'InteractiveMode',false);

    for simNum = 1:numSim

        randNum = rand(1,8);
        rdc.taumin = raTaumin(1) + tauminDiff*randNum(1);
        rdc.taumax = raTaumax(1) + taumaxDiff*randNum(2);
        rdc.P = raP(1) + pDiff*randNum(3);
        rdc.xk = raXk(1) + xkDiff*randNum(4);
        rdc.xmeff = raXmeff(1) + xmeffDiff*randNum(5);
        rdc.rfact = raRfact(1) + rfactDiff*randNum(6);

        [declusCat, is_mainshock] = rdc.ReasenbergDeclus();

        % [declusCat,is_mainshock] = ReasenbergDeclus(lTaumin,lTaumax,lXk,lXmeff,lP,lRfact,lErr,lDerr,Cat);

        Cat(is_mainshock,10) = Cat(is_mainshock,10) + 1;
        nIst=zeros(length(Cat),1);
        nIst(is_mainshock)=1;
        nIst=logical(nIst);
        mNumDeclus=[mNumDeclus,(nIst==1)];
        save(resFileOut,'Cat');

        monteParms(simNum) = {[rdc.taumin;rdc.taumax;rdc.P;rdc.xk;rdc.xmeff;rdc.rfact;rdc.err;rdc.derr]};
        save(parmFileOut,'monteParms');
        disp(num2str(simNum));

    end
end


function Population = subNSGAII(Population,N)
% The environmental selection of NSGA-II

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Cheng He

    %% Non-dominated sorting
    CC = min([N,length(Population)]);
    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,CC);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:CC-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
end


function a = r8mat_test ( trans, lda, m, n )

%*****************************************************************************80
%
%  Purpose:
%
%    R8MAT_TEST sets up a test matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2014
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, character TRANS, indicates whether matrix is to be
%    transposed.
%    'N', no transpose.
%    'T', transpose the matrix.
%
%    Input, integer LDA, the leading dimension of the matrix.
%
%    Input, integer M, N, the number of rows and columns of
%    the matrix.
%
%    Output, real A(LDA,*), the matrix.
%    if TRANS is 'N', then the matrix is stored in LDA*N entries,
%    as an M x N matrix;
%    if TRANS is 'T', then the matrix is stored in LDA*M entries,
%    as an N x M matrix.
%
  if ( trans == 'N' )

    a = zeros ( lda, n );

    for j = 1 : n
      for i = 1 : m
        a(i,j) = 10 * i + j;
      end
    end

  else

    a = zeros ( lda, m );

    for j = 1 : n
      for i = 1 : m
        a(j,i) = 10 * i + j;
      end
    end

  end

  return
end


% VL_IKMEANSPUSH  Project data on integer K-means paritions
%   I = VL_IKMEANSPUSH(X,C) projects the data X to the integer K-meanns
%   clusters of centers C returning the cluster indeces I.
%
%   See also: VL_IKMEANS(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function Tbig = divide_by_pot(Tbig, Tsmall)
% DIVIDE_BY_POT Tbig /= Tsmall
% Tbig = divide_by_pot(Tbig, Tsmall)
%
% Tsmall's domain must be a subset of Tbig's domain.

smallp = extend_domain_table(Tsmall.p, Tsmall.domain, Tsmall.sizes, Tbig.domain, Tbig.sizes);
smallp = smallp + (smallp==0);
Tbig.p = Tbig.p ./ smallp;

smallu = extend_domain_table(Tsmall.u, Tsmall.domain, Tsmall.sizes, Tbig.domain, Tbig.sizes);
Tbig.u = Tbig.u - smallu;


function T = permute(T,order)
%PERMUTE Permute tensor dimensions.
%
%   B = PERMUTE(A,ORDER) rearranges the dimensions of A so that they
%   are in the order specified by the vector ORDER. The result has the
%   same values of A, but the order of the subscripts needed to access
%   any particular element are rearranged as specified by ORDER.
%
%   See also TENSOR, TENSOR/SIZE, TENSOR/NDIMS, PERMUTE.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

if ndims(T) ~= numel(order)
  error('Invalid permutation order');
end

% Check for special case of permuting an order-1 object (which has
% no effect but confuses MATLAB's permute command which doesn't
% think that there is such a thing as a 1D-array).
if isequal(order,1)
    return;
end

% Check for special case of empty object (which has
% no effect but confuses MATLAB's permute command which doesn't
% think that there is such a thing as an empty array).
if isempty(order)
    return;
end

% Note that permute does error checking on order, so we don't worry
% about it.
T.data = permute(T.data,order);
T.size = T.size(order);

return;


function cumsum = cfg_example_cumsum1
% Example script that creates an cfg_exbranch to sum two numbers. The
% inputs are entered as vector, the output is a vector containing the
% cumulative sums. This function differs from cfg_example_sum (except from
% names) only in the specification of the output subscript.
%
% This code is part of a batch job configuration system for MATLAB. See
%      help matlabbatch
% for a general overview.
%_______________________________________________________________________
% Copyright (C) 2007 Freiburg Brain Imaging

% Volkmar Glauche
% $Id: cfg_example_cumsum1.m 1716 2008-05-23 08:18:45Z volkmar $

rev = '$Rev: 1716 $'; %#ok

%% Input Item
input1         = cfg_entry; % This is the generic data entry item
input1.name    = 'Input a'; % The displayed name
input1.tag     = 'a';       % The name appearing in the harvested job structure. This name must be unique among all items in the val field of the superior node
input1.strtype = 'e';       % No restriction on what type of data is entered. This could be used to restrict input to real numbers, integers ...
input1.num     = [1 Inf];   % Number of inputs required (2D-array with exactly one row and two column)
input1.help    = {'This is the input vector.','The elements will be added together.'}; % help text displayed

%% Executable Branch
cumsum      = cfg_exbranch;       % This is the branch that has information about how to run this module
cumsum.name = 'cumsum1';             % The display name
cumsum.tag  = 'cfg_example_cumsum1'; % The name appearing in the harvested job structure. This name must be unique among all items in the val field of the superior node
cumsum.val  = {input1};    % The items that belong to this branch. All items must be filled before this branch can run or produce virtual outputs
cumsum.prog = @cfg_example_run_cumsum1;  % A function handle that will be called with the harvested job to run the computation
cumsum.vout = @cfg_example_vout_cumsum1; % A function handle that will be called with the harvested job to determine virtual outputs
cumsum.help = {'Compute the cumulative sum of two numbers.'};

%% Local Functions
% The cfg_example_vout_cumsum1 function can go here, it is not useful outside
% the batch environment.
function vout = cfg_example_vout_cumsum1(job)
% Determine what outputs will be present if this job is run. In this case,
% the structure of the inputs is fixed, and the output is always a single
% number. Note that input items may not be numbers, they can also be
% dependencies.

vout = cfg_dep;                        % The dependency object
vout.sname      = 'cumsum(a)';       % Displayed dependency name
vout.src_output = substruct('.','cs'); % The output subscript reference. The length of the output vector depends on the unknown length of the input vector. Therefore, the vector output needs to be assigned to a struct field.


function phiFaceAverage = upwindMean(phi, u)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the upwind average on
% the cell faces, for a uniform mesh based on the direction of the velocity
% vector u
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract data from the mesh structure

d = phi.domain.dimension;
if (d ==1) || (d==1.5) || (d==1.8)
	phiFaceAverage = upwindMean1D(phi, u);
elseif (d == 2) || (d == 2.5) || (d==2.8)
	phiFaceAverage = upwindMean2D(phi, u);
elseif (d == 3) || (d==3.2)
    phiFaceAverage = upwindMean3D(phi, u);
end


function test_suite = test_normalizeVector3d
%TESTNORMALIZEVECTOR3D  One-line description here, please.
%
%   output = testNormalizeVector3d(input)
%
%   Example
%   testNormalizeVector3d
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-11-16,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

test_suite = functiontests(localfunctions);

function testOrthoVectors(testCase) %#ok<*DEFNU>

vec1    = [3 0 0];
exp1    = [1 0 0];
norm1   = normalizeVector3d(vec1);
testCase.assertEqual(exp1, norm1);

vec2    = [0 3 0];
exp2    = [0 1 0];
norm2   = normalizeVector3d(vec2);
testCase.assertEqual(exp2, norm2);

vec3    = [0 0 3];
exp3    = [0 0 1];
norm3   = normalizeVector3d(vec3);
testCase.assertEqual(exp3, norm3);

function testDiagoVector(testCase)

vec = [6 8 0];
norm = normalizeVector3d(vec);
exp = [3/5 4/5 0];
testCase.assertEqual(exp, norm);

vec = [0 6 8];
norm = normalizeVector3d(vec);
exp = [0 3/5 4/5];
testCase.assertEqual(exp, norm);

function testArray(testCase)

vecs = [2 0 0;0 3 0;0 0 4];
exp = [1 0 0;0 1 0;0 0 1];
norm = normalizeVector3d(vecs);
testCase.assertEqual(exp, norm);

vecs = [2 0 0;0 3 0;0 0 4;3 0 4];
exp = [1 0 0;0 1 0;0 0 1;3/5 0 4/5];
norm = normalizeVector3d(vecs);
testCase.assertEqual(exp, norm);


function C = mtimes(A,B)
%MTIMES Implement A*B (scalar multiply) for ktensor.
%
%   C = mtimes(A,B) computes A * B where A is a Kruskal tensor and B is
%   a scalar (or vice versa). The result C is the same size as A.
%
%   See also KTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% Note: We can do scalar times a tensor, but anything more complex is
% an error.

if isa(B,'numeric') && isequal(size(B),[1 1])
    C = ktensor(B * A.lambda, A.u);
elseif isa(A,'numeric') && isequal(size(A),[1 1])
    C = ktensor(A * B.lambda, B.u);
else
    error('Use mtimes(full(A),full(B)).');
end


%-------------------------------------------------------------------------------------------------------------------%
%
% 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:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	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 (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the JELLYFISH-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Jellyfish_Geometry()

% FLUID GRID PARAMETERS %
L = 8;                              % Length of computational domain (m)
N = 256;                            % # of Cartesian grid meshwidths
dx = L/N;                           % Cartesian mesh width (m)
ds = L/(2.0*N);                     % Ideal Lagrangian spacing (m)

% Construct Geometry
[xLag,yLag,ds] = give_Me_Immsersed_Boundary_Geometry(N,L);
plot(xLag,yLag,'*'); hold on;

% Translate Geometry
xLag = xLag + L/8;
yLag = yLag + L/4;
plot(xLag,yLag,'r*'); hold on;

% NAMING CONVENTION FOR SIMULATION
struct_name = 'jelly';      % structure name

%
% PRINT INPUT FILES (.vertex, .spring, .beam, etc) %
%

% print vertices
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% print springs
k_Spring = 500*1.2750000000000000e+07; %500   % spring constant (Newton) dt=2.5e-6
print_Lagrangian_Springs(xLag,yLag,k_Spring,ds,struct_name);

% print beams
k_Beam = 1.0363359375000002e+13;  %100 % beam stiffness constant (Newton m^2) %5.1816796875000010e+12
print_Lagrangian_nonInv_Beams(xLag,yLag,k_Beam,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called rubberband.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N-1 );

    %spring_force = kappa_spring*ds/(ds^2);

    % SPRINGS BETWEEN VERTICES ON RHS
    for s = 1:ceil(N/2)
            if s <= floor(N/2)
                x1 = xLag(s); x2 = xLag(s+1);
                y1 = yLag(s); y2 = yLag(s+1);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            elseif s == ceil(N/2)
                x1 = xLag(1); x2 = xLag( ceil(N/2)+1 );
                y1 = yLag(1); y2 = yLag( ceil(N/2)+1 );
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', 1, ceil(N/2)+1, k_Spring, ds);
            end
    end

    % SPRINGS BETWEEN VERTICES ON LHS
    for s=1:floor(N/2)-1
                s1 = ceil(N/2)+s;
                s2 = ceil(N/2)+s+1;
                x1 = xLag(s1); x2 = xLag(s2);
                y1 = yLag(s1); y2 = yLag(s2);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s1, s2, k_Spring, ds);
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (NON-INVARIANT) points to a file called rubberband.nonInv_beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_nonInv_Beams(xLag,yLag,k_Beam,struct_name)

    % k_Beam: beam stiffness
    % Cx/Cy:  beam curvatures in x/y respestively

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.nonInv_beam'], 'w');

    fprintf(beam_fid, '%d\n', N-2 );

    % beam_force = kappa_beam*ds/(ds^4)

    %BEAMS BETWEEN VERTICES ON RHS
    for s = 1:ceil(N/2)+1
            if s <= floor(N/2)-1
                Cx = xLag(s) - 2*xLag(s+1) + xLag(s+2);
                Cy = yLag(s) - 2*yLag(s+1) + yLag(s+2);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', s,s+1,s+2, k_Beam, Cx, Cy);
            elseif s == ceil(N/2)
                Cx = xLag(ceil(N/2)+1) - 2*xLag(1) + xLag(2);
                Cy = yLag(ceil(N/2)+1) - 2*yLag(1) + yLag(2);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', ceil(N/2)+1,1,2, k_Beam, Cx,Cy);
            elseif s == ceil(N/2)+1
                Cx = xLag(ceil(N/2)+2) - 2*xLag( ceil(N/2)+1 ) + xLag(1);
                Cy = yLag(ceil(N/2)+2) - 2*yLag( ceil(N/2)+1 ) + yLag(1);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', ceil(N/2)+2,ceil(N/2)+1,1, k_Beam, Cx,Cy);
            end
    end

    % BEAMS BETWEEN VERTICES ON LHS
    for s=1:floor(N/2)-2
                s1 = ceil(N/2)+s;
                s2 = ceil(N/2)+s+1;
                s3 = ceil(N/2)+s+2;
                Cx = xLag(s3) - 2*xLag(s2) + xLag(s1);
                Cy = yLag(s3) - 2*yLag(s2) + yLag(s1);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', s3, s2, s1, k_Beam, Cx, Cy);
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag,ds] = give_Me_Immsersed_Boundary_Geometry(N,L)

% JELLYFISH GEOMETRY PARAMETERS %

bell_length = 2;                    % bell length (m)
npts_bell = ceil(2.0*(bell_length/L)*N); % number of points along the length of the entire bell
npts_circ = 1;                    %number of points along the circumference (=1 for 2D)
npts = npts_bell*npts_circ;	      % total number points
ds = bell_length/(npts_bell-1);   % mesh spacing along the length of bell (m)
Zs = 0;                          %distance to top of bell (m)
xb = zeros(npts);                 %holds resting positions to calculate curvatures
zb=zeros(npts);                   %holds resting positions to calculate curvatures

% Values to describe bell shape given in Alben, Peng and Miller %
betao = 0.5;
betam = 0.3;
to = 0.5;
t=0;
gamma = 1;

% Starting values
z = Zs;
r = 0;
x = 0;

% center line of jelly
xLag(1) = x;
yLag(1) = z;
zl=z;
rl=r;

%right side of bell
for s = 1:(ceil(npts_bell/2)-1)
    beta = betao+(betam-betao)*(t/to)^gamma;
    theta = -1.55*(1-exp(-(s)*ds/beta));
    z = zl + ds*sin(theta);
    r = rl + ds*cos(theta);
    x = r;
    zl=z;
    rl=r;
    xLag(s+1)=x;
    yLag(s+1)=z;
   %fprintf(vertex_fid, '%1.16e %1.16e\n', x, z);
end

% reinitialize values for centerline
z = Zs;
r = 0;
zl=z;
rl=r;

%left side of bell
for s = (ceil(npts_bell/2)):npts_bell-2
    s2=s-(ceil(npts_bell/2)-1);
    beta = betao+(betam-betao)*(t/to)^gamma;
    theta = -1.55*(1-exp(-(s2)*ds/beta));
    z = zl + ds*sin(theta);
    r = rl + ds*cos(theta);
    x = -1*r;
    zl=z;
    rl=r;
    xLag(s+1)=x;
    yLag(s+1)=z;
   %fprintf(vertex_fid, '%1.16e %1.16e\n', x, z);
end

%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% % Write out the beam information
%  beam_fid = fopen([mesh_name num2str(N) '.beam'], 'w');
%
%  fprintf(beam_fid, '%d\n', npts-3);
%
%  %right side of bell
%  for q = 0:npts_circ-1
%  for s = 0:(ceil(npts_bell/2)-3)
%     C1 = xb(s+1)+xb(s+3)-2*xb(s+2);
%     C2 = zb(s+1)+zb(s+3)-2*zb(s+2);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+s+1, q*npts_circ+s+2, kappa_beam*ds/(ds^4), C1, C2);
%  end
%
%  %top of bell
%     s=ceil(npts_bell/2);
%     C1 = xb(s+1)+xb(2)-2*xb(1);
%     C2 = zb(s+1)+zb(2)-2*zb(1);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+0, q*npts_circ+1, kappa_beam*ds/(ds^4), C1, C2);
%     s=ceil(npts_bell/2)+1;
%     C1 = xb(s+1)+xb(1)-2*xb(s);
%     C2 = zb(s+1)+zb(1)-2*zb(s);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+s-1, q*npts_circ+0, kappa_beam*ds/(ds^4), C1, C2);
%
%  %left side of bell
%  for s = ceil((npts_bell/2)+2):npts_bell-2
%     C1 = xb(s+1)+xb(s-1)-2*xb(s);
%     C2 = zb(s+1)+zb(s-1)-2*zb(s);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+s-1, q*npts_circ+s-2, kappa_beam*ds/(ds^4), C1, C2);
%  end
%  end
% %
% %
%  fclose(beam_fid);
%


function msm_to_mm_test01 ( )

%*****************************************************************************80
%
%% MSM_TO_MM_TEST01 tests MSM_TO_MM_ARRAY_COMPLEX_GENERAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MSM_TO_MM_TEST01\n' );
  fprintf ( 1, '  Convert an MSM to MM array complex general format.\n' );

  output_filename = 'msm_to_mm_test01.mm';
  a = c8mat_indicator ( 5, 3 );
%
%  Have MSM_TO_MM write the matrix to a file.
%
  msm_to_mm ( output_filename, a, 'array', 'complex', 'general' );

  return
end


function embedding = optimize_layout(head_embedding, tail_embedding, ...
    head, tail, n_epochs, n_vertices, epochs_per_sample, a, b, ...
    gamma, initial_alpha, negative_sample_rate, verbose)
%OPTIMIZE_LAYOUT Improve an embedding using stochastic gradient descent to
% minimize the fuzzy set cross entropy between the 1-skeletons of the high
% dimensional and low dimensional fuzzy simplicial sets. In practice this
% is done by sampling edges based on their membership strength (with the
% (1-p) terms coming from negative sampling similar to word2vec). This
% function is only called if the UMAP method is 'MATLAB'.
%
% embedding = OPTIMIZE_LAYOUT(head_embedding, tail_embedding, head, tail,
% n_epochs, n_vertices, epochs_per_sample, a, b)
%
% Parameters
% ----------
% head_embedding: array of size (n_samples, n_components)
%     The initial embedding to be improved by SGD.
%
% tail_embedding: array of size (source_samples, n_components)
%     The reference embedding of embedded points. If not embedding new
%     previously unseen points with respect to an existing embedding this
%     is simply the head_embedding (again); otherwise it provides the
%     existing embedding to embed with respect to.
%
% head: array of size (n_1_simplices, 1)
%     The indices of the heads of 1-simplices with non-zero membership.
%
% tail: array of size (n_1_simplices, 1)
%     The indices of the tails of 1-simplices with non-zero membership.
%
% n_epochs: double
%     The number of training epochs to use in optimization.
%
% n_vertices: double
%     The number of vertices (0-simplices) in the dataset.
%
% epochs_per_samples: array of size (n_1_simplices, 1)
%     A double value of the number of epochs per 1-simplex. 1-simplices with
%     weaker membership strength will have more epochs between being sampled.
%
% a: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% b: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% gamma: double (optional, default 1)
%     Weight to apply to negative samples.
%
% initial_alpha: double (optional, default 1)
%     Initial learning rate for the SGD.
%
% negative_sample_rate: double (optional, default 5)
%     Number of negative samples to use per positive sample.
%
% verbose: boolean (optional, default false)
%     Whether to report information on the current progress of the algorithm.
%
% Returns
% -------
% embedding: array of size (n_samples, n_components)
%     The optimized embedding.
%
% See also: OPTIMIZE_LAYOUT2
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause

    if nargin < 13
        verbose = false;
        if nargin < 12
            negative_sample_rate = 5;
            if nargin < 11
                initial_alpha = 1;
                if nargin < 10
                    gamma = 1;
                end
            end
        end
    end

    dim = size(head_embedding, 2);
    n_1_simplices = size(epochs_per_sample, 1);
    same_embedding = isequal(head_embedding, tail_embedding);
    alpha = initial_alpha;
    ONES=ones(1, dim);
    BG2S=2*gamma* b*ONES;
    FOURS=4*ONES;
    ABNEG2=-2.0*a*b;
    BNEG1=b-1;

    epochs_per_negative_sample = epochs_per_sample / single(negative_sample_rate);
    epoch_of_next_negative_sample = epochs_per_negative_sample;
    epoch_of_next_sample = epochs_per_sample;
    if verbose
        fprintf('\t0/%d epochs done\n', int32(n_epochs));
    end
    for n = 1:n_epochs
        for i = 1:n_1_simplices
            if epoch_of_next_sample(i) <= n
                j = head(i);
                k = tail(i);

                current = head_embedding(j,:);
                other = tail_embedding(k,:);

                dist_squared = norm(current - other).^2;

                grad_coeff = (dist_squared > 0)*(ABNEG2*(dist_squared).^BNEG1)./ (a*dist_squared.^b + 1);
                grad_coeff(isnan(grad_coeff)) = 0;

                grad= max(-4, min(4, grad_coeff .* (current - other)));
                current = current + grad * alpha;

                epoch_of_next_sample(i) = epoch_of_next_sample(i) + epochs_per_sample(i);

                n_neg_samples = floor((single(n) - epoch_of_next_negative_sample(i)) / epochs_per_negative_sample(i));

                if same_embedding
                    other = other - grad * alpha;
                    head_embedding(k,:) = other;
                    tail_embedding(k,:) = other;
                end

                for p = 1:n_neg_samples
                    k = int32(randi(n_vertices));

                    other = tail_embedding(k,:);

                    dist_squared = norm(current - other).^2;

                    if same_embedding && j == k
                        continue
                    end

                    grad_coeff = (dist_squared > 0)*BG2S./(0.001 + dist_squared)./(a*dist_squared.^b + 1);
                    grad_coeff(isnan(grad_coeff)) = 0;
                    grad=(grad_coeff > 0).*max(-4, min(4, grad_coeff .* (current - other))) + ~(grad_coeff > 0).*FOURS;
                    current = current + grad * alpha;
                end

                head_embedding(j,:) = current;
                if same_embedding
                    tail_embedding(j,:) = current;
                end

                epoch_of_next_negative_sample(i) = epoch_of_next_negative_sample(i)+(n_neg_samples * epochs_per_negative_sample(i));
            end

        end
        alpha = initial_alpha * (1 - single(n)/single(n_epochs));

        progress_checkpoint = min(floor(n_epochs / 10), 50);

        if verbose && mod(n, progress_checkpoint) == 0
            fprintf('\t%d/%d epochs done\n', int32(n), int32(n_epochs));
        end
    end

    embedding = head_embedding;
 end


function  [X,Y,vals,labI]=mp_utm(optn,varargin)
% MP_UTM   Universal Transverse Mercator projection
%           This function should not be used directly; instead it is
%           is accessed by various high-level functions named M_*.

% mp_utm.m, Peter Lemmond (peter@whoi.edu)

% created mp_utm.m 13Aug98 from mp_tmerc.m, v1.2d distribution, by:
%
% Rich Pawlowicz (rich@ocgy.ubc.ca) 2/Apr/1997
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Mathematical formulas for the projections and their inverses are taken from
%
%      Snyder, John P., Map Projections used by the US Geological Survey,
%      Geol. Surv. Bull. 1532, 2nd Edition, USGPO, Washington D.C., 1983.
%

% 10/Dec/98 - PL added various ellipsoids.
% 17/May/12 - clarified hemisphere setting

global MAP_PROJECTION MAP_VAR_LIST

% define a structure of various ellipsoids. each has a name, and
% a vector consisting of equatorial radius and flattening. the first
% two are somewhat special cases.

MAP_ELLIP = struct ( 'normal', [1.0, 0], ...
    'sphere', [6370997.0, 0], ...
    'grs80' , [6378137.0, 1/298.257], ...
    'grs67' , [6378160.0, 1/247.247], ...
    'wgs84' , [6378137.0, 1/298.257], ...
    'wgs72' , [6378135.0, 1/298.260], ...
    'wgs66' , [6378145.0, 1/298.250], ...
    'wgs60' , [6378165.0, 1/298.300], ...
    'clrk66', [6378206.4, 1/294.980], ...
    'clrk80', [6378249.1, 1/293.466], ...
    'intl24', [6378388.0, 1/297.000], ...
    'intl67', [6378157.5, 1/298.250]);

name={'UTM'};

switch optn

  case 'name'

    X=name;

  case {'usage','set'}

    m_names=fieldnames(MAP_ELLIP);

    X=char({['     ''' varargin{1} ''''],...
	'     <,''lon<gitude>'',[min max]>',...
	'     <,''lat<itude>'',[min max]>',...
	'     <,''zon<e>'',value>',...
	'     <,''hem<isphere>'',[1|0] (0 for N)>',...
	'     <,''ell<ipsoid>'', one of',...
    reshape(sprintf('         %6s',m_names{:}),15,length(m_names))',...
        '                               >',...
	'     <,''rec<tbox>'', ( ''on'' | ''off'' )>'});

  case 'get'

    X=char([' Projection: ' MAP_PROJECTION.name '  (function: ' ...
	  MAP_PROJECTION.routine ')'],...
	[' longitudes: ' num2str(MAP_VAR_LIST.ulongs) ],...
	[' latitudes: ' num2str(MAP_VAR_LIST.ulats) ],...
	[' zone: ' num2str(MAP_VAR_LIST.zone) ],...
	[' hemisphere: ' num2str(MAP_VAR_LIST.hemisphere) ],...
	[' ellipsoid: ' MAP_VAR_LIST.ellipsoid ], ...
	[' Rectangular border: ' MAP_VAR_LIST.rectbox ]);

  case 'initialize'

    MAP_VAR_LIST=[];
    MAP_PROJECTION.name=varargin{1};
    MAP_VAR_LIST.ulongs = [-72 -68];
    MAP_VAR_LIST.ulats = [40 44];
    MAP_VAR_LIST.zone = 0;		% will be computed if not there
    MAP_VAR_LIST.hemisphere = -1;
    MAP_VAR_LIST.ellipsoid = 'wgs84';
    MAP_VAR_LIST.rectbox='off';
    k=2;

    while k<length(varargin)
      switch varargin{k}(1:3)
	case 'lon'
	  MAP_VAR_LIST.ulongs=varargin{k+1}(:)';
	case 'lat'
	  MAP_VAR_LIST.ulats=varargin{k+1}(:)';
	case 'zon'
	  MAP_VAR_LIST.zone=varargin{k+1};
	case 'hem'
	  MAP_VAR_LIST.hemisphere=varargin{k+1};
        case 'ell'
	  MAP_VAR_LIST.ellipsoid=varargin{k+1};
        case 'rec'
          MAP_VAR_LIST.rectbox=varargin{k+1};
	otherwise
	  disp(['Unknown option: ' varargin{k}]);
      end
      k=k+2;
    end

    % set the zone and hemisphere if not specified

    if (~MAP_VAR_LIST.zone)
      MAP_VAR_LIST.zone = 1 + fix((mod(mean(MAP_VAR_LIST.ulongs)+180,360))/6);
    end

    if MAP_VAR_LIST.hemisphere==-1
      MAP_VAR_LIST.hemisphere=mean(MAP_VAR_LIST.ulats)<0;
    end

    % check for a valid ellipsoid. if not, use the normalized sphere

    if ~isfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid)
       MAP_VAR_LIST.ellipsoid = 'normal';
    end

    % Get X/Y and (if rectboxs are desired) recompute lat/long limits.

    mu_util('xylimits');
    if strcmp(MAP_VAR_LIST.rectbox,'on'), mu_util('lllimits'); end

  case 'll2xy'

    long=varargin{1};
    lat=varargin{2};
    vals=zeros(size(long));

    % Clip out-of-range values (lat/long boxes)

    if  strcmp(MAP_VAR_LIST.rectbox,'off') && ~strcmp(varargin{4},'off')
        vals=vals | long<=MAP_VAR_LIST.longs(1)+eps*10 | long>=MAP_VAR_LIST.longs(2)-eps*10 | ...
	              lat<=MAP_VAR_LIST.lats(1)+eps*10 |   lat>=MAP_VAR_LIST.lats(2)-eps*10;
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(1),long<MAP_VAR_LIST.longs(1),lat);
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(2),long>MAP_VAR_LIST.longs(2),lat);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(1),lat<MAP_VAR_LIST.lats(1),long);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(2),lat>MAP_VAR_LIST.lats(2),long);
    end

    % do the forward transformation

    [X,Y] = mu_ll2utm(lat,long,MAP_VAR_LIST.zone,MAP_VAR_LIST.hemisphere, ...
	getfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid));

    % Clip out-of-range values (rectboxes)

    if strcmp(MAP_VAR_LIST.rectbox,'on')  && ~strcmp(varargin{4},'off')
        vals= vals | X<=MAP_VAR_LIST.xlims(1)+eps*10 | X>=MAP_VAR_LIST.xlims(2)-eps*10 | ...
                     Y<=MAP_VAR_LIST.ylims(1)+eps*10 | Y>=MAP_VAR_LIST.ylims(2)-eps*10;
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(1),X<MAP_VAR_LIST.xlims(1),Y);
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(2),X>MAP_VAR_LIST.xlims(2),Y);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(1),Y<MAP_VAR_LIST.ylims(1),X);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(2),Y>MAP_VAR_LIST.ylims(2),X);
    end

  case 'xy2ll'

    [Y,X] = mu_utm2ll(varargin{1}, varargin{2}, MAP_VAR_LIST.zone, ...
	MAP_VAR_LIST.hemisphere, getfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid));

  case 'xgrid'

    [X,Y,vals,labI]=mu_util('xgrid',MAP_VAR_LIST.longs,MAP_VAR_LIST.lats,varargin{1},31,varargin{2:3});

  case 'ygrid'

    [X,Y,vals,labI]=mu_util('ygrid',MAP_VAR_LIST.lats,MAP_VAR_LIST.longs,varargin{1},31,varargin{2:3});

  case 'box'

    [X,Y]=mu_util('box',31);

end

%-------------------------------------------------------------------

function [x,y] = mu_ll2utm (lat,lon, zone, hemisphere,ellipsoid)
%mu_ll2utm		Convert geodetic lat,lon to X/Y UTM coordinates
%
%	[x,y] = mu_ll2utm (lat, lon, zone, hemisphere,ellipsoid)
%
%	input is latitude and longitude vectors, zone number,
%		hemisphere(N=0,S=1), ellipsoid info [eq-rad, flat]
%	output is X/Y vectors
%
%	see also	mu_utm2ll, utmzone

% some general constants

DEG2RADS    = 0.01745329252;
RADIUS      = ellipsoid(1);
FLAT        = ellipsoid(2);
K_NOT       = 0.9996;
FALSE_EAST  = 500000;
FALSE_NORTH = 10000000;

% check for valid numbers

if (max(abs(lat)) > 90)
  error('latitude values exceed 89 degree');
  return;
end

if ((zone < 1) || (zone > 60))
  error ('utm zones only valid from 1 to 60');
  return;
end

% compute some geodetic parameters

lambda_not  = ((-180 + zone*6) - 3) * DEG2RADS;

e2  = 2*FLAT - FLAT*FLAT;
e4  = e2 * e2;
e6  = e4 * e2;
ep2 = e2/(1-e2);

% some other constants, vectors

lat = lat * DEG2RADS;
lon = lon * DEG2RADS;

sinL = sin(lat);
tanL = tan(lat);
cosL = cos(lat);

T = tanL.*tanL;
C = ep2 * (cosL.*cosL);
A = (lon - lambda_not).*cosL;
A2 = A.*A;
A4 = A2.*A2;
S = sinL.*sinL;

% solve for N

N = RADIUS ./ (sqrt (1-e2*S));

% solve for M

M0 = 1 - e2*0.25 - e4*0.046875 - e6*0.01953125;
M1 = e2*0.375 + e4*0.09375 + e6*0.043945313;
M2 = e4*0.05859375 + e6*0.043945313;
M3 = e6*0.011393229;
M = RADIUS.*(M0.*lat - M1.*sin(2*lat) + M2.*sin(4*lat) - M3.*sin(6*lat));

% solve for x

X0 = A4.*A/120;
X1 = 5 - 18*T + T.*T + 72*C - 58*ep2;
X2 = A2.*A/6;
X3 = 1 - T + C;
x = N.*(A + X3.*X2 + X1.* X0);

% solve for y

Y0 = 61 - 58*T + T.*T + 600*C - 330*ep2;
Y1 = 5 - T + 9*C + 4*C.*C;

y = M + N.*tanL.*(A2/2 + Y1.*A4/24 + Y0.*A4.*A2/720);

% finally, do the scaling and false thing. if using a unit-normal radius,
% we don't bother.

x = x*K_NOT + (RADIUS>1) * FALSE_EAST;

y = y*K_NOT;
if (hemisphere)
  y = y + (RADIUS>1) * FALSE_NORTH;
end

return

%-------------------------------------------------------------------

function [lat,lon] = mu_utm2ll (x,y, zone, hemisphere,ellipsoid)
%mu_utm2ll		Convert X/Y UTM coordinates to geodetic lat,lon
%
%	[lat,lon] = mu_utm2ll (x,y, zone, hemisphere,ellipsoid)
%
%	input is X/Y vectors, zone number, hemisphere(N=0,S=1),
%		ellipsoid info [eq-rad, flat]
%	output is lat/lon vectors
%
%	see also	mu_ll2utm, utmzone

% some general constants

DEG2RADS    = 0.01745329252;
RADIUS      = ellipsoid(1);
FLAT        = ellipsoid(2);
K_NOT       = 0.9996;
FALSE_EAST  = 500000;
FALSE_NORTH = 10000000;

if ((zone < 1) || (zone > 60))
  error ('utm zones only valid from 1 to 60');
  return;
end

% compute some geodetic parameters

e2  = 2*FLAT - FLAT*FLAT;
e4  = e2 * e2;
e6  = e4 * e2;
eps = e2 / (1-e2);
em1 = sqrt(1-e2);
e1  = (1-em1)/(1+em1);
e12 = e1*e1;

lambda_not  = ((-180 + zone*6) - 3) * DEG2RADS;

% remove the false things

x = x - (RADIUS>1)*FALSE_EAST;
if (hemisphere)
  y = y - (RADIUS>1)*FALSE_NORTH;
end

% compute the footpoint latitude

M = y/K_NOT;
mu = M/(RADIUS * (1 - 0.25*e2 - 0.046875*e4 - 0.01953125*e6));
foot = mu + (1.5*e1 - 0.84375*e12*e1)*sin(2*mu) ...
    + (1.3125*e12 - 1.71875*e12*e12)*sin(4*mu) ...
    + (1.57291666667*e12*e1)*sin(6*mu) ...
    + (2.142578125*e12*e12)*sin(8*mu);

% some other terms

sinF = sin(foot);
cosF = cos(foot);
tanF = tan(foot);

N = RADIUS ./ sqrt(1-e2*(sinF.*sinF));
T = tanF.*tanF;
T2 = T.*T;
C = eps * cosF.*cosF;
C2 = C.*C;
denom = sqrt(1-e2*(sinF.*sinF));
R = RADIUS * em1*em1 ./ (denom.*denom.*denom);
D = x./(N*K_NOT);
D2 = D.*D;
D4 = D2.*D2;

% can now compute the lat and lon

lat = foot - (N.*tanF./R) .* (0.5*D2 - (5 + 3*T + 10*C - 4*C2 - 9*eps).*D4/24 ...
    + (61 + 90*T + 298*C + 45*T2 - 252*eps - 3*C2) .* D4 .* D2/720);

lon = lambda_not + (D - (1 + 2*T +C).*D2.*D/6 + ...
    (5 - 2*C + 28*T - 3*C2 + 8*eps + 24*T2).*D4.*D./120)./cosF;

% convert back to degrees;

lat=lat/DEG2RADS;
lon=lon/DEG2RADS;

return


function [x,u,y] = spm_Markov_blanket(J,z,m,mj)
% FORMAT [x,u,y] = spm_Markov_blanket(J,z,m,mj)
% Markovian partition
% J  - Jacobian
% z  - {1 x N}  partition of states (indices)
% m  - number of internal states [default: 3]
%
% x  - {3 x n} particular partition of state indices
%     x{1,j} - active states of j-th partition
%     x{2,j} - sensory states of j-th partition
%     x{3,j} - internal states of j-th partition
%
% u  - location of partitions in scaling or embedding space
%
% y  - {3 x n} particular partition of partition indices
%     y{1,j} - active states of j-th partition
%     y{2,j} - sensory states of j-th partition
%     y{3,j} - internal states of j-th partition
%
% Partition or Grouping (coarse-scaling) operator
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_Markov_blanket.m 7655 2019-08-25 20:10:20Z karl $

% preliminaries
%--------------------------------------------------------------------------
GRAPHICS  = 1;                      % Graphics switch
nz        = length(z);              % number of partitions
if nargin < 3
    m = 3;                          % maximum size of internal states
end
if nargin < 4
   mj = ones(nz,1);                 % eligible internal states
end
if isempty(mj)
   mj = ones(nz,1);                 % eligible internal states
end

% Adjacency matrix (over z)
%--------------------------------------------------------------------------
for i = 1:nz
    for j = 1:nz
        Lij    = J(z{i},z{j});
        if any(any(Lij))
            L(i,j) = abs(norm(full(Lij)) > 1/128);
        else
            L(i,j) = 0;
        end
    end
end
L     = double(L);

% get Markov blanket
%--------------------------------------------------------------------------
B     = L + L' + L'*L;
B     = B - diag(diag(B));

% scaling space (defined by graph Laplacian)
%--------------------------------------------------------------------------
G     = L + L';
G     = G - diag(diag(G));
G     = G - diag(sum(G));
G     = expm(G);

% get principal dimensions of scaling space (u)
%--------------------------------------------------------------------------
if GRAPHICS
    [u,v] = eig(G,'nobalance');
    v     = abs(diag(v));
    for i = 1:nz
        [p,h] = hist(real(u(:,i)),16);
        dh    = h(2) - h(1) + exp(-16);
        p     = p(:)/sum(p)/dh;
        v(i)  = log(v(i)) - p'*log(p + exp(-16))*dh;
    end
    [v,j] = sort(real(v),'descend');
    u     = real(u(:,j));
end

% recursive (particular) partition into internal, sensory and active states
%--------------------------------------------------------------------------
nn    = zeros(nz,1);
for i = 1:nz

    % internal states (defined by graph Laplacian)
    %----------------------------------------------------------------------
    jj = ~(B*nn) & ~nn & mj;
    if any(jj)

        % find densely coupled internal states (using the graph Laplacian)
        %------------------------------------------------------------------
        [g,j] = max(diag(G).*jj);
        if m > 1
            g      = G(:,j);
            g(j)   = 0;
            g(~jj) = 0;
            [g,k]  = sort(g,'descend');
            try
                j = [j; k(1:m - 1)];
            end
        end

        jj    = sparse(j,1,1,size(L,1),1) & jj;         % internal states
        bb    = B*jj & ~jj & ~nn;                       % Markov blanket
        ee    =  ~bb & ~jj & ~nn;                       % external states
        b     = find(bb);
        e     = find(ee);
        s     = b(find( any(L(b,e),2)));
        a     = b(find(~any(L(b,e),2)));

        % indices of individual states in the i-th particle
        %------------------------------------------------------------------
        x{1,i} = spm_cat(z(a));
        x{2,i} = spm_cat(z(s));
        x{3,i} = spm_cat(z(j));

        % states accounted for (nn)
        %------------------------------------------------------------------
        nn   = nn | bb | jj;

    else

        % no internal states - find active states (not influenced by e)
        %------------------------------------------------------------------
        j = ~any(L(~nn,nn),2);
        if any(j)

            % sensory states connected with active states
            %--------------------------------------------------------------
            a  = find(~nn);
            a  = a(find(j,1));
            aa = sparse(a,1,1,size(L,1),1);
            ss = (L*aa | L'*aa) & ~aa & ~nn;
            a  = find(aa);
            s  = find(ss);
            j  = [];

            % indices of individual states in the i-th particle
            %--------------------------------------------------------------
            x{1,i} = spm_cat(z(a));
            x{2,i} = spm_cat(z(s));
            x{3,i} = [];

            % states accounted for (nn)
            %--------------------------------------------------------------
            nn   = nn | aa | ss;

        elseif any(~nn)

            % sensory states connected with sensory states
            %--------------------------------------------------------------
            s  = find(~nn);
            ss = sparse(s(1),1,1,nz,1);
            ss = ss | B*ss & ~nn;
            s  = find(ss);
            a  = [];
            j  = [];

            % indices of individual states in the i-th particle
            %--------------------------------------------------------------
            x{1,i} = [];
            x{2,i} = spm_cat(z(s));
            x{3,i} = [];

            % states accounted for (nn)
            %--------------------------------------------------------------
            nn   = nn | ss;
        end
    end

    % indices of partitions (i.e., n-states) in the i-th particle
    %----------------------------------------------------------------------
    y{1,i} = a;
    y{2,i} = s;
    y{3,i} = j;

    % plot
    %----------------------------------------------------------------------
    if all(nn) && numel(u) > 1

        % remove isolated (internal) states
        %--------------------------------------------------------------
        j     = [];
        for n = 1:size(x,2)
            if any(x{1,n}) || any(x{2,n})
                j = [j,n];
            end
        end
        x  = x(:,j);
        y  = y(:,j);

        if GRAPHICS,clf

            % colours for different particles
            %--------------------------------------------------------------
            nx            = size(x,2);
            [col,bol,msz] = spm_MB_col(nx);

            % plot partitions in embedding space (which particle)
            %--------------------------------------------------------------
            subplot(3,2,3)
            for k = 1:nx
                plot(u(y{1,k},1),u(y{1,k},2),'.','color',bol{k},'MarkerSize',msz), hold on
                plot(u(y{2,k},1),u(y{2,k},2),'.','color',bol{k},'MarkerSize',msz), hold on
                plot(u(y{3,k},1),u(y{3,k},2),'.','color',col{k},'MarkerSize',msz), hold on
            end
            axis square
            title(sprintf('Particles [%i n-states]',nz),'Fontsize',16)

            % plot particles in embedding space (which sort of state)
            %--------------------------------------------------------------
            subplot(3,2,4)
            for k = 1:nx
                plot(u(y{1,k},1),u(y{1,k},2),'.r','MarkerSize',msz), hold on
                plot(u(y{2,k},1),u(y{2,k},2),'.m','MarkerSize',msz), hold on
                plot(u(y{3,k},1),u(y{3,k},2),'.b','MarkerSize',msz), hold on
            end
            axis square
            title(sprintf('Markov partition [%i particles]',nx),'Fontsize',16)

            % plot particles in three embedding dimensions
            %--------------------------------------------------------------
            subplot(3,2,2)
            try
                for k = 1:nx
                    plot3(u(y{1,k},1),u(y{1,k},2),u(y{1,k},3),'.r','MarkerSize',msz), hold on
                    plot3(u(y{2,k},1),u(y{2,k},2),u(y{2,k},3),'.m','MarkerSize',msz), hold on
                    plot3(u(y{3,k},1),u(y{3,k},2),u(y{3,k},3),'.b','MarkerSize',msz), hold on
                end
            catch
                for k = 1:nx
                    plot(u(y{1,k},1),u(y{1,k},2),'.r','MarkerSize',msz), hold on
                    plot(u(y{2,k},1),u(y{2,k},2),'.m','MarkerSize',msz), hold on
                    plot(u(y{3,k},1),u(y{3,k},2),'.b','MarkerSize',msz), hold on
                end
            end

            axis square
            title('Embedding space','Fontsize',16)
            rotate3d(gca,'on')

            % Jacobian (ordered by partition and type)
            %--------------------------------------------------------------
            j = spm_vec(x');
            k = spm_vec(x );
            subplot(3,2,5),imagesc(-log(abs(J(k,k)) + exp(-4))),axis square
            subplot(3,2,6),imagesc(-log(abs(J(j,j)) + exp(-4))),axis square

            % Colors
            %--------------------------------------------------------------
            nj   = spm_length(x);
            msz  = fix(16 + 128/nj);
            j    = 1:nj;
            k    = spm_unvec(j,x')';
            j    = spm_unvec(j,x);
            subplot(3,2,5),hold on
            for q = 1:nx
                plot(j{1,q},ones(size(x{1,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(j{2,q},ones(size(x{2,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(j{3,q},ones(size(x{3,q})),'.','color',col{q},   'MarkerSize',msz)
                plot(j{1,q},zeros(size(x{1,q})) + nj,'.','color','r','MarkerSize',msz)
                plot(j{2,q},zeros(size(x{2,q})) + nj,'.','color','m','MarkerSize',msz)
                plot(j{3,q},zeros(size(x{3,q})) + nj,'.','color','b','MarkerSize',msz)
            end
            title(sprintf('Jacobian (by %i particles)',nx),'Fontsize',16)

            subplot(3,2,6),hold on
            for q = 1:nx
                plot(k{1,q},ones(size(x{1,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(k{2,q},ones(size(x{2,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(k{3,q},ones(size(x{3,q})),'.','color',col{q},   'MarkerSize',msz)
                plot(k{1,q},zeros(size(x{1,q})) + nj,'.','color','r','MarkerSize',msz)
                plot(k{2,q},zeros(size(x{2,q})) + nj,'.','color','m','MarkerSize',msz)
                plot(k{3,q},zeros(size(x{3,q})) + nj,'.','color','b','MarkerSize',msz)
            end
            title('Jacobian (by type)','Fontsize',16)

        end
        break
    end

end

return


function [PA, P, PS] = reduce(disc, A, S)
%REDUCE   Dimension reduction for operator matrix.
%   PA = REDUCE(DISC, A) reduces the row dimension of each block column in the
%   cell array A (which is typically a discretization of DISC.SOURCE) so that
%   the reduced discretization, PA, can be formed as a matrix. In particular, PA
%   will have sum(DISC.dimension) rows and sum(cellfun(@(a) size(A, 2), A(1,:))
%   columns.
%
%   [PA, P] = REDUCE(DISC, A) returns also the block-diagonal reduction matrix
%   P. For COLLOC discretizations, this blocks are BARYMAT projections.
%
%   [PA, P, PS] = REDUCE(DISC, A, S) is required for consistency with other
%   opDiscretization reductions. Here S is ignored and PS = P.

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

% Setup:
r = disc.projOrder;
dim = disc.dimension;
dimAdjust = disc.dimAdjust(1,:);
if ( numel(dimAdjust) == 1 )
    dimAdjust = repmat(dimAdjust, 1, size(A, 2));
end
PA = cell(1, size(A, 2));
P = cell(1, size(A, 2));

% Do reduction for each block column:
for k = 1:size(A, 2)
    [PA{k}, P{k}] = reduceOne(disc, A(:,k), r(k), dim + dimAdjust(k));
%     [PS{k}, ignored] = reduceOne(disc, S(:,k), r(k), dim + dimAdjust(k));
end

% Convert cell arrays to matrices:
P = blkdiag(P{:});
PA = cell2mat(PA);
% PS = cell2mat(PS);
PS = P;

end

function [PA, P] = reduceOne(disc, A, m, n)
%REDUCEONE   Reduce one block column.
%   [PA, P] = REDUCEONE(DISC, A, M, N) reduces entries of the column cell arrays
%   A from a sum(N)xsum(N) discretization to sum(N-M)xsum(N) version, PA, using
%   the block-projection operator P.

% Step by intervals in the domain.
domain = disc.domain;
numInt = disc.numIntervals;
P = cell(1, numInt);

% Loop through intervals
for k = 1:numInt
    disc.domain = domain(k:(k+1));
    disc.dimension = n(k)-m;
    [xOut, ignored, ignored, tOut] = equationPoints(disc);
    disc.dimension = n(k);
    [xIn, ignored, baryWt, tIn] = functionPoints(disc);
    % Store the kth projection matrix in the cell P
%     P{k} = barymat(xOut, xIn, baryWt);
    P{k} = barymat(xOut, xIn, baryWt, tOut, tIn, 1);
end
% Convert the projection matrices P into a blockdiagonal matrix.
P = blkdiag(P{:});

% Project each of the entries of A:
PA = cell(size(A));
for j = 1:numel(A)
    if ( size(P, 2) == size(A{j}, 1) )
        PA{j} = P*A{j};
    else
        PA{j} = A{j};
    end
end
PA = cell2mat(PA);

if ( (m == 0) && (size(A{1}, 2) < sum(n)) )
    % We don't want to project scalars.
    P = eye(size(A, 2));
end

end


%%******************************************************************
%% ops:
%%
%%   Z = ops(X,operand,Y,alpha);
%%
%%  INPUT:        X = a matrix or a scalar
%%                    or a CELL ARRAY consisting only of matrices
%%          operand = sym, transpose, triu, tril,
%%                    real, imag, sqrt, abs, max, min, nnz,
%%                    spdiags, ones, zeros, norm, sum, row-norm, blk-norm
%%                    rank1, rank1inv, inv
%%                    +,  -, *, .*,  ./, .^
%%     Y (optional) = a matrix or a scalar
%%                    or a CELL ARRAY consisting only of matrices
%% alpha (optional) = a scalar
%%                    or the variable blk.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function Z = ops(X,operand,Y,alpha);

   spdensity = 0.4;

   if (nargin == 2)
      if strcmp(operand,'sym');
         if ~iscell(X);
            [m,n] = size(X);
            if (m == n);
               Z = (X+X')/2;
            elseif (n == 1);
               Z = X;
            else;
               error('X must be square matrix or a column vector');
            end;
         else
            Z = cell(size(X));
            for p = 1:length(X);
                [m,n] = size(X{p});
                if (m == n);
                   Z{p} = (X{p}+X{p}')/2;
                elseif (n == 1);
                   Z{p} = X{p};
                else;
                   error('X{p} must be square matrix or a column vector');
                end;
            end;
         end;
      elseif strcmp(operand,'sqrt') | strcmp(operand,'abs') | ...
         strcmp(operand,'real') | strcmp(operand,'imag');
         if ~iscell(X);
            eval(['Z = ',operand,'(X);']);
         else;
            Z = cell(size(X));
            for p = 1:length(X);
               eval(['Z{p} = ',operand,'(X{p});']);
            end;
         end;
      elseif strcmp(operand,'max') | strcmp(operand,'min') | ...
             strcmp(operand,'sum');
         if ~iscell(X);
            eval(['Z = ',operand,'(X);']);
         else;
            Z = [];
            for p = 1:length(X);
                eval(['Z = [Z  ',operand,'(X{p})','  ];']);
            end;
         end;
         eval(['Z = ',operand,'(Z);']);
      elseif strcmp(operand,'transpose') | strcmp(operand,'triu') | ...
             strcmp(operand,'tril');
         if ~iscell(X);
            if (size(X,1) == size(X,2));
               eval(['Z = ',operand,'(X);']);
            elseif (size(X,2) == 1);
               eval(['Z = X;']);
            else;
               error('X must be square matrix or a column vector');
            end;
         else
            Z = cell(size(X));
            for p = 1:length(X);
                if (size(X{p},1) == size(X{p},2));
                   eval(['Z{p} = ',operand,'(X{p});']);
                elseif (size(X{p},2) == 1);
                   eval(['Z{p} = X{p};']);
                else;
                   error('X{p} must be square matrix or a column vector');
                end;
            end;
         end;
      elseif strcmp(operand,'norm');
         if ~iscell(X);
            Z = full(sqrt(sum(sum(X.*X))));
         else
            Z = 0;
            for p = 1:length(X); Z = Z + sum(sum(X{p}.*X{p})); end;
            Z = sqrt(Z);
         end;
      elseif strcmp(operand,'blk-norm');
         if ~iscell(X);
            Z = full(sqrt(sum(sum(X.*X))));
         else
            Z = zeros(length(X),1);
            for p = 1:length(X); Z(p) = sum(sum(X{p}.*X{p})); end;
            Z = sqrt(Z);
         end;
      elseif strcmp(operand,'inv');
         if ~iscell(X);
            [m,n] = size(X); n2 = n*n;
            if (m==n)
               Z = inv(X);
               if (nnz(Z) > spdensity*n2)
                  Z = full(Z);
               else
                  Z = sparse(Z);
               end
            elseif (m > 1 & n == 1);
               Z = 1./X;
               if (nnz(Z) > spdensity*n)
                  Z = full(Z);
	       else
	          Z = sparse(Z);
               end
            end
         else
            Z = cell(size(X));
            for p = 1:length(X);
               [m,n] = size(X{p}); n2 = n*n;
               if (m==n)
                  Z{p} = inv(X{p});
                  if (nnz(Z{p}) > spdensity*n2)
                     Z{p} = full(Z{p});
		  else
                     Z{p} = sparse(Z{p});
                  end
               elseif (m > 1 & n == 1);
                  Z{p} = 1./X{p};
                  if (nnz(Z{p}) > spdensity*n)
                     Z{p} = full(Z{p});
		  else
                     Z{p} = sparse(Z{p});
                  end
               end
            end
         end
      elseif strcmp(operand,'getM');
         if ~iscell(X);
            Z = size(X,1);
         else
            for p = 1:length(X); Z(p) = size(X{p},1); end;
            Z = sum(Z);
         end;
      elseif strcmp(operand,'nnz');
         if ~iscell(X);
            Z = nnz(X);
         else;
            for p = 1:length(X);
               Z(p) = nnz(X{p});
            end;
            Z = sum(Z);
         end;
      elseif strcmp(operand,'ones');
         if ~iscell(X);
            Z = ones(size(X));
         else
            Z = cell(size(X));
            for p = 1:length(X);
                Z{p} = ones(size(X{p}));
            end
         end
      elseif strcmp(operand,'zeros');
         if ~iscell(X);
            [m,n] = size(X);
            Z = sparse(m,n);
         else
            Z = cell(size(X));
            for p = 1:length(X);
               [m,n] = size(X{p});
               Z{p} = sparse(m,n);
            end
         end
      elseif strcmp(operand,'identity');
         blk = X;
         Z = cell(size(blk,1),1);
         for p = 1:size(blk,1)
            pblk = blk(p,:); n = sum(pblk{2});
            if strcmp(pblk{1},'s')
               Z{p} = speye(n,n);
            elseif strcmp(pblk{1},'q')
               s = 1+[0, cumsum(pblk{2})];
               len = length(pblk{2});
               Z{p} = zeros(n,1);
               Z{p}(s(1:len)) = ones(len,1);
            elseif strcmp(pblk{1},'l')
               Z{p} = ones(n,1);
            elseif strcmp(pblk{1},'u')
               Z{p} = zeros(n,1);
            end
         end
      elseif strcmp(operand,'row-norm');
         if ~iscell(X);
            if (size(X,2) == size(X,1));
               Z = sqrt(sum((X.*conj(X))'))';
            elseif (size(X,2) == 1);
               Z = abs(X);
            end
         else
            Z = cell(size(X));
            for p = 1:length(X);
               if (size(X{p},2) == size(X{p},1));
                  Z{p} = sqrt(sum((X{p}.*conj(X{p}))'))';
               elseif (size(X{p},2) == 1);
                  Z{p} = abs(X{p});
               end
            end
         end
      end
   end
%%
   if (nargin == 3)
      if strcmp(operand,'spdiags');
         if ~iscell(Y);
            [m,n] = size(Y);
            if (m == n);
               Z = spdiags(X,0,m,n);
            else
               Z = X;
            end
         else
            Z = cell(size(Y));
            for p = 1:length(Y);
               [m,n] = size(Y{p});
               if (m == n);
                  Z{p} = spdiags(X{p},0,m,n);
               else;
                  Z{p} = X{p};
               end
            end
         end
      elseif strcmp(operand,'inprod')
         if ~iscell(X) & ~iscell(Y)
    	    Z = (Y'*X)';
	 elseif iscell(X) & iscell(Y)
    	    Z = zeros(size(X{1},2),1);
   	    for p=1:length(X)
	        Z = Z + (Y{p}'*X{p})';
            end
         end
      elseif strcmp(operand,'+') | strcmp(operand,'-') | ...
             strcmp(operand,'/') | strcmp(operand,'./') | ...
             strcmp(operand,'*') | strcmp(operand,'.*') | ...
             strcmp(operand,'.^');
         if (~iscell(X) & ~iscell(Y));
            eval(['Z = X',operand,'Y;']);
         elseif (iscell(X) & iscell(Y))
            Z = cell(size(X));
            for p = 1:length(X);
 	       if (size(X{p},2) == 1) & (size(Y{p},2) == 1) & ...
                  (strcmp(operand,'*') | strcmp(operand,'/'));
                  eval(['Z{p} = X{p}.',operand,'Y{p};']);
               else
                  eval(['Z{p} = X{p} ',operand,'Y{p};']);
               end
            end
         elseif (iscell(X) & ~iscell(Y));
	    if (length(Y) == 1); Y = Y*ones(length(X),1); end
            Z = cell(size(X));
            for p = 1:length(X);
                eval(['Z{p} = X{p}',operand,'Y(p);']);
            end
         elseif (~iscell(X) & iscell(Y));
            Z = cell(size(Y));
	    if (length(X) == 1); X = X*ones(length(Y),1); end
            for p = 1:length(Y);
                eval(['Z{p} = X(p)',operand,'Y{p};']);
            end
         end
      else
         error([operand,' is not available, check input arguments']);
      end
   end
%%
   if (nargin == 4)
      if strcmp(operand,'rank1') | strcmp(operand,'rank1inv');
         Z = cell(size(alpha,1),1);
         for p = 1:size(alpha,1);
            if ~strcmp(alpha{p,1},'diag');
               blktmp = alpha{p,2};
               if (length(blktmp) == 1);
                  if strcmp(operand,'rank1');
                     Z{p} = (X{p}*Y{p}' + Y{p}*X{p}')/2;
                  else;
                     Z{p} = 2./(X{p}*Y{p}' + Y{p}*X{p}');
                  end
               else
                  Xp = X{p};
                  Yp = Y{p};
                  n = sum(blktmp);
                  Zp = sparse(n,n);
                  s = [0 cumsum(blktmp)];
                  if strcmp(operand,'rank1');
                     for i = 1:length(blktmp)
                        pos = [s(i)+1 : s(i+1)];
                        x = Xp(pos);
                        y = Yp(pos);
                        Zp(pos,pos) = sparse((x*y' + y*x')/2);
                     end;
                     Z{p} = Zp;
                  else
                     for i = 1:length(blktmp)
                        pos = [s(i)+1 : s(i+1)];
                        x = Xp(pos);
                        y = Yp(pos);
                        Zp(pos,pos) = sparse(2./(x*y' + y*x'));
                     end
                     Z{p} = Zp;
                  end
               end
            elseif strcmp(alpha{p,1},'diag');
               if strcmp(operand,'rank1');
                  Z{p} = X{p}.*Y{p};
               else
                  Z{p} = 1./(X{p}.*Y{p});
               end
            end
         end
      elseif strcmp(operand,'+') | strcmp(operand,'-');
         if ~iscell(X) & ~iscell(Y);
            eval(['Z = X',operand,'alpha*Y;']);
         elseif (iscell(X) & iscell(Y));
            Z = cell(size(X));
	    if (length(alpha) == 1);
               alpha = alpha*ones(length(X),1);
            end
            for p = 1:length(X);
               eval(['Z{p} = X{p}',operand,'alpha(p)*Y{p};']);
            end
         else
            error('X, Y are different objects');
         end
      else
         error([operand,' is not available']);
      end
   end
%%============================================================


function A = sledges2adjmat(n, nt, edges, varargin)
%SLEDGES2ADJMAT Creates an adjacency matrix from edge set
%
% $ Syntax $
%   - A = sledges2adjmat(n, nt, edges, ...)
%
% $ Arguments $
%   - n:            The number of (source) nodes
%   - nt:           The number of (target) nodes
%   - edges:        The matrix of edge set
%
% $ Description $
%   - A = sledges2adjmat(n, nt, edges) creates an adjacency matrix
%     from the edge set. You can specify the following properties:
%       - 'valtype':        the value type of the target matrix
%                           - 'auto': if has value, make numeric matrix
%                                     if no value, make logical matrix
%                           - 'logical': make logical matrix always
%                           - 'numeric': make numeric matrix always
%                           (default = 'auto')
%       - 'sparse':         whether to create a sparse matrix
%                           (default = true)
%       - 'preprune':       whether to prune the edges first
%                           (default = false)
%       - 'prunemethod':    the method used to prune the edge set
%                           (default = [], means using default method)
%                           refer to slpruneedgeset for the specification
%                           of the prune methods.
%       - 'sym':            whether to create symmetric graph
%       - 'symmethod':      the method to symmetrize the graph
%                           (default = [], means using default method)
%                           refer to slsymedgeset for the specification.
%
% $ Remarks $
%   - The property sym can only be true when n == nt.
%
%   - It is an integrated wrapper for slmakeadjmat, slsymgraph and
%     slpruneedgeset.
%
% $ History $
%   - Created by Dahua Lin, on Sep 9, 2006
%

%% parse and verify input arguments

if nargin < 3
    raise_lackinput('sledges2adjmat', 3);
end

if ~isempty(edges)
    ncols = size(edges, 2);
    if ndims(edges) ~= 2 || (ncols ~= 2 && ncols ~= 3)
        error('sltoolbox:invalidarg', ...
            'The edges should be a 2D matrix with two or three columns');
    end
end

opts.valtype = 'auto';
opts.sparse = true;
opts.preprune = false;
opts.prunemethod = [];
opts.sym = false;
opts.symmethod = [];
opts = slparseprops(opts, varargin{:});

if opts.sym
    if n ~= nt
        error('sltoolbox:rterror', ...
            'The sym can only be true when n == nt');
    end
end

switch opts.valtype
    case 'auto'
        islogic = (ncols == 2);
    case 'numeric'
        islogic = false;
    case 'logical'
        islogic = true;
    otherwise
        error('sltoolbox:invalidarg', ...
        'Invalid value type of adjacency matrix: %s', opts.valtype);
end

%% main skeleton

% prune
if opts.preprune
    edges = slpruneedgeset(n, nt, edges, opts.prunemethod);
end

% make adjmat

A = slmakeadjmat(n, nt, edges, [], islogic, opts.sparse);

% symmetrize
if opts.sym
    A = slsymgraph(A, opts.symmethod);
end