function output = callmaxdet(interfacedata)

[F_struc,F_blksz,G_struc,G_blksz] = sedumi2maxdet(interfacedata.F_struc,interfacedata.K);

c = interfacedata.c;
options = interfacedata.options;
x0 = interfacedata.x0;
onlyfeasible = (nnz(c)==0) & isempty(G_blksz);

if isempty(G_blksz)
    G_blksz = 1;
    G_struc = [1 zeros(1,length(c))];
else
end

if isempty(F_blksz)
    x = zeros(length(c),1)+NaN;
    problem = 11;
    infostr = yalmiperror(problems,'MAXDET failed since there are no inequality constraints in F(x)');
    solveroutput = [];
    solvertime = 0;
    D_struc = [];
end

problem = 0;
solvertimephase1=0;
D_struc = [];
if isempty(x0)
    solvertimephase1 = tic;
    showprogress('Calling MAXDET/phase1',options.showprogress);
    [x0,z0,w0,problem,infostr,solveroutput] = callmaxdetphase1(full(F_struc),full(F_blksz), full(G_struc),full(G_blksz), full(c), options);
    solvertimephase1 = toc(solvertimephase1);
end
if (problem~=0) | (onlyfeasible==1)
    if isempty(x0)
        x = repmat(nan,length(c),1);
    else
        x = x0;
    end
    solvertime = solvertimephase1;
else
    z0=zeros(size(F_struc,1),1);
    w0=zeros(size(G_struc,1),1);
    solvertime = tic;
    showprogress('Calling MAXDET',options.showprogress);
    if options.verbose==0
        evalc('[x,Z,W,ul,hist,infostr]=maxdet(full(F_struc),F_blksz,full(G_struc), G_blksz,c'',x0,z0,w0,options.maxdet.AbsTol,options.maxdet.RelTol,options.maxdet.gam,options.maxdet.NTiters);');
    else
        [x,Z,W,ul,hist,infostr]=maxdet(full(F_struc),F_blksz,full(G_struc), G_blksz,c',x0,z0,w0,options.maxdet.AbsTol,options.maxdet.RelTol,options.maxdet.gam,options.maxdet.NTiters);
    end
    solvertime = toc(solvertime)+solvertimephase1;

    D_struc = [Z;W];

    if options.savesolveroutput
        solveroutput.x = x;
        solveroutput.Z = Z;
        solveroutput.W = W;
        solveroutput.ul = ul;
        solveroutput.hist = hist;
        solveroutput.infostr = infostr;
    else
        solveroutput = [];
    end

    switch  infostr
        case {'target reached','relative accuracy reached','absolute accuracy reached','absolute tolerance reached','relative tolerance reached'}
            problem = 0;
        case 'maximum Newton iteration exceeded'
            problem = 3;
        otherwise
            problem = 1;
    end
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,interfacedata.solver.tag,[],solveroutput,solvertime);

function [x0,z0,w0,problem,infostr,solveroutput] = callmaxdetphase1(F_struc,F_blksz, G_struc,G_blksz, c, options);

try
    if options.verbose==0
        evalc('[x0,z0,w0,ul] = phase1(full(F_struc),F_blksz,full(G_struc), G_blksz,options.maxdet.gam,options.maxdet.AbsTol,options.maxdet.RelTol,options.maxdet.NTiters);');
    else
        [x0,z0,w0,ul] = phase1(full(F_struc),F_blksz,full(G_struc), G_blksz,options.maxdet.gam,options.maxdet.AbsTol,options.maxdet.RelTol,options.maxdet.NTiters);
    end
    if ul(1)<0
        problem = 0;
    elseif ul(2)>=0
        problem = 1;
    else
        problem = 8;
    end
    %  % Problems currently detected outside
%  problem = (ul(1)>0);

% In case of problems, phase1 outputs the extended vector instead
% of last primal iterate
if problem
    x0 = x0(1:length(c));
end

catch
    problem = 9;
    x0 = [];
    z0 = [];
    w0 = [];
    ul = [];
end
infostr = yalmiperror(problem,'MAXDET/phase1');
if options.savesolveroutput
    solveroutput.x = x0;
    solveroutput.Z = z0;
    solveroutput.W = w0;
    solveroutput.ul = ul;
    solveroutput.infostr = infostr;
else
    solveroutput = [];
end

% CODE TAKEN DIRECTLY FROM MAXDET/PHASE1
% THE REASON FOR HAVING A COPY HERE IS THAT
% A FILE NAMED PHASE1 EXIST FOR SP ALSO
%MAXDET, version ALPHA, April 1996.

%COPYRIGHT 1996 SHAO-PO WU, LIEVEN VANDENBERGHE AND STEPHEN BOYD
%Permission to use, copy, modify, and distribute this software for
%any purpose without fee is hereby granted, provided that this entire
%notice is included in all copies of any software which is or includes
%a copy or modification of this software and in all copies of the
%supporting documentation for such software.
%This software is being provided "as is", without any express or
%implied warranty.  In particular, the authors do not make any
%representation or warranty of any kind concerning the merchantability
%of this software or its fitness for any particular purpose.
function [x,Z,W,ul] = phase1(F,F_blkszs,G,G_blkszs,gam,abstol,reltol,NTiters);

m = size(F,2)-1;
if (m ~= size(G,2)-1)
    error('F and G must have the same number of columns.');
end
if (size(F,1) ~= sum(F_blkszs.^2))
    error('Dimensions of F do not match F_blkszs.');
end;
if (size(G,1) ~= sum(G_blkszs.^2))
    error('Dimensions of G do not match G_blkszs.');
end;

% mineigF is the smallest eigenvalue of F_0
mineigF = 0.0;
k=0; for n=F_blkszs,
    mineigF = min(mineigF, min(eig(reshape(F(k+[1:n*n],1),n,n))));
    k=k+n*n;   % k = sum n_i*n_i
end;
% mineigG is the smallest eigenvalue of G_0
mineigG = 0.0;
k=0; for n=G_blkszs,
    mineigG = min(mineigG, min(eig(reshape(G(k+[1:n*n],1),n,n))));
    k=k+n*n;   % k = sum n_i*n_i
end;

% eyeF is the identity
eyeF = zeros(size(F,1),1);
k=0; for n=F_blkszs,
    eyeF(k+[1:n*n]) = reshape(eye(n),n*n,1);   % identity
    k=k+n*n;   % k = sum n_i*n_i
end;
% eyeG is the identity
eyeG = zeros(size(G,1),1);
k=0; for n=G_blkszs,
    eyeG(k+[1:n*n]) = reshape(eye(n),n*n,1);   % identity
    k=k+n*n;   % k = sum n_i*n_i
end;

% initial x0
x0 = [zeros(m,1); max(-1.1*min(mineigF,mineigG), 1)];

% linear objective
c = [zeros(m,1); 1];

% call maxdet
[x,Z,W,ul,hist,infostr]=maxdet([F,eyeF; G,eyeG],...
    [F_blkszs(:)',G_blkszs(:)'],...
    [1 zeros(1,m+1)],1,c,x0,...
    zeros(size(F,1)+size(G,1),1),0,...
    abstol,reltol,gam,NTiters);

% prepare output
x = x(1:m);
W = Z(size(F,1)+1:size(F,1)+size(G,1));
Z = Z(1:size(F,1));
%if ul(1)<0
%  infostr = 'feasible';
%elseif ul(2)>=0
%  infostr = 'infeasible';
%else
%  infostr = 'feasibility cannot be determined';
%end

function [F_struc,blksz] = deblock(F_struc,blksz);
X = any(F_struc(end-blksz^2+1:end,:),2);
X = reshape(X,blksz,blksz);
[v,dummy,r,dummy2]=dmperm(X);
blks = diff(r);

logt = F_struc;

newlogt = [];
for i = 1:size(logt,2)
    temp = reshape(logt(:,i),blksz,blksz);
    temp = temp(v,v);
    newlogt = [newlogt temp(:)];
end
logt = newlogt;

pattern = [];
for i = 1:length(blks)
    pattern = blkdiag(pattern,ones(blks(i)));
end

F_struc = logt(find(pattern),:);
blksz = blks;


%%
% Test for OT color transfer.

addpath('../toolbox/');
addpath('../colors_functions/');
addpath('../image_blur/');
addpath('../blur_functions//');
addpath('../convolutional_wasserstein/');
addpath('../../data/meshes/');
addpath('../mesh_functions/');

% #bins
N = 40;
N = 60;

%%
% helpers

mmin = @(x)min(x(:));
mmax = @(x)max(x(:));
normalize = @(h)h/sum(h(:));
setfigname = @(name)set(gcf, 'Name', name, 'NumberTitle','off');
% plot histograms
delta = .1/N^2;
dispHist = @(x)-log(x+delta);
dispCell = @(H)cellfun(dispHist, H, 'UniformOutput', false);
Entropy = @(x)-sum(x(x>0).*log(x(x>0)));

%%
% Blur kernel

mu = N/50;
mu = N/40;
mu = N/25;
blur = load_filtering('imgaussian', N);
K = @(x)blur(x,mu);
Kv = @(x)apply_3d_func(K,x);

%%
% Load densities

if 1 % not(exist('names'))
    names = {'spiky', 'spheres'};
    names = {'spiky', 'sphere'};
    names = {'sphere', 'boxes'};
    names = {'duck', 'horse'};
    names = {'duck' 'horse' 'shears' 'moomoo_s0'};
    names = {'spot_flipped','duck','torus'};
    names = {'spiky','sphere','boxes','cone_rotated'};
    names = {'duck','spiky','moomoo_s0','double-torus'};
    names = {'shears' 'chair' 'wolf0' 'duck' };
    names = {'mushroom' 'torus' 'hand1' 'trim-star'}; % 'star_subdivided'
    names = {'torus'  'duck' 'mushroom' 'spiky'}; % 'star_subdivided'
end
p = length(names);

f = {};
for i=1:p
    name = names{i};
    f{i} = normalize( load_volume(names{i}, N) );
    if isempty(f{i})
        %% try to load a mesh %%
        [V,F] = read_off([name '.off']);
        V = rescale(V, .03, .97);
        % check if rotation is needed
        A = eye(3);
        switch name
            case 'duck'
                A = [-0.0193 -0.2900 0.9568; 0.9677 0.2352 0.0908; -0.2514  0.9277 0.2761];
            case 'moomoo_s0'
                A = [-0.0971 0.7483 -0.6562; -0.9928 -0.1191 0.0111; -0.0699 0.6525 0.7545];
            case 'homer'
                A = [-0.4287  0.5566 -0.7116; -0.7577 -0.6505 -0.0524; -0.4921 0.5167 0.7006];
            case 'dinosaur'
                A = [-0.8362    0.0082   -0.5483   -0.2390   -0.9054    0.3509   -0.4935    0.4245    0.7591];
            case 'chair'
                A = [-0.8165    0.2728   -0.5089    0.3447   -0.4768   -0.8086   -0.4632   -0.8356    0.2952];
            case 'trim-star'
                A = [-0.7297    0.3243    0.6020   -0.6814   -0.4187   -0.6003    0.0574   -0.8483    0.5265];
            case 'hand1'
                A = [-0.4030   -0.7111   -0.5761    0.8945   -0.4391   -0.0837   -0.1934   -0.5491    0.8131];
            case 'mushroom'
                A = [-0.1212   -0.9430    0.3099    0.9227    0.0081    0.3855   -0.3660    0.3326    0.8691];
        end
        V = reshape(A,[3 3])*(V-.5)+.5;
        %  V0 = V; [A,~] = qr(randn(3)); V = A*(V0-.5)+.5;
        t = linspace(0,1,N);
        f{i} = inpolyhedron(F',V',t,t,t);
        opts.isolevel = .5;
        clf; plot_isosurface(f{i},opts);
        a=1;
    end
end

opts.alpha = 1; % transparency
opts.color = [0 1 0];
opts.isolevel = .5;
clf;
for i=1:p
    subplot(1,length(names),i);
    plot_isosurface(f{i},opts);
end

rep = ['../results/volumetric/' names{1}];
for i=2:p
    rep = [rep '-' names{i}];
end
rep = [rep '/'];

if not(exist(rep))
    mkdir(rep);
end

%%
% Compute transport coupling
%   pi = diag(w1)*K*diag(w0)
% and
%   pi*1 = w1.*K(w0) = p1
% and
%   pi'*1 = w1.*K(w0) = p0

options.tol = 1e-9;
options.tol = 0;
options.niter = 100;
options.verb = 2;
slicing = @(x)x(:,:,end/2);
options.disp = @(w0,w1)imageplot( slicing(w0.*K(w1)) );

% clf;
% [distances,w0,w1] = convolutionalDistance(f{1}, f{2}, [], K,[], options);

%%
% Compute displacement interpolation

q = 5;
switch p
    case 2
        % displacement interpolation
        t = linspace(0,1,q);
        W = [t;1-t];
    case 3
        % triangle interpolation
        W = [ ...
            [0, 0, 1]; ...
            [1, 0, 3]; [0, 1, 3]; ...
            [1,0,1]; [1,1,2]; [0,1,1]; ...
            [3,0,1]; [2,1,1]; [1,2,1]; [0,3,1]; ...
            [1,0,0]; [3,1,0]; [1,1,0]; [1,3,0]; [0,1,0] ...
            ]';
    case 4
        % bilinear interpolation
        t = linspace(0,1,q);
        [T,S] = meshgrid(t,t); S = S(:); T = T(:);
        W = [(1-S).*(1-T) S.*(1-T) (1-S).*T S.*T]';
end
Q = size(W,2);

%%
% Do the computations.

for i=1:Q
    w = W(:,i)'; w = w/sum(w);
    % select entropy bound
	entropyLimit = [];
    % store as 2D matrix
    Hv = [];
    for k = 1:p
        Hv = [Hv f{k}(:)/sum(f{k}(:))];
    end
    % do the computation
    options.disp = @(x)plot_isosurface(reshape(x,[N N N]));
    [B,u] = convolutionalBarycenter( Hv, w, [], Kv, [],entropyLimit, options);
    B = reshape(B, [N N N]);
    B = B/max(B(:));
    Bsvg{i} = B;
    if isnan(max(B(:)))
        warning('NaN problem.');
    end
end

%%
% Do the rendering.

col = [ [1 0 0]; [0 1 0]; [0 0 1]; [1 1 0]; [1/2 1 1/2]; [1/2 1/2 1] ];

for i=1:Q
    w = W(:,i)'; w = w/sum(w);
    B = Bsvg{i};
    % display
    clf;
    opts.alpha = 1; % transparency
	opts.color = sum(col(1:p,:) .* repmat(w(:),[1 3]) );
    opts.isolevel = median(B(:));
    opts.isolevel = (max(B(:))-min(B(:)))/2;
    F = plot_isosurface(B,opts);
    % save as image
    if p==4
        str = ['barycenter-' num2str(S(i)*(q-1)) '-' num2str(T(i)*(q-1))];
    else
        str = ['barycenter-' num2str(i)];
    end
    saveas(gcf, [rep str '.png'], 'png');
    % save as object
    if 0
    mesh.vertices = F.vertices;
    mesh.triangles = F.faces;
    mesh.numVertices = size(F.vertices, 1);
    vertexTexture = zeros(mesh.numVertices,1);
    writeTexturedObj([rep 'barycenter-' str '.obj'], mesh, vertexTexture);
    end
end


report_this_filefun(mfilename('fullpath'));

bv2 = [];
bv3 = [] ;
me = [];
def = {'150'};
ni2 = inputdlg('Number of events in each window?','Input',1,def);
l = ni2{:};
ni = str2double(l);

think

for i = 1:ni/10:length(newt2)-ni
    % [bv magco,  stan] =  bvalcalc(newt2(i:i+ni,:));
    [bv magco stan ] =  bvalca2(newt2(i:i+ni,:));

    bv2 = [bv2 ; magco newt2(i,3)];
end

% Find out of figure already exists
%
[existFlag,figNumber]=figure_exists('Mc with time',1);
newdepWindowFlag=~existFlag;
bdep= figNumber;

% Set up the window

if newdepWindowFlag
    Mcfig = figure_w_normalized_uicontrolunits( ...
        'Name','Mc with time',...
        'NumberTitle','off', ...
        'MenuBar','none', ...
        'NextPlot','replace', ...
        'backingstore','on',...
        'Visible','on');

    uicontrol('Units','normal',...
        'Position',[.0 .85 .08 .06],'String','Info ',...
         'Callback','infoz(1)');
    matdraw
end

hold on
figure_w_normalized_uicontrolunits(Mcfig)
hold on
delete(gca)
delete(gca)
axis off

rect = [0.15 0.30 0.7 0.45];
axes('position',rect)
pl = plot(bv2(:,2),bv2(:,1),'^r');
set(pl,'LineWidth',1.5,'MarkerSize',10,...
    'MarkerFaceColor','y','MarkerEdgeColor','r')
hold on
pl = plot(bv2(:,2),bv2(:,1),'b')
set(pl,'LineWidth',1.0)

grid
set(gca,'Color',color_bg)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
%
ylabel('Mc')
%set(gca,'Xlim',[t0b teb]);

xlabel('Time')
tist = [  name ' - b(t), ni = ' num2str(ni) ];
title(tist)
done

nu = [];
for i = 1:length(bv2)
    l = newt2.Date >= bv2(i,2) &  newt2.Magnitude >= bv2(i,1);
    nu = [nu length(newt2(l,1))];
end

figure
plot(bv2(:,2),nu,'o')
hold on
plot(bv2(:,2),nu)

figure
plot(bv2(:,1),nu,'o')
hold on
plot(bv2(:,1),nu)


classdef net_model_dc < mp.net_model & mp.form_dc

%   MATPOWER
%   Copyright (c) 2019-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

    properties
        va = [];
        z  = [];
    end

    methods
        %% constructor
        function obj = net_model_dc()
            obj@mp.net_model();
            obj.element_classes = { ...
                @mp.nme_bus_dc, @mp.nme_gen_dc, @mp.nme_load_dc, ...
                    @mp.nme_branch_dc, @mp.nme_shunt_dc };

            %% Due to a bug related to inheritance in constructors in
            %% Octave 5.2 and earlier (https://savannah.gnu.org/bugs/?52614),
            %% INIT_SET_TYPES() cannot be called directly in the
            %% MP_IDX_MANAGER constructor, as desired.
            %%
            %% WORKAROUND:  INIT_SET_TYPES() is called explicitly as needed
            %%              (if obj.node is empty) in BUILD() and DISPLAY(),
            %%              after object construction, but before object use.
        end

        function obj = def_set_types(obj)
            def_set_types@mp.net_model(obj);        %% call parent first
            obj.set_types.va = 'VOLTAGE VARS (va)';
            obj.set_types.z  = 'NON-VOLTAGE VARS (z)';
        end

        function obj = build_params(obj, nm, dm)
            %% call parent to build individual element parameters
            build_params@mp.net_model(obj, nm, dm);

            %% aggregate parameters from individual elements
            obj.B = obj.stack_matrix_params('B', 1);
            obj.K = obj.stack_matrix_params('K', 0);
            obj.p = obj.stack_vector_params('p');
        end

        function obj = port_inj_soln(obj)
            %% compute port injections
            obj.soln.gp = obj.port_inj_power(obj.soln.x);
        end
    end     %% methods
end         %% classdef


function [Lden,Ld] = dpr1fact(x, d, Lsym, smult, maxu) %#ok
% [Lden,L.d] = dpr1fact(x, d, Lsym, smult, maxu)
%
% DPR1FACT  Factor d[iag] p[lus] r[ank] 1:
%    [Lden,L.d] = dpr1fact(x, d, Lsym, smult, maxu)
%    Computes fi and d such that
%       diag(d_IN) + x*diag(smult)*x' =
%(PI_{i=1}^n L(p_OUT^i,beta_i)) * diag(d_OUT) * (PI_{i=1}^n L(p_OUT^i,beta_i))'
%    where L(p,beta) = eye(n) + tril(p*beta',-1).
%
% Lden.dopiv(k) = 1 if p(:,k) has been reordered, with permutation in
% Lden.pivperm.
% We reorder if otherwise |p(i,k)*beta(j,k)| > maxu.
%
% ******************** INTERNAL FUNCTION OF SEDUMI ********************
%
% See also fwdpr1,bwdpr1,sedumi

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

%Indicate to the user Matlab cannot find the SeDuMi binaries
sedumi_binary_error();


%% SPACEXPLOSION Analyze the SpaceX explosion that occurred on 1 Sep 2016
% All supporting functions are in the infrasoundGT directory (or GISMO)
%

%% setup
clear all
close all
matfile = '/Users/glennthompson/Dropbox/Rockets/analysis/20160901_SpaceXplosion/explosion2.mat';
if exist(matfile,'file')
    load(matfile);
else

    % waveform data parameters
    %ds = datasource('antelope', '/raid/data/rockets/dbspacexplosion');
    ds = datasource('antelope', '/Users/glennthompson/Dropbox/Rockets/db/20160901_explosion');
    snum=datenum(2016,9,1,13,0,0);
    enum = snum + 1/24;
    scnl = scnlobject('BCHH', '*', 'FL');

    % Geographical coordinates
    % %%%%%%%%%%%%% SCAFFOLD - could load these from Antelope or Excel
    lat = [28.574182 28.573894 28.574004 28.574013 28.574013 28.574013];
    lon = [-80.572410 -80.572352 -80.572561 -80.572360  -80.572360 -80.572360];
    source.lat = 28.562106; % SLC40 - SpaceX launch complex
    source.lon = -80.57718;
    % Wind tower data - could read this from Excel instead
    relativeHumidity = 92; % percent from NASA weather tower data
    temperatureF = 80; % 80 Fahrenheit according to weather tower data from NASA
    wind_direction_from = 150; % degrees - this is the direction FROM according to NASA, see Lisa Huddleston email of Oct 10th
    wind_direction = mod(wind_direction_from + 180, 360);
    wind_speed_knots = 10; % knots
    wind_speed = wind_speed_knots * 0.514444; % m/s
    % rmpath(genpath('/raid/apps/src/GISMO'))

    %% compute speed of sound based on temperature & rel. humidity
    temperatureC = fahrenheit2celsius(temperatureF);
    speed_of_sound = computeSpeedOfSound(temperatureC, relativeHumidity);
    disp(sprintf('speed of sound at %.1f Celsius and %f percent relative humidity is %.1f',temperatureC, relativeHumidity, speed_of_sound));

    %% load waveform data
    disp('Loading waveform data...')
    w=waveform(ds,scnl,snum,enum);

    save(matfile);
end

make_figures = true
%addpath('infrasoundGT')
figureOutDirectory = '20160901_results';
mkdir('.',figureOutDirectory);

save(matfile)

%% plot raw waveform data
if make_figures
    figure
    plot_panels(w);
    outfile = sprintf('%s/waveforms_raw.png',figureOutDirectory);
    feval('print', '-dpng', outfile);
    close
end

%% compute predicted travel times for infrasound waves based on GPS coords & wind
%% also add lat, lon, distance and bacaz fields to waveform objects
disp('Predicting travel times based on GPS coordinates and wind vector...')
fprintf('\n_______________________________________________\n');
fprintf('PREDICTED TRAVEL TIME BASED ON:\n');
fprintf('  sound speed    %.1fm/s\n', speed_of_sound);
fprintf('  wind speed     %.1fm/s\n', wind_speed);
fprintf('  wind direction %.1f degrees\n', wind_direction);
fprintf('------\t--------\t-----------\t----------\n');
fprintf('Channel\tDistance\tBackAzimuth\tTravelTime\n');
fprintf('------\t--------\t-----------\t----------\n');
for c=1:length(lat)
    [arclen(c), backaz(c)] = distance(lat(c), lon(c), source.lat, source.lon, 'degrees');
    arclen(c) = deg2km(arclen(c))*1000;
    effective_speed = speed_of_sound + wind_speed * cos(deg2rad( (180+backaz(c)) - wind_direction) );
    predicted_traveltime_seconds(c) = arclen(c)/effective_speed;
    fprintf('%s\t%.1fm\t\t%.1f degrees\t%.3fs\n',get(w(c),'channel'), arclen(c), backaz(c), predicted_traveltime_seconds(c));
    w(c) = addfield(w(c), 'lat', lat(c));
    w(c) = addfield(w(c), 'lon', lon(c));
    w(c) = addfield(w(c), 'distance', arclen(c));
    w(c) = addfield(w(c), 'backaz', backaz(c));

end
fprintf('_______________________________________________\n');
fprintf('Program name: %s\n',mfilename('fullpath'))
save spacexplosion.mat

%% plot array map & compute eastings and northings
if make_figures
    disp('Plotting array map')
    close all
    deg2m = deg2km(1) * 1000;
    cols = 'rwbggg';
    for c=1:length(lat)
        chan = get(w(c),'channel');
        easting(c) = distance(lat(c), lon(c), lat(c), source.lon) * deg2m;
        northing(c) = distance(lat(c), lon(c), source.lat, lon(c)) * deg2m;
        plot(easting(c),northing(c),'o','MarkerFaceColor',cols(c),'MarkerSize',10)
        hold on
        quiver(easting(c),northing(c),-easting(c)/100,-northing(c)/100,0); % /100 just gives arrow length
        text(easting(c)+1,northing(c),chan(1:3));
    end
    grid on
    quiver(440,1325,wind_speed*sin(deg2rad(wind_direction)), wind_speed*cos(deg2rad(wind_direction)) ,0,'k');
    text(440,1325,'wind')
    hold off
    title('Beach House array position relative to SLC40');
    xlabel('metres east');
    ylabel('metres north');
    axis equal;
    outfile = sprintf('%s/arraymap.png',figureOutDirectory);
    feval('print', '-dpng', outfile);
    close
    save spacexplosion.mat
end

%% Load arrivals
disp('Loading arrivals...')
arrivals=Arrival.retrieve('antelope', '/raid/data/rockets/dbspacexplosion');
save spacexplosion.mat

%% Subset out X1 arrivals
disp('Subsetting arrivals...')
arrivals = arrivals.subset('iphase', 'X1');
save spacexplosion.mat

%% Associate events
disp('Associating arrivals into events...')
maxTimeDiff = 1; % seconds
eventOn = false;
eventNumber = 0;
for c=2:numel(arrivals.daynumber)
    if arrivals.daynumber(c-1) + maxTimeDiff/86400 > arrivals.daynumber(c)
        if ~eventOn % start new event
            eventOn = true;
            eventNumber = eventNumber + 1;
            infrasoundEvent(eventNumber).FirstArrivalTime = arrivals.daynumber(c-1);
        else % event already in progress
        end
        infrasoundEvent(eventNumber).LastArrivalTime = arrivals.daynumber(c);
    else
        if eventOn % write out last event
            eventOn = false;
        end
    end
end
numEvents = numel(infrasoundEvent);
save spacexplosion.mat

%% Filter waveform data to center around zero
disp('Filtering waveform data...')
wfilt = detrend(w);
f=filterobject('h',[10],3);
wfilt=filtfilt(f,wfilt);
save spacexplosion.mat

%% Segment event waveforms
pretrigger = 1;
posttrigger = 1;
wevent = segment_event_waveforms(wfilt, infrasoundEvent, pretrigger, posttrigger);
save spacexplosion.mat

%% Plot infrasound events
if make_figures
    plot_events(wevent, 'waveforms_infrasoundEvent', figureOutDirectory);
end

%% correlate
% loop through infrasound channels
% take a 0.3-second snippet starting 0.1s before FirstArrivalTime, till 0.2s
% after it
% correlate this against the whole wevent for each infrasound
% trace
% this should result in a correlation matrix
% from this record the time lag matrix for each infrasound channel against each other
% infrasound channel
disp('CORRELATION ...')
disp('_______________')
%infrasoundEvent = xcorr3C(wevent, infrasoundEvent, make_figures, figureOutDirectory, pretrigger);
infrasoundEvent = xcorr3C(wevent, infrasoundEvent, false, figureOutDirectory, pretrigger);

% for eventNumber=1:numEvents
%     fprintf('- processing event %d of %d\n', eventNumber, numEvents);
%     haystacks = wevent{eventNumber};
%     infrasoundEvent(eventNumber).maxCorr = eye(3);
%     infrasoundEvent(eventNumber).secsDiff = eye(3);
%     precorrtime = 0.1; % NEEDLE seconds of data to add before first arrival
%     postcorrtime = 0.2; % NEEDLE seconds of data to add after first arrival
%     for chanNumber=1:3
%         needle = extract(haystacks(chanNumber), 'time', infrasoundEvent(eventNumber).FirstArrivalTime-precorrtime/86400, infrasoundEvent(eventNumber).FirstArrivalTime+postcorrtime/86400);
%         for haystackNum = 1:3
%             fprintf('  - looking for needle %d in haystack %d\n', chanNumber, haystackNum);
%             haystack = haystacks(haystackNum);
%             [acor,lag] = xcorr(get(needle,'data'),get(haystack,'data'));
%             [m,I] = max(abs(acor));
%             infrasoundEvent(eventNumber).maxCorr(chanNumber,haystackNum) = m;
%             infrasoundEvent(eventNumber).secsDiff(chanNumber,haystackNum) = lag(I)/get(haystack,'freq') + pretrigger - precorrtime;
%             if make_figures
%                 figure;
%                 subplot(3,1,1),plot(haystack);
%                 subplot(3,1,2),plot(needle);
%                 subplot(3,1,3),plot(lag,acor);
%                 outfile = sprintf('figs/xcorr_infrasoundEvent%03d_%d_%d.png',eventNumber,chanNumber,haystackNum);
%                 feval('print', '-dpng', outfile);
%                 close
%             end
%         end
%     end
% end
save spacexplosion.mat

%% Construct a master infrasound event, from all the individual ones
disp('Constructing master infrasound event from individual event statistics')
masterEvent.FirstArrivalTime = infrasoundEvent(1).FirstArrivalTime;
masterEvent.LastArrivalTime = infrasoundEvent(1).LastArrivalTime;

% find the mean xcorr time lag difference for non-identical infrasound components - it should be close to zero if only one event in time window
% e.g. needle 1 and haystack 2 should have same magnitude but opposite sign
% time delay to needle 2 and haystack 1 if there is only one clear N wave
% in the haystacks
disp('- finding events with a mean time lag difference of close to 0 - these are the events we can use')
indexes = find(abs([infrasoundEvent.meanSecsDiff]) < 0.01); % these are events with probably only one event in wevent time window
fprintf('- found %d events we can use\n', numel(indexes));
disp('- event indexes to use');
disp(indexes)

masterEvent.secsDiff = zeros(3,3);
masterEvent.stdSecsDiff = zeros(3,3);
for row=1:3
    for column=1:3
        a = [];
        for eventNumber = indexes
            thisEvent = infrasoundEvent(eventNumber);
            a = [a thisEvent.secsDiff(row, column)];
        end

        % eliminate any events which have a difference from the mean of
        % greater than the standard deviation
        diffa = abs( (a-mean(a))/std(a) );

        % now set the mean and std for the master event
        masterEvent.secsDiff(row, column) = mean(a(diffa<1.0));
        masterEvent.stdSecsDiff(row, column) = std(a(diffa<1.0));
    end
end
disp('  - mean:');
disp(masterEvent.secsDiff)
disp('  - std:')
disp(masterEvent.stdSecsDiff)
disp('  - fractional std:')
disp(masterEvent.stdSecsDiff ./ masterEvent.secsDiff)

%% compute sound speed based on GPS coordinates and master event differential travel times
disp('- Estimating sound speed for each component pair using GPS coordinates')
clear speed
speed = zeros(3,3)*NaN;
for row=1:3
    for column = 1:3
        if row ~= column
            radialDistanceDifference = ( get(w(row),'distance') - get(w(column),'distance') );
            timeDifference = masterEvent.secsDiff(row, column);
            s = radialDistanceDifference / timeDifference;
            disp(sprintf('row %d column %d distance difference %.1f time difference %.4f speed %.1fm/s',row,column,radialDistanceDifference,timeDifference,s));
            speed(row,column) = s;
        end
    end
end
speed
meanspeed = nanmean(nanmean(abs(speed)));
stdspeed =  nanstd(nanstd(abs(speed)));
fprintf('- mean sound speed %.1f, std sound speed %.1f\n', meanspeed, stdspeed);
save spacexplosion.mat

%% beamforming
disp('Beamforming to estimate backazimuth of source from array')
[bestbackaz,bestsoundspeed,distanceDiff,speedMatrix] = beamform2(easting(1:3), northing(1:3), masterEvent.secsDiff, 199.0, 348.6);
distanceDiff
speedMatrix
sourceDist = get(w(2),'distance');
[beamformingsourcelat, beamformingsourcelon] = reckon(lat(2), lon(2), km2deg(sourceDist/1000), bestbackaz);
distFromSLC40 = deg2km(distance(beamformingsourcelat, beamformingsourcelon, source.lat, source.lon)) * 1000;
disp(sprintf('- source location estimated to be lat = %.4f lon = %.4f, which is %1.fm from true position',beamformingsourcelat, beamformingsourcelon,distFromSLC40));

%%
if make_figures
    outfile = sprintf('%s/beamforming.png',figureOutDirectory);
    feval('print', '-dpng', outfile);
    close
end
save spacexplosion.mat

%% estimate travel times from cross-correlation derived differential travel times and actual source location, wind speed, and predicted sound speed
[minimumPredictedTravelTime,index] =  min(predicted_traveltime_seconds);
for component=1:3
    traveltime_secs(component) = minimumPredictedTravelTime + mean([masterEvent.secsDiff(component,index) -masterEvent.secsDiff(index,component)]);
end
traveltime_secs(4) = minimumPredictedTravelTime + (get(w(4),'distance') - get(w(index),'distance')) / bestsoundspeed;
traveltime_secs(5) = traveltime_secs(4);
traveltime_secs(6) = traveltime_secs(4);
save spacexplosion.mat

%% Shift waveforms based on travel times
wshift = wfilt;
for c=1:numel(wshift)
     starttime = get(w(c), 'start');
     newstarttime = starttime - traveltime_secs(c)/86400;
     disp(sprintf('moving channel %d from %s to %s\n', c, datestr(starttime, 'HH:MM:SS.FFF'), datestr(newstarttime, 'HH:MM:SS.FFF')));
     wshift(c)=set(wshift(c),'start', newstarttime);
     disp(sprintf('%s\n',datestr(get(wshift(c),'start'))));
end
save spacexplosion.mat

%% Segment time shifted event waveforms
disp('Segmenting traveltime-corrected event waveforms...')
arrivalTimeCorrection = minimumPredictedTravelTime;
preplot = 0.15;
postplot = 0.15;
weventshift = segment_event_waveforms(wshift, infrasoundEvent, preplot, postplot, arrivalTimeCorrection);
save spacexplosion.mat

%% Plot shifted events
if make_figures
    plot_events(weventshift, 'waveforms_infrasoundEvent_shifted', figureOutDirectory)
end

%% Pick events
close all
for eventNumber=1:numEvents

    % plot waveforms for this event
    w2=weventshift{eventNumber};
    fh=plot_panels(w2);
    ah=get(fh,'Children');
    set(fh, 'Position', [0 0 1600 1000]);
    infrasoundEvent(eventNumber).maxAmp=zeros(1,6);
    infrasoundEvent(eventNumber).minAmp=zeros(1,6);
    infrasoundEvent(eventNumber).maxTime=zeros(1,6);
    infrasoundEvent(eventNumber).minTime=zeros(1,6);

    for chanNum=1:6

        % scan over this event on this channel and find the greatest
        % max/min difference in 0.1s
        y = get(w2(chanNum),'data');
        fs = get(w2(chanNum),'freq');
        numSamples = length(y);
        windowSize = round(fs/25);
        maxA = 0;
        for startSamp = 1:windowSize:round(numSamples*0.7)-windowSize
            samples = startSamp:startSamp+windowSize-1;
            [maxy, maxindex] = max(y(samples));
            [miny, minindex] = min(y(samples));
            if (maxy-miny) > maxA
                maxSecs = ((maxindex+samples(1)-1)/fs);
                minSecs = ((minindex+samples(1)-1)/fs);
                maxA = maxy-miny;

                infrasoundEvent(eventNumber).maxTime(chanNum) = tstart + maxSecs/86400;
                infrasoundEvent(eventNumber).minTime(chanNum) = tstart + minSecs/86400;
                infrasoundEvent(eventNumber).maxAmp(chanNum) = maxy;
                infrasoundEvent(eventNumber).minAmp(chanNum) = miny;
            end
        end

        % plot max & min
        axisnum = 8 - chanNum;
        axes(ah(axisnum));
        hold on
        plot(ah(axisnum),maxSecs, infrasoundEvent(eventNumber).maxAmp(chanNum), 'g*');
        plot(ah(axisnum),minSecs, infrasoundEvent(eventNumber).minAmp(chanNum), 'r*');

    end
    feval('print', '-dpng', sprintf('%s/picked_event_%03d',figureOutDirectory, eventNumber) );
    close
end
save spacexplosion.mat

%% Compute relative calibrations of infrasound sensors
clc
tolerance = 0.02; % times have to be within 2 hundreds of a second
component_pairs = [ [1,3]; [1,2]; [2,3] ];

for component_pair_num = 1:length(component_pairs)
    chanNum = nan(2,1);
    chanNum(1) = component_pairs(component_pair_num, 1);
    chanNum(2) = component_pairs(component_pair_num, 2);
    maxA = nan(2, numEvents);
    minA = nan(2, numEvents);
    for eventNum = 1:numEvents
        ev = infrasoundEvent(eventNum);
        if std(ev.maxTime) < tolerance/86400 & std(ev.minTime) < tolerance/86400
            for c=1:2
                maxA(c, eventNum) = ev.maxAmp(chanNum(c));
                minA(c, eventNum) = ev.minAmp(chanNum(c));
            end
        end
    end
    avA = (maxA - minA)/2;

    % first compute raw ratio, mean and std
    ratio = avA(1,:)./avA(2,:);
    m=nanmean(ratio);
    s=nanstd(ratio);

    % now recompute after throwing out any measurement that has an error larger
    % than the standard deviation
    ratio2 = ratio(ratio>m-s & ratio<m+s);
    m2=nanmean(ratio2);
    s2=nanstd(ratio2);
    distratio = get(w(chanNum(2)),'distance') / get(w(chanNum(1)), 'distance')

    fprintf('relative amplitude of signals on sensors %d and %d: %f (+/- %f)\n',chanNum(1), chanNum(2), m2, s2);
    fprintf('distance ratio = %f\n', distratio );

    relAmp(component_pair_num) = m2/distratio;
    fprintf('relative amplitude of signals corrected for distance is %f\n', m2/distratio);

end

%% compute new calibration constants assume sensor 3 has correct calib
oldcalib = get(w,'calib');
newcalib(1) = oldcalib(1) / relAmp(1);
newcalib(2) = oldcalib(2) / relAmp(3);
newcalib(3) = get(w(3),'calib');
for chanNum=1:3
    fprintf('Using channel 3 as a reference, channel 1 calib old = %f, new = %f\n',oldcalib(chanNum),newcalib(chanNum))
end

%% compute reduced pressures
for eventNum = 1:numEvents
    for chanNum = 1:3
        ev = infrasoundEvent(eventNum);
        pchange = ev.maxAmp(chanNum) - ev.maxAmp(chanNum);
        distanceInKm = get(w(chanNum),'distance') / 1000.0;
        infrasoundEvent(eventNum).pchange(chanNum) = pchange;
        infrasoundEvent(eventNum).preduced(chanNum) = pchange / 2 * distanceInKm;
    end
end
% add origin time - time of earliest arrival corrected for travel time

%%

SCAFFOLD

%% write out events
fout = fopen(sprintf('%s/eventlist.csv',figureOutDirectory),'w');
for eventNum = 1:numEvents
    ev = infrasoundEvent(eventNum);
    % event number
    fprintf(fout, '%d,', eventNum);
    % origin time
    fprintf(fout, '%s,', datestr(ev.originTime));
    % pressure change (from stack)
    fprintf(fout, '%.1f,', nanmean(ev.pchange));
    % reduced pressure (from stack)
    fprintf(fout, '%.1f,', nanmean(ev.preduced));
    % energy (from stack)
    fprintf(fout, '%.1f\n', nanmean(ev.preduced)*some_correction_factor);
    % by component
        % arrival time (not time corrected)
        % maxA
        % minA
end
fclose(fout)

%% Particle motion analysis
thisw = wevent{1};
t = threecomp(thisw([6 5 4])',backaz(1));
tr = t.rotate()
tr2 = tr.particlemotion();
tr2.plotpm()
tr2.plot3()


function sVF1 = minus(sVF1, sVF2)
%
% Syntax
%   sVF = sVF1 - sVF2
%   sVF = sVF - v
%
% Input
%  sVF1 - @S2VectorFieldTri
%  sVF2 - @S2VectorField
%  v    - @vector3d
%
% Output
%  sF - @S2FunTri
%

sVF1 = sVF1 + (-sVF2);


function appdata = sliceomaticdnorm(d,norm)
% SLICEOMATICDNORM(rawdata,norm) - Normalize relevant data.

% Simplify the isonormals
d.reduce=d.reduce./norm.*100;
d.reducesmooth=d.reducesmooth./norm.*100;
d.smooth=d.smooth./norm.*100;
d.data=d.data./norm.*100;
appdata = d;


function [orderedPoints,biggest,goodPerimEdges]=orderMeshPerimeterPointsAll_new(mesh,perimeterEdges)
%
% orderedPoints=orderMeshPerimeterPointsAll(perimeterEdges)
%
% AUTHOR:  Wade
% DATE: 062100
% PURPOSE:
% Takes a list of edges defined as point pairs..
% Returns the points in the list in their connection order...
% If more than one (unlinked) perimeter exists, return a structure containing all the separate ordered perimeters
%
%  N.B.  Each vertex should appear in exactly two rows.
%  Starting with an initial position on the perimeter, startNode, the routine finds
%  the other node in the row. By selectively deleting entries in the connection matrix
%  as we progress, we can use a simple find operation each time to give us the next node.
%  ARW 031401 : This routine is critical to the success of the opration. It will break when
%  a perimeter point is connected to more than two other perimeter points. How can this happen? (you ask).
%  Well it can happen when two perimeters share a common point - like interlocking rings. In theory, I think you could have two perimeters sharing many
%  points. We deal with this in some sense in findGroupPerimeter when we eliminate nodes that are not part of any face but the 'just touching' condition
%  is a headache. Here's one solution...
%  The perimeter connection matrix should have 2 entries per row. If it doesn't then iteratively generate new ones with fewer connections until you have a
% (large?) set of perimeter connection matrices with 2 entries per row. E.G. if row t has entries at x,y,z then generate three copies of the conmat,
% one with entries at x,y one with y,z and one with x,z.
% If it has four entries, then you generate  nchoosek(1:4,2) new matrices.
% (You can see how this might mount up if we have many bad points -  if you have two bad points, each generating
% n conMats then you end up with n^2 conmats - fortunately they seem to be rare
% and anyway, connection matrices are sparse and perimeter connection matrices are small and sparse.
% Then just run the regular perimeter detection algorithm on all the conmats and pick the biggest final perimeter.

nVerts=length(mesh.connectionMatrix); % Total number of vertices in the full connection matrix

edgePoints=unique(perimeterEdges(:)); % List of nodes that are on the perimeter
[y x]=size(mesh.connectionMatrix); % = nVerts

% Generate a connection matrix using only the edges - make sure it's symmetric
ec=sparse(perimeterEdges(:,1),perimeterEdges(:,2),1,nVerts,nVerts);
ec=ec+ec';
edgeConMat=sparse((ec~=0));
nnz(edgeConMat)
[eY,eX]=size(edgeConMat);

% Now do something interesting: go through the edge connecton matrix and find all the rows that have more than 2 entries
% Split these up and do some record keeping - we generate new nodes
% Used for checking which perimeter is biggest
nConnects=sum(edgeConMat,2); % How many points on each row?

badPoints=find(nConnects>2);
nBadPoints=length(badPoints);
fprintf('There are %d bad points',nBadPoints);
conMatStack{1}=edgeConMat;

for thisBadPoint=1:nBadPoints
    fprintf('\nLegalizing bad point number %d',thisBadPoint);

	conMatStack=legaliseConMatRow(conMatStack,badPoints(thisBadPoint));
end

nConMats=length(conMatStack);
fprintf('\n%d different edge connection matrices',nConMats);

biggest=-Inf;
biggestSize=-Inf;
goodEdgeConMat=-1;

thisPerim=1; % Index of the current perimeter
nextRow=-99999; % Dummy

foundAllPerimeters=0; % Flag to say we've finished

% Now loop over all the connection matrices
for thisConMat=1:nConMats
	edgeConMat=conMatStack{thisConMat};
	nnz(edgeConMat)
	while (~foundAllPerimeters)
   counter=1;
   [startRow startCol]=find(edgeConMat);
   if (~isempty(startRow)) % If there is one...

      startRow=startRow(1); % ...pick the first non-zero row in the edge connection matrix
      startCol=startCol(1); % Pick the first non-zero column.
      orderedPoints{thisPerim}.points=startRow;

      thisRow=startRow;
      deleteCol=startCol;
      nextRow=-Inf;
      foundEnd=0;
      edgeConMat(thisRow,deleteCol)=0; % Zero one of the entries in this row

      while ((~foundEnd)&(counter<10000))

         % Do the deletions
         edgeConMat(thisRow,deleteCol)=0; % Zero one of the entries in this row
         edgeConMat(deleteCol,thisRow)=0; % Zero one of the entries in this row

         [thisCol]=find(edgeConMat(thisRow,:));

         thisCol=unique(thisCol(:));
         if (length(thisCol)>1)
            disp ('Warning!! - thiscol has more than 1 entry:');
            disp (thisCol);
            disp ('Perim node is connected to many other perim nodes. Carrying on but expect trouble....');
            % Can we just ignore superfluous ones? We'll press ahead regardless...
         	thisCol=thisCol(1);
         end

         if (~isempty(thisCol)) % If there >is< another point in this perimeter
            orderedPoints{thisPerim}.points=[orderedPoints{thisPerim}.points;thisCol];
             deleteCol=thisRow;
             thisRow=thisCol;

         else % We've found the end of the perimeter.
           % fprintf ('\n.');

            foundEnd=1;
            orderedPoints{thisPerim}.size=counter;

         end % endif
         counter=counter+1;

      end % end while not startRow

      if (orderedPoints{thisPerim}.size>biggestSize) % Have to return the index of the larges tperim
         biggestSize=orderedPoints{thisPerim}.size;
         biggest=thisPerim;
		 goodEdgeConMat=conMatStack{thisConMat};
      end % end size check

      fprintf('\n%d points');

   end % End if anything found to start with

     foundAllPerimeters=(nnz(edgeConMat)==0);
     thisPerim=thisPerim+1;
	end % End while not all perims found
end % Next con mat

fprintf('\nFound %d perimeters...',thisPerim-1);
[goodY,goodX]=find(triu(goodEdgeConMat));
goodPerimEdges=[goodY(:),goodX(:)];


function [trg] = read_eep_trg(fn);

% READ_EEP_TRG reads triggers from an EEProbe *.trg file
%
% This function returns an Nx1 array with the N triggers
%
% [trg] = read_eep_trg(filename)
%
% trg(i).time   ... trigger latency in ms
% trg(i).offset ... byte offset
% trg(i).code   ... trigger code (string)
% trg(i).type   ... numeric value of trg.code
%
% where i as number between 1 and N (the number of triggers found)
%
% An EEProbe trigger file is formatted like
%   0.00195312 256
%   0.000     10350  __
%   17.033   2242926   1
%   20.535   2701934   5
%   21.096   2775406  13
%   21.098   2775662   8
%   ...
% where the first column is the trigger latency in seconds, the second
% column is the byte offset in the file and the third column is the triggercode.
% The triggercode can be numeric or a string. The first line of the file contains the
% sample duration.
%
% Author: Robert Oostenveld, Aalborg University, Denmark, 11 March 2003
%
% See also READ_EEP_CNT, READ_EEP_REJ, READ_EEP_AVR
%

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

% $Log: not supported by cvs2svn $
% Revision 1.1  2004/11/26 13:17:02  jwiskerke
% Added m-files without binary code in maple distribution.
%
% Revision 1.2  2003/10/24 13:34:41  Maarten-Jan Hoeve
% Added GNU Licence and updated revision history
%
% Revision 1.1.1.2  2003/10/17 09:55:20  mvelde
% updated: consistent copyrights, arguments/data labels, fixed some typos
%
% Revision 1.1.1.1  2003/03/11 15:24:51  roberto
% updated help and copyrights
% ANT Software BV, The Netherlands, www.ant-neuro.com / info@ant-neuro.com
%

trg = [];

fid = fopen(fn, 'rb');
if fid<0
   return
end

header = fgetl(fid);
while ~feof(fid)
  tmp = fscanf(fid, '%f %d %s', 3);
  if ~isempty(tmp)
    new.time   = tmp(1)*1000;			% in ms
    new.offset = tmp(2)+1;			% offset 1
    new.code   = char(tmp(3:end));		% string
    new.type   = str2double(new.code);		% numeric
    trg = [trg; new];
  end
end

fclose(fid);


function [v,H,R,azel,exc,stat,rtk]=ppp_res(post,x,rtk,obs,nav,sv,dr,exc)

global glc
stat=1; opt=rtk.opt; MAXSAT=glc.MAXSAT; VAR_GLO_IFB=0.6^2;%#ok
nobs=size(obs,1); nf=rtk.NF;
v=zeros(2*nobs*nf,1); H=zeros(2*nobs*nf,rtk.nx); var=zeros(2*nobs*nf,1);
azel=zeros(nobs,2); dants=zeros(3,1); dgrav=0;
obsi=zeros(64,1); frqi=zeros(64,1); ve=zeros(64,1);

for i=1:glc.MAXSAT
    for j=1:opt.nf
        rtk.sat(i).vsat(j)=0;
    end
end

% earth tide correction
rr=x(1:3)+dr;
pos=ecef2pos(rr);

nv=1;ne=0;
for i=1:nobs

    sat=obs(i).sat; lam=nav.lam(sat,:);lam_=[nav.lam(sat,:),nav.lam(sat+1,:)];
    if lam_(fix(j/2)+1)==0||lam(1)==0,continue;end

    % satellite information
    rs=sv(i).pos; dts=sv(i).dts; var_rs=sv(i).vars; vs=sv(i).vel; svh=sv(i).svh;

    % distance/light of sight/azimuth/elevation
    [r,LOS]=geodist(rs,rr); azel(i,:)=satazel(pos,LOS);
    if r<=0||azel(i,2)<opt.elmin,continue;end

    [sys,~]=satsys(sat);
    if sys==0||rtk.sat(sat).vs==0||satexclude(sat,var_rs,svh,opt)==0||exc(i)==1
        exc(i)=1; continue;
    end

    % tropospheric and ionospheric model
    [dtdx,dtrop,vart,stat_t]=model_trop(obs(i).time,pos,azel(i,:),rtk,x,nav);
    [diono,vari,stat_i]    =model_iono(obs(i).time,pos,azel(i,:),rtk,x,nav,sat);
    if stat_t==0||stat_i==0,continue;end

    % satellite and receiver antenna model
    if opt.posopt(1)==1
        dants=satantpcv(rs,rr,nav.pcvs(sat));
    end
    dantr=antmodel(sat,opt.pcvr,opt.antdel(1,:),azel(i,:),opt.posopt(2));

    % phase windup model
    [rtk.sat(sat).phw,stat_tmp]=model_phw(rtk.sol.time,sat,nav.pcvs(sat).type,opt.posopt(3),rs,rr,vs,rtk.sat(sat).phw);
    if stat_tmp==0,continue;end

    % gravitational delay correction
    if opt.posopt(7)==1
        dgrav=model_grav(sys,rr,rs);
    end

    % corrected phase and code measurements
    [L,P,Lc,Pc]=corr_meas(rtk,obs(i),nav,dantr,dants,rtk.sat(sat).phw);

    j=0;
    while j<2*nf
        dcb=0; bias=0;

        if opt.ionoopt==glc.IONOOPT_IFLC
            if rem(j,2)==0,y=Lc;else,y=Pc;end
            if y==0,j=j+1;continue;end
        else
            if rem(j,2)==0,y=L(fix(j/2)+1);else,y=P(fix(j/2)+1);end
            if y==0,j=j+1;continue;end
            if sys==glc.SYS_GLO,mm=2;else,mm=1;end
            if fix(j/2)==1,dcb=-nav.rbias(mm,1);end
        end

        if rem(j,2)==0,C_K1=-1;else,C_K1=1;end
        gama=(lam(fix(j/2)+1)/lam(1))^2;
        C=gama*ionmapf(pos,azel(i,:))*C_K1;

        H(nv,:)=zeros(1,rtk.nx);
        H(nv,1:3)=-LOS;

        % receiver clock
        dtr=0;
        if     sys==glc.SYS_GPS
            dtr=x(rtk.ic+1);
            H(nv,rtk.ic+1)=1;
        elseif sys==glc.SYS_GLO
            dtr=x(rtk.ic+1)+x(rtk.ic+2);
            H(nv,rtk.ic+1)=1; H(nv,rtk.ic+2)=1;
            % for GLONASS icb
            if opt.gloicb==glc.GLOICB_LNF
                if (nf==1&&(fix(j/2)==0&&rem(j,2)==1))||(nf==2&&(fix(j/2)==1&&rem(j,2)==1))
                    frq=get_glo_fcn(sat,nav);
                    dtr=dtr+frq*x(rtk.iicb+1);
                    H(nv,rtk.iicb+1)=frq;
                end
            elseif opt.gloicb==glc.GLOICB_QUAD
                if (nf==1&&(fix(j/2)==0&&rem(j,2)==1))||(nf==2&&(fix(j/2)==1&&rem(j,2)==1))
                    frq=get_glo_fcn(sat,nav);
                    dtr=dtr+frq*x(rtk.iicb+1);
                    H(nv,rtk.iicb+1)=frq;
                    dtr=dtr+frq^2*x(rtk.iicb+2);
                    H(nv,rtk.iicb+2)=frq^2;
                end
            end
        elseif sys==glc.SYS_GAL
            dtr=x(rtk.ic+1)+x(rtk.ic+3);
            H(nv,rtk.ic+1)=1; H(nv,rtk.ic+3)=1;
        elseif sys==glc.SYS_BDS
            dtr=x(rtk.ic+1)+x(rtk.ic+4);
            H(nv,rtk.ic+1)=1; H(nv,rtk.ic+4)=1;
        elseif sys==glc.SYS_QZS
            dtr=x(rtk.ic+1)+x(rtk.ic+5);
            H(nv,rtk.ic+1)=1; H(nv,rtk.ic+5)=1;
        end

        % troposphere
        if opt.tropopt==glc.TROPOPT_EST||opt.tropopt==glc.TROPOPT_ESTG
            H(nv,rtk.it+1)=dtdx(1);
            if opt.tropopt==glc.TROPOPT_ESTG
                H(nv,rtk.it+2)=dtdx(2);H(nv,rtk.it+3)=dtdx(3);
            end
        end

        % ionosphere
        if opt.ionoopt==glc.IONOOPT_EST
            if rtk.x(rtk.ii+sat)==0,j=j+1;continue;end
            H(nv,rtk.ii+sat)=C;
        end

        % L5-receiver-dcb
        if fix(j/2)==2&&rem(j,2)==1
            dcb=dcb+rtk.x(rtk.id+1);
            H(nv,rtk.id+1)=1;
        end

        % ambiguity
        if rem(j,2)==0
            bias=x(rtk.ib+fix(j/2)*MAXSAT+sat);
            if bias==0,j=j+1;continue;end
            H(nv,rtk.ib+fix(j/2)*MAXSAT+sat)=1;
        end

        % residual
        dtS=dts*glc.CLIGHT;
        v(nv,1)=y-(r+dtr-dtS+dtrop+C*diono+dcb+bias-dgrav);

        if rem(j,2)==0,rtk.sat(sat).resc(fix(j/2)+1)=v(nv,1);
        else          ,rtk.sat(sat).resp(fix(j/2)+1)=v(nv,1);
        end

        % variance
        var_rr=varerr_ppp(sys,azel(i,2),fix(j/2),rem(j,2),opt);
        var(nv,1)=var_rr+var_rs+vart+C^2*vari;
        %if sys==glc.SYS_GLO&&rem(j,2)==1,var(nv,1)=var(nv,1)+VAR_GLO_IFB;end

        % reject satellite by pre-fit residuals
        if post==0&&opt.maxinno>0&&abs(v(nv))>opt.maxinno
            exc(i)=1;
            rtk.sat(sat).rejc(rem(j,2)+1)=rtk.sat(sat).rejc(rem(j,2)+1)+1;
            j=j+1; continue;
        end

        % record large post-fit residuals
        if post~=0&&abs(v(nv))>sqrt(var(nv))*4
            obsi(ne+1)=i;frqi(ne+1)=j;ve(ne+1)=v(nv);
            ne=ne+1;
        end

        if rem(j,2)==0,rtk.sat(sat).vsat(fix(j/2)+1)=1;end

        nv=nv+1;
        j=j+1;
    end

end

if nv-1==0
    v=NaN;H=NaN;var=NaN;
elseif nv-1<2*nobs*nf
    v(nv:end)=[]; H(nv:end,:)=[]; var(nv:end)=[];
end
obsi(ne+1:end,:)=[];frqi(ne+1:end,:)=[];ve(ne+1:end,:)=[];

% reject satellite with large and max post-fit residual
if post~=0 && ne>0
    vmax=ve(1);maxobs=obsi(1);maxfrqi=frqi(1);rej=1; %#ok
    j=2;
    while j<=ne
        if abs(vmax)>=abs(ve(j))
            j=j+1; continue;
        end
        vmax=ve(j);maxobs=obsi(j);maxfrqi=frqi(j);rej=j; %#ok
        j=j+1;
    end
    sat=obs(maxobs).sat;
    exc(maxobs)=1;
    rtk.sat(sat).rejc(rem(j,2)+1)=rtk.sat(sat).rejc(rem(j,2)+1)+1;
    stat=0;
    ve(rej)=0; %#ok
end

% measurement noise matrix
if nv-1>0
    R=zeros(nv-1,nv-1);
    for i=1:nv-1
        for j=1:nv-1
            if i==j
                R(i,j)=var(i);
            else
                R(i,j)=0;
            end
        end
    end
else
    R=NaN;
end

if post==0
    stat=nv-1;
end

return


%-------------------------------------------------------------------------------------------------------------------%
%
% 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


function [boundingbox] = bbreg(boundingbox,reg)
	%calibrate bouding boxes
	if size(reg,2)==1
		reg=reshape(reg,[size(reg,3) size(reg,4)])';
	end
	w=[boundingbox(:,3)-boundingbox(:,1)]+1;
	h=[boundingbox(:,4)-boundingbox(:,2)]+1;
	boundingbox(:,1:4)=[boundingbox(:,1)+reg(:,1).*w boundingbox(:,2)+reg(:,2).*h boundingbox(:,3)+reg(:,3).*w boundingbox(:,4)+reg(:,4).*h];
end


% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function qp = plus(q1, q2)
%PLUS Add two quaternion objects
%
% Q1+Q2 is the element-wise sum of quaternion elements.

    if isa(q1, 'Quaternion') & isa(q2, 'Quaternion')

        qp = Quaternion(double(q1) + double(q2));
    end
end


function volvisGUI(x,y,z,v)
% volvisGUI Interactive volume visualization.
%
%   Example:
%   [x,y,z,v] = flow;
%   volvisGUI(x,y,z,v)

%   Copyright 2007 The MathWorks, Inc.

%% Initalize visualization
figure;
s = volumeVisualization(x,y,z,v);
s.addSlicePlane(s.xMin);

%% Add uicontrol
hSlider = uicontrol(...
    'Units','normalized', ...
    'Position',[.75 .05 .2 .05], ...
    'Style','slider', ...
    'Min',s.xMin, ...
    'Max',s.xMax, ...
    'Value',s.xMin, ...
    'Callback',@updateSliderPosition);

%%
    function updateSliderPosition(varargin)
        s.deleteLastSlicePlane();
        x = get(hSlider,'Value');
        s.addSlicePlane(x);
    end

end