text:
function [L,S,errHist] = solver_RPCA_constrained(AY,lambda_S, tau, A_cell, opts)
% [L,S,errHist] = solver_RPCA_constrained(Y,lambda_S, tau, A_cell, opts)
% Solves the problem
%   minimize_{L,S} .5|| L + S - Y ||_F^2
%   subject to
%   if opts.sum = true
%       (1)  ||L||_* + lambda_S ||S||_1 <= tau
%   if opts.max = true
%       (2) max(  ||L||_* , lambda_S ||S||_1 ) <= tau
%
%   if opts.max and opts.sum are false and tau is a negative number, then
%   we solve the problem:
%       minimize_{L,S} .5|| L + S - Y ||_F^2  + abs(tau)*( ||L||_* + lambda_S ||S||_1 )
%   (but see solver_RPCA_Lagrangian.m for a simpler interface)
%
%   or if A_cell is provided, where A_cell = {A, At}
%   (A is a function handle, At is a function handle to the transpose of A)
%   then
%
%   minimize_{L,S} .5|| A(L + S) - Y ||_F^2
%       subject to ...
%   (here, Y usually represents A(Y); if Y is not the same size
%    as A(L), then we will automatically set Y <-- A(Y) )
%
%   errHist(:,1) is a record of the residual
%   errHist(:,2) is a record of the full objective (that is, .5*resid^2 )
%   errHist(:,3) is the output of opts.errFcn if provided
%
% opts is a structure with options:
%   opts.sum, opts.max  (as described above)
%   opts.L0         initial guess for L (default is 0)
%   opts.S0         initial guess for S (default is 0)
%   opts.size       [n1,n2] where L and S are n1 x n2 matrices. The size is automatically
%       determined in most cases, but when providing a linear operator
%       it may be necessary to provide an explicit size.
%   opts.tol        sets stopping tolerance
%   opts.maxIts     sets maximum number of iterations
%   opts.printEvery will print information this many iterations
%   opts.displayTime will print out timing information (default is true for large problems)
%   opts.errFcn     a function of (L,S) that records information
%   opts.trueObj    if provided, this will be subtracted from errHist(2,:)
%   opts.Lip        Lipschitz constant, i.e., 2*spectralNorm(A)^2
%                       by default, assume 2 (e.g., good if A = P_Omega)
%   opts.FISTA      whether to use FISTA or not. By default, true
%     opts.restart  how often to restart FISTA; set to -Inf to make it automatic
%   opts.BB         whether to use the Barzilai-Borwein spectral steplength
%     opts.BB_type  which BB stepsize to take. Default is 1, the larger step
%     opts.BB_split whether to calculate stepslengths for S and L independently.
%       Default is false, which is recommended.
%   opts.quasiNewton  uses quasi-Newton-like Gauss-Seidel scheme.
%                     Only available in "max" mode
%     opts.quasiNewton_stepsize     stepsize length. Default is .8*(2/Lip)
%     opts.quasinewton_SLS          whether to take S-L-S sequence (default is true)
%                                   otherwise, takes a L-S Gauss-Seidel sequence
%   opts.SVDstyle   controls what type of SVD is performed.
%       1 = full svd using matlab's "svd". Best for small problems
%       2 = partial svd using matlab's "svds". Not recommended.
%       3 = partial svd using PROPACK, if installed. Better than option 2, worse than 4
%       4 = partial svd using randomized linear algebra, following
%           the Halko/Tropp/Martinnson "Structure in Randomness" paper
%       in option 4, there are additional options:
%       opts.SVDwarmstart   whether to "warm-start" the algorithm
%       opts.SVDnPower  number of power iterations (default is 2 unless warm start)
%       opts.SVDoffset  oversampling, e.g., "rho" in Tropp's paper. Default is 5
%
%   opts.L1L2      instead of using l1 penalty, e.g., norm(S(:),1), we can
%       also use block norm penalties, such as (if opts.L1L2 = 'rows')
%       the sum of the l2-norm of rows (i.e., l1-norm of rows),
%       or if opts.L1L2='cols', the sum of the l2-norms of colimns.
%       By default, or if opts.L1L2 = [] or false, then uses usual l1 norm.
%       [Feature added April 17 2015]
%
%  Features that may be added later: [email developers if these are
%    important to you]
%       - Allow Huber loss function.
%
% Stephen Becker, March 6 2014. Edited March 14 2014, April 2015.
%   stephen.becker@colorad.edu
% See also solver_RPCA_Lagrangian.m, solver_RPCA_SPGL1.m

% todo: allow S >= 0 constraints, since this is easy
% todo: allow Huber loss function

error(nargchk(3,5,nargin,'struct'));
if nargin < 5, opts = []; end
% == PROCESS OPTIONS ==
function out = setOpts( field, default )
    if ~isfield( opts, field )
        opts.(field)    = default;
    end
    out = opts.(field);
    opts    = rmfield( opts, field ); % so we can do a check later
end

if nargin < 4 || isempty(A_cell)
    A   = @(X) X(:);
    [n1,n2] = size(AY);
    At  = @(x) reshape(x,n1,n2);

    if ~iscell(tau)
        AY  = A(AY);
    end
    % The factor of 2 is since we are in both L and S
else
    A   = A_cell{1};
    At  = A_cell{2};
    % Y could be either Y or A(Y)
    if size(AY,2) > 1
        % AY is a vector, so it is probably Y and not AY
        disp('Changing Y to A(Y)');
        AY = A(AY);
        [n1,n2] = size(AY); % April 24 '14
    else
        % April 24, '14: we need to know the (n1,n2)
        sz      = setOpts('size',[] );
        if isempty(sz)
            error('Cannot determine the size of the variables; please specify opts.size=[n1,n2]');
        end
        n1 = sz(1);
        n2 = sz(2);
    end
    %[n1,n2] = size(AY); % comment out April 24 '14
end
normAY =    norm(AY(:));

vec = @(X) X(:);

% Some problem sizes. Feel free to tweak. Mainly affect the defaults
SMALL   = ( n1*n2 <= 50^2 );
MEDIUM  = ( n1*n2 <= 200^2 ) && ~SMALL;
LARGE   = ( n1*n2 <= 1000^2 ) && ~SMALL && ~MEDIUM;
HUGE    = ( n1*n2 > 1000^2 );

% -- PROCESS OPTIONS -- (some defaults depend on problem size )
tol     = setOpts('tol',1e-6*(SMALL | MEDIUM) + 1e-4*LARGE + 1e-3*HUGE );
maxIts  = setOpts('maxIts', 1e3*(SMALL | MEDIUM ) + 400*LARGE + 200*HUGE );
printEvery  = setOpts('printEvery',100*SMALL + 50*MEDIUM + 5*LARGE + 1*HUGE);
errFcn      = setOpts('errFcn', [] );
Lip         = setOpts('Lip', 2 );
restart     = setOpts('restart',-Inf);
trueObj     = setOpts('trueObj',0);
sumProject  = setOpts('sum', false );
maxProject  = setOpts('max', false );

if tau < 0
	Lagrangian = true;
	if sumProject || maxProject
		error('in Lagrangian mode (when tau<0 significies lambda=|tau|), turn off sum/maxProject');
	end
	lambda = abs(tau);
	tau = []; % help us track down bugs
else
	Lagrangian = false;
	if (sumProject && maxProject) || (~sumProject && ~maxProject), error('must choose either "sum" or "max" type projection'); end
end

QUASINEWTON = setOpts('quasiNewton', maxProject || Lagrangian );
FISTA       = setOpts('FISTA',~QUASINEWTON);
BB          = setOpts('BB',~QUASINEWTON);
% Note: BB with FISTA is sometimesm not so good
if BB && FISTA, warning('solver_RPCA:convergence','Convergence not guaranteed with FISTA if opts.BB=true'); end
BB_split    = setOpts('BB_split',false);
BB_type     = setOpts('BB_type',1); % 1 or 2
stepsizeQN  = setOpts('quasiNewton_stepsize', .8*2/Lip );
S_L_S       = setOpts('quasiNewton_SLS', true );
displayTime = setOpts('displayTime',LARGE | HUGE );
SVDstyle    = setOpts('SVDstyle', 1*SMALL + 4*(~SMALL) ); % 1 is full SVD
% and even finer tuning (only matter if SVDstyle==4)
SVDwarmstart= setOpts('SVDwarmstart', true );
SVDnPower   = setOpts('SVDnPower', 1 + ~SVDwarmstart ); % number of power iteratiosn
SVDoffset   = setOpts('SVDoffset', 5 );
SVDopts = struct('SVDstyle', SVDstyle,'warmstart',SVDwarmstart,...
    'nPower',SVDnPower,'offset',SVDoffset );

if QUASINEWTON
    if sumProject
        error('Can not run quasi-Newton mode when in "sum" formulation. Please change to "max"');
    elseif FISTA
        error('Can not run quasi-Newton with FISTA');
    elseif BB
        error('Can not run quasi-Newton with BB');
    end
end

% April 17 2015
L1L2        = setOpts('L1L2',0);
if isempty(L1L2), L1L2=0; end
if L1L2,
    if ~isempty(strfind(lower(L1L2),'row')),  L1L2 = 'rows';
    elseif ~isempty(strfind(lower(L1L2),'col')),  L1L2 = 'cols';
        % so col, COL, cols, columns, etc. all acceptable
    else
        error('unrecognized option for L1L2: should be row or column or 0');
    end
end

projNuclear(); % remove any persistent variables
if maxProject
    project = @(L,S,varargin) projectMax(L1L2,tau,lambda_S,SVDopts, L,S);
elseif sumProject
    if any(L1L2), error('with opts.sum=true, need opts.L1L2=0'); end
    project = @(L,S,varargin) projectSum(tau,lambda_S,L,S);
elseif Lagrangian
    project = @(L,S,varargin) projectMax(L1L2,lambda,lambda*lambda_S,SVDopts, L,S, varargin{:});
end

L           = setOpts('L0',zeros(n1,n2) );
S           = setOpts('S0',zeros(n1,n2) );

% Check for extra options that were not processed
if ~isempty( fieldnames(opts ) )
    disp( 'warning, found extra guys in opts');
    disp( opts )
    error('Found unprocessed options in "opts"');
end

stepsize    = 1/Lip;
errHist     = zeros(maxIts, 2 + ~isempty(errFcn) );
Grad        = 0;
if FISTA || BB || QUASINEWTON
    L_old   = L;
    S_old   = S;
end
L_fista     = L;
S_fista     = S;
BREAK   = false;
kk      = 0; % counter for FISTA
timeRef = tic;
for k = 1:maxIts
    % Gradient in (L,S) (at the fista point) is (R,R) where...
    R   = A(L_fista + S_fista) - AY;
    Grad_old    = Grad;
    Grad        = At(R);

    objL        = Inf;
    if QUASINEWTON
%         stepsizeQN  = 1 - min(0.1, 1/k );
%         stepsizeQN = 1 - .3/sqrt(k);

        if S_L_S
            % we solve for S, update L, then re-update S
            % Exploits the fact that projection for S is faster
            dL      = L - L_old;
            S_old   = S;
            [~,S_temp]   = project( [], S - stepsizeQN*( Grad + dL ), stepsizeQN ); % take small step...

            dS      = S_temp - S_old;
            L_old   = L;
            [L,~,rnk,objL]   = project( L - stepsizeQN*( Grad + dS ), [], stepsizeQN );

            dL      = L - L_old;
            [~,S]   = project( [], S - stepsizeQN*( Grad + dL ) , stepsizeQN);
        else
            % Gauss-Seidel update, starting with L, then S
            % Changing order seemed to not work as well
            dS      = S - S_old;
            L_old   = L;
            [L,~,rnk,objL]   = project( L - stepsizeQN*( Grad + dS ), [] , stepsizeQN);
            dL      = L - L_old;
            S_old   = S;
            [~,S]   = project( [], S - stepsizeQN*( Grad + dL ) , stepsizeQN);
        end
%         stepsizeQN = (1+3*stepsizeQN)/4;
    else
        if BB && k > 1
            [stepsizeL, stepsizeS]  = compute_BB_stepsize( Grad, Grad_old, L, L_old, S, S_old, BB_split, BB_type);
            if isnan(stepsizeL) || isnan(stepsizeS)
                fprintf(2,'Warning: no BB stepsize possible since iterates have not changed!\n');
                [stepsizeL, stepsizeS]   = deal( stepsize );
            end
        else
            [stepsizeL, stepsizeS]   = deal( stepsize );
        end

        % Now compute proximity step
        if FISTA || BB
            L_old   = L;
            S_old   = S;
        end
        L           = L_fista - stepsizeL*Grad;
        S           = S_fista - stepsizeS*Grad;
        if any(isnan(L(:))) || any(isnan(S(:))), fprintf(2,'DEBUG!\n'); keyboard; end
        [L,S,rnk,objL]   = project( L, S, stepsizeL, stepsizeS);
        if any(isnan(L(:))) || any(isnan(S(:))), fprintf(2,'DEBUG!\n'); keyboard; end
    end

    DO_RESTART = false;
    if FISTA
        if k>1 && restart > 0 && ~isinf(restart) && ~mod(kk,restart)
            kk = 0;
            DO_RESTART  = true;
        elseif restart==-Inf && kk > 5
            % In this case, we restart if the function has significantly increased
            if (errHist(k-1,2)-errHist(k-5,2)) > 1e-8*abs(errHist(k-5,2))
                DO_RESTART = true;
                kk = 0;
            end
        end
        L_fista = L + kk/(kk+3)*( L - L_old );
        S_fista = S + kk/(kk+3)*( S - S_old );
        kk      = kk + 1;
    else
        L_fista = L;
        S_fista = S;
    end

%     res          = norm(R(:)); % this is for L_fista, not L
    R            = A(L + S) - AY;
    % sometimes we already have R pre-computed, so if this turns out to
    %   be a signficant computational cost we can make fancier code...
    res          = norm(R(:));
    errHist(k,1) = res;
    errHist(k,2) = 1/2*(res^2); % + lambda_L*objL + lambda_S*objS;
    if Lagrangian
        errHist(k,2) = errHist(k,2) + lambda*objL;
        if any(L1L2)
            if strcmpi(L1L2,'rows')
                errHist(k,2) = errHist(k,2) + lambda*lambda_S*sum( sqrt( sum(S.^2,2) ) );
            else
                errHist(k,2) = errHist(k,2) + lambda*lambda_S*sum( sqrt( sum(S.^2,1) ) );
            end
        else
            errHist(k,2) = errHist(k,2) + lambda*lambda_S*norm(S(:),1);
        end
    end
    if k > 1 && abs(diff(errHist(k-1:k,1)))/res < tol
        BREAK = true;
    end
    PRINT   = ~mod(k,printEvery) | BREAK | DO_RESTART;
    if PRINT
        fprintf('Iter %4d, rel. residual %.2e, objective %.2e', k, res/normAY, errHist(k,2) -trueObj);
    end
    if ~isempty(errFcn)
        err     = errFcn(L,S);
        errHist(k,3)    = err;
        if PRINT, fprintf(', err %.2e', err ); end
    end
    if ~isempty(rnk) && PRINT, fprintf(', rank(L) %3d', rnk ); end
    if PRINT
        fprintf(', sparsity(S) %5.1f%%', 100*nnz(S)/numel(S) );
    end
    if displayTime && PRINT
        tm = toc( timeRef ); timeRef = tic;
        fprintf(', time %.1f s', tm );
    end
    if DO_RESTART, fprintf(' [restarted FISTA]'); end
    if PRINT, fprintf('\n'); end
    if BREAK
        fprintf('Reached stopping criteria (based on change in residual)\n');
        break;
    end
end
if BREAK
    errHist = errHist(1:k,:);
else
    fprintf('Reached maximum number of allowed iterations\n');
end

end

% subfunctions for projection
function [L,S,rnk,nuclearNorm] = projectMax( L1L2, tau, lambda_S,SVDopts, L, S , stepsize, stepsizeS)
 if nargin >= 7 && ~isempty(stepsize)
     % we compute proximity, not projection
     tauL 	= -abs( tau*stepsize );
     if nargin < 8 || isempty( stepsizeS ), stepsizeS = stepsize; end
     tauS 	= -abs( lambda_S*stepsizeS );
 else
     tauL 	= abs(tau);
     tauS 	= abs(tau/lambda_S );
 end

 % We project separately, so very easy
 if ~isempty(L)
     [L,rnk,nuclearNorm]  = projNuclear(tauL, L,SVDopts);
     if tauL > 0
         % we did projection, so this should be feasible
         nuclearNorm = 0;
     end
 else
     rnk = [];
     nuclearNorm = 0;
 end

 if ~isempty(S)
     if tauS > 0
         if ~any(L1L2)
             % use the l1 norm
             projS  = project_l1(tauS);
         elseif strcmpi(L1L2,'rows')
             projS  = project_l1l2(tauS,true);
         elseif strcmpi(L1L2,'cols')
             projS  = project_l1l2(tauS,false);
         else
             error('bad value for L1L2: should be [], ''rows'' or ''cols'' ');
         end
         S  = projS( S );
     else
         if ~any(L1L2)
             % use the l1 norm
             % simple prox
             S = sign(S).*max(0, abs(S) - abs(tauS));
         elseif strcmpi(L1L2,'rows')
             projS  = prox_l1l2(abs(tauS));
             S      = projS(S,1);
         elseif strcmpi(L1L2,'cols')
             projS  = prox_l1l2(abs(tauS));
             S      = projS(S',1)';
         else
             error('bad value for L1L2: should be [], ''rows'' or ''cols'' ');
         end

     end
 end
end

function [X,rEst,nrm] = projNuclear( tau, X, SVDopts )
 % Input must be a matrix, not a vector
 % Computes either the proximity operator of the nuclear norm (if tau<0)
 % or projection onto the nuclear norm ball of radius tau (if tau>0)

 persistent oldRank Vold iteration
 if nargin==0, oldRank=[]; Vold = []; iteration = 0; return; end
 if isempty(oldRank), rEst = 10;
 else, rEst = oldRank + 2;
 end
 if isempty(iteration), iteration = 0; end
 iteration   = iteration + 1;
 [n1,n2]     = size(X);
 minN       = min( [n1,n2] ); % could set smaller to make nonconvex
 % For the first few iterations, we constrain rankMax
 switch iteration
     case 1
         rankMax     = round(minN/4);
     case 2
         rankMax     = round(minN/2);
     otherwise
         rankMax     = minN;
 end

 style = SVDopts.SVDstyle;
 if tau==0, X=0*X; return; end

 switch style
     case 1
         % full SVD
         [U,S,V] = svd(X,'econ');
         s   = diag(S);
         if tau < 0 % we do prox
             s = max( 0, s - abs(tau) );
         else
             s = project_simplex( tau, s );
         end
         tt      = s > 0;
         rEst    = nnz(tt);
         U       = U(:,tt);
         S       = diag(s(tt));
         V       = V(:,tt);
         nrm     = sum(s(tt));
     case {2,3,4}
         % 2: use Matlab's sparse SVD
         % 3: use PROPACK
         % 4: use Joel Tropp's randomized SVD
         if style==2
             opts = struct('tol',1e-4);
             if rankMax==1, opts.tol = min(opts.tol,1e-6); end % important!
             svdFcn = @(X,rEst)svds(X,rEst,'L',opts);
         elseif style==3
             opts = struct('tol',1e-4,'eta',eps);
             opts.delta = 10*opts.eta;
             % set eta to eps, but not 0 otherwise reorth is very slow
             if rankMax==1, opts.tol = min(opts.tol,1e-6); end % important!
             svdFcn = @(X,rEst)lansvd(X,rEst,'L',opts);
         elseif style == 4
             opts = [];
             if isfield(SVDopts,'nPower') && ~isempty( SVDopts.nPower )
                 nPower = SVDopts.nPower;
             else
                 nPower = 2;
             end
             if isfield(SVDopts,'offset') && ~isempty( SVDopts.offset )
                 offset = SVDopts.offset;
             else
                 offset = 5;
             end
             if isfield( SVDopts, 'warmstart' ) && SVDopts.warmstart==true ...
                     && ~isempty(Vold)
                 opts = struct( 'warmStart', Vold );
             end
             ell     = @(r) min([r+offset,n1,n2]); % number of samples to take
             svdFcn = @(X,rEst)randomizedSVD(X,rEst,ell(rEst),nPower,[],opts );
         end

         ok  = false;
         while ~ok
             rEst    = min( [rEst,rankMax] );
             [U,S,V] = svdFcn(X,rEst);
             s       = diag(S);
             if tau < 0
                 % we are doing prox
                 lambda = abs(tau);
             else
                 lambda  = findTau(s,tau);
             end
             ok      = ( min(s) < lambda ) || (rEst == rankMax);
             if ok, break; end
             rEst    = 2*rEst;
         end
         rEst = min( length(find(s>lambda)), rankMax );
         S   = diag( s(1:rEst) - lambda );
         U   = U(:,1:rEst);
         V   = V(:,1:rEst);
         nrm = sum( s(1:rEst) - lambda );
     otherwise
         error('bad value for SVDstyle');
 end
 if isempty(U)
     X = 0*X;
 else
     X = U*S*V';
 end
 oldRank = size(U,2);
 if isfield( SVDopts, 'warmstart' ) && SVDopts.warmstart==true
     Vold = V;
 end
end

function x = project_simplex( q, x )
 % projects onto the constraints sum(x)=q and x >= 0
 % Update: projects onto sum(x) <= q and x >= 0

 x     = x.*( x > 0 ); % March 11 '14
 % March 11, fixing bug: we want to project onto the volume, not
 %   the actual simplex (surface)
 if sum(x) <= q, return; end
 if q==0, x = 0*x; return; end

 s     = sort( x, 'descend' );
 if q < eps(s(1))
     % eps(x) is the distance from abs(x) to the next larger
     %   floating point number, i.e., in floating point arithmetic,
     %   x + eps(x)/2 = x
     % eps(1) is about 2.2e-16

     error('Input is scaled so large compared to q that accurate computations are difficult');
     % since then cs(1) = s(1) - q is  not even guaranteed to be
     % smaller than q !
 end
 cs    = ( cumsum(s) - q ) ./ ( 1 : numel(s) )';
 ndx   = nnz( s > cs );
 x     = max( x - cs(ndx), 0 );
end

function tau = findTau( s, lambda )
 % Returns the shrinkage value necessary to shrink the vector s
 %   so that it is in the lambda scaled simplex
 %   Usually, s is the diagonal part of an SVD
 if all(s==0)||lambda==0, tau=0; return; end
 if numel(s)>length(s), error('s should be a vector, not a matrix'); end
 if ~issorted(flipud(s)), s = sort(s, 'descend'); end
 if any( s < 0 ), error('s should be non-negative'); end

 % project onto the simplex of radius lambda (not tau)
 % and use this to find "tau" (the shrinkage amount)
 % If we know s_i > tau for i = 1, ..., k, then
 %   tau = ( sum(s(1:k)) - lambda )/k
 % But we don't know k, so find it:
 cs  = (cumsum(s) - abs(lambda) )./(1:length(s))';
 ndx = nnz( s > cs ); % >= 1 as long as lambda > 0
 tau = max(0,cs(ndx));
 % We want to make sure we project onto sum(sigma) <= lambda
 %   and not sum(sigma) == lambda, so do not allow negative tau

end

function [L,S,rnk,nuclearNorm] = projectSum( tau, lambda_S, L, S )
  [m,n]           = size(L);
  [U,Sigma,V]     = svd(L,'econ');
  s       = diag(Sigma);
  wts     = [ ones(length(s),1); lambda_S*ones(m*n,1) ];
  proj    = project_l1(tau,wts);
  sS      = proj( [s;vec(S)] );
  sProj   = sS(1:length(s));
  S       = reshape( sS(length(s)+1:end), m, n );
  L       = U*diag(sProj)*V';
  rnk     = nnz( sProj );
  nuclearNorm = sum(sProj);
end

function [stepsizeL, stepsizeS]  = compute_BB_stepsize( ...
    Grad, Grad_old, L, L_old, S, S_old, BB_split, BB_type)

  if ~BB_split
      % we take a Barzilai-Borwein stepsize in the full variable
      yk  = Grad(:) - Grad_old(:);
      yk  = [yk;yk]; % to account for both variables
      sk  = [L(:) - L_old(:); S(:) - S_old(:) ];
      if BB_type == 1
          % Default. The bigger stepsize
          stepsize    = norm(sk)^2/(sk'*yk);
      elseif BB_type == 2
          stepsize    = sk'*yk/(norm(yk)^2);
      end
      [stepsizeL, stepsizeS] = deal( stepsize );

  elseif BB_split
      % treat L and S variables separately
      % Doesn't seem to work well.
      yk  = Grad(:) - Grad_old(:);
      skL  = L(:) - L_old(:);
      skS  = S(:) - S_old(:);
      if BB_type == 1
          % Default. The bigger stepsize
          stepsizeL   = norm(skL)^2/(skL'*yk);
          stepsizeS   = norm(skS)^2/(skS'*yk);
      elseif BB_type == 2
          stepsizeL   = skL'*yk/(norm(yk)^2);
          stepsizeS   = skS'*yk/(norm(yk)^2);
      end
  end
end

function op = project_l1( q , d)
%PROJECT_L1   Projection onto the scaled 1-norm ball.
%    OP = PROJECT_L1( Q ) returns an operator implementing the
%    indicator function for the 1-norm ball of radius q,
%    { X | norm( X, 1 ) <= q }. Q is optional; if omitted,
%    Q=1 is assumed. But if Q is supplied, it must be a positive
%    real scalar.
%
%    OP = PROJECT_L1( Q, D ) uses a scaled 1-norm ball of radius q,
%    { X | norm( D.*X, 1 ) <= 1 }. D should be the same size as X
%    and non-negative (some zero entries are OK).

% Note: theoretically, this can be done in O(n)
%   but in practice, worst-case O(n) median sorts are slow
%   and instead average-case O(n) median sorts are used.
%   But in matlab, the median is no faster than sort
%   (the sort is probably quicksort, O(n log n) expected, with
%    good constants, but O(n^2) worst-case).
%   So, we use the naive implementation with the sort, since
%   that is, in practice, the fastest.

 if nargin == 0,
     q = 1;
 elseif ~isnumeric( q ) || ~isreal( q ) || numel( q ) ~= 1 || q <= 0,
     error( 'Argument must be positive.' );
 end
 if nargin < 2 || isempty(d) || numel(d)==1
     if nargin>=2 && ~isempty(d)
         % d is a scalar, so norm( d*x ) <= q is same as norm(x)<=q/d
         if d==0
             error('If d==0 in proj_l1, the set is just {0}, so use proj_0');
         elseif d < 0
             error('Require d >= 0');
         end
         q = q/d;
     end
     op = @(varargin)proj_l1_q(q, varargin{:} );
 else
     if any(d<0)
         error('All entries of d must be non-negative');
     end
     op = @(varargin)proj_l1_q_d(q, d, varargin{:} );
 end

 % This is modified from TFOCS, Nov 26 2013, Stephen Becker
 % Note: removing "v" (value) output from TFOCS code
 %   Also removing extraneous inputs
    function x = proj_l1_q( q, x, varargin )
        myReshape   = @(x) x; % do nothing
        if size(x,2) > 1
            if ndims(x) > 2, error('You must modify this code to deal with tensors'); end
            myReshape     = @(y) reshape( y, size(x,1), size(x,2) );
            x   = x(:); % make it into a vector
        end
        s      = sort(abs(nonzeros(x)),'descend');
        cs     = cumsum(s);
        % ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= q, 1 );
        ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= q+2*eps(q), 1 ); % For stability
        if ~isempty( ndx )
            thresh = ( cs(ndx) - q ) / ndx;
            x      = x .* ( 1 - thresh ./ max( abs(x), thresh ) ); % May divide very small numbers
        end
        x   = myReshape(x);
    end

% Allows scaling. Added Feb 21 2014
    function x = proj_l1_q_d( q, d,  x, varargin )
        myReshape   = @(x) x; % do nothing
        if size(x,2) > 1
            if ndims(x) > 2, error('You must modify this code to deal with tensors'); end
            myReshape     = @(y) reshape( y, size(x,1), size(x,2) );
            x   = x(:); % make it into a vector
        end
        [goodInd,j,xOverD] = find( x./ d );
        [lambdas,srt]      = sort(abs(xOverD),'descend');
        s   = abs(x(goodInd).*d(goodInd));
        s   = s(srt);
        dd  = d(goodInd).^2;
        dd  = dd(srt);
        cs  = cumsum(s);
        cd  = cumsum(dd);
        ndx    = find( cs - lambdas.*cd >= q+2*eps(q), 1, 'first');
        if ~isempty( ndx )
            ndx     = ndx - 1;
            lambda  = ( cs(ndx) - q )/cd(ndx);
            x       = sign(x).*max( 0, abs(x) - lambda*d );
        end
        x   = myReshape(x);
    end

end

% Copied from TFOCS, April 17 2015
function op = project_l1l2( q, rowNorms )
%PROJ_L1L2    L1-L2 block norm: sum of L2 norms of rows.
%    OP = PROJ_L1L2( q ) implements the constraint set
%        {X | sum_{i=1:m} norm(X(i,:),2) <= 1 }
%    where X is a m x n matrix.  If n = 1, this is equivalent
%    to PROJ_L1. If m=1, this is equivalent to PROJ_L2
%
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be positive and real and a scalar.
%
%   OP = PROJ_L1L2( q, rowNorms )
%     will either do the sum of the l2-norms of rows if rowNorms=true
%       (the default), or the sum of the l2-norms of columns if
%       rowNorms = false.
%
%   Known issues: doesn't yet work with complex-valued data.
%       Should be easy to fix, so email developers if this is
%       needed for your problem.
%
% Dual: prox_linfl2.m [not yet available]
% See also prox_l1l2.m, proj_l1.m, proj_l2.m

    if nargin == 0 || isempty(q),
        q = 1;
    elseif ~isnumeric( q ) || ~isreal( q ) || any(q <= 0) ||numel(q)>1,
        error( 'Argument must be positive and a scalar.' );
    end

    if nargin<2 || isempty(rowNorms)
        rowNorms = true;
    end

    if rowNorms
        op = @(x,varargin)prox_f_rows(q,x);
    else
        op = @(x,varargin)prox_f_cols(q,x);
    end

    function X = prox_f_rows(tau,X)
        nrms    = sqrt( sum( X.^2, 2 ) );
        % When we include a row of x, corresponding to row y of Y,
        % its contribution is norm(y)-lambda
        % So we have sum_{i=1}^m max(0, norm(y_0)-lambda)
        % So, basically project nrms onto the l1 ball...
        s      = sort( nrms, 'descend' );
        cs     = cumsum(s);

        ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= tau+2*eps(tau), 1 ); % For stability

        if ~isempty( ndx )
            thresh = ( cs(ndx) - tau ) / ndx;
            % Apply to relevant rows
            d   = max( 0, 1-thresh./nrms );
            m   = size(X,1);
            X   = spdiags( d, 0, m, m )*X;
        end
    end

    function X = prox_f_cols(tau,X)
        nrms    = sqrt( sum( X.^2, 1 ) ).';
        s      = sort( nrms, 'descend' );
        cs     = cumsum(s);

        ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= tau+2*eps(tau), 1 ); % For stability

        if ~isempty( ndx )
            thresh = ( cs(ndx) - tau ) / ndx;
            d   = max( 0, 1-thresh./nrms );
            n   = size(X,2);
            X   = X*spdiags( d, 0, n,n );
        end
    end

end % end projection_l1l2.m

% Copied from TRFOCS April 17 2015
function op = prox_l1l2( q )
%PROX_L1L2    L1-L2 block norm: sum of L2 norms of rows.
%    OP = PROX_L1L2( q ) implements the nonsmooth function
%        OP(X) = q * sum_{i=1:m} norm(X(i,:),2)
%    where X is a m x n matrix.  If n = 1, this is equivalent
%    to PROX_L1
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be positive and real.
%    If Q is a vector, it must be m x 1, and in this case,
%    the weighted norm OP(X) = sum_{i} Q(i)*norm(X(i,:),2)
%    is calculated.
%
% Dual: proj_linfl2.m
% See also proj_linfl2.m, proj_l1l2.m

    if nargin == 0,
        q = 1;
    elseif ~isnumeric( q ) || ~isreal( q ) || any(q <= 0),
        error( 'Argument must be positive.' );
    end
    op = @(x,t)prox_f(q,x,t);

    function x = prox_f(q,x,t)
        if nargin < 3,
            error( 'Not enough arguments.' );
        end
        v = sqrt( sum(x.^2,2) );
        s = 1 - 1 ./ max( v ./ ( t .* q ), 1 );
        m = length(s);
        x = spdiags(s,0,m,m)*x;
    end

end

author: stephenbeckr

repo: fastRPCA


text:
%DBINV Convert from decibels.
%
% (c) 2008-2011 Daniel Halperin <dhalperi@cs.washington.edu>
%
function ret = dbinv(x)
    ret = 10.^(x/10);
end

author: linteresa

repo: WiAR

save_path: github-repos/MATLAB/linteresa-WiAR


text:
%-------------------------------------------------------------------------------------------------------------------%
%
% 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:  nickabattista@gmail.com
% 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 (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Computes the components of the force term in Navier-Stokes from
%           deformations of the boundary of the immersed boundary
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Fx, Fy, F_Mass, F_Lag, F_Poro, aggregate_list] = please_Find_Lagrangian_Forces_On_Eulerian_grid(dt, current_time, xLag, yLag,xLag_P,yLag_P, x, y, grid_Info, model_Info, springs, targets, beams, nonInv_beams, muscles, muscles3, masses, electro_potential, d_Springs, general_force,poroelastic_info, coagulation, aggregate_list, flag_Geo_Connect, geo_Connect_MAT)

%
% The components of the force are given by
% F(x,y) = int{ f(s) * delta(x - xLag(s)) * delta(y - yLag(s)) * ds }
%
% where s parameteriizes the Lagrangian structure.

% xLag:           x positions of Lagrangian structure
% yLag:           y positions of Lagrangian structure
% x:              x positions on Eulerian grid
% y:              y positions on Eulerian grid
% grid_Info:      holds lots of geometric pieces about grid / simulations
% model_Info:     Stores if springs, if update_springs, if target_pts, if update_target_pts (as 0 (no) or 1 (yes) )
% springs:        Stores Master Node, Slave Node, Spring Stiffness, Restling-Lengths, all in column vecs
% beams:          Stores 1st Node, 2nd (MIDDLE-MAIN) Node, 3rd Nodes, Beam Stiffnesses, and Beam curvatures
% targets:        Stores target point index, correponding xLag, yLag and target point stiffness
% masses:         Stores mass point index, correponding xLag, yLag, "spring" stiffness, and mass value parameter
%    .
%    .
% coagulation:    Stores coagulation data: first index of lag pt. of cell, threshold radii, fracture force, # of pts in each cell
% aggregate_list: Stores list of bonds between Lag. Pt. Indices (INDICES OF CELLS AS A WHOLE)
% flag_Geo_Connect: If User provides geometrical details about neighboring pts
% geo_Connect_MAT: Gives which LAG IDs are geometrical neighbors
% current_time:   Current time of simulation (in seconds)

% Force density is computed using a SIMPLE LINEAR SPRING model w/ resting length L.
% This leads to a force density of the form,
%               f = ( \LagPts_s * ( 1 - L / abs(\LagPts_s) )  )/ds^2
%

% Grid Info %
Nx =    grid_Info(1); % # of Eulerian pts. in x-direction
Ny =    grid_Info(2); % # of Eulerian pts. in y-direction
Lx =    grid_Info(3); % Length of Eulerian grid in x-coordinate
Ly =    grid_Info(4); % Length of Eulerian grid in y-coordinate
dx =    grid_Info(5); % Spatial-size in x
dy =    grid_Info(6); % Spatial-size in y
supp =  grid_Info(7); % Delta-function support
Nb =    grid_Info(8); % # of Lagrangian pts.
ds =    grid_Info(9); % Lagrangian spacing

% Model Potential Forces %
springs_Yes = model_Info(1);        % Springs: 0 (for no) or 1 (for yes)
target_pts_Yes = model_Info(3);     % Target_Pts: 0 (for no) or 1 (for yes)
beams_Yes = model_Info(5);          % Beams (Torsional Springs): 0 (for no) or 1 (for yes)
nonInv_beams_Yes = model_Info(7);
muscle_LT_FV_Yes = model_Info(9);   % Length-Tension/Force-Velocity Muscle: 0 (for no) or 1 (for yes) (Length/Tension - Hill Model)
muscle_3_Hill_Yes = model_Info(10); % 3-Element Hill Model: 0 (for no) or 1 (for yes) (3 Element Hill + Length-Tension/Force-Velocity)
mass_Yes = model_Info(13);          % Mass Pts: 0 (for no) or 1 (for yes)
electro_phys_Yes = model_Info(18);  % Electrophysiology (FitzHugh-Nagumo): 0 (for no) or 1 (for yes)
d_Springs_Yes = model_Info(19);     % Damped Springs: 0 (for no) or 1 (for yes)
gen_force_Yes = model_Info(23);     % General User-Defined Force: 0 (for no) or 1 (for yes)
poroelastic_Yes = model_Info(24);   % Poroelastic Media: 0 (for no) or 1 (for yes)
coagulation_Yes = model_Info(25);   % Coagulation Model: 0 (for no) or 1 (for yes)

%----------------------------------------------------------------------------------------------
% Compute MUSCLE LENGTH-TENSION/FORCE-VELOCITY (if using combined length/tension-Hill model) %
%----------------------------------------------------------------------------------------------
%
if ( ( muscle_LT_FV_Yes == 1 ) && ( electro_phys_Yes == 0 ) )
    [fx_muscles, fy_muscles] = give_Muscle_Force_Densities(Nb,xLag,yLag,xLag_P,yLag_P,muscles,current_time,dt);
elseif ( ( muscle_LT_FV_Yes == 1 ) && ( electro_phys_Yes == 1 ) )
    [fx_muscles, fy_muscles] = give_ElectroPhys_Ca_Muscle_Force_Densities(Nb,xLag,yLag,xLag_P,yLag_P,muscles,current_time,dt,electro_potential);
else
    fx_muscles = zeros(length(xLag),1);
    fy_muscles = fx_muscles;
end

%-----------------------------------------------------------------------------------------
% Compute 3-ELEMENT HILL MUSCLE MODEL FORCE DENSITIES (if using combined 3-HILL + LT/FV) %
%-----------------------------------------------------------------------------------------
%
if ( muscle_3_Hill_Yes == 1)
    [fx_muscles3, fy_muscles3] = give_3_Element_Muscle_Force_Densities(Nb,xLag,yLag,xLag_P,yLag_P,muscles3,current_time,dt);
else
    fx_muscles3 = zeros(length(xLag),1);
    fy_muscles3 = fx_muscles3;
end

%----------------------------------------------------------------------
% Compute SPRING FORCE DENSITIES (if there are springs!)
%----------------------------------------------------------------------
if ( springs_Yes == 1 )

    % Compute "Connections Matrix" for what springs are attached to whom     %
    % (and "Connections Stiffness Matrix" and "Connections Restling Lengths" %
    %connects = give_Me_Spring_Connections_Matrix(Nb,Nsprings,sp_1,sp_2,K_Vec,L_Vec);

    % Compute distances between Lag-Pts w/ Springs for Spring-Tension Calc.
    %dLag_x = give_Spring_Lagrangian_Distance(xLag, Lx, springs);
    %dLag_y = give_Spring_Lagrangian_Distance(yLag, Ly, springs);

    % Compute the Lagrangian SPRING tensions!
    %[Tx Ty] = give_Me_Spring_Lagrangian_Tension(Nb,dLag_x,dLag_y,springs);

    % Compute the Lagrangian SPRING force densities!
    [fx_springs, fy_springs] = give_Me_Spring_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,springs,Lx,Ly);

else
    fx_springs = zeros(Nb,1); %No x-forces coming from springs
    fy_springs = fx_springs;  %No y-forces coming from springs
end

%----------------------------------------------------------------------
% Compute MASS PT FORCE DENSITIES (if there are mass points!)
%----------------------------------------------------------------------
if ( mass_Yes == 1)

    % Compute the Lagrangian MASSIVE PT force densities!
    [fx_mass, fy_mass, F_Mass] = give_Me_Mass_Lagrangian_Force_Densities(ds,xLag,yLag,masses);
else
    fx_mass = zeros(Nb,1); %No x-forces coming from mass points
    fy_mass = fx_mass;     %No y-forces coming from mass points
    F_Mass = 0;            %Dummy to pass along
end

%--------------------------------------------------------------------
% Compute TARGET FORCE DENSITIES (if there are target points!)
%--------------------------------------------------------------------
if ( target_pts_Yes == 1)

    % Compute the Lagrangian TARGET force densities!
    [fx_target, fy_target] = give_Me_Target_Lagrangian_Force_Densities(ds,xLag,yLag,targets,Lx,Ly);

else
    fx_target = zeros(Nb,1); %No x-forces coming from target points
    fy_target = fx_target;   %No y-forces coming from target points
end

%------------------------------------------------------------------------
% Compute BEAM (TORSIONAL SPRINGS) FORCE DENSITIES (if there are beams!)
%------------------------------------------------------------------------
if ( beams_Yes == 1 )

    % Compute the Lagrangian BEAM (TORSIONAL SPRINGS) force densities!
    [fx_beams, fy_beams] = give_Me_Beam_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,beams,Lx,Ly);

else
    fx_beams = zeros(Nb,1); %No x-forces coming from beams
    fy_beams = fx_beams;    %No y-forces coming from beams
end

%---------------------------------------------------------------------------------
% Compute BEAM (NON-INVARIANT) FORCE DENSITIES (if there are non-invariant beams!)
%---------------------------------------------------------------------------------
if ( nonInv_beams_Yes == 1 )

    % Compute the Lagrangian NON-INVARIANT BEAM force densities!
    [fx_nonInv_beams, fy_nonInv_beams] = give_Me_nonInv_Beam_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,nonInv_beams,Lx,Ly);

else
    fx_nonInv_beams = zeros(Nb,1);        %No x-forces coming from beams
    fy_nonInv_beams = fx_nonInv_beams;    %No y-forces coming from beams
end

%----------------------------------------------------------------------
% Compute DAMPED SPRING FORCE DENSITIES (if there are damped springs!)
%----------------------------------------------------------------------
if ( d_Springs_Yes == 1 )

    % Compute the Lagrangian DAMPED SPRING force densities!
    [fx_dSprings, fy_dSprings] = give_Me_Damped_Springs_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,d_Springs,xLag_P,yLag_P,dt,Lx,Ly);

else
    fx_dSprings = zeros(Nb,1);    %No x-forces coming from damped springs
    fy_dSprings = fx_dSprings;    %No y-forces coming from damped springs
end

%---------------------------------------------------------------------------------
% Compute GENERAL USER-DEFINED FORCE DENSITIES (if there is a user-defined force!)
%---------------------------------------------------------------------------------
if ( gen_force_Yes == 1 )

    % Compute the Lagrangian GENERAL USER force densities!
    [fx_genForce, fy_genForce] = give_Me_General_User_Defined_Force_Densities(ds,Nb,xLag,yLag,xLag_P,yLag_P,dt,current_time,general_force);

else
    fx_genForce = zeros(Nb,1);    %No x-forces coming from general force model
    fy_genForce = fx_genForce;    %No y-forces coming from general force model
end

%--------------------------------------------------------------------------
% Compute COAGULATION MODEL AND FORCE DENSITIES (if there is coagulation!)
%--------------------------------------------------------------------------
if ( coagulation_Yes == 1 )

    % Compute the Lagrangian COAGULATION force densities!
    [fx_coag, fy_coag, aggregate_list] = give_Me_Coagulation_Force_Densities(Nb,xLag,yLag,coagulation,aggregate_list,Lx,Ly);

else
    fx_coag = zeros(Nb,1);   % No x-forces coming from coagulation
    fy_coag = fx_coag;       % No y-forces coming from coagulation
end

%-----------------------------------------------------
% SUM TOTAL FORCE DENSITY! %
%-----------------------------------------------------
fx = fx_springs + fx_target + fx_beams + fx_nonInv_beams + fx_muscles + fx_muscles3 + fx_mass + fx_dSprings + fx_genForce + fx_coag;
fy = fy_springs + fy_target + fy_beams + fy_nonInv_beams + fy_muscles + fy_muscles3 + fy_mass + fy_dSprings + fy_genForce + fy_coag;

%-----------------------------------------------------
% Save Poro-Elastic Forces, if poroelastic elements %
%-----------------------------------------------------
if poroelastic_Yes
    F_Poro(:,1) = fx_springs(poroelastic_info(:,1)) + fx_nonInv_beams(poroelastic_info(:,1));
    F_Poro(:,2) = fy_springs(poroelastic_info(:,1)) + fy_nonInv_beams(poroelastic_info(:,1));
else
    F_Poro = 0;
end

%----------------------------------
% SAVE LAGRANGIAN FORCES
%----------------------------------
F_Lag(:,1) = fx;
F_Lag(:,2) = fy;

%-----------------------------------------
% Give me delta-function approximations!
%-----------------------------------------
[delta_X, delta_Y] = give_Me_Delta_Function_Approximations_For_Force_Calc(x,y,grid_Info,xLag,yLag);

%------------------------------------------------------------
% Transform the force density vectors into diagonal matrices
%------------------------------------------------------------
fxds = zeros(Nb,Nb); fyds = zeros(Nb,Nb);
%
if flag_Geo_Connect % Compute -actual- distances between
                    % neighboring Lagrangian pts in geometry

    for i=1:Nb

        % compute real distances btwn LAG_i and Attached Points
        %                           and sum distances together

        % find all Lag Pts that are Lag_i's geometric neighbors
        indsVec = find(geo_Connect_MAT(:,1)==i);

        % loop to (1) distances between Lag_i and each neighbor
        %         (2) sum all such distances together
        ds_sum = 0;
        for j=1:length(indsVec)
            id1 = geo_Connect_MAT( indsVec(j),1);
            id2 = geo_Connect_MAT( indsVec(j),2);
            ds_sum = ds_sum + sqrt( ( xLag(id1,1)-xLag(id2,1) )^2 + ( yLag(id1,1)-yLag(id2,1) )^2 );
        end

        % Note: (1) 0.5 coming from Trapezoid Rule
        %       (2) ds_sum is sum of distances to neighboring pts
        fxds(i,i) = 0.5*fx(i,1)*ds_sum;
        fyds(i,i) = 0.5*fy(i,1)*ds_sum;

    end

else % Use Peskin's Constant 'ds' assumption
    for i=1:Nb
       fxds(i,i) = fx(i,1)*ds;
       fyds(i,i) = fy(i,1)*ds;
    end
end

%-----------------------------------------------------------------------
% Find Eulerian forces on grids by approximating the line integral,
%       F(x,y) = int{ f(s) delta(x - xLag(s)) delta(y - yLag(s)) ds }
%-----------------------------------------------------------------------
Fx = delta_Y * fxds * delta_X;
Fy = delta_Y * fyds * delta_X;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION computes the Lagrangian SPRING Force Densities.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx, fy] = give_Me_Spring_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,springs,Lx,Ly)

Nsprings = length(springs(:,1));  % # of Springs
sp_1 = springs(:,1);              % Initialize storage for MASTER NODE Spring Connection
sp_2 = springs(:,2);              % Initialize storage for SLAVE NODE Spring Connection
K_Vec = springs(:,3);             % Stores spring stiffness associated with each spring
RL_Vec = springs(:,4);            % Stores spring resting length associated with each spring
alpha_pow = springs(:,5);         % Degree of linearity (1=linear, >1 = non-linear)

fx = zeros(Nb,1);                 % Initialize storage for x-forces
fy = fx;                          % Initialize storage for y-forces

for i=1:Nsprings

    id_Master = sp_1(i);          % Master Node index
    id_Slave = sp_2(i);           % Slave Node index
    k_Spring = K_Vec(i);          % Spring stiffness of i-th spring
    L_r = RL_Vec(i);              % Resting length of i-th spring
    alpha = alpha_pow(i);         % Degree of linearity of i-th spring

    dx = xLag(id_Slave) - xLag(id_Master); % x-Distance btwn slave and master node
    dy = yLag(id_Slave) - yLag(id_Master); % y-Distance btwn slave and master node

    %
    % TESTING FOR LAG PT. PASSED THRU BNDRY; MAY NEED TO CHANGE TOLERANCE HERE, DEPENDENT ON APPLICATION
    %
    if abs(dx) > Lx/2
        dx = sign(dx)*( Lx - sign(dx)*dx );
    end

    if abs(dy) > Ly/2
        dy = sign(dy)*( Ly - sign(dy)*dy );
    end

    sF_x = 0.5*(alpha+1) * k_Spring * ( sqrt( dx^2 + dy^2 ) - L_r )^(alpha) * ( dx / sqrt(dx^2+dy^2) );
    sF_y = 0.5*(alpha+1) * k_Spring * ( sqrt( dx^2 + dy^2 ) - L_r )^(alpha) * ( dy / sqrt(dx^2+dy^2) );

    fx(id_Master,1) = fx(id_Master,1) + sF_x;  % Sum total forces for node, i in x-direction (this is MASTER node for this spring)
    fy(id_Master,1) = fy(id_Master,1) + sF_y;  % Sum total forces for node, i in y-direction (this is MASTER node for this spring)

    fx(id_Slave,1) = fx(id_Slave,1) - sF_x;    % Sum total forces for node, i in x-direction (this is SLAVE node for this spring)
    fy(id_Slave,1) = fy(id_Slave,1) - sF_y;    % Sum total forces for node, i in y-direction (this is SLAVE node for this spring)

end

% MIGHT NOT NEED THESE!
%fx = fx/ds^2;
%fy = fy/ds^2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION computes the Lagrangian **DAMPED** SPRING Force Densities .
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx, fy] = give_Me_Damped_Springs_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,d_Springs,xLag_P,yLag_P,dt,Lx,Ly)

Ndsprings = length(d_Springs(:,1));  % # of DAMPED Springs
sp_1 = d_Springs(:,1);               % Initialize storage for MASTER NODE Spring Connection
sp_2 = d_Springs(:,2);               % Initialize storage for SLAVE NODE Spring Connection
K_Vec = d_Springs(:,3);              % Stores spring stiffness associated with each spring
RL_Vec = d_Springs(:,4);             % Stores spring resting length associated with each spring
b_Vec = d_Springs(:,5);              % Damping coefficient

fx = zeros(Nb,1);                    % Initialize storage for x-forces
fy = fx;                             % Initialize storage for y-forces

for i=1:Ndsprings

    id_Master = sp_1(i);          % Master Node index
    id_Slave = sp_2(i);           % Slave Node index
    k_Spring = K_Vec(i);          % Spring stiffness of i-th spring
    L_r = RL_Vec(i);              % Resting length of i-th spring
    b = b_Vec(i);                 % Damping Coefficient

    dx = xLag(id_Slave) - xLag(id_Master);      % x-Distance btwn slave and master node
    dy = yLag(id_Slave) - yLag(id_Master);      % y-Distance btwn slave and master node

    %
    % TESTING FOR LAG PT. PASSED THRU BNDRY; MAY NEED TO CHANGE TOLERANCE HERE, DEPENDENT ON APPLICATION
    %
    if abs(dx) > Lx/2
       dx = sign(dx)*( Lx - sign(dx)*dx );
    end

    if abs(dy) > Lx/2
        dy = sign(dy)*( Ly - sign(dy)*dy );
    end

    dV_x = ( xLag(id_Master) - xLag_P(id_Master) ); % dt*(x-Velocity) between current and prev. steps
    dV_y = ( yLag(id_Master) - yLag_P(id_Master) ); % dt*(y-Velocity) between current and prev. steps

    %
    % TESTING FOR LAG PT. PASSED THRU BNDRY; MAY NEED TO CHANGE TOLERANCE HERE, DEPENDENT ON APPLICATION
    %
    if abs(dV_x) > Lx/2
       dV_x = sign(dV_x)*( Lx - sign(dV_x)*dV_x );
    end

    if abs(dV_y) > Lx/2
        dV_y = sign(dV_y)*( Ly - sign(dV_y)*dV_y );
    end

    dV_x = ( dV_x ) / dt ; % x-Velocity between current and prev. steps
    dV_y = ( dV_y ) / dt ; % y-Velocity between current and prev. steps

    sF_x = k_Spring * ( sqrt( dx^2 + dy^2 ) - L_r ) * ( dx / sqrt(dx^2+dy^2) ) - b*dV_x; %added negative for testing
    sF_y = k_Spring * ( sqrt( dx^2 + dy^2 ) - L_r ) * ( dy / sqrt(dx^2+dy^2) ) - b*dV_y;

    fx(id_Master,1) = fx(id_Master,1) + sF_x ;  % Sum total forces for node, i in x-direction (this is MASTER node for this spring)
    fy(id_Master,1) = fy(id_Master,1) + sF_y ;  % Sum total forces for node, i in y-direction (this is MASTER node for this spring)

    fx(id_Slave,1) = fx(id_Slave,1) - sF_x ;    % Sum total forces for node, i in x-direction (this is SLAVE node for this spring)
    fy(id_Slave,1) = fy(id_Slave,1) - sF_y ;    % Sum total forces for node, i in y-direction (this is SLAVE node for this spring)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION computes the Lagrangian MUSCLE Force Densities for LENGTH-TENSION/FORCE-VELOCITY MUSCLE MODEL.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx,fy] = give_ElectroPhys_Ca_Muscle_Force_Densities(Nb,xLag,yLag,xLag_P,yLag_P,muscles,current_time,dt,electro_potential)

Nmuscles = length(muscles(:,1));  % # of Muscles
m_1 = muscles(:,1);               % Initialize storage for MASTER NODE Muscle Connection
m_2 = muscles(:,2);               % Initialize storage for SLAVE NODE Muscle Connection
LFO_Vec = muscles(:,3);           % Stores length for max. muscle tension
SK_Vec = muscles(:,4);            % Stores muscle constant
a_Vec = muscles(:,5);             % Stores Hill Parameter, a
b_Vec = muscles(:,6);             % Stores Hill Parameter, b
FMAX_Vec = muscles(:,7);          % Stores Force-Maximum for Muscle

fx = zeros(Nb,1);                 % Initialize storage for x-forces
fy = fx;                          % Initialize storage for y-forces

ct = current_time/dt;             % gives count of time-steps for simulation

for i=1:Nmuscles

    id_Master = m_1(i);          % Master Node index for i-th muscle
    id_Slave = m_2(i);           % Slave Node index for i-th muscle
    LFO = LFO_Vec(i);            % Length for max. muscle tension for i-th muscle
    sk = SK_Vec(i);              % Muscle constant for i-th muscle
    a = a_Vec(i);                % Hill parameter, a, for i-th muscle
    b = b_Vec(i);                % Hill parameter, b, for i-th muscle
    Fmax = FMAX_Vec(i);          % Force-Maximum for i-th muscle

    %xPt = xLag( id_Master );     % x-Pt of interest at the moment to drive muscle contraction

    dx = xLag(id_Slave) - xLag(id_Master); % x-Distance btwn slave and master node
    dy = yLag(id_Slave) - yLag(id_Master); % y-Distance btwn slave and master node
    LF = sqrt( dx^2 + dy^2 );              % Euclidean DISTANCE between master and slave node

    dx_P = xLag_P(id_Slave) - xLag_P(id_Master); % x-Distance btwn slave and master node
    dy_P = yLag_P(id_Slave) - yLag_P(id_Master); % y-Distance btwn slave and master node
    LF_P = sqrt( dx_P^2 + dy_P^2 );              % Euclidean DISTANCE between master and slave node

    v =  abs(LF-LF_P)/dt;        % How fast the muscle is contracting/expanding

    % Find actual muscle activation magnitude
    Fm = give_Ca_ElectroPhys_Muscle_Activation(v,LF,LFO,sk,a,b,Fmax,ct,id_Master,electro_potential);

    mF_x = Fm*(dx/LF);           % cos(theta) = dx / LF;
    mF_y = Fm*(dy/LF);           % sin(theta) = dy / LF;

    fx(id_Master,1) = fx(id_Master,1) + mF_x;  % Sum total forces for node, i in x-direction (this is MASTER node for this spring)
    fy(id_Master,1) = fy(id_Master,1) + mF_y;  % Sum total forces for node, i in y-direction (this is MASTER node for this spring)

    fx(id_Slave,1) = fx(id_Slave,1) - mF_x;    % Sum total forces for node, i in x-direction (this is SLAVE node for this spring)
    fy(id_Slave,1) = fy(id_Slave,1) - mF_y;    % Sum total forces for node, i in y-direction (this is SLAVE node for this spring)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION computes the Lagrangian MUSCLE Force Densities for LENGTH-TENSION/FORCE-VELOCITY MUSCLE MODEL.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx,fy] = give_Muscle_Force_Densities(Nb,xLag,yLag,xLag_P,yLag_P,muscles,current_time,dt)

Nmuscles = length(muscles(:,1));  % # of Muscles
m_1 = muscles(:,1);               % Initialize storage for MASTER NODE Muscle Connection
m_2 = muscles(:,2);               % Initialize storage for SLAVE NODE Muscle Connection
LFO_Vec = muscles(:,3);           % Stores length for max. muscle tension
SK_Vec = muscles(:,4);            % Stores muscle constant
a_Vec = muscles(:,5);             % Stores Hill Parameter, a
b_Vec = muscles(:,6);             % Stores Hill Parameter, b
FMAX_Vec = muscles(:,7);          % Stores Force-Maximum for Muscle

fx = zeros(Nb,1);                 % Initialize storage for x-forces
fy = fx;                          % Initialize storage for y-forces

for i=1:Nmuscles

    id_Master = m_1(i);          % Master Node index for i-th muscle
    id_Slave = m_2(i);           % Slave Node index for i-th muscle
    LFO = LFO_Vec(i);            % Length for max. muscle tension for i-th muscle
    sk = SK_Vec(i);              % Muscle constant for i-th muscle
    a = a_Vec(i);                % Hill parameter, a, for i-th muscle
    b = b_Vec(i);                % Hill parameter, b, for i-th muscle
    Fmax = FMAX_Vec(i);          % Force-Maximum for i-th muscle

    xPt = xLag( id_Master );     % x-Pt of interest at the moment to drive muscle contraction

    dx = xLag(id_Slave) - xLag(id_Master); % x-Distance btwn slave and master node
    dy = yLag(id_Slave) - yLag(id_Master); % y-Distance btwn slave and master node
    LF = sqrt( dx^2 + dy^2 );              % Euclidean DISTANCE between master and slave node

    dx_P = xLag_P(id_Slave) - xLag_P(id_Master); % x-Distance btwn slave and master node
    dy_P = yLag_P(id_Slave) - yLag_P(id_Master); % y-Distance btwn slave and master node
    LF_P = sqrt( dx_P^2 + dy_P^2 );              % Euclidean DISTANCE between master and slave node

    v =  abs(LF-LF_P)/dt;        % How fast the muscle is contracting/expanding

    % Find actual muscle activation magnitude
    Fm = give_Muscle_Activation(v,LF,LFO,sk,a,b,Fmax,current_time,xPt,xLag);

    mF_x = Fm*(dx/LF);           % cos(theta) = dx / LF;
    mF_y = Fm*(dy/LF);           % sin(theta) = dy / LF;

    fx(id_Master,1) = fx(id_Master,1) + mF_x;  % Sum total forces for node, i in x-direction (this is MASTER node for this spring)
    fy(id_Master,1) = fy(id_Master,1) + mF_y;  % Sum total forces for node, i in y-direction (this is MASTER node for this spring)

    fx(id_Slave,1) = fx(id_Slave,1) - mF_x;    % Sum total forces for node, i in x-direction (this is SLAVE node for this spring)
    fy(id_Slave,1) = fy(id_Slave,1) - mF_y;    % Sum total forces for node, i in y-direction (this is SLAVE node for this spring)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the Lagrangian MUSCLE Force Densities for 3-ELEMENT
%           HILL MODEL! ( coupled w/ force-velocity/length-tension model for
%           contractile element!)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx,fy] = give_3_Element_Muscle_Force_Densities(Nb,xLag,yLag,xLag_P,yLag_P,muscles3,current_time,dt)

Nmuscles = length(muscles3(:,1));  % # of Muscles
m_1 = muscles3(:,1);               % Initialize storage for MASTER NODE Muscle Connection
m_2 = muscles3(:,2);               % Initialize storage for SLAVE NODE Muscle Connection
LFO_Vec = muscles3(:,3);           % Stores length for max. muscle tension
SK_Vec = muscles3(:,4);            % Stores muscle constant
a_Vec = muscles3(:,5);             % Stores Hill Parameter, a
b_Vec = muscles3(:,6);             % Stores Hill Parameter, b
FMAX_Vec = muscles3(:,7);          % Stores Force-Maximum for Muscle
kSpr_Vec = muscles3(:,8);          % Stores Spring Coeffs
alpha_pow= muscles3(:,9);          % Stores deg. of non-linearity for springs

fx = zeros(Nb,1);                 % Initialize storage for x-forces
fy = fx;                          % Initialize storage for y-forces

for i=1:Nmuscles

    id_Master = m_1(i);          % Master Node index for i-th muscle
    id_Slave = m_2(i);           % Slave Node index for i-th muscle
    LFO = LFO_Vec(i);            % Length for max. muscle tension for i-th muscle
    sk = SK_Vec(i);              % Muscle constant for i-th muscle
    a = a_Vec(i);                % Hill parameter, a, for i-th muscle
    b = b_Vec(i);                % Hill parameter, b, for i-th muscle
    Fmax = FMAX_Vec(i);          % Force-Maximum for i-th muscle
    kSpr = kSpr_Vec(i);          % Spring coefficient for PARALLEL ELEMENT NL-spring
    alpha = alpha_pow(i);        % Degree of linearity of PARALLEL ELEMENT i-th spring

    xPt = xLag( id_Master );     % x-Pt of interest at the moment to drive muscle contraction

    dx = xLag(id_Slave) - xLag(id_Master); % x-Distance btwn slave and master node
    dy = yLag(id_Slave) - yLag(id_Master); % y-Distance btwn slave and master node
    LF = sqrt( dx^2 + dy^2 );              % Euclidean DISTANCE between master and slave node

    dx_P = xLag_P(id_Slave) - xLag_P(id_Master); % x-Distance btwn slave and master node
    dy_P = yLag_P(id_Slave) - yLag_P(id_Master); % y-Distance btwn slave and master node
    LF_P = sqrt( dx_P^2 + dy_P^2 );              % Euclidean DISTANCE between master and slave node

    v =  abs(LF-LF_P)/dt;        % How fast the muscle is contracting/expanding

    % Find actual muscle activation magnitude for CONTRACTILE ELEMENT
    [Fm,on_Parallel,PE_Coeff,af_Val] = give_3_Element_Muscle_Activation(v,LF,LFO,sk,a,b,Fmax,current_time,xPt,xLag,i);

    % Compute muscle force from CONTRACTILE ELEMENT in each direction
    mF_x = Fm*(dx/LF);           % cos(theta) = dx / LF;
    mF_y = Fm*(dy/LF);           % sin(theta) = dy / LF;

    % Find muscle force from SERIES ELEMENT in each direction
    bDamp = 1.0;
    dV_x = ( xLag(id_Master) - xLag_P(id_Master) )/dt;      % Compute velocity gradient terms for damping
    dV_y = ( yLag(id_Master) - yLag_P(id_Master) )/dt;      % Compute velocity gradient terms for damping
    sF_SE_x = af_Val * kSpr * ( (LFO-LF) - LFO ) * ( dx / sqrt(dx^2+dy^2) ) + bDamp*dV_x; % Note: L_con (LF) + L_ser = L_tot = LFO
    sF_SE_y = af_Val * kSpr * ( (LFO-LF) - LFO ) * ( dy / sqrt(dx^2+dy^2) ) + bDamp*dV_y;

    if on_Parallel == 1
        % Find force from series element (tendons)
        sF_PE_x = PE_Coeff * 0.5*(alpha+1) * kSpr * ( sqrt( dx^2 + dy^2 ) - LFO )^(alpha) * ( dx / sqrt(dx^2+dy^2) );
        sF_PE_y = PE_Coeff * 0.5*(alpha+1) * kSpr * ( sqrt( dx^2 + dy^2 ) - LFO )^(alpha) * ( dy / sqrt(dx^2+dy^2) );
    else
        % Parallel Element is off
        sF_PE_x = 0;
        sF_PE_y = 0;
    end

    fx(id_Master,1) = fx(id_Master,1) + mF_x + sF_SE_x + sF_PE_x;  % Sum total forces for node, i in x-direction (this is MASTER node for this spring)
    fy(id_Master,1) = fy(id_Master,1) + mF_y + sF_SE_y + sF_PE_y;  % Sum total forces for node, i in y-direction (this is MASTER node for this spring)

    fx(id_Slave,1) = fx(id_Slave,1) - mF_x - sF_SE_x - sF_PE_x;    % Sum total forces for node, i in x-direction (this is SLAVE node for this spring)
    fy(id_Slave,1) = fy(id_Slave,1) - mF_y - sF_SE_y - sF_PE_y;    % Sum total forces for node, i in y-direction (this is SLAVE node for this spring)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FUNCTION computes the Lagrangian BEAM (NON-INVARIANT) Force Densities
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx, fy] = give_Me_nonInv_Beam_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,beams,Lx,Ly)

Nbeams = length(beams(:,1));     % # of Beams
pts_1 = beams(:,1);              % Initialize storage for 1ST NODE for BEAM
pts_2 = beams(:,2);              % Initialize storage for MIDDLE NODE (2ND Node) for BEAM
pts_3 = beams(:,3);              % Initialize storage for 3RD NODE for BEAM
K_Vec = beams(:,4);              % Stores beam stiffness associated with each beam
CX_Vec = beams(:,5);             % Stores beam curvature in x-direction
CY_Vec = beams(:,6);             % Stores beam curvature in y-direction

fx = zeros(Nb,1);                % Initialize storage for x-forces
fy = fx;                         % Initialize storage for y-forces

for i=1:Nbeams

    id_1 = pts_1(i);          % 1ST Node index
    id_2 = pts_2(i);          % (MIDDLE) 2nd Node index -> index that gets force applied to it!
    id_3 = pts_3(i);          % 3RD Node index
    k_Beam = K_Vec(i);        % Beam stiffness of i-th spring
    Cx = CX_Vec(i) ;          % x-Curvature of the beam between these three nodes
    Cy = CY_Vec(i) ;          % y-Curvature of the beam between these three nodes

    Xp = xLag(id_1);          % xPt of 1ST Node Pt. in beam
    Xq = xLag(id_2);          % xPt of 2ND (MIDDLE) Node Pt. in beam
    Xr = xLag(id_3);          % xPt of 3RD Node Pt. in beam

    Yp = yLag(id_1);          % yPt of 1ST Node Pt. in beam
    Yq = yLag(id_2);          % yPt of 2ND (MIDDLE) Node Pt. in beam
    Yr = yLag(id_3);          % yPt of 3RD Node Pt. in beam

    % Checks if Lag. Pts. have passed through the boundary and translates appropriately
    [Xp,Xq,Xr] = check_If_Beam_Points_Pass_Through_Boundary(ds,Lx,Xp,Xq,Xr);
    [Yp,Yq,Yr] = check_If_Beam_Points_Pass_Through_Boundary(ds,Ly,Yp,Yq,Yr);

    % CALCULATE BENDING IN X
    fx(id_3,1) = fx(id_3,1) -   k_Beam*( Xr - 2*Xq + Xp - Cx);
    fx(id_1,1) = fx(id_1,1) -   k_Beam*( Xr - 2*Xq + Xp - Cx);
    fx(id_2,1) = fx(id_2,1) + 2*k_Beam*( Xr - 2*Xq + Xp - Cx );

    % CALCULATE BENDING IN Y
    fy(id_3,1) = fy(id_3,1) -   k_Beam*( Yr - 2*Yq + Yp - Cy );
    fy(id_1,1) = fy(id_1,1) -   k_Beam*( Yr - 2*Yq + Yp - Cy );
    fy(id_2,1) = fy(id_2,1) + 2*k_Beam*( Yr - 2*Yq + Yp - Cy );

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION computes the Lagrangian BEAM (TORSIONAL SPRING) Force Densities
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx, fy] = give_Me_Beam_Lagrangian_Force_Densities(ds,Nb,xLag,yLag,beams,Lx,Ly)

Nbeams = length(beams(:,1));     % # of Beams
pts_1 = beams(:,1);              % Initialize storage for 1ST NODE for BEAM
pts_2 = beams(:,2);              % Initialize storage for MIDDLE NODE (2ND Node) for BEAM
pts_3 = beams(:,3);              % Initialize storage for 3RD NODE for BEAM
K_Vec = beams(:,4);              % Stores spring stiffness associated with each spring
C_Vec = beams(:,5);              % Stores spring resting length associated with each spring

fx = zeros(Nb,1);                % Initialize storage for x-forces
fy = fx;                         % Initialize storage for y-forces

for i=1:Nbeams

    id_1 = pts_1(i);          % 1ST Node index
    id_2 = pts_2(i);          % (MIDDLE) 2nd Node index -> index that gets force applied to it!
    id_3 = pts_3(i);          % 3RD Node index
    k_Beam = K_Vec(i);        % Beam stiffness of i-th spring
    C = C_Vec(i) ;            % Curvature of the beam between these three nodes

    Xp = xLag(id_1);          % xPt of 1ST Node Pt. in beam
    Xq = xLag(id_2);          % xPt of 2ND (MIDDLE) Node Pt. in beam
    Xr = xLag(id_3);          % xPt of 3RD Node Pt. in beam

    Yp = yLag(id_1);          % yPt of 1ST Node Pt. in beam
    Yq = yLag(id_2);          % yPt of 2ND (MIDDLE) Node Pt. in beam
    Yr = yLag(id_3);          % yPt of 3RD Node Pt. in beam

    % Checks if Lag. Pts. have passed through the boundary and translates appropriately
    [Xp,Xq,Xr] = check_If_Beam_Points_Pass_Through_Boundary(ds,Lx,Xp,Xq,Xr);
    [Yp,Yq,Yr] = check_If_Beam_Points_Pass_Through_Boundary(ds,Ly,Yp,Yq,Yr);

    % Compute Cross-Product
    cross_prod = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp);

    % FORCES FOR LEFT NODE
    bF_x_L = -k_Beam * ( cross_prod - C ) * ( Yr-Yq );
    bF_y_L =  k_Beam * ( cross_prod - C ) * ( Xr-Xq );

    % FORCES FOR MIDDLE NODE
    bF_x_M =  k_Beam * ( cross_prod - C ) * (  (Yq-Yp) + (Yr-Yq) );
    bF_y_M = -k_Beam * ( cross_prod - C ) * (  (Xr-Xq) + (Xq-Xp) );

    % FORCES FOR RIGHT NODE
    bF_x_R = -k_Beam * ( cross_prod - C ) * ( Yq-Yp );
    bF_y_R =  k_Beam * ( cross_prod - C ) * ( Xq-Xp );

    fx(id_1,1) = fx(id_1,1) - bF_x_L;  % Sum total forces for left node, in x-direction (this is LEFT node for this beam)
    fy(id_1,1) = fy(id_1,1) - bF_y_L;  % Sum total forces for left node, in y-direction (this is LEFT node for this beam)

    fx(id_2,1) = fx(id_2,1) + bF_x_M;  % Sum total forces for middle node, in x-direction (this is MIDDLE node for this beam)
    fy(id_2,1) = fy(id_2,1) + bF_y_M;  % Sum total forces for middle node, in y-direction (this is MIDDLE node for this beam)

    fx(id_3,1) = fx(id_3,1) - bF_x_R;  % Sum total forces for right node, in x-direction (this is RIGHT node for this beam)
    fy(id_3,1) = fy(id_3,1) - bF_y_R;  % Sum total forces for right node, in y-direction (this is RIGHT node for this beam)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the Target-Pt Force Densities!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx_mass, fy_mass, F_Mass] = give_Me_Mass_Lagrangian_Force_Densities(ds,xLag,yLag,masses)

IDs = masses(:,1);                 % Stores Lag-Pt IDs in col vector
xPts= masses(:,2);                 % Original x-Values of x-Mass Pts.
yPts= masses(:,3);                 % Original y-Values of y-Mass Pts.
kStiffs = masses(:,4);             % Stores "spring" stiffness parameter

N_masses = length(IDs);            % # of target points!

fx = zeros(length(xLag),1);         % Initialize storage for x-force density from TARGET PTS
fy = fx;                            % Initialize storage for y-force density from TARGET PTS

for i=1:N_masses

    fx(IDs(i),1) = fx(IDs(i),1) + kStiffs(i)*( xPts(i) - xLag(IDs(i)) );
    fy(IDs(i),1) = fy(IDs(i),1) + kStiffs(i)*( yPts(i) - yLag(IDs(i)) );

end

fx_mass = fx;
fy_mass = fy;

F_Mass(:,1) = fx;  % Store for updating massive boundary pts
F_Mass(:,2) = fy;  % Store for updating massive boundary pts

% MIGHT NOT NEED THESE!
%fx_target = fx/ds^2;
%fy_target = fy/ds^2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the Target-Pt Force Densities!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fx_target, fy_target] = give_Me_Target_Lagrangian_Force_Densities(ds,xLag,yLag,targets,Lx,Ly)

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

N_targets = length(IDs);            % # of target points!

fx = zeros(length(xLag),1);         % Initialize storage for x-force density from TARGET PTS
fy = fx;                            % Initialize storage for y-force density from TARGET PTS

for i=1:N_targets

    dx = xPts(i) - xLag(IDs(i)); % x-Distance btwn Lag Pt. and Virtual pt
    dy = yPts(i) - yLag(IDs(i)); % y-Distance btwn Lag Pt. and Virtual pt

    %
    % TESTING FOR LAG PT. PASSED THRU BNDRY; MAY NEED TO CHANGE TOLERANCE HERE, DEPENDENT ON APPLICATION
    %
    if abs(dx) > Lx/2
        dx = sign(dx)*( Lx - sign(dx)*dx );
    end

    if abs(dy) > Ly/2
        dy = sign(dy)*( Ly - sign(dy)*dy );
    end

    fx(IDs(i),1) = fx(IDs(i),1) + kStiffs(i)*( dx );
    fy(IDs(i),1) = fy(IDs(i),1) + kStiffs(i)*( dy );

end

fx_target = fx;
fy_target = fy;

% MIGHT NOT NEED THESE!
%fx_target = fx/ds^2;
%fy_target = fy/ds^2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION computes the Delta-Function Approximations
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [delta_X delta_Y] = give_Me_Delta_Function_Approximations_For_Force_Calc(x,y,grid_Info,xLag,yLag)

% Grid Info
Nx =   grid_Info(1);
Ny =   grid_Info(2);
Lx =   grid_Info(3);
Ly =   grid_Info(4);
dx =   grid_Info(5);
dy =   grid_Info(6);
supp = grid_Info(7);
Nb =   grid_Info(8);

% Find the indices of the points (xi, yj) where the 1D delta functions are non-zero in EULERIAN FRAME
indX = give_1D_NonZero_Delta_Indices(xLag, Nx, dx, supp);
indY = give_1D_NonZero_Delta_Indices(yLag, Ny, dy, supp)';

% Matrix of possible indices, augmented by "supp"-copies to perform subtractions later in LAGRANGIAN FRAME
indLagAux = [1:1:Nb]';
ind_Lag = [];
for i=1:supp
   ind_Lag = [ind_Lag indLagAux];
end

% Compute distance between Eulerian Pts. and Lagrangian Pts. by passing correct indices for each
try
    distX = give_Eulerian_Lagrangian_Distance(x(indX),xLag(ind_Lag),Lx);
    distY = give_Eulerian_Lagrangian_Distance(y(indY),yLag(ind_Lag'),Ly);
catch
    fprintf('\n\n\n - ERROR - \n');
    fprintf('\n\n - ERROR ERROR - \n');
    fprintf('\n\n - ERROR ERROR ERROR - \n');
    fprintf('\n\n - ERROR ERROR ERROR ERROR - \n\n\n');
    error('BLOW UP! (*forces TOO large*) -> try decreasing the time-step or decreasing material property stiffnesses');
end

% Initialize delta_X and delta_Y matrices for storing delta-function info for each Lag. Pt.
delta_X = zeros(Nb, Nx);
delta_Y = zeros(Ny, Nb);

delta_1D_x = give_Delta_Kernel(distX, dx);
delta_1D_y = give_Delta_Kernel(distY, dy);

[row,col] = size(ind_Lag);
for i=1:row
    for j=1:col

        % Get Eulerian/Lagrangian indices to use for saving non-zero delta-function values
        xID = indX(i,j);
        indy= ind_Lag(i,j);
        yID = indY(j,i);

        % Store non-zero delta-function values into delta_X / delta_Y matrices at correct indices!
        delta_X(indy,xID) = delta_1D_x(i,j);
        delta_Y(yID,indy) = delta_1D_y(j,i);

    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION CHECK if BEAM points have passed through boundary, and translates them accordingly for calculation
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Xp_N,Xq_N,Xr_N] = check_If_Beam_Points_Pass_Through_Boundary(ds,Lx,Xp,Xq,Xr)

    % CHECKS FOR IF POINTS PASSED THRU BNDRY
    dX_pq = ( Xp - Xq );
    dX_qr = ( Xq - Xr );

    if abs(dX_pq) > 5*ds
       %if abs(dX_pq) > abs(dX_qr)

            % MEANS point p has moved thru; check if moved through right/left bndry
            if dX_pq < 0
                Xp_N = Lx + Xp;
            else
                Xp_N = -Lx+Xp;
            end
            Xq_N = Xq;
            Xr_N = Xr;
       %else
            % MEANS point q has moved thru; check if moved through right/left bndry
            %if dX_pq > 0
            %    Xq_N = Lx + Xq;
            %else
            %    Xq_N = -Xq;
            %end
            %Xp_N = Xp;
            %Xr_N = Xr;
       %end

%        fprintf('\n\n\n\n');
%        Xp
%        Xq
%        Xr
%        Xp_N
%        Xq_N
%        Xr_N
%        pause();

    else
       Xp_N = Xp;
       Xq_N = Xq;
       Xr_N = Xr;
    end

    if abs(dX_qr) > 5*ds
       %if abs(dX_qr) > abs(dX_pq)
            % MEANS point r has moved thru; check if moved through right/left bndry
            if dX_qr < 0
                Xr_N = -Lx+Xr;
            else
                Xr_N = Lx + Xr;
            end
            Xq_N = Xq;
            if abs(dX_pq) < 5*ds
                Xp_N = Xp;
            end
%        else
%             % MEANS point q has moved thru; check if moved through right/left bndry
%             if dX_qr > 0
%                 Xq_N = Lx + Xq;
%             else
%                 Xq_N = -Xq;
%             end
%             Xr_N = Xr;
%             if abs(dX_pq) < 5*ds
%                 Xp_N = Xp;
%             end
       %end

%        fprintf('\n\n\n\n');
%        Xp
%        Xq
%        Xr
%        Xp_N
%        Xq_N
%        Xr_N
%        pause();

    end

author: nickabattista


text:
%compute vy_cm
function [data,units]=compute_vy_cm(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
vy_cm=cell(1,numlarvae);

for i=1:numlarvae
    larva=larvae(i);
    vy_cm{1,i}=(trx(larva).y_mm(2:end)-trx(larva).y_mm(1:end-1))./trx(larva).dt;
end

units=parseunits('mm/s');
data=vy_cm;

author: kristinbranson

repo: JAABA


text:
function [newTS,newTBase] = mrSliceTiming(ts,frameAdjustment,method)
% Adjust the time series of a slice/scan based on the slice position
%
%   [newTS,newTBase] = mrSliceTiming(ts,frameAdjustment,method)
%
% This function takes in the time series (ts) and recreates a new time
% series at each voxel.  It calculates the time at which the ts was
% actually measured and then interpolates to a new time frame.  All of the
% different slices are interpolated to the same time frame, which is the
% basis of slice timing adjustment.  The adjustment to all of the slices is
% done by calling this routine repeated from AdjustSliceTiming.
%
% This is  the core routine for time series slice timing adjustment.
% AdjustSliceTiming has overhead associated with creating the data type and
% so forth.  This routine is just the calculation.
%
% ts:               Time series from a single slice within a scan
% frameAdjustment:  The fraction of the frame (TR) this time series must
%                   be resampled (not in seconds)
% method:           'linear' or 'spline'
%
% Slice timing adjustment smooths noisy time series a little bit.  The
% linear choice smooths more than the spline choice.
%
% Example:
%   We don't call this routine on its own.  We call it from the
%   wrapper AdjustSliceTiming().
%

if notDefined('ts'), error('Time series required'); end
if notDefined('frameAdjustment'), error('frameAdjustment value required'); end
if notDefined('method'), method = 'spline'; end

% get nFrames, the number of time samples, from data
nFrames    = size(ts,1);

% Pad the with a replication of the first and last frames to deal with
% extrapolation:
ts = [ts(1, :); ts; ts(nFrames, :)];

% deal w/ NaNs. These may occur due to motion correction, for example.
% For now, replace with zero. We should probably replace with the mean of
% the neighbors at some point.
%
% nanInds = find(isnan(ts));
% ts(nanInds) = 0;
ts(isnan(ts)) = 0;

% These are the time samples for the amended (padded) ts.  We don't
% bother multiplying by TR, though of course we could.
tBaseRef = (0:nFrames+1);

% If the timing between frames is deltaFrame, and the difference in slice
% between this one and the standard slice is refSlice - slice, then we need
% to adjust the times between this slice and the reference as here.  This
% calculation should be replaced by a routine that includes the slice
% ordering; the slice ordering should be saved!
% newTBase = (1:nFrames) + deltaFrame * (refSlice - slice);
newTBase = (1:nFrames) + frameAdjustment;

switch lower(method)
    case 'spline'
        % Ress used spline temporal interpolation routine
        newTS = spline(tBaseRef, ts', newTBase)';
    case 'linear'
        % This should be an option, as well as others.
        newTS = interp1(tBaseRef(:), ts, newTBase(:));
    otherwise
        error('Undefined method %s\n',method);
end

return

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
% Lorenz Attractor equations solved by ODE Solve
%% x' = sigma*(y-x)
%% y' = x*(rho - z) - y
%% z' = x*y - beta*z
function dx = lorenzatt(X)
    rho = 28; sigma = 10; beta = 8/3;
    dx = zeros(3,1);
    dx(1) = sigma*(X(2) - X(1));
    dx(2) = X(1)*(rho - X(3)) - X(2);
    dx(3) = X(1)*X(2) - beta*X(3);
    return
end

repo: Aero-matlab


text:
function H = normalize_H(H, type)
% function H = normalize_H(H, type)
%
% Normalize rows of H using type which can be:
%  1   - use 1-norm [default]
%  2   - use 2-norm
%  k   - multiply the 1-norm by k
%  'a' - means make sum(H(:))=1
%
% 2010-01-14 Graham Grindlay (grindlay@ee.columbia.edu)

% Copyright (C) 2008-2028 Graham Grindlay (grindlay@ee.columbia.edu)
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

if nargin < 2
    type = 0;
end

switch type
    case 1
        for i = 1:size(H,1)
            H(i,:) = H(i,:) ./ norm(H(i,:),1);
        end

    case 2
        for i = 1:size(H,1)
            H(i,:) = H(i,:) ./ norm(H(i,:),2);
        end

    case 'a'
        H = H./sum(H(:));

    case 0

    otherwise
        for i = 1:size(H,1)
            H(i,:) = type*H(i,:) ./ norm(H(i,:),1);
        end
end

author: hiroyuki-kasai

repo: NMFLibrary


text:
function [rc,var,ASAcontrol] = burg_s(f,b,max_order,last)
%BURG_S Burg type AR estimator for multiple segments
%   [RC,VAR] = BURG_S(SIG_MTX,MAX_ORDER) estimates a single vector of AR-
%   reflectioncoefficients RC up to order MAX_ORDER, using data from
%   multiple segments simultaneously. The data segments must be of equal
%   length and arranged columnwise in SIG_MTX. VAR is the estimated
%   variance based on all elements of SIG_MTX.
%
%   RC = BURG_S(F,B,ADD_ORDER) estimates ADD_ORDER additional
%   reflectioncoefficients from matrices of forward and backward
%   residuals F and B, that have been internally used in a previous Burg
%   estimation procedure.
%
%   Since forward and backward residual vectors are internally assigned
%   to the ASAglob variables 'ASAglob_final_f' and 'ASAglob_final_b', an
%   appropriate F and B can be retrieved after the program has finished
%   execution. This is most easily done by typing:
%     F = ASAglobretr('ASAglob_final_f');
%     B = ASAglobretr('ASAglob_final_b');
%
%   BURG_S is an ARMASA main function.
%
%   See also: BURG, ASAGLOB, ASAGLOBRETR.

%   References: S. de Waele and P. M. T. Broersen, The Burg Algorithm for
%               Segments, IEEE Transactions on Signal Processing,
%               vol. 48, no. 10, pp. 2876-2880, October 2000.
%               W. Wunderink, The Introduction of a Computationally
%               Efficient Burg Estimator Algorithm,
%               Tech. Report SSC-March00-1, Delft University of
%               Technology, Department of Applied Physics, Systems
%               Signals and Control Group, the Netherlands, March 2000.

%Header
%==============================================================================

%Declaration of variables
%------------------------

%Declare and assign values to local variables
%according to the input argument pattern
switch nargin
case 1
   if isa(f,'struct'), ASAcontrol=f;
   else, error(ASAerr(39))
   end
   f=[]; b=[]; max_order=[];
case 2
   if isa(b,'struct'), error(ASAerr(2,mfilename))
   end
   max_order=b; b=[]; ASAcontrol=[];
case 3
   if isa(max_order,'struct'), ASAcontrol=max_order; max_order=b; b=[];
   else, ASAcontrol=[];
   end
case 4
   if isa(last,'struct'), ASAcontrol=last;
   else, error(ASAerr(39))
   end
otherwise
   error(ASAerr(1,mfilename))
end

state = 0;
if isequal(nargin,1) & ~isempty(ASAcontrol)
      %ASAcontrol is the only input argument
   ASAcontrol.error_chk = 0;
   ASAcontrol.run = 0;
elseif (isequal(nargin,3) & isempty(ASAcontrol)) | ...
      isequal(nargin,4) %f and b have been provided
      %as input arguments (empty b not excluded)
   state = 1;
end

%Declare ASAglob variables
ASAglob = {'ASAglob_final_f';'ASAglob_final_b'};

%Assign values to ASAglob variables by screening the
%caller workspace
for ASAcounter = 1:length(ASAglob)
   ASAvar = ASAglob{ASAcounter};
   eval(['global ' ASAvar]);
   if evalin('caller',['exist(''' ASAvar ''',''var'')'])
      eval([ASAvar '=evalin(''caller'',ASAvar);']);
   else
      eval([ASAvar '=[];']);
   end
end

%ARMASA-function version information
%-----------------------------------

%This ARMASA-function is characterized by
%its current version,
ASAcontrol.is_version = [2000 12 30 20 0 0];
%and its compatability with versions down to,
ASAcontrol.comp_version = [2000 12 30 20 0 0];

%This function calls other functions of the ARMASA
%toolbox. The versions of these other functions must
%be greater than or equal to:
ASAcontrol.req_version.convolrev = [2000 12 6 12 17 20];

%Checks
%------

if ~any(strcmp(fieldnames(ASAcontrol),'error_chk')) | ASAcontrol.error_chk
      %Perform standard error checks
   %Input argument format checks
   ASAcontrol.error_chk = 1;
   if ~isnum(f)
      error(ASAerr(11,'f'))
   elseif isavector(f) & size(f,2)>1
      f = f(:);
      warning(ASAwarn(25,{'row';'f';'column'},ASAcontrol))
   elseif ~(length(size(f))==2)
      error(ASAerr(35,'f'))
   end
   if ~isavector(f)
      warning(ASAwarn(36,ASAcontrol))
   end
   if ~isempty(b)
      if ~isnum(b)
         error(ASAerr(11,'b'))
      elseif isavector(b) & size(b,2)>1
         b = b(:);
         warning(ASAwarn(25,{'row';'b';'column'},ASAcontrol))
      elseif ~(length(size(b))==2)
         error(ASAerr(35,'b'))
      end
   end
   if ~isnum(max_order) | ~isintscalar(max_order) | ...
         max_order<0
      error(ASAerr(17,'max_order'))
   end

   %Input argument value checks
   if ~isreal(f) | (~isempty(b) & ~isreal(b))
      error(ASAerr(13))
   end
   if isempty(b)
      if max_order>size(f,1)-1
         error(ASAerr(21))
      end
   elseif ~isequal(size(f),size(b))
      error(ASAerr(28,{'f','b'}))
   elseif max_order>size(f,1)-2
      error(ASAerr(29,'max_order'))
   end
end

if ~any(strcmp(fieldnames(ASAcontrol),'version_chk')) | ASAcontrol.version_chk
      %Perform version check
   ASAcontrol.version_chk = 1;

   %Make sure the requested version of this function
   %complies with its actual version
   ASAversionchk(ASAcontrol);

   %Make sure the requested versions of the called
   %functions comply with their actual versions
   convolrev(ASAcontrol);
end

if ~any(strcmp(fieldnames(ASAcontrol),'run')) | ASAcontrol.run
      %Run the computational kernel
   ASAcontrol.run = 1;
   ASAcontrol.version_chk = 0;
   ASAcontrol.error_chk = 0;

%Main
%==========================================================================

[N,n] = size(f);
P = max_order;

if P>0
   %The protocol is tuned using Matlab R12,
   %but also works fine on R11.
   if ~isempty(b)
      P = P+1;
      protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
   else
      if N<128
         protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
      elseif N<12000 %C_7 (& C_13)
         if P<(0.1+6.7e-006*N)*N %C_14 & S
            protocol = [0 1 0;0 0 0;0 0 0;0 P 0];
         else
            protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
         end
      else
         if P<0.18*N %S
            protocol = [0 1 0;0 0 0;0 0 0;0 P 0];
         else
            protocol = [0 1 3 0;0 0 0 0;0 1 0 0;0 round(0.03*N) P 0]; %C_5
         end
      end
   end
else
   protocol = zeros(4,2);
end

p = 0;
p_max = 0;
i2 = 2;
while protocol(4,i2)~=0;
   p = protocol(4,i2)-protocol(4,i2-1);
   if p>p_max
      p_max = p;
   end
   i2 = i2+1;
end
i9 = 1;
i10 = 2;
if n>N
   i9 = 2;
   i10 = 1;
end

v = zeros(N+p_max,n,2);
a = zeros(N+p_max,n,2);
if isempty(b)
   v(1:N,:,2) = f;
   a(1+p_max:N+p_max,:,2) = f;
   if protocol(1,2)==3 | protocol(1,2)==0
      var = sum(sum(f(1:N,:).^2,i9),i10)/(n*N);
      den = 2*var*n*N;
   end
else
   v(1:N,:,2) = f;
   a(1+p_max:N+p_max,:,2) = b;
   den = sum(sum(f(1:N,:).^2+b(1:N,:).^2,i9),i10);
   var = [];
end

r1 = p_max+1;
v1 = [1 1];
v2 = [N N];
v3 = [N N];
a1 = [r1 r1];
a2 = [r1 r1];
a3 = [r1+N-1 r1+N-1];
rc = zeros(1,P+1);
i1 = 0;
i2 = 2;
i4 = 0;
c = 0;

while protocol(4,i2)~=0
   i1 = i1+1;
   p = protocol(4,i2);

   if protocol(1,i2)==3 %SP
      r2 = a2(1)+1;
      r3 = v2(1)-1;
      r4 = a2(2)+1;
      r5 = v2(2)-1;
      i5 = i1;
      for i1 = i1:p
         i5 = i5+1;
         if c, c0 = 2; c1 = 1;
         else c0 = 1; c1 = 2;
         end
         v1(c0) = v1(c1)+1;
         a3(c0) = a3(c1)-1;
         v(v1(c0):v2(c0),:,c0) = v(v1(c0):v2(c1),:,c1)+...
            rc(i1)*a(r4:a3(c1),:,c1);
         a(a2(c0):a3(c0),:,c0) = a(a2(c1):a3(c0),:,c1)+...
            rc(i1)*v(v1(c1):r5,:,c1);
         den = (1-rc(i1)^2)*den-...
            sum((v(v1(c1),:,c1)+rc(i1)*a(a2(c1),:,c1)).^2)-...
            sum((a(a3(c1),:,c1)+rc(i1)*v(v2(c1),:,c1)).^2);
         rc(i5) = -2*sum(sum(v(v1(c0):v2(c0),:,c0).*...
            a(a2(c0):a3(c0),:,c0),i9),i10)/den;
         c = ~c;
      end
      v(v1(c1),:,c0) = v(v1(c1),:,c1)+rc(i1)*a(a2(c1),:,c1);
      a(a3(c1),:,c0) = a(a3(c1),:,c1)+rc(i1)*v(v2(c1),:,c1);

   elseif protocol(1,i2)==1 %ACC
      i13 = 2*p;
      i14 = p+1;
      i15 = 0;
      i16 = 0;
      i17 = 0;
      jj = zeros(2,i13);
      jjm_h = zeros(2,i14);
      jj_h = zeros(2,i14);
      jj(2,1) = 1;
      jjm_h(2,1) = 1;
      jj_h(2,2) = 1;
      for i4 = 1:n
         acov_x(:,i4) = convolrev(f(:,i4),p,ASAcontrol);
      end
      i1 = i1+1;
      var = sum(acov_x(1,:));
      den = (2*var-sum(v(v1(2),:,2).^2)-sum(a(a3(2),:,2).^2));
      rc(i1) = -2*sum(acov_x(2,:))/den;
      var = var/(n*N);
      v1(2) = v1(2)+1;
      a3(2) = a3(2)-1;
      t = 0;
      r5 = v1(2)+p-2;
      r6 = a3(2)-p+2;
      if protocol(3,i2)==1
         r11 = r5+1;
         r12 = r6-1;
         i33 = 0;
      else
         r11 = v2(2)-p+2;
         r12 = a2(2)+p-2;
         i33 = 1;
      end
      r7 = v2(2)+1;
      r8 = a2(2)-1;
      r9 = v1(2);
      r10 = a3(2);
      r13 = r5-r9;
      r14 = r10-r6;
      r15 = r11-r9;
      r16 = r10-r12;

      for i1 = i1:p
         i13 = i1+1;
         i14 = i1-1;
         i15 = 2*i1-1;
         i16 = 2*i1-2;
         i17 = 2*i1-3;
         rc_h1 = rc(i1)^2;
         rc_h2 = 2*rc(i1);
         if c, c0 = 2; c1 = 1;
         else c0 = 1; c1 = 2;
         end
         jj(c0,1:2) = jj(c1,1:2);
         jj(c0,3:i15) = jj(c1,3:i15)+rc_h1*jj(c1,i17:-1:1);
         jj(c0,i1:i16) = jj(c0,i1:i16)+rc_h2*jjm_h(c1,1:i14);
         jj(c0,2:i14) = jj(c0,2:i14)+rc_h2*jjm_h(c1,i14:-1:2);
         jjm_h(c0,1:i1) = rc(i1)*jj_h(c1,1:i1);
         jjm_h(c0,1:i14) = jjm_h(c0,1:i14)+(1+rc(i1)^2)*jjm_h(c1,1:i14);
         jj_h(c0,1:i13) = jj(c0,i13:-1:1);
         jj_h(c0,1:i14) = jj_h(c0,1:i14)+jj(c0,i13:i15);
         va_t(1,:) = jj(c0,1:i1)*acov_x(i13:-1:2,:);
         va_t(2,:) = jj(c0,i13:i15)*acov_x(1:i14,:);
         v1(c0) = v1(c1)+1; v3(c0) = v3(c1)+1;
         a1(c0) = a1(c1)-1; a3(c0) = a3(c1)-1;
         t = t+i33;
         v(r9:r5,:,c0) = v(r9:r5,:,c1)+...
            rc(i1)*a(a1(c1):a1(c1)+r13,:,c1);
         v(r11+t:v3(c1),:,c0) = v(r11+t:v3(c1),:,c1)+...
            rc(i1)*a(a1(c1)+r15+t:r10,:,c1);
         v(v3(c0),:,c0) = rc(i1)*f(N,:);
         a(r6:r10,:,c0) = a(r6:r10,:,c1)+...
            rc(i1)*v(v3(c1)-r14:v3(c1),:,c1);
         a(a1(c1):r12-t,:,c0) = a(a1(c1):r12-t,:,c1)+...
            rc(i1)*v(r9:v3(c1)-r16-t,:,c1);
         a(a1(c0),:,c0) = rc(i1)*f(1,:);
         omega_va(c0) = sum(sum(v(r9:v1(c1),:,c0).*a(a1(c0):r8,:,c0)+...
            v(r7:v3(c0),:,c0).*a(a3(c1):r10,:,c0),1));
         den = den*(1-rc_h1)-sum(v(v1(c1),:,c0).^2)-sum(a(a3(c1),:,c0).^2);
         rc(i13) = -2*(sum(sum(va_t,2),1)-omega_va(c0))/den;
         c=~c;
      end
      i1 = p;
   end
   i2 = i2+1;
end

if state %f and b have been provided as input
      %arguments (empty b not excluded)
      if isempty(b)
         rc = rc(2:end);
      else
         rc = rc(3:end);
      end
else
   rc(1) = 1;
end

i2 = i2-1;
ASAglob_final_f = [];
ASAglob_final_b = [];
if protocol(3,i2)==1 | protocol(1,i2)==3
   ASAglob_final_f = v(v1(c1):v2(c0),:,c0);
   ASAglob_final_b = a(a2(c0):a3(c1),:,c0);
elseif isequal(P,0)
   ASAglob_final_f = f;
end

%Footer
%=====================================================

else %Skip the computational kernel
   %Return ASAcontrol as the first output argument
   if nargout>1
      warning(ASAwarn(9,mfilename,ASAcontrol))
   end
   rc = ASAcontrol;
   ASAcontrol = [];
end

%Program history
%======================================================================
%
% Version                Programmer(s)          E-mail address
% -------                -------------          --------------
% former version         S. de Waele            waele@tn.tudelft.nl
% [2000 12 30 20 0 0]    W. Wunderink           wwunderink01@freeler.nl

author: Sable

repo: mcbench-benchmarks


text:
load hershey
palabra='D';
sz=size(palabra);
figure
%hold on
xprev=0;
%yprev=0;
% accede a cada letra B
for i= 1:sz(2)
B = hershey{palabra(i)};

%Se muestran por pantalla las coordenadas y se marca con NaN cada uno de
%los segmentos de la letra. Son los trazos que pueden dibujarse sin
%levantar el lapiz de la superficie
B.stroke;
%X=[(B.stroke(1,:)+xprev+0.2) B.stroke(2,:)].*3; %Esfera
%x= [2*X(1)/(1+X(1)^2+X(2)^2), 2*X(2)/(1+X(1)^2+X(2)^2), (-1+X(1)^2+X(2)^2)/(2+X(1)^2+X(2)^2)];

%obten coordenadas "unitarias" x, y
x=B.stroke(1,:)+xprev+0.2;
y=B.stroke(2,:);

%plotea mostrando las coordenadas de los puntos en coordenadas unitarias
%%figure
hold on
%plot(x,y,'*')
xprev=max(x);

end
% Con las lineas siguientes obtenemos una trayectoria que permite levantar
% el lapiz cuando se finaliza cada trazo
path = [ 0.25*B.stroke; zeros(1,size(B.stroke,2))];
k = find(isnan(path(1,:)));
path(:,k) = path(:,k-1);
path(3,k) = 0.2;

% % plotea el camino
plot3(path(1,:),path(2,:), path(3,:),'*')

repo: ARTE


text:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample program for blind source separation using independent low-rank   %
% matrix analysis (ILRMA)                                                 %
%                                                                         %
% Coded by D. Kitamura (d-kitamura@ieee.org)                              %
%                                                                         %
% Copyright 2020 Daichi Kitamura                                          %
%                                                                         %
% These programs are distributed only for academic research at            %
% universities and research institutions.                                 %
% It is not allowed to use or modify these programs for commercial or     %
% industrial purpose without our permission.                              %
% When you use or modify these programs and write research articles,      %
% cite the following references:                                          %
%                                                                         %
% # Original paper (The algorithm was called "Rank-1 MNMF" in this paper) %
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined  %
% blind source separation unifying independent vector analysis and        %
% nonnegative matrix factorization," IEEE/ACM Trans. ASLP, vol. 24,       %
% no. 9, pp. 1626-1641, September 2016.                                   %
%                                                                         %
% # Book chapter (The algorithm was renamed as "ILRMA")                   %
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined  %
% blind source separation with independent low-rank matrix analysis,"     %
% Audio Source Separation. Signals and Communication Technology.,         %
% S. Makino, Ed. Springer, Cham, pp. 125-155, March 2018.                 %
%                                                                         %
% See also:                                                               %
% http://d-kitamura.net                                                   %
% http://d-kitamura.net/demo-ILRMA_en.html                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear;
close all;

% Set parameters
seed = 1; % pseudo random seed
refMic = 1; % reference microphone for back projection
resampFreq = 16000; % resampling frequency [Hz]
nSrc = 2; % number of sources
fftSize = 4096; % window length in STFT [points]
shiftSize = 2048; % shift length in STFT [points]
windowType = "hamming"; % window function used in STFT
nBases = 10; % number of bases (for ilrmaType=1, nBases is # of bases for "each" source. for ilrmaType=2, nBases is # of bases for "all" sources)
nIter = 100; % number of iterations (define by checking convergence behavior with drawConv=true)
ilrmaType = 1; % 1 or 2 (1: ILRMA w/o partitioning function, 2: ILRMA with partitioning function)
dofParam = 1; % degree-of-freedom parameter of Student's t distribution (positive value, for t-ILRMA, 1: Cauchy)
sigDom = 2; % domain of signal for low-rank source model (positive value, for t-ILRMA, 2: power, 1: amplitude)
applyNormalize = 1; % 0 or 1 or 2 (0: do not apply normalization in each iteration, 1: apply average-power-based normalization in each iteration to improve numerical stability (the monotonic decrease of the cost function may be lost), 2: apply back projection in each iteration)
applyWhitening = false; % true or false (true: apply whitening to the observed multichannel spectrograms)
drawConv = true; % true or false (true: plot cost function values in each iteration and show convergence behavior, false: faster and do not plot cost function values)

% Fix random seed
RandStream.setGlobalStream(RandStream('mt19937ar','Seed',seed))

% Input data and resample
[srcSig(:,:,1), sampFreq] = audioread('./input/drums.wav'); % signal x channel x source (source image)
[srcSig(:,:,2), sampFreq] = audioread('./input/piano.wav'); % signal x channel x source (source image)
srcSigResample(:,:,1) = resample(srcSig(:,:,1), resampFreq, sampFreq, 100); % resampling for reducing computational cost
srcSigResample(:,:,2) = resample(srcSig(:,:,2), resampFreq, sampFreq, 100); % resampling for reducing computational cost

% Mix source images of each channel to produce observed mixture signal
mixSig(:,1) = srcSigResample(:,1,1) + srcSigResample(:,1,2);
mixSig(:,2) = srcSigResample(:,2,1) + srcSigResample(:,2,2);
if abs(max(max(mixSig))) > 1 % check clipping
    error('Cliping detected while mixing.\n');
end

% Blind source separation based on ILRMA
[estSig, cost] = ILRMA(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv);

% Blind source separation based on t-ILRMA
% [estSig, cost] = tILRMA(mixSig, nSrc, resampFreq, nBases, dofParam, sigDom, fftSize, shiftSize, windowType, nIter, refMic, applyNormalize, applyWhitening, drawConv);

% Blind source separation based on consistent ILRMA
% [estSig, cost] = consistentILRMA(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, refMic, applyWhitening, drawConv);

% Blind source separation based on ILRMA-ISS
% [estSig, cost] = ILRMAISS(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv);

% Output separated signals
outputDir = sprintf('./output');
if ~isdir( outputDir )
    mkdir( outputDir );
end
audiowrite(sprintf('%s/observedMixture.wav', outputDir), mixSig, resampFreq); % observed signal
audiowrite(sprintf('%s/originalSource1.wav', outputDir), srcSigResample(:,refMic,1), resampFreq); % source signal 1
audiowrite(sprintf('%s/originalSource2.wav', outputDir), srcSigResample(:,refMic,2), resampFreq); % source signal 2
audiowrite(sprintf('%s/estimatedSignal1.wav', outputDir), estSig(:,1), resampFreq); % estimated signal 1
audiowrite(sprintf('%s/estimatedSignal2.wav', outputDir), estSig(:,2), resampFreq); % estimated signal 2

fprintf('The files are saved in "./output".\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% EOF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

author: d-kitamura

repo: ILRMA

save_path: github-repos/MATLAB/d-kitamura-ILRMA