% Q = vbfa(D, Y, W_module, X_module, noise_module, ...)
%
% Variational Bayesian (VB) factor analysis (FA) learning algorithm with
% changeable modules for the latent variables.

function Q = vbfa(D, Y, W_module, X_module, noise_module, varargin)

[M,N] = size(Y);

options = struct('maxiter', 100, ...
                 'update_x', 1, ...
                 'update_w', 1, ...
                 'update_noise', 1, ...
                 'rotate', 1, ...
                 'rotation_checkgrad', false, ...
                 'rotation_show', false, ...
                 'rotation_maxiter', 10, ...
                 'debug', false, ...
                 'autosave', false,...
                 'autosavefile', 'vbfa_autosave');

[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

% Initialize
disp('Initializing variables..')
[Q.v_W,Q.W,Q.CovW] = W_module.initialize();
[Q.v_X,Q.X,Q.CovX] = X_module.initialize();
[Q.Tau] = noise_module.initialize();
Q.W_module = W_module;
Q.X_module = X_module;
Q.noise_module = noise_module;

% Observed values
Obs = ~isnan(Y);
MN_obs = sum(Obs(:));

% Replace missing values with zeros for computational reasons
Y(~Obs) = 0;

%%
%% VB learning

loglikelihood_old = -inf;
Q.loglikelihood = nan(options.maxiter,1);

Q.Y = Y;

KL_W = -inf;
KL_X = -inf;
KL_Tau = -inf;

%f = zeros(4,options.maxiter);

disp('Starting the VB iteration..')
for ind=1:options.maxiter

  startitercpu = cputime;

  %
  % Update variables
  %

  if index_selected(ind, options.update_x)
    if index_selected(ind, options.debug)
      disp('Update X')
    end
    [Q.v_X,Q.X,Q.CovX,KL_X] = X_module.update(ind, Y, Obs, Q.v_W, Q.W, Q.CovW, Q.Tau);
  end

  if index_selected(ind, options.update_w)
    if options.debug, disp('Update W'), end
    if index_selected(ind, options.debug)
      disp('Update W')
    end
    [Q.v_W,Q.W,Q.CovW,KL_W] = W_module.update(ind, Y', Obs', Q.v_X, Q.X, Q.CovX, Q.Tau');
  end

  % Compute squared errors <(y_mn-w_m*x_n)^2>
  % (used by noise update and loglikelihood)
  E2 = zeros(size(Y));
  Yh = Q.W'*Q.X;
  E2 = Y.*Y;
  E2 = E2 - 2*Y.*Yh;
  if ndims(Q.CovW) == 2 && ndims(Q.CovX) == 2
    E2 = E2 + Yh.^2 + Q.W'.^2*Q.CovX + Q.CovW'*Q.X.^2 + Q.CovW'*Q.CovX;
  elseif ndims(Q.CovW) == 3 && ndims(Q.CovX) == 3
    xx = bsxfun(@times, reshape(Q.X,[D,1,N]), reshape(Q.X,[1,D,N])) + Q.CovX;
    xx = reshape(xx, [D*D,N]);
    ww = bsxfun(@times, reshape(Q.W,[D,1,M]), reshape(Q.W,[1,D,M])) + Q.CovW;
    ww = reshape(ww, [D*D,M]);
    E2 = E2 + ww'*xx;
  else
    % TODO: Optimize this..
    warning('Optimize this..');
    for m=1:M
      for n=1:N
        if Obs(m,n)
          if ndims(Q.CovW) == 2
            ww = Q.W(:,m)*Q.W(:,m)' + diag(Q.CovW(:,m));
          else
            ww = Q.W(:,m)*Q.W(:,m)' + Q.CovW(:,:,m);
          end
          if ndims(Q.CovX) == 2
            xx = Q.X(:,n)*Q.X(:,n)' + diag(Q.CovX(:,n));
          else
            xx = Q.X(:,n)*Q.X(:,n)' + Q.CovX(:,:,n);
          end
          %WX_WX = WX_WX + traceprod(ww, xx);
          E2(m,n) = E2(m,n) + traceprod(ww, xx);
        end
      end
    end
  end
  E2(~Obs) = 0;

  if index_selected(ind, options.update_noise)
    if index_selected(ind, options.debug)
      disp('Update Tau')
    end

    [Q.Tau,LogTau,KL_Tau] = noise_module.update(ind, E2, Obs);
%    [Tau,lowerbound] = noise_module.update(Y, Obs, v_W, W, CovW, v_X, X, CovX);
  end

  %
  % Rotate
  %

  if index_selected(ind, options.rotate)

    % TODO: You could optimize the hyperparameters at the same time?
    disp('Rotating..')
    A = eye(D);

    if index_selected(ind, options.rotation_checkgrad)
      mycheckgrad(@rotation_cost, A(:) + 0.5*randn(D^2,1), 1e-3, W_module, ...
                  X_module);
    end
    A = minimize(A(:), @rotation_cost, options.rotation_maxiter, W_module, ...
                 X_module);
    A = reshape(A, [D D]);
    if index_selected(ind, options.rotation_show)
      A
    end
    [Q.W, Q.CovW] = W_module.rotate(A);
    [Q.X, Q.CovX] = X_module.rotate(inv(A)');

  end

  %
  % Evaluate VB lower bound
  %

  % Likelihood part: <log p(Y|...)>
  logpdf_y = gaussian_logpdf(Q.Tau(Obs)'*E2(Obs), ...
                             0, ...
                             0, ...
                             -sum(LogTau(Obs)), ...
                             MN_obs);

  % Lower bound
  Q.loglikelihood(ind) = logpdf_y - KL_W - KL_X - KL_Tau;

% $$$   if ~isreal(Q.loglikelihood(ind))
% $$$     KL_W
% $$$     KL_X
% $$$     KL_Tau
% $$$     error('Loglikelihood imaginary')
% $$$   end

  if Q.loglikelihood(ind) < loglikelihood_old
    warning(sprintf('Loglikelihood lower bound decreased relatively %e!', ...
                    (loglikelihood_old - Q.loglikelihood(ind)) / loglikelihood_old));
  end

  fprintf('Iteration step %d: loglikelihood=%e (%.2f seconds)\n', ind, ...
          Q.loglikelihood(ind), cputime()-startitercpu);

% $$$   f(1,ind) = KL_W;
% $$$   f(2,ind) = KL_X;
% $$$   f(3,ind) = KL_Tau;
% $$$   f(4,ind) = loglikelihood(ind);

  loglikelihood_old = Q.loglikelihood(ind);

  if index_selected(ind, options.autosave)
    fprintf('Saving results to %s...', options.autosavefile);
    save(options.autosavefile, '-struct', 'Q');
    fprintf(' done.\n');
  end

end

% $$$ figure
% $$$ plot(f');

% $$$ if nargout >= 1
% $$$
% $$$   Q.W_module = W_module;
% $$$   Q.W = W;
% $$$   Q.CovW = CovW;
% $$$
% $$$   Q.X_module = X_module;
% $$$   Q.X = X;
% $$$   Q.CovX = CovX;
% $$$
% $$$   Q.noise_module = noise_module;
% $$$   Q.Tau = Tau;
% $$$
% $$$   Q.loglikelihood = loglikelihood;
% $$$
% $$$ end

%function rotate(W_module, X_module)
%end

function [c, dc] = rotation_cost(a, W_module, X_module)
N = sqrt(length(a));
A = reshape(a, N, N);
[U,S,V] = svd(A);
invS = diag(1./diag(S));
invAt = U*invS*V';
[c_w, dc_w] = W_module.rotation_cost(A, U, S, V);
[c_x, dc_x] = X_module.rotation_cost(invAt, U, invS, V);
dc_x = -invAt*dc_x'*invAt;
c = c_w + c_x;
dc = dc_w(:) + dc_x(:);


% THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION IS RELEASED "AS IS."  THE U.S. GOVERNMENT MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, CONCERNING THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT WILL THE U.S. GOVERNMENT BE LIABLE FOR ANY DAMAGES, INCLUDING ANY LOST PROFITS, LOST SAVINGS OR OTHER INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE, OR INABILITY TO USE, THIS SOFTWARE OR ANY ACCOMPANYING DOCUMENTATION, EVEN IF INFORMED IN ADVANCE OF THE POSSIBILITY OF SUCH DAMAGES.
%
% file: interval_est_r.m
% computes confidence interval estimates for rates.
%
% 022400 tdr created
% 031600 tdr created rate version from interval_est_p.m
% 032200 tdr added cs1, cs2, and tl methods.
% 012601 tdr added input checking.
% 012901 tdr added 1-sided CIs for CS1 method
% 031501 tdr added minimum length method
%

function ci = interval_est_r(x,A,alpha,method)

% ------------------------------------------------------------
% start checking inputs

if nargin ~= 4,
    error('Requires four input arguments (x,A,alpha,method)');
end

% inputs must all be scalars
if (max(max([size(x); size(A); size(alpha)])) ~= 1)
    error('Non-scalar input');
end;

% x must be integer
if (round(x) ~= x)
    x = round(x);
    warning('Non-integer input x');
end;

% A must be > 0
if (A <= 0),
   ci = [ NaN NaN NaN];
   warning('A <= 0');
   return;
end;

% x must be >= 0
if ( x < 0),
   ci = [ NaN NaN NaN];
   warning('x < 0');
   return;
end;

% alpha must be > 0.0 and < 1.0
if ( alpha < 0 | alpha > 1),
   ci = [ NaN NaN NaN];
   warning('alpha < 0 or alpha > 1');
   return;
end;

% normalize case
if strcmpi(method,'cs1'), method = 'cs1'; end;
if strcmpi(method,'cs1_1s'), method = 'cs1_1s'; end;
if strcmpi(method,'cs2'), method = 'cs2'; end;
if strcmpi(method,'ibp'), method = 'ibp'; end;
%if strcmpi(method,'ibp_1s'), method = 'ibp_1s'; end;
if strcmpi(method,'ml'), method = 'ml'; end;
if strcmpi(method,'nibp'), method = 'nibp'; end;
%if strcmpi(method,'nibp_1s'), method = 'nibp_1s'; end;
if strcmpi(method,'na'), method = 'na'; end;
if strcmpi(method,'tl'), method = 'tl'; end;

% end checking inputs
% ------------------------------------------------------------

switch method
case 'cs1',
   ci = get_r_ci_cs1(x,A,alpha);
case 'cs1_1s',
   ci = get_r_ci_cs1_1s(x,A,alpha);
case 'cs2',
   ci = get_r_ci_cs2(x,A,alpha);
case 'ibp',
   ci = get_r_ci_ibp(x,A,alpha);
case 'ml',
   ci = get_r_ci_ml(x,A,alpha);
case 'na',
   ci = get_r_ci_na(x,A,alpha);
case 'tl',
   ci = get_r_ci_tl(x,A,alpha);
otherwise
   error('Method not recognized. Methods Available: cs1/cs2 - COMPASE Standards, ibp - Integration of Bayes Posterior; ml - minimum length; na - Normal Approximation; tl - Table Lookup');
end;


function adjList = meshFaceAdjacency(vertices, edges, faces)
%MESHFACEADJACENCY Compute adjacency list of face around each face.
%
%
%   Example
%     % Create a sample 3D mesh
%     [v, e, f] = createDodecahedron;
%     adjList = meshFaceAdjacency(v, e, f);
%     figure; hold on; axis equal; view([100 40]);
%     drawMesh(v, f);
%     % draw sample face in a different color
%     drawMesh(v, f(1, :), 'faceColor', 'b');
%     % draw the neighbors of a sample face
%     drawMesh(v, f(adjList{1}, :), 'faceColor', 'g')
%
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2010-10-04, using Matlab 7.9.0.529 (R2009b)
% Copyright 2010-2022 INRA - Cepia Software Platform

edgeFaceList = meshEdgeFaces(vertices, edges, faces);

% allocate memory for adjacency list
nFaces = max(edgeFaceList(:));
adjList = cell(1, nFaces);

% iterate over edges to populate adjacency list
for iEdge = 1:size(edgeFaceList)
    f1 = edgeFaceList(iEdge, 1);
    f2 = edgeFaceList(iEdge, 2);
    adjList{f1} = [adjList{f1} f2];
    adjList{f2} = [adjList{f2} f1];
end


function [xh,pf, P, L] = RBPF(x_true, y, LRS, u, a, pf, Nsamples, resampling_strategy)

    % x_true: true position of robot
    % y: True laser scan
    % LRS: laser scan inforamtion
    % u: Movement control
    % a: Odometry parameters found via Motion_Model_Velocity_Test
    % pf: pf class with all particle filter information
    % Nsamples: number of samples for the Particle Filter
    % resampling_startegy: DO NOT CHANGE

    %% Variable Initialization

    k = pf.k;
    NPC = pf.NPC;
    q_past = pf.q(:,k-1);
    x_past= pf.particles(:,:,k-1);

    % Initialise  new variables
    x_initial_estimate=zeros(3, NPC);           % Initial estimate from previous Xt-1 and Ut
    yh=zeros(2, 361, NPC);                      % Laser scan estimate from x_initial_estimate
    x_new_sm=zeros(3, NPC);                     % New position after scan matching (ICP)
    x_sampled=zeros(3,Nsamples, NPC);           % Smaple position around every particle position
    p_xt=zeros(Nsamples, NPC);                  % Density function for odometry (Motion_Model_velocity)
    p_zt=zeros(Nsamples, NPC);                  % Density function for scans (Scan Matching)
    tau_xt=zeros(Nsamples, NPC);                % Optimal Proposal Distribution
    n=zeros(1,NPC);                             % Sample normalizer
    mu=zeros(3, NPC);                           % Mean for Gaussian distribution from all particle samples
    cov=zeros(1, NPC);                          % Covariance for Gaussian distribution from all particle samples
    q=zeros(size(q_past));                      % Particle weights
    x_final=zeros(3, NPC);                      % final estimate particle position after RBPF

    %% RBPF for every particle
    for i = 1:NPC

        %Estimate particle position from x_(t-1) after Ut
        x_initial_estimate(:,i) = Kinematics(x_past(:,i),u+sqrt(pf.R1)*randn(length(u),1));

        if  x_initial_estimate(3,i)>2*pi
            x_initial_estimate(3,i)=x_initial_estimate(3,i)-2*pi;
        elseif x_initial_estimate(3,i)<-2*pi
            x_initial_estimate(3,i)=x_initial_estimate(3,i)+2*pi;
        end

        %Scan Matcher returns estimate of laser given an initial particle position estimate
        yh(:,:,i) = Measurement_Estimate_From_Grid(x_initial_estimate(:,i),pf.P(:,:, i),LRS.Resolution,LRS.FoV, LRS.MaxDistance, pf.UsableArea);

        % Scan pre-processing prior to ICP algortithm
            y_help=y;
            yh_help=yh(:,:,i);

            % Use data inside Usable Area only
            y_help(y_help>pf.UsableArea)=nan;
            yh_help(yh_help>pf.UsableArea)=nan;

            % Chose data that whose angle is usable in both scans
            find_y_help=isnan(y_help(2,:));
            find_yh_help=isnan(yh_help(2,:));
            del_cols=or(find_y_help, find_yh_help);
            y_help(:,(del_cols(1,:)==1))=[];
            yh_help(:,(del_cols(1,:)==1))=[];

            % Transform to Polar coordinates and World frame
            y_help=Polar2Cart(y_help);
            yh_help=Polar2Cart(yh_help);
            y_help=Rotate_Data(y_help, x_true);
            yh_help=Robot2World([x_initial_estimate(1,i); x_initial_estimate(2,i); x_initial_estimate(3,i)], yh_help);

            % Add the particles position so that its also transformed
            y_help=horzcat(y_help, x_true(1:2));
            yh_help=horzcat(yh_help, x_initial_estimate(1:2,i));

        failure=false;                               % Initialse ICP failure to FALSE

        try
        [TR,TT, data] = ICP(y_help, yh_help);        % ICP algortithm returns the translational and rotational matrices and the resulting data
        catch
            failure=true;
        end
        % Check if ICP failed
        if abs(TT(1))>50  || abs(TT(2))>50
            failure=true;
        end

        if failure
            disp('ICP failed')
            x_final(:,i)=x_initial_estimate(:,i);    % If ICP failed used last known position estimate
            q(i)=q_past(i);                          % do not change particle weights
        else

            theta_new_sm=x_initial_estimate(3,i)+asin(TR(2,1));     % Modify theta angle according to the rotation given by ICP
            x_new_sm(:,i)=[data(1:2,end); theta_new_sm];            % Use rotated+translated initila_estimate as new (and better) positon

            %Plot grid-map, all positions and all scans (Black: True postion, Blue: Position and scan initial estimates, red:
            %Converged position and laser data
                figure(42)
                set(gcf,'units','normalized','outerposition',[1 0 1 1]);
                clf;
                caxis([0, 1]);
                colormap gray;
                colormap(flipud(colormap));
                hold on;
                imagesc(flipud(pf.P(:,:, i)));

                % Plot scans
                plot(y_help(1,:), y_help(2,:), '.g')
                plot(yh_help(1,:), yh_help(2,:), '.b')
                plot(data(1,:), data(2,:), '.r')

                % Plot positions
                plot(x_true(1), x_true(2), 'dk')
                plot(x_initial_estimate(1,i), x_initial_estimate(2, i), 'db')
                plot(x_new_sm(1,i), x_new_sm(2,i), 'dr')
                hold off;

                % Save each iteration map into as png
                % str=sprintf('path_images/fig%d.png', pf.k);
                % saveas(gcf, str)

            % Randomize samples around good estimate (x_new_sm) of particle position
            theta_samples=x_new_sm(3,i);
            if theta_samples > 2*pi
                theta_samples=theta_samples-2*pi;
            elseif theta_samples < -2*pi
                theta_samples=theta_samples-2*pi;
            end

            x_sampled(:,:, i) = [ normrnd([x_new_sm(1,i).*ones(1, Nsamples); x_new_sm(2,i).*ones(1, Nsamples)],0.5, [2 Nsamples]); theta_samples.*ones(1, Nsamples)];

            for h=1:Nsamples

                % Calculate odometry probability for that sample
                p_xt(h,i)= Motion_Model_Velocity(x_sampled(:,h,i), u, x_past(:,i), a);

                % Calculate scan matching probability for that sample
                ytru=y;
                yest = Measurement_Estimate_From_Grid(x_sampled(:,h,i),pf.P(:,:, i),LRS.Resolution,LRS.FoV, LRS.MaxDistance, pf.UsableArea);
                yest(yest>pf.UsableArea)=nan;
                find_yest=isnan(yest(2,:));
                yest(:,(find_yest(1,:)==1))=[];
                ytru(:,(find_yest(1,:)==1))=[];
                p_zt(h,i) = Measurement_Model(ytru,yest,[1 1]);

                % Calculate optimal proposal distribution
                tau_xt(h,i)=abs(p_xt(h,i)*p_zt(h,i));

            end

            n(i)=sum(tau_xt(:,i));                                  % Calculate sample normalizer
            mu(1, i)=(1/n(i))*(x_sampled(1,:,i)*tau_xt(:,i));       % Calculate mu for gaussian distribution from samples
            mu(2, i)=(1/n(i))*(x_sampled(2,:,i)*tau_xt(:,i));
            mu(3, i)=theta_samples;                                 % Orientation of samples is constant (the one from ICP)

            % Calculate covariance for gaussian distribution from samples
            cov_sum=0;
            for h=1:Nsamples
                cov_sum=cov_sum+(x_sampled(1:2,h,i)-mu(1:2,i))'*(x_sampled(1:2,h,i)-mu(1:2,i))*tau_xt(h,i);
            end
            cov(i)=(1/n(i))*cov_sum;

            %Calculate new weights
            q(i)=q_past(i)*n(i);

            %Sample gaussian for final position of particle
            x_final(:,i)=[normrnd([mu(1,i); mu(2,i)],cov(i), [2 1]); mu(3, i)];
        end

        %Update GridMap for taht particle
        [pf.P(:,:,i), pf.L(:,:,i)] = Occupancy_Grid_Mapping(pf.L(:,:,i), x_final(:,i), y, pf.UsableArea, pf.L0, pf.gridsize, LRS);

    end

    q=q/norm(q,1);                      % Normalize weights
    Neff = 1/sum(q.^2);                 % Calcuklate Neff for possible resampling

    %% Resampling step

    resample_percentage = 0.3;          % 0.5 is the best resample percentage proposed by Cyrill Stachniss
    Nt = resample_percentage*NPC;
    if Neff < Nt
        disp('Resampling');
        [x_final(:,:), q, idx] = Resample(x_final(:,:), q, resampling_strategy);
        P_save_resamp=pf.P(:,:,:);
        L_save_resamp=pf.L(:,:,:);
        for n=1:NPC
           pf.P(:,:,n)=P_save_resamp(:,:,idx(n));
           pf.L(:,:,n)=L_save_resamp(:,:,idx(n));
        end
    end

    pf.q(:,k) = q;                      % Save particle weights (might have changed during resampling)

    %% Calculate estimate of true position (xh) given all weighted particles
    xh = zeros(3,1);
    for i=1:NPC
            xh = xh +(q(i).*x_final(:,i));
    end
    pf.xh(:, k)= xh;

    %% Store final position of all particles
    pf.particles(:,:,k) = x_final(:,:);

    % Make sure no errors have occured. If so probably becasue of p_xt or p_zt being equal to 0
    if isnan(x_final(1,i)) || isnan(x_final(2,i))
                1; %debugger
                x_final=x_initial_estimate;
    end

    %% Visualize results: Plot Particles
%         figure(89)
%         set(gcf,'units','normalized','outerposition',[-1 0 1 1]);
%         clf;
%         hold on;
%         for g = 1:size(map,2)
%             plot([map(1,g) map(3,g)],[map(2,g) map(4,g)],'k')
%         end
%         plot(squeeze(pf.particles(1,:,pf.k-1))',squeeze(pf.particles(2,:,pf.k-1))','bo');
%         plot(squeeze(pf.particles(1,:,pf.k))',squeeze(pf.particles(2,:,pf.k))','k+');
%         plot(squeeze(pf.xh(1,pf.k))',squeeze(pf.xh(2,pf.k))','ro');
%         plot(x_true(1), x_true(2), 'go')
%         hold off;

end


function g = rbfhKernGradient(kern, x1, x2, covGrad)

% RBFHKERNGRADIENT Gradient of RBFH kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% radial basis function heat kernel's parameters. As well as the kernel
% structure and the input positions, the user provides a matrix PARTIAL
% which gives the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations for which the gradients are being
% computed.
% ARG covGrad : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG covGrad : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO: rbfhKernParamInit, kernGradient
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    covGrad = x2;
    x2 = x1;
end
if size(x1, 2) ~= 2 || size(x2, 2) ~= 2
    error('Input can only have two columns');
end

% Split the domain into time domain and spatial domain and account for
% missing values. If there are no missing values the computation of the
% kernel is a pointwise prodruct, otherwise it is a kronecker product.
t1 = x1(x1(:,1)~=Inf,1);
t2 = x2(x2(:,1)~=Inf,1);
s1 = x1(x1(:,2)~=Inf,2);
s2 = x2(x2(:,2)~=Inf,2);
if (length(t1) == length(s1)) && (length(t2) == length(s2))
    ut1 = unique(t1);
    ut2 = unique(t2);
    us1 = unique(s1);
    us2 = unique(s2);
    if (length(ut1)*length(us1) == length(t1)) && ...
            (length(ut2)*length(us2) == length(t2))
        t1 = ut1; s1 = us1; t2 = ut2; s2 = us2;
        isPointwise = false;
    else
        isPointwise = true;
    end
else
    isPointwise = false;
end

kern.rbf.inverseWidth = kern.inverseWidthTime;
Kt = rbfKernCompute(kern.rbf, t1, t2);
kern.rbf.inverseWidth = kern.inverseWidthSpace;
Ks = rbfKernCompute(kern.rbf, s1, s2);

if isPointwise
    covGradt = covGrad.*Ks;
    covGrads = covGrad.*Kt;
else
    covGradt = zeros(length(t1), length(t2));
    covGrads = zeros(length(s1), length(s2));
    endOne = 0;
    startOne = 1;
    for k=1:length(t1)
        endOne = endOne + length(s1);
        startTwo = 1;
        endTwo = 0;
        for l=1:length(t2)
            endTwo = endTwo + length(s2);
            covGradt(k,l) = sum(sum(covGrad(startOne:endOne, startTwo:endTwo).*Ks));
            startTwo = endTwo + 1;
        end
        startOne = endOne + 1;
    end
    for k=1:length(s1)
        indRows = (k:length(s1):(length(t1)*length(s1)))';
        for l=1:length(s2)
            indCols = l:length(s2):(length(t2)*length(s2));
            covGrads(k,l) = sum(sum(covGrad(indRows, indCols).*Kt));
        end
    end
end

kern.rbf.inverseWidth = kern.inverseWidthTime;
gt = rbfKernGradient(kern.rbf, t1, t2, covGradt);
kern.rbf.inverseWidth = kern.inverseWidthSpace;
gs = rbfKernGradient(kern.rbf, s1, s2, covGrads);

g = [gt(1) gs(1)];


function [rtk,sat_,stat]=estpos(rtk,obs,nav,sv,opt)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%estimate reciever position and clock bias
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global  glc
NX=3+glc.NSYS; MAXITER=10; iter=1;
time=obs(1).time; xr0=[rtk.sol.pos';zeros(glc.NSYS,1)];

while iter<=MAXITER

    % compute residual,measurement model,weight matrix
    [v,H,P,vsat,azel,resp,nv,ns]=rescode(iter,obs,nav,sv,xr0,opt);
    sat_.vsat=vsat;
    sat_.azel=azel;
    sat_.resp=resp;

    if nv<NX, stat=0;return;end

    % least square algrithm
    [dx,Q]=least_square(v,H,P);

    xr0=xr0+dx;
    iter=iter+1;

    if dot(dx,dx)<1e-4
        rtk.sol.time=timeadd(obs(1).time,-xr0(4)/glc.CLIGHT);
        rtk.sol.ns  =ns;
        rtk.sol.ratio=0;
        rtk.sol.pos =xr0(1:3)';
        rtk.sol.vel =zeros(1,3);
        rtk.sol.posP(1)=Q(1,1);rtk.sol.posP(2)=Q(2,2);rtk.sol.posP(3)=Q(3,3);
        rtk.sol.posP(4)=Q(1,2);rtk.sol.posP(5)=Q(2,3);rtk.sol.posP(6)=Q(1,3);
        rtk.sol.dtr(1) =xr0(4)/glc.CLIGHT;
        rtk.sol.dtr(2) =xr0(5)/glc.CLIGHT;
        rtk.sol.dtr(3) =xr0(6)/glc.CLIGHT;
        rtk.sol.dtr(4) =xr0(7)/glc.CLIGHT;
        rtk.sol.dtr(5) =xr0(8)/glc.CLIGHT;

        % validate solution
        stat=valsol(time,v,P,vsat,azel,opt);
        if stat==1
            rtk.sol.stat=glc.SOLQ_SPP; return;
        end
        return;
    end

end

if iter>MAXITER
    stat=0;
    [week,sow]=time2gpst(time);
    fprintf('Warning:GPS week = %d sow = %.3f,SPP iteration divergent!\n',week,sow);
    return;
end

return


function radDepthV = getRadiologicalDepth(xV, yV, zV, gantryAngle, isocenter, isodistance, structNum, scanSet, planC)
%"getRadiologicalDepth"
%   Returns the radiological depth at the points in xV, yV, zV, as
%   determined from scanSet with a point source at sourceV.  structNum
%   tells the interpolation routine to only include the densities of
%   points within a certain structure when calculating the radiological
%   depth.
%
%JRA 06/16/05
%
%Usage:
%  function radDepthV = getRadiologicalDepth(xV, yV, zV, gA, iC, iD, structNum, scanSet, planC)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('planC')
    global planC
end
indexS = planC{end};

siz = size(xV);

%Get mask of requested structure.
rS = getRasterSegments(structNum, planC);
[structMask, slicesV] = rasterToMask(rS, scanSet, planC);
% rS = getRasterSegments(structNum, planC);
% [structMask, slicesV] = rasterToMask(rS, scanSet, planC);
% [surfPoints] = getSurfacePoints(structMask);

%Get coordinates of scan voxels inside the structure.
[xS, yS, zS] = getScanXYZVals(planC{indexS.scan}(scanSet));
[xM, yM, zM] = meshgrid(xS, yS, zS(slicesV));
xM = xM(structMask);
yM = yM(structMask);
zM = zM(structMask);
% [xS, yS, zS] = getScanXYZVals(planC{indexS.scan}(scanSet));
% zS = zS(slicesV);
% xM = xS(surfPoints(:,1));
% yM = yS(surfPoints(:,2));
% zM = zS(surfPoints(:,3));
% clear surfPoints

%Convert points to collimator coordinates.
coll3V = scan2Collimator([xM(:) yM(:) zM(:)], gantryAngle, 0, 0, isocenter, isodistance);
clear xM yM zM;

%Get distance from source to all points.
distsquared = sepsq(coll3V', [0 0 0]');

%Project vector to points to the perpendicular plane at isocenter.
coll3V = coll3V./repmat(coll3V(:,3), [1 3]) * -isodistance;

%Find the extents of X,Y,D for the region covering the structure (ie, all
%nonzero scan numbers since we will set scan densities outside of structure
%to be 0.
minExt = min(coll3V);
maxExt = max(coll3V);
clear coll3V;

minProjX = minExt(1);
maxProjX = maxExt(1);

minProjY = minExt(2);
maxProjY = maxExt(2);

maxDsq = max(distsquared);
minDsq = min(distsquared);
clear distsquared;

%Set points in scanArray outside of structure to 0.
sA = planC{indexS.scan}(scanSet).scanArray;
for i=1:size(sA, 3);
    [ismem, loc] = ismember(i, slicesV);
    if ismem
        slc = sA(:,:,i);
        slc(~structMask(:,:,loc)) = 0;
        sA(:,:,i) = slc;
    else
        sA(:,:,i) = zeros(size(sA(:,:,i)));
    end
end
clear structMask;

%Convert requested points to collimator coordinates.
coll3V = scan2Collimator([xV(:) yV(:) zV(:)], gantryAngle, 0, 0, isocenter, isodistance);
clear xM yM zM;

%Get distance from source to all requested points.
distsquared = sepsq(coll3V', [0 0 0]');

%Project vector to points to the perpendicular plane at isocenter.
coll3V = coll3V./repmat(coll3V(:,3), [1 3]) * -isodistance;

clear xV yV zV

%Find out of bounds points, for which calculation is not necessary,
%radDepth is zero.
OOBPoints = coll3V(:,1) < minProjX | coll3V(:,1) > maxProjX | coll3V(:,2) < minProjY | coll3V(:,2) > maxProjY;

%Clip the r values so they take the density of the nearest calculated
%point (points less than minDsq would take zero, and greater than maxDsq
%would take the last radDepthV value along that ray.
r = clip(distsquared, minDsq, maxDsq, 'limits');

%Set the resolution of the radiological depth calculation (at isocenter).
dX = .5;
dY = .5;
dR = .25;

%Mesh the radiological depth calculation grid in collimator coordinates.
projXV = minProjX:dX:maxProjX;
projYV = minProjY:dY:maxProjY;
distV = sqrt(minDsq):dR:sqrt(maxDsq);
[projXVM, projYVM, distVM] = meshgrid(projXV, projYV, distV);

projDistV = sqrt(sepsq([projXVM(:) projYVM(:) -isodistance*ones(size(projXVM(:)))]', [0 0 0]'));
projXVM = projXVM(:) .* (distVM(:) ./ projDistV);
projYVM = projYVM(:) .* (distVM(:) ./ projDistV);
projZVM = -isodistance*ones(size(distVM(:))) .* (distVM(:) ./ projDistV);

%Convert the depth calculation grid to rectangular coordinates.
newXYZ = collimator2Scan([projXVM(:) projYVM(:) projZVM(:)], gantryAngle, 0, 0, isocenter, isodistance);
% clear projXVM projYVM distVM

%Get the scan values at each point in the radiological depth grid.
[xVS, yVS, zVS] = getScanXYZVals(planC{indexS.scan}(scanSet));

%Interpolate to values in xV/yV/zV from the scanarray, 0 if out of bounds.
scanVals = finterp3(newXYZ(:,1), newXYZ(:,2), newXYZ(:,3), sA, [xVS(1) xVS(2)-xVS(1) xVS(end)], [yVS(1) yVS(2)-yVS(1) yVS(end)], zVS, 0);

scanVals = reshape(scanVals, length(projXV), length(projYV), length(distV));

%Scale values by the resolution of R to get the linear radiological depth.
%Divide by 1024 to get the radiological depth in units of water.
scanVals = scanVals .* (dR / 1024);

%Take the cumsum to get the actual radiological depth at each point in the
%depth calculation grid.
cumDens = cumsum(scanVals, 3);
clear scanVals

cumDens = reshape(cumDens, size(distVM));

%Interpolate to values in xV/yV/zV from the scanarray, 0 if out of bounds.
radDepthV = finterp3(coll3V(:,1), coll3V(:,2), sqrt(distsquared), cumDens, [projXV(1) projXV(2)-projXV(1) projXV(end)], [projYV(1) projYV(2)-projYV(1) projYV(end)], distV, 0);

radDepthV(OOBPoints) = 0;
radDepthV = reshape(radDepthV, siz);


%DEMO_MEMORYSAVE  Demonstration of memory save option in GPstuff
%
%  Description
%    GP = GP_SET(..., 'savememory', 'on');
%
%    This demo consists of various combinations of covariance
%    functions, likelihoods and full/sparse approximations. This demo
%    is intended for showing how to use memory save option in
%    GPstuff. Unfortunately MATLAB doesn't have memory usage
%    monitoring, so the purpose of the demo is only to test that
%    results are the same whether you use memory save or not and to
%    show that the running times with memory saving are little bit
%    longer (more overhead). Memory saving can be enabled with the
%    following command
%
%    Usually GPstuff computes covariance matrix derivatives with
%    respect to all covariance function parameters at once which
%    avoids repeating computations common to each derivatibe, but
%    requires storage of Q NxN matrices, where Q is the number of
%    covariance function parameters. Q can be large, for example, if
%    there are many covariates each having own lengthsacle
%    parameter. With memory save option on, each matrix derivative is
%    computed separately, increasing the computation time little, but
%    reducing the memory usage. In an application, which motivated the
%    implementation of the option, the memory requirement of GPstuff
%    was reduced from over 60GB to less than 6GB.
%
%  See also
%    GP_SET
%
% Copyright (c) 2012 Ville Tolvanen

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

% Classification data with full GP's

S = which('demo_classific');
L = strrep(S,'demo_classific.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];

lik = lik_logit();

pl = prior_t();
pm = prior_sqrtunif();

gpcf = gpcf_neuralnetwork('weightSigma2', [0.9 0.9], 'biasSigma2', 10);
gpcf = gpcf_neuralnetwork(gpcf, 'weightSigma2_prior', pl,'biasSigma2_prior', pm); %

gpcf2 = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 10);
gpcf2 = gpcf_sexp(gpcf2, 'lengthScale_prior', pl,'magnSigma2_prior', pm); %

% GP without memory saving
gp = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-4, 'latent_method', 'EP');
% GP with memory saving option enabled
gp2 = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-4, 'latent_method', 'EP', 'savememory', 'on');

fprintf('Classification (EP), Neuralnetwork covariance function without and with memory saving (optimization and prediction)\n');
opt=optimset('TolX',1e-3,'TolFun',1e-3, 'Display', 'off');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'opt',opt);
[Eft,Varft, lpyt]=gp_pred(gp,x,y,x, 'yt', y);toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'opt',opt);
[Eft2,Varft2, lpyt2]=gp_pred(gp2,x,y,x, 'yt', y);toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft==Eft2)); assert(all(Varft==Varft2)); assert(all(lpyt==lpyt2));

gp = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-6, 'latent_method', 'Laplace');
gp2 = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-6, 'latent_method', 'Laplace', 'savememory', 'on');

fprintf('Classification (Laplace), Squared-Exponential covariance function without and with memory saving (optimization and prediction)\n');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'opt',opt);
[Eft,Varft, lpyt]=gp_pred(gp,x,y,x, 'yt', y);toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'opt',opt);
[Eft2,Varft2, lpyt2]=gp_pred(gp2,x,y,x, 'yt', y);toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft==Eft2)); assert(all(Varft==Varft2)); assert(all(lpyt==lpyt2));

% Regression data with sparse approximations

prevstream=setrandstream();
xx=linspace(1,10,901);
x1=logspace(0,1,100);
x1=round(x1*100)-99;
x=xx(x1)';
y=2*sin(4*x)+0.2*randn(size(x));
xt=[1:0.01:14]';
[n,nin] = size(x);

pl = prior_t('s2', 1);
pm = prior_logunif();
pn = prior_logunif();
gpcfse = gpcf_matern32('lengthScale',0.5,'magnSigma2',1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
lik = lik_gaussian('sigma2', 0.1, 'sigma2_prior', pn);
gp = gp_set('lik', lik, 'cf', gpcfse, 'jitterSigma2', 1e-6);
opt=optimset('TolFun',1e-4,'TolX',1e-4);

fprintf('Regression, FIC GP, Matern-3/2 covariance function, without and with memory saving (optimization and prediction)\n')
Xu=round(10+90*rand(18,1))/10; % Random placement

gp_fic = gp_set(gp, 'type','FIC','X_u',Xu,'infer_params','covariance+likelihood+inducing');
gp_fic2 = gp_set(gp, 'type','FIC','X_u',Xu,'infer_params','covariance+likelihood+inducing', 'savememory', 'on');

opt=optimset('TolFun',1e-4,'TolX',1e-4, 'Display', 'off');

% Optimization and prediction without memory saving
tic,gp_fic=gp_optim(gp_fic,x,y,'opt',opt);
[Eft_fic, Varft_fic] = gp_pred(gp_fic, x, y, xt);
Varft_fic = Varft_fic + gp_fic.lik.sigma2;toc

% Optimization and prediction with memory saving
tic,gp_fic2=gp_optim(gp_fic2,x,y,'opt',opt);
[Eft_fic2, Varft_fic2] = gp_pred(gp_fic2, x, y, xt);
Varft_fic2 = Varft_fic2 + gp_fic.lik.sigma2;toc
setrandstream([],prevstream);

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft_fic==Eft_fic2)); assert(all(Varft_fic==Varft_fic2));

gpcfse = gpcf_sexp('lengthScale',0.5,'magnSigma2',1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gp = gp_set('lik', lik, 'cf', gpcfse, 'jitterSigma2', 1e-6);

gp_fic = gp_set(gp, 'type','VAR','X_u',Xu,'infer_params','covariance+likelihood+inducing');
gp_fic2 = gp_set(gp, 'type','VAR','X_u',Xu,'infer_params','covariance+likelihood+inducing', 'savememory', 'on');

fprintf('Regression, VAR GP, Squared-Exponential covariance function, without and with memory saving (optimization and prediction)\n')

% Optimization and prediction without memory saving
tic,gp_fic=gp_optim(gp_fic,x,y,'opt',opt);
[Eft_fic, Varft_fic] = gp_pred(gp_fic, x, y, xt);
Varft_fic = Varft_fic + gp_fic.lik.sigma2;toc

% Optimization and prediction with memory saving
tic,gp_fic2=gp_optim(gp_fic2,x,y,'opt',opt);
[Eft_fic2, Varft_fic2] = gp_pred(gp_fic2, x, y, xt);
Varft_fic2 = Varft_fic2 + gp_fic.lik.sigma2;toc
setrandstream([],prevstream);

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Eft_fic==Eft_fic2)); assert(all(Varft_fic==Varft_fic2));

% Spatial data with sparse approximations

S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));
x = data(1:200,1:2);
ye = data(1:200,3);
y = data(1:200,4);
dims = [1    60     1    35];
[trindex, Xu] = set_PIC(x, dims, 5, 'corners', 0);
[n,nin] = size(x);

pl = prior_t('s2',10);
pm = prior_sqrtunif();

gpcf1 = gpcf_ppcs3('nin',nin,'lengthScale', 1, 'magnSigma2', 0.1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gpcf2 = gpcf_sexp('lengthScale', 5, 'magnSigma2', 0.05, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
gpcf3 = gpcf_neuralnetwork('weightSigma2', [1 1], 'biasSigma2', 0.05, 'weightSigma2_prior', pl, 'biasSigma2_prior', pm);

lik = lik_negbin();
% GP without memory save
gp = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, ...
            'jitterSigma2', 1e-4, 'infer_params', 'covariance+inducing');
% GP with memory saving option enabled
gp2 = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, ...
            'jitterSigma2', 1e-4, 'infer_params', 'covariance+inducing', 'savememory', 'on');
opt=optimset('TolFun',1e-2,'TolX',1e-2, 'Display', 'off');

fprintf('Spatial process (Laplace), FIC GP, PPCS3 covariance function, without and with memory saving (optimization and prediction)\n');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'z',ye,'opt',opt);
[Ef, Varf] = gp_pred(gp, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'z',ye,'opt',opt);
[Ef2, Varf2] = gp_pred(gp2, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Ef==Ef2)); assert(all(Varf==Varf2));

gp = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-4, 'latent_method', 'EP');
gp2 = gp_set('lik', lik, 'cf', gpcf2, 'jitterSigma2', 1e-4, 'latent_method', 'EP', 'savememory', 'on');

fprintf('Spatial process (EP), FIC GP, Squared-Exponential covariance function, without and with memory saving (optimization and prediction)\n');

% Optimization and prediction without memory saving
tic,gp=gp_optim(gp,x,y,'z',ye,'opt',opt);
[Ef, Varf] = gp_pred(gp, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Optimization and prediction with memory saving
tic,gp2=gp_optim(gp2,x,y,'z',ye,'opt',opt);
[Ef2, Varf2] = gp_pred(gp2, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Ef==Ef2)); assert(all(Varf==Varf2));

gp = gp_set('lik', lik, 'cf', gpcf3, 'jitterSigma2', 1e-4);
gp2 = gp_set('lik', lik, 'cf', gpcf3, 'jitterSigma2', 1e-4, 'savememory', 'on');

fprintf('Spatial process (Laplace), FIC GP, Neuralnetwork covariance function, without and with memory saving (optimization and prediction)\n');
tic,gp=gp_optim(gp,x,y,'z',ye,'opt',opt);
[Ef, Varf] = gp_pred(gp, x, y, x, 'z', ye, 'tstind', [1:n]); toc

tic,gp2=gp_optim(gp2,x,y,'z',ye,'opt',opt);
[Ef2, Varf2] = gp_pred(gp2, x, y, x, 'z', ye, 'tstind', [1:n]); toc

% Check that predictions (and optimization) with and without memory saving are same
assert(all(Ef==Ef2)); assert(all(Varf==Varf2));


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;


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


% Copyright and terms of use (DO NOT REMOVE):
% The code is made freely available for non-commercial uses only, provided that the copyright
% header in each file not be removed, and suitable citation(s) (see below) be made for papers
% published based on the code.
%
% The code is not optimized for speed, and we are not responsible for any errors that might
% occur in the code.
%
% The copyright of the code is retained by the authors.  By downloading/using this code you
% agree to all the terms stated above.
%
%   Lin, J., Keogh, E., Lonardi, S. & Chiu, B.
%   "A Symbolic Representation of Time Series, with Implications for Streaming Algorithms."
%   In proceedings of the 8th ACM SIGMOD Workshop on Research Issues in Data Mining and
%   Knowledge Discovery. San Diego, CA. June 13, 2003.
%
%
%   Lin, J., Keogh, E., Patel, P. & Lonardi, S.
%   "Finding Motifs in Time Series". In proceedings of the 2nd Workshop on Temporal Data Mining,
%   at the 8th ACM SIGKDD International Conference on Knowledge Discovery and Data Mining.
%   Edmonton, Alberta, Canada. July 23-26, 2002
%
% This function takes in a time series and convert it to string(s).
% There are two options:
%   1. Convert the entire time series to ONE string
%   2. Use sliding windows, extract the subsequences and convert these subsequences to strings
%
% For the first option, simply enter the length of the time series as "N"
%   ex. We have a time series of length 32 and we want to convert it to a 8-symbol string,
%       with alphabet size 3:
%       timeseries2symbol(data, 32, 8, 3)
% For the second option, enter the desired sliding window length as "N"
%   ex. We have a time series of length 32 and we want extract subsequences of length 32 using
%       sliding windows, and convert the subsequences to 8-symbol strings, with alphabet size 3:
%       timeseries2symbol(data, 16, 8, 3)
%
%
% Input:
%   data              is the raw time series.
%   N                 is the length of sliding window (use the length of the raw time series
%                     instead if you don't want to have sliding windows)
%   n                 is the number of symbols in the low dimensional approximation of the sub sequence.
%   alphabet_size     is the number of discrete symbols. 2 <= alphabet_size <= 10, although alphabet_size = 2 is a special "useless" case.
%   NR_opt            1: no numerosity reduction (record everything)
%                     2: numerosity reduction (record only if the string is different from the last recorded string)
%                        (default)
%                     3: advanced numerosity reduction (record only if the mindist between current string and
%                        last recorded string > 0)
%                     4: more reduction (record only if the subsequence is NOT monotonic)
%
% Output:
%   symbolic_data:    matrix of symbolic data (no-repetition).  If consecutive subsequences
%                     have the same string, then only the first occurrence is recorded, with
%                     a pointer to its location stored in "pointers"
%   pointers:         location of the first occurrences of the strings
%
% N/n must be an integer, otherwise the program will give a warning, and abort.
%
% The variable "win_size" is assigned to N/n, this is the number of data points on the raw
% time series that will be mapped to a single symbol, and can be imagined as the
% "compression rate".
%
% The symbolic data is returned in "symbolic_data", with pointers to the subsequences
%
%
%
%
% Copyright (c) 2003, Eamonn Keogh, Jessica Lin, Stefano Lonardi, Pranav Patel.  All rights reserved.
%
function [symbolic_data, pointers] =  timeseries2symbol(data, N, n, alphabet_size, NR_opt)

if nargin < 4
    disp('usage: sax_modified(data, window_len, num_segment, alphabet_size, [numerosity_reduction_option]');
    return;
end

%if (N/n - floor(N/n))                               % N/n must be an integer.
%    disp('N/n must be an integer. Aborting '); , return;
%end;

if alphabet_size > 20
    disp('Currently alphabet_size cannot be larger than 20.  Please update the breakpoint table if you wish to do so');
    return;
end

if nargin < 5
    NR_opt = 2;
end

win_size      = floor(N/n);                         % win_size is the number of data points on the raw time series that will be mapped to a single symbol

pointers      = [];                                 % Initialize pointers,
symbolic_data = zeros(1,n);                         % Initialize symbolic_data with a void string, it will be removed later.
all_string    = zeros(length(data)-N+1,n);

% Scan across the time series extract sub sequences, and converting them to strings.
for i = 1 : length(data) - (N -1)

    if mod(i, 1000) == 0
        %disp(num2str(i));
    end

    % Remove the current subsection.
    sub_section = data(i:i + N -1);

    % Z normalize it.
    sub_section = (sub_section - mean(sub_section))/std(sub_section);

    % take care of the special case where there is no dimensionality reduction
    if N == n
        PAA = sub_section;

    % N is not dividable by n
    else
        if (N/n - floor(N/n))
            temp = zeros(n, N);
            for j = 1 : n
                temp(j, :) = sub_section;
            end
            expanded_sub_section = reshape(temp, 1, N*n);
            PAA = [mean(reshape(expanded_sub_section, N, n))];
        % N is dividable by n
        else
            PAA = [mean(reshape(sub_section,win_size,n))];
        end
    % Convert to PAA.
    %else
   %     PAA = [mean(reshape(sub_section,win_size,n))];
    end

    current_string = map_to_string(PAA,alphabet_size);          % Convert the PAA to a string.

    % no numerosity reduction: record everything
    if NR_opt == 1
        symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
        pointers         = [pointers ; i];                      % ... and add a new pointer.

    % with numerosity reduction: record a string only if it differs from its leftmost neighbor
    elseif NR_opt == 2

        if ~all(current_string == symbolic_data(end,:))             % If the string differs from its leftmost neighbor...
            symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
            pointers         = [pointers ; i];                      % ... and add a new pointer.
        end;

    % advanced numerosity reduction: record a string only if its mindist to the last recorded
    % string > 0
    elseif NR_opt == 3

        % always record the first string
        if i == 1
            symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
            pointers         = [pointers ; i];                      % ... and add a new pointer.

        % subsequent strings
        else

            % we only need to check if two sliding windows have different strings (if they are
            % the same then their mindist is 0)
            if ~all(current_string == symbolic_data(end,:))            % If the string differs from its leftmost neighbor...

                % Here we're doing a simplified version of mindist. Since we're only interested
                % in knowing if the distance of two strings is 0, we can do so without any extra
                % computation.  Since only adjacent symbols have distance 0, all we have to
                % do is check if any two symbols are non-adjacent.
                if any(abs(symbolic_data(end,:) - current_string) > 1)
                    symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
                    pointers         = [pointers ; i];                      % ... and add a new pointer.
                end
            end
        end

    else

        % we only need to check if two sliding windows have different strings (if they are
        % the same then their mindist is 0)
        if ~all(current_string == symbolic_data(end,:))            % If the string differs from its leftmost neighbor...
            if any(abs(symbolic_data(end,:) - current_string) > 1)
                if ~all(sign(diff(current_string)) >= 0) && ~all(sign(diff(current_string)) <= 0)
                    symbolic_data    = [symbolic_data; current_string];     % ... add it to the set...
                    pointers         = [pointers ; i];                      % ... and add a new pointer.
                end
            end
        end
    end

end;
% Delete the first element, it was just used to initialize the data structure
symbolic_data(1,:) = [];

%--------------------------------------------------------------------------------------------------------------------------------------------------------
%----------------Local Functions----------------------Local Functions----------------Local Functions----------------------Local Functions----------------
%--------------------------------------------------------------------------------------------------------------------------------------------------------

function string = map_to_string(PAA,alphabet_size)

string = zeros(1,length(PAA));

switch alphabet_size
        case 2, cut_points  = [-inf 0];
        case 3, cut_points  = [-inf -0.43 0.43];
        case 4, cut_points  = [-inf -0.67 0 0.67];
        case 5, cut_points  = [-inf -0.84 -0.25 0.25 0.84];
        case 6, cut_points  = [-inf -0.97 -0.43 0 0.43 0.97];
        case 7, cut_points  = [-inf -1.07 -0.57 -0.18 0.18 0.57 1.07];
        case 8, cut_points  = [-inf -1.15 -0.67 -0.32 0 0.32 0.67 1.15];
        case 9, cut_points  = [-inf -1.22 -0.76 -0.43 -0.14 0.14 0.43 0.76 1.22];
        case 10, cut_points = [-inf -1.28 -0.84 -0.52 -0.25 0. 0.25 0.52 0.84 1.28];
        case 11, cut_points = [-inf -1.34 -0.91 -0.6 -0.35 -0.11 0.11 0.35 0.6 0.91 1.34];
        case 12, cut_points = [-inf -1.38 -0.97 -0.67 -0.43 -0.21 0 0.21 0.43 0.67 0.97 1.38];
        case 13, cut_points = [-inf -1.43 -1.02 -0.74 -0.5 -0.29 -0.1 0.1 0.29 0.5 0.74 1.02 1.43];
        case 14, cut_points = [-inf -1.47 -1.07 -0.79 -0.57 -0.37 -0.18 0 0.18 0.37 0.57 0.79 1.07 1.47];
        case 15, cut_points = [-inf -1.5 -1.11 -0.84 -0.62 -0.43 -0.25 -0.08 0.08 0.25 0.43 0.62 0.84 1.11 1.5];
        case 16, cut_points = [-inf -1.53 -1.15 -0.89 -0.67 -0.49 -0.32 -0.16 0 0.16 0.32 0.49 0.67 0.89 1.15 1.53];
        case 17, cut_points = [-inf -1.56 -1.19 -0.93 -0.72 -0.54 -0.38 -0.22 -0.07 0.07 0.22 0.38 0.54 0.72 0.93 1.19 1.56];
        case 18, cut_points = [-inf -1.59 -1.22 -0.97 -0.76 -0.59 -0.43 -0.28 -0.14 0 0.14 0.28 0.43 0.59 0.76 0.97 1.22 1.59];
        case 19, cut_points = [-inf -1.62 -1.25 -1 -0.8 -0.63 -0.48 -0.34 -0.2 -0.07 0.07 0.2 0.34 0.48 0.63 0.8 1 1.25 1.62];
        case 20, cut_points = [-inf -1.64 -1.28 -1.04 -0.84 -0.67 -0.52 -0.39 -0.25 -0.13 0 0.13 0.25 0.39 0.52 0.67 0.84 1.04 1.28 1.64];
        otherwise disp('Error! alphabet_size is too big');
end;

for i = 1 : length(PAA)
    string(i) = sum( (cut_points <= PAA(i)), 2 );         % order is now: a = 1, b = 2, c = 3..
end;


function res = PDD_Profile_res(params, doseV_obj, doseV_array,doseV_arrayFF, doseV_e, ...
    doseProfile1_obj, doseP1p5cm_array, doseP1p5cm_arrayFF, doseP1p5cm_e, ...
    doseProfile2_obj, doseP5cm_array, doseP5cm_arrayFF,  doseP5cm_e, ...
    doseProfile3_obj, doseP10cm_array, doseP10cm_arrayFF, doseP10cm_e, ...
    doseProfile4_obj, doseP20cm_array, doseP20cm_arrayFF, doseP20cm_e, ...
    energy, numBin, extraBin, numBinFF)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

%Start the function.
%Calculate the residue of the PDD and dose profile difference between DPM and the measurement.

doseV_sum = zeros(size(doseV_obj));

ener = energy/numBin: energy/numBin:energy*(1+extraBin/numBin);
a = Fatigue(params(1:4), ener);
a(find(isnan(a))) = 0;

cutoff = 0.85;
Ef = energy*cutoff;
kt = energy*0.15;  % change kt
f = 1./(1+exp((ener-Ef)/kt));
a = a.*f;

% Sum the contribution of primary and "OnlyHorn" effect
doseV_sum = (doseV_array) * real(a)';
doseProfile1_sum = (doseP1p5cm_array) * real(a)';
doseProfile2_sum = (doseP5cm_array) * real(a)';
doseProfile3_sum = (doseP10cm_array)  * real(a)';
doseProfile4_sum = (doseP20cm_array) * real(a)';

% Get the weights/spectrum for the flattening filter
enerFF = energy/numBin : energy/numBin : numBinFF*energy/numBin;
aFF = Fatigue(params(1:4), params(5)* enerFF);
aFF(find(isnan(aFF))) = 0;

% Get modified Flattening filter spectrum/weights
f = 1./(1+exp((params(5)*enerFF-Ef)/kt));
aFF = aFF.* f;
aFF = aFF * (sum(a)/sum(aFF)*params(6)) ;  % scale FF weight respect to sum(a)

%calculate the dose contribution of the flattening filter
doseV_sumFF = doseV_arrayFF * real(aFF)';
doseProfile1_sumFF = doseP1p5cm_arrayFF * real(aFF)';
doseProfile2_sumFF = doseP5cm_arrayFF * real(aFF)';
doseProfile3_sumFF = doseP10cm_arrayFF * real(aFF)';
doseProfile4_sumFF = doseP20cm_arrayFF * real(aFF)';

% Nov. 1 2006
% Add the asd
filterSize = 7;
filterWindow = [-3 -2 -1 0 1 2 3];
G1 = gauss(filterWindow, params(9));  %p(9) is sigma
G1 = G1/sum(G1);   % normalize, to make sum(G1)==1;

doseV_DPM = doseV_sum + doseV_sumFF + params(8) * sum(a)* doseV_e;
doseProfile1_DPM = doseProfile1_sum + doseProfile1_sumFF + params(8) * sum(a)* doseP1p5cm_e;
doseProfile1_DPM = conv(G1, doseProfile1_DPM);
doseProfile2_DPM = doseProfile2_sum + doseProfile2_sumFF + params(8) * sum(a)* doseP5cm_e;
doseProfile2_DPM = conv(G1, doseProfile2_DPM);
doseProfile3_DPM = doseProfile3_sum + doseProfile3_sumFF + params(8) * sum(a)* doseP10cm_e;
doseProfile3_DPM = conv(G1, doseProfile3_DPM);
doseProfile4_DPM = doseProfile4_sum + doseProfile4_sumFF + params(8) * sum(a)* doseP20cm_e;
doseProfile4_DPM = conv(G1, doseProfile4_DPM);

% Use relative weight for PDD as 5; for lateral dose profile, use weight
% as 1.
% for doseV_obj, dose different absolute dose value make a difference?
% Should it be weighed by the dose values at those points?

diffsqr = 1.0*sum ((doseV_obj - doseV_DPM).^2) ...
      + sum((doseProfile1_obj - doseProfile1_DPM(4:end-3)).^2) ...
    + sum((doseProfile2_obj - doseProfile2_DPM(4:end-3)).^2) ...
    + sum((doseProfile3_obj - doseProfile3_DPM(4:end-3)).^2) ...
      + sum((doseProfile4_obj - doseProfile4_DPM(4:end-3)).^2);
res = sqrt(diffsqr);
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:  nick.battista@unc.edu
% Date Created: September 9th, 2016
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Make_Your_Plates_and_Input_Files()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  64;       % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
dx = Lx/Nx;      % Grid spatial resolution

%
% Immersed Structure Geometric / Dynamic Parameters %
%
ds = 0.5*dx;              % Lagrangian Pt. Spacing (2x resolution of Eulerian grid)
struct_name = 'corals';   % Name for .vertex, .spring, etc files. (must match what's in 'input2d')

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,Nx,Lx);

% Plot Geometry to test
plot(xLag,yLag,'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Lx]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
% k_Spring = 2.5e4;                    % Spring stiffness (does not need to be equal for all springs)
% ds_Plates = dist;                    % Spring resting length (does not need to be equal for all springs)
% print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Plates,ds,struct_name);

% Prints .beam file!
% k_Beam = 0.5;                      % Beam Stiffness (does not need to be equal for all beams)
% C = compute_Curvatures(xLag,yLag)  % Computes curvature of initial configuration
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e7;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

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

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag); % Total # of Lag. Pts

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

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

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

    fclose(vertex_fid);

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

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Plates,ds,struct_name)

    N = length(xLag);

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

    fprintf(spring_fid, '%d\n', N-2 + N/2 );    % Print # of springs

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            elseif s > N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            end
    end

    % SPRINGS ACROSS PLATES
    for s=1:N/2
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+N/2, k_Spring, ds_Plates);
    end

    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

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

    %Loops over all Lagrangian Pts.
    for s = 1:N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called 'struct_name'.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

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

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

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

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

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s) );
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C(s) );
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes "curvature" of starting configuration
%
% NOTE: not curvature in the traditional geometric sense, in the 'discrete'
% sense through cross product.
%
% NOTE: assumes a CLOSED structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = compute_Curvatures(xLag,yLag)

N = length(xLag);
C = zeros( N );

%Note: needs to be done same order as you print .beam file!
for i=1:N

   % Pts Xp -> Xq -> Xr (same as beam force calc.)

   if ( (i > 1) && (i < N) )

        Xp = xLag(i-1); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(i-1); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==1)

        Xp = xLag(N); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(N); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==N)

        Xp = xLag(N-1); Xq = xLag(N); Xr = xLag(1);
        Yp = yLag(N-1); Yq = yLag(N); Yr = yLag(1);

   end

   C(i) = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp); %Cross product btwn vectors

end

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

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

% ds: Lagrangian pt. spacing
% Nx: Eulerian grid resolution
% L:  Length of computational domain

ratio = 1024 / (2*Nx);

% Gives LEFT Coral
[x1,y1,N] = please_Give_Single_Polyp_Geometry(ratio);
x1 = x1 + L/10;

% Makes RIGHT Coral
x2 = x1 + L/5;
y2 = y1;

% TESTS GEOMETRY BY PLOTTING
plot(x1,y1,'k*'); hold on;
plot(x1(1),y1(1),'g*'); hold on;
plot(x1(end/2),y1(end/2),'b*'); hold on;
plot(x1(end/2+1),y1(end/2+1),'g*'); hold on;
plot(x1(end),y1(end),'b*'); hold on;
%
plot(x2,y2,'r*'); hold on;
plot(x2(1),y2(1),'g*'); hold on;
plot(x2(end/2),y2(end/2),'b*'); hold on;
plot(x2(end/2+1),y2(end/2+1),'g*'); hold on;
plot(x2(end),y2(end),'b*'); hold on;

xRef = [x1 x2]; yRef = [y1 y2];
ang = pi/4;

% Store Values for Centers of Rotation
xL_1 = xRef(1);       yL_1 = yRef(1);           % Left side of Left Pair
xR_1 = xRef(end/4+1); yR_1 = yRef(end/4+1);     % Right side of Left Pair

xL_2 = xRef(end/2+1);   yL_2 = yRef(end/2+1);   % Left side of Right Pair
xR_2 = xRef(3*end/4+1); yR_2 = yRef(3*end/4+1); % Right side of Right Pair

% -> Rotate Geometry <- %
% LEFT PAIR %
[xR_Ref_1,yR_Ref_1] = rotate_Geometry(ang,xR_1,yR_1,xRef(end/4+1:end/2),yRef(end/4+1:end/2) );
[xL_Ref_1,yL_Ref_1] = rotate_Geometry(-ang,xL_1,yL_1,xRef(1:end/4),yRef(1:end/4) );
% RIGHT PAIR %
[xR_Ref_2,yR_Ref_2] = rotate_Geometry(ang,xR_2,yR_2,xRef(3*end/4+1:end),yRef(3*end/4+1:end) );
[xL_Ref_2,yL_Ref_2] = rotate_Geometry(-ang,xL_2,yL_2,xRef(end/2+1:3*end/4),yRef(end/2+1:3*end/4) );

xLag = [xL_Ref_1 xR_Ref_1 xL_Ref_2 xR_Ref_2];
yLag = [yL_Ref_1 yR_Ref_1 yL_Ref_2 yR_Ref_2];

plot(xLag,yLag,'ro'); hold on;
axis([0 1 0 1]);

% Combine into ONE Vector
%xLag = [xLag_L xLag_R];
%yLag = [yLag yLag];

% Plot the Geometry
% plot(xLag,yLag,'*'); hold on;
% axis([0 L 0 L]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: rotate geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y] = rotate_Geometry(ang,xC,yC,xRef,yRef)

len = length(xRef);
x = zeros(1,len); y=x;

xRef = xRef - xC;
yRef = yRef - yC;

for i=1:len
   x(i) =  xRef(i)*cos(ang) - yRef(i)*sin(ang);
   y(i) =  xRef(i)*sin(ang) + yRef(i)*cos(ang);
end

x = x + xC;
y = y + yC;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the vertices for one coral polyp
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y,N] = please_Give_Single_Polyp_Geometry(ratio)

xoffset = 0.3; % To put geometry into QUADRANT-1
yoffset = 0.5; % To put geometry into QUADRANT-1

% Note: (1) ds for stored data is 0.6/(2*1024)
%       (2) 'ratio' is comparing 1024:desired resolution

% Get LEFT side geometry
struct_name1 = 'coral2d_left_1024';
[~,x1,y1] = read_Vertex_Points(struct_name1);

% Reverse order so first point is on bottom
x1 = x1(end:-1:1); y1 = y1(end:-1:1);

% Get RIGHT side geometry
struct_name2 = 'coral2d_right_1024';
[~,x2,y2] = read_Vertex_Points(struct_name2);

% Reverse order so first point is on bottom
x2 = x2(end:-1:1); y2 = y2(end:-1:1);

% Put Geometry Together for One Polyp
xAux = [x1; x2]; yAux = [y1; y2];
xAux = xAux+xoffset; yAux = yAux+yoffset;

% Pull out correct resolution
x=xAux(1:ratio:end)';
y=yAux(1:ratio:end)';
N = length(x)/2;

%plot(xAux,yAux,'*'); hold on;
%plot(x,y,'ro'); hold on;

%NOTE: (1) N here is the # of pts. on one arm (NOT ENTIRE POLYP)!!
%      (2) The geometry here is listed for 1024x1024 meshes -> will need
%          to take every 8 or 16 pts to render geometry usable for MATLAB
%          code

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the # of vertex pts and all the vertex pts from the
%           .vertex file.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [N,xLag,yLag] = read_Vertex_Points(struct_name)

filename = [struct_name '.vertex'];  %Name of file to read in
fileID = fopen(filename);

% Read in the file, use 'CollectOutput' to gather all similar data together
% and 'CommentStyle' to to end and be able to skip lines in file.
C = textscan(fileID,'%f %f','CollectOutput',1);

fclose(fileID);     %Close the data file.

vertices = C{1};    %Stores all read in data in vertices (N+1,2) array

N = vertices(1,1);  % # of Lagrangian Pts
xLag = zeros(N,1);  % Initialize storage for Lagrangian Pts.
yLag = xLag;        % Initialize storage for Lagrangian Pts.

for i=1:N
   xLag(i,1) = vertices(i+1,1); %Stores x-values of Lagrangian Mesh
   yLag(i,1) = vertices(i+1,2); %Stores y-values of Lagrangian Mesh

end