function [param] = get_parameters2(hdrf,t)
% Find model parameters
%
% Height - h
%
% Time to peak - p (in time units of TR seconds)
%
% Width (at half peak) - w
%
% Calculate Heights and Time to peak:
%
% delta = 1/(t(2)-t(1));

n = round(length(t)*0.8);
% n = round(t(end)*0.6*delta)

[~,p] = max(abs(hdrf(1:n)));
h = hdrf(p);

%if (p > t(end)*0.6*delta), warning('Late time to peak'), end;
if (p > t(end)*0.8), warning('Late time to peak'), end;

if (h >0)
    v = (hdrf >= h/2);
else
    v = (hdrf <= h/2);
end;

[~,b] = min(diff(v));
v(b+1:end) = 0;
w = sum(v);

cnt = p-1;
g =hdrf(2:end) - hdrf(1:(end-1));
while((cnt > 0) && (abs(g(cnt)) <0.001)),
    h = hdrf(cnt);
    p = cnt;
    cnt = cnt-1;
end;

param = zeros(3,1);
param(1) = h;
param(2) = p;
param(3) = w;

return;


function  [fy,f]=FFT(y,Fs)

%==========================================================================
%  (Updated Version 03/09/2013)
%  Usage :
%  Function  [fy]=FFT(y,Fs)
%
%  1)computes the Power spectral density and Amplitude spectrum (P(f),F(f))
%  of 1d signal y(t) with sample rate  Fs (Nyquist rate) which is known
%  apriori. The results are plotted in 3 figures which correspond to simple
%  PSD,logarithmic PSD (dB) and  Amplitude Specturm respectively.
%                             _______________
%  such that  Ampitude(f) = \/    PSD(f)
%
%  2)The usefulness of this function is the adjustment of the frequency axis
%
%  3)The fast Fourier transform is computed with Matlab built-in function
%  fft, but for signals whose lengths <1000 points, one can use the nested
%  function   y=Fast_Fourier_Transform(X,N) .
%
%  Demo :
%
%                 Fs=800;
%                 Tf=2;
%                 t=0:1/Fs:Tf;
%                 f=[40 75];
%                 Amp=[4.5 9.22];
%                 sigma=1.33;
%                 y=Amp(1)*exp(j*2*pi*t*f(1))+Amp(2)*exp(j*2*pi*t*f(2));
%                 N=(sigma/sqrt(2))*(randn(size(t))+j*randn(size(t)));
%                 y=y+N;
%                 figure, plot(t,y),xlabel('time (s)'),ylabel('Voltage (v)'),
%                 title(strcat('Signal corrupted with AWGN, \sigma=',num2str(sigma))),
%                 fy=FFT(y,Fs);
%
% (c) KHMOU Youssef , Signal Processing 2013
%==========================================================================

if nargin<2
    error(' Sampling frequency is required to sompute (PSD,F(y)) ! ');
end

% In case that the input vector is matrix :  Maping with vect{} .
y=y(:).';
L=length(y);

%  (2^N) :Number of points for computing the FFT
N=ceil(log2(length(y)));

% FFT
fy=fft(y,2^N)/(L/2);
%------------------------------------------------
% for length<1000 one can replace fft with function :
% fy=Fast_Fourier_Transform(y,2^N)/(L/2); (line 84)
%------------------------------------------------

% Amplitude adjustment by checking for complex input y
if isreal(y)==0
    fy=fy/2;
end

% PSD
Power=fy.*conj(fy);
%Phase Angle
phy=angle(fy);

%  Frequency axis
f=(Fs/2^N)*(0:2^(N-1)-1);

%if nargin==4

% Figures------------------------------------------------------------------
ff1=figure;
plot(f,Power(1:2^(N-1)),'r'),  xlabel('  Frequency (Hz)'), ylabel(' Magnitude (w)'),
title('  Power Spectral Density'), grid on;
set(ff1,'Name','PSD');

ff2=figure;
plot(f,10*log10(Power(1:2^(N-1))),'r'),  xlabel('  Frequency (Hz)'), ylabel(' Magnitude  (dB)'),
title('  Power Spectral Density, logarithmic scale '), grid on;
set(ff2,'Name','10*log10(PSD)');

ff3=figure;
plot(f,sqrt(Power(1:2^(N-1))),'r'),  xlabel('  Frequency (Hz)'), ylabel('|F(Y)|'),
title('  Amplitude Spectrum'), grid on;
set(ff3,'Name','|F(y)|');

ff4=figure;
plot(f,phy(1:2^(N-1)),'b'), xlabel(' Frequency (Hz)'), ylabel(' arg(F(Y))'),
title(' Phase spectrum'), grid on;
set(ff4,'Name','arg(F(Y))');
%end
%==========================================================================
% function z=Fast_Fourier_Transform(x,nfft)
%
% N=length(x);
% z=zeros(1,nfft);
% Sum=0;
% for k=1:nfft
%     for jj=1:N
%         Sum=Sum+x(jj)*exp(-2*pi*j*(jj-1)*(k-1)/nfft);
%     end
% z(k)=Sum;
% Sum=0;% Reset
% end
% return
%=========================================================================

% Additional plot :  PDF
%M=max(real(y));
%M=2*M;
%x_range=-M:M/20:M;
%ff4=figure;
%hist(real(y),x_range),xlabel('Signal values'), ylabel('PDF');
%title('  Probability density function of the input signal'), grid minor;
%set(ff4,'Name','PDF');
%clear M x_range


function [mixed, component1, component2] = MixSpeechWaveforms(wav1, wav2, SPR)

wav1 = wav1(:)';
n1 = length(wav1);
wav2 = wav2(:)';
n2 = length(wav2);

% scale wav2 to obtain desired SPR, the signal power ratio in dB
power1 = mean(wav1.^2);
power2 = mean(wav2.^2);
scale2 = sqrt(power1/power2 * 10^(-SPR/10));
if scale2<1
    wav2 = scale2*wav2;
else
    wav1 = wav1/scale2;
end

% if the two waveforms have different length, repeat the shorter one to
% match the longer one

if n1>n2
    nRepeat = ceil(n1/n2);
    wav2 = repmat(wav2, 1, nRepeat);
    wav2(n1+1:end) = [];
else
    nRepeat = ceil(n2/n1);
    wav1 = repmat(wav1, 1, nRepeat);
    wav1(n2+1:end) = [];
end

component1 = wav1;
component2 = wav2;
mixed = component1 + component2;

end


function [T] = cubic_cubic_intersect(CA,CB,tol)
  % CUBIC_CUBIC_INTERSECT Intersect two cubic Bezier curves.
  %
  % [T] = cubic_cubic_intersect(CA,CB,tol)
  %
  % Inputs:
  %   CA  #4 by 2 list of control point locations
  %   CB  #4 by 2 list of control point locations
  % Outputs:
  %   T  #T by 2 list of parameters between [0,1] of intersects, so that
  %     intersect i occurs at T(i,1) on curve A and at T(i,2) on curve B.
  %
  % See also: cubic_eval, cubic_is_flat, cubic_split
  %

  if nargin<3
    tol = 1e-7;
  end

  %plot_cubic(CA);
  %hold on;
  %arrayfun(@(p) set(p,'Color','r'), plot_cubic(CB));
  %hold off;
  %axis equal;
  %axis([-1 1 -1 1]);
  %drawnow

  %if ~bounding_box_overlap(CA,CB)
  % Avoid function overhead
  if (any(max(CA)<min(CB)) || any(max(CB)<min(CA)))
    T = zeros(0,2);
    return;
  end
  A_flat = cubic_is_flat(CA,tol);
  B_flat = cubic_is_flat(CB,tol);
  % lazy just wait until they're both flat
  if A_flat && B_flat
    s = lineSegmentIntersect([CA(1,:) CA(4,:)],[CB(1,:) CB(4,:)]);
    if s.intAdjacencyMatrix
      T = [s.intNormalizedDistance1To2 s.intNormalizedDistance2To1];
    else
      T = zeros(0,2);
    end
    return;
  end

  [CA1,CA2] = cubic_split(CA,0.5);
  [CB1,CB2] = cubic_split(CB,0.5);
  T11 = cubic_cubic_intersect(CA1,CB1,tol)*0.5+0.5*[0 0];
  T12 = cubic_cubic_intersect(CA1,CB2,tol)*0.5+0.5*[0 1];
  T21 = cubic_cubic_intersect(CA2,CB1,tol)*0.5+0.5*[1 0];
  T22 = cubic_cubic_intersect(CA2,CB2,tol)*0.5+0.5*[1 1];
  T = [T11;T12;T21;T22];
end


classdef MixtureTheoryHomogenizer < handle

    properties (Access = public)
        Ch
    end

    properties (Access = private)
        C1
        C0
        angle
        Ex
        Ey
        nu_xy
        nu_yx
        mu
        dir
        ChHorizontal
        Vfrac
    end

    methods (Access = public)

        function obj = MixtureTheoryHomogenizer(C1,C0,Dir,angle,Vfrac)
            obj.init(C1,C0,Dir,angle,Vfrac)
            obj.computeOrthotropicProperties()
            obj.computeHorizontalHomogenizedTensor()
            obj.rotateHorizontalHomogenizedTensor()
        end

    end

    methods (Access = private)

        function init(obj,C1,C0,Dir,angle,Vfrac)
            obj.C1 = C1;
            obj.C0 = C0;
            obj.angle = angle;
            obj.dir = Dir;
            obj.Vfrac = Vfrac;
        end

        function computeOrthotropicProperties(obj)
            E1 = obj.C1.getYoung();
            E0 = obj.C0.getYoung();
            nu1 = obj.C1.getPoisson();
            nu0 = obj.C0.getPoisson();
            mu1 = obj.C1.getMu();
            mu0 = obj.C0.getMu();
            Vol = obj.Vfrac;

            obj.Ex = obj.serialize(E1,E0,Vol);
            obj.Ey = obj.parelalize(E1,E0,Vol);
            obj.nu_xy = obj.serialize(nu1,nu0,Vol);
            obj.nu_yx = obj.nu_xy*obj.Ey/obj.Ex;
            obj.mu = obj.parelalize(mu1,mu0,Vol);
        end

        function computeHorizontalHomogenizedTensor(obj)
            E1    = obj.Ex;
            E2    = obj.Ey;
            nu_12 = obj.nu_xy;
            nu_21 = obj.nu_yx;
            Mu    = obj.mu;

            C = zeros(3,3);
            C(1,1) = E1/(1-nu_12*nu_21);
            C(2,2) = E2/(1-nu_12*nu_21);
            C(1,2) = E1*nu_21/(1-nu_12*nu_21);
            C(2,1) = E2*nu_12/(1-nu_12*nu_21);
            C(3,3) = Mu;
            obj.ChHorizontal  = C;
        end

        function rotateHorizontalHomogenizedTensor(obj)
            d = obj.dir;
            a = obj.angle;
            ChHor = obj.ChHorizontal;
            C = SymmetricFourthOrderPlaneStressVoigtTensor();
            C.setValue(ChHor);
            obj.Ch = Rotator.rotate(C,a,d);
        end

    end

    methods (Access = private, Static)
        function fSerial = serialize(f1,f0,rho)
            fSerial = rho*f1 + (1-rho)*f0;
        end

        function fParalel = parelalize(f1,f0,rho)
            fParalel = 1/(rho/f1 + (1-rho)/f0);
        end

    end

end


% make histogram of average within-cluster distance for all clusters,
% pooled for all fish

clear all; close all; clc;

outputDir = GetOutputDataDir;

%% Init load
hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);

%% Load fish
range_fish = 1:18;

M_Count = cell(length(range_fish),1);

%%
% i_set = 1;
for i_fish = range_fish
    %% load fish
    ClusterIDs = [6,1];
    [cIX_load,gIX_load] = LoadSingleFishDefault(i_fish,hfig,ClusterIDs);
    CellXYZ_norm = getappdata(hfig,'CellXYZ_norm');
    absIX = getappdata(hfig,'absIX');
    cIX_abs = absIX(cIX_load);
    M_xyz_norm = CellXYZ_norm(cIX_abs,:);

    %% compute average within-cluster-distance for each cluster
    numClus = length(unique(gIX_load));
    D = zeros(numClus,1);
    for i_clus = 1:numClus
        IX = find(gIX_load == i_clus);
        XYZ_clus = M_xyz_norm(IX,:);
       D(i_clus) = mean(pdist(XYZ_clus,'euclidean'));
    end

    M_Count{i_fish} = D;
end

%% fig4f: Distribution of within-cluster distance
figure('Position',[500,500,150,120]);hold on;
xbins = 0:50:400;

% pool
N = zeros(length(range_fish),length(xbins)-1);
for i_fish = range_fish
    [N(i_fish,:),edges,bin] = histcounts(M_Count{i_fish},xbins);
end

for i = 1:size(N,2)
    meanN = mean(N(:,i));
    semN = std(N(:,i))/sqrt(length(range_fish));
    plot([edges(i),edges(i)],[0,meanN],'color',[1,0.5,0.5],'linewidth',7.5)
    plot([edges(i),edges(i)],[meanN-semN,meanN+semN],'color',[0.2,0.2,0.2],'linewidth',0.5);
end

%
% h = findobj(gca,'Type','patch');
% h.FaceColor = [0.5 0.5 0.5];
% h.EdgeColor = 'w';
xlim([-25,max(xbins)])
% set(gca,'XTick', 0:0.2:1);
xlabel('avr within-clus dist')
ylabel('count')
ylim([0,60])


function reg_filter = get_reg_filter(sz, target_sz, params, reg_window_edge)

% Compute the spatial regularization function and derive the corresponding
% filter operation used for the optimization

if nargin < 3 || isempty(reg_window_edge)
    reg_window_edge = params.reg_window_edge;
end

if params.use_reg_window
    % create weight window
    reg_window_power = params.reg_window_power;

    % normalization factor
    reg_scale = 0.5 * target_sz;

    % construct grid
    wrg = -(sz(1)-1)/2:(sz(1)-1)/2;
    wcg = -(sz(2)-1)/2:(sz(2)-1)/2;
    [wrs, wcs] = ndgrid(wrg, wcg);

    % construct the regukarization window
    reg_window = (reg_window_edge - params.reg_window_min) * (abs(wrs/reg_scale(1)).^reg_window_power + abs(wcs/reg_scale(2)).^reg_window_power) + params.reg_window_min;

    % compute the DFT and enforce sparsity
    reg_window_dft = fft2(reg_window) / prod(sz);
    reg_window_dft(abs(reg_window_dft) < params.reg_sparsity_threshold * max(abs(reg_window_dft(:)))) = 0;

    % do the inverse transform, correct window minimum
    reg_window_sparse = real(ifft2(reg_window_dft));
    reg_window_dft(1,1) = reg_window_dft(1,1) - prod(sz) * min(reg_window_sparse(:)) + params.reg_window_min;
    reg_window_dft = fftshift(reg_window_dft);

    % find the regularization filter by removing the zeros
    reg_filter = single(real(reg_window_dft(~all(reg_window_dft==0,2), ~all(reg_window_dft==0,1))));
else
    % else use a scaled identity matrix
    reg_filter = single(params.reg_window_min);
end


function R = dark_channel_prior_dehaze(I)
%DARK_CHANNEL_PRIOR_DEHAZE  Dehaze input image following the pipeline proposed
%in Single Image Haze Removal Using Dark Channel Prior.
%
%   INPUTS:
%
%   -|I|: H-by-W-by-|image_channels| hazy image.
%
%   OUTPUTS:
%
%   -|R|: output dehazed image representing the estimate for the true radiance
%   of the scene, with same size as |I|.

% Add required paths.
current_script_full_name = mfilename('fullpath');
current_script_directory = fileparts(current_script_full_name);
addpath(fullfile(current_script_directory, '..', '..', 'utilities'));
addpath_relative_to_caller(current_script_full_name,...
    fullfile('..', '..', 'Fog_simulation'));
addpath_relative_to_caller(current_script_full_name,...
    fullfile('..', '..', 'Dehazing'));

% Set parameters.
neighborhood_size_dark_channel = 15;
t_thresh = 0.1;
window_size_guided_filter = 41;
epsilon = 1e-3;

% Dehaze using Dark Channel Prior, following the paper of He, Sun and Tang (CVPR
% 2009). The refinement of the raw transmission map is performed according to
% Guided Image Filtering by He, Sun and Tang (ECCV 2010), in order to gain
% speed.
[I_dark, I_eroded] = get_dark_channel(I, neighborhood_size_dark_channel);
L = estimate_atmospheric_light_dcp(I_dark, I);
t_initial = transmission_initial(I_eroded, L);
t = transmission_guided_filtering(t_initial, I,...
    window_size_guided_filter, epsilon);
t = clip_to_unit_range(t);
R = inverse_haze_linear(I, t, L, t_thresh);
R = clip_to_unit_range(R);

end


classdef MPSOD < ALGORITHM
% <multi/many> <real/integer>
% Multi-objective particle swarm optimization algorithm based on
% decomposition

%------------------------------- Reference --------------------------------
% C. Dai, Y. Wang, and M. Ye, A new multi-objective particle swarm
% optimization algorithm based on decomposition, Information Sciences,
% 2015, 325: 541-557.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate the weight vectors
            [W,Problem.N] = UniformPoint(Problem.N,Problem.M);
            W = W./repmat(sqrt(sum(W.^2,2)),1,size(W,2));
            T = ceil(Problem.N/10);

            %% Detect the neighbours of each solution
            B = pdist2(W,W);
            [~,B] = sort(B,2);
            B = B(:,1:T);

            %% Generate random population
            Population = Problem.Initialization(2*Problem.N);
            Z          = min(Population.objs,[],1);
            Population = Classification(Problem,Population,W,Z);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                [Parent,Pbest,Gbest] = MatingSelection(Population.objs,B,W,Z);
                Offspring  = Operator(Problem,Population(Parent),Population(Pbest),Population(Gbest));
                Z          = min([Z;Offspring.objs],[],1);
                Population = Classification(Problem,[Population,Offspring],W,Z);
            end
        end
    end
end


function [p,d] = check(X)
% CHECK(F)  Displays/calculates constraint residuals on constraint F
%
% [pres,dres] = CHECK(F)
%
% pres : Primal constraint residuals
% dres : Dual constraint residuals
%
% If no output argument is supplied, tabulated results are displayed
%
% Primal constraint residuals are calculated as:
%
%  Semidefinite constraint F(x)>0 : min(eig(F))
%  Element-wise constraint F(x)>0 : min(min(F))
%  Equality constraint F==0       : -max(max(abs(F)))
%  Second order cone t>||x||      : t-||x||
%  Integrality constraint on x    : max(abs(x-round(x)))
%  Sum-of-square constraint       : Minus value of largest (absolute value) coefficient
%                                   in the polynomial p-v'*v
%
% Dual constraints are evaluated similarily.
%
%  See also  SOLVESDP, SOLVESOS, SOSD, DUAL

switch nargout
    case 0
        check(lmi(X));
    case 1
        p = check(lmi(X));
    case 2
        [p,d] = check(lmi(X));
end


function S = globMatrix3DStiff(fun,mesh,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
if strcmp(fem1.type,'P1')||strcmp(fem1.type,'DGP1')||strcmp(fem1.type,'CR')
    feEvalBas1 = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem1.type,'DGP2')
    feEvalBas1 = @evalP2Bas3D;
end

if strcmp(fem2.type,'P1')||strcmp(fem2.type,'DGP1')||strcmp(fem2.type,'CR')
    feEvalBas2 = @evalP1Bas3D;
elseif strcmp(fem2.type,'P2')||strcmp(fem2.type,'DGP2')
    feEvalBas2 = @evalP2Bas3D;
end

dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
nt = length(mesh.t);
A = fem1.area; gx = fem1.gx; gy = fem1.gy; gz = fem1.gz; gw = fem1.gw;
X = zeros(nloc*nt, 1);

coef = feval(fun,gx,gy,gz);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);
for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem1.bas(:,:,i), gx, gy, gz, [1,0,0]);
    Ibasy{i} = feEvalBas1(fem1.bas(:,:,i), gx, gy, gz, [0,1,0]);
    Ibasz{i} = feEvalBas1(fem1.bas(:,:,i), gx, gy, gz, [0,0,1]);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem2.bas(:,:,j), gx, gy, gz, [1,0,0]);
    Jbasy{j} = feEvalBas2(fem2.bas(:,:,j), gx, gy, gz, [0,1,0]);
    Jbasz{j} = feEvalBas2(fem2.bas(:,:,j), gx, gy, gz, [0,0,1]);
end

I = reshape(repmat(fem1.t,4,1),nloc*nt,1);
J = repmat(reshape(fem2.t,dof2*nt,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        X(ind+1:ind+nt) = A.*(sum(((Ibasx{i}.*(coef.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coef.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coef.*Jbasz{j})).*gw'),2));
        ind = ind + nt;
    end
end
ID = find(X~=0);
S = sparse(I(ID),J(ID),X(ID),size(fem1.p,1),size(fem2.p,1));


function [U_final, V_final, dnorm, nIter_final, objhistory_final] = GNMF(X, k, W, options, U, V)
% Graph regularized Non-negative Matrix Factorization (GNMF)
%
% where
%   X
% Notation:
% X ... (mFea x nSmp) data matrix
%       mFea  ... number of words (vocabulary size)
%       nSmp  ... number of documents
% k ... number of hidden factors
% W ... weight matrix of the affinity graph
%
% options ... Structure holding all settings
%               options.alpha ... the regularization parameter.
%                                 [default: 100]
%                                 alpha = 0, GNMF boils down to the ordinary NMF.
%
%
% You only need to provide the above four inputs.
%
% X = U*V'
%
% References:
% [1] Deng Cai, Xiaofei He, Xiaoyun Wu, and Jiawei Han. "Non-negative
% Matrix Factorization on Manifold", Proc. 2008 Int. Conf. on Data Mining
% (ICDM'08), Pisa, Italy, Dec. 2008.
%
% [2] Deng Cai, Xiaofei He, Jiawei Han, Thomas Huang. "Graph Regularized
% Non-negative Matrix Factorization for Data Representation", IEEE
% Transactions on Pattern Analysis and Machine Intelligence, , Vol. 33, No.
% 8, pp. 1548-1560, 2011.
%
%
%   version 2.0 --April/2009
%   version 1.0 --April/2008
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

if min(min(X)) < 0
    error('Input should be nonnegative!');
end

if ~isfield(options,'error')
    options.error = 1e-5;
end
if ~isfield(options, 'maxIter')
    options.maxIter = [];
end

if ~isfield(options,'nRepeat')
    options.nRepeat = 10;
end

if ~isfield(options,'minIter')
    options.minIter = 30;
end

if ~isfield(options,'meanFitRatio')
    options.meanFitRatio = 0.1;
end

if ~isfield(options,'alpha')
    options.alpha = 100;
end

nSmp = size(X,2);

if isfield(options,'alpha_nSmp') && options.alpha_nSmp
    options.alpha = options.alpha*nSmp;
end

if isfield(options,'weight') && strcmpi(options.weight,'NCW')
    feaSum = full(sum(X,2));
    D_half = X'*feaSum;
    X = X*spdiags(D_half.^-.5,0,nSmp,nSmp);
end

if ~isfield(options,'Optimization')
    options.Optimization = 'Multiplicative';
end

if ~exist('U','var')
    U = [];
    V = [];
end

[U_final, V_final, nIter_final, objhistory_final] = GNMF_Multi(X, k, W, options, U, V);
V_final = V_final';
dnorm = norm(X - U_final * V_final, 'fro');


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: plots a B-spline
%
%==============================================================================

FAIRfigure(1,'color','w'); clf
tt = linspace(-3,11,1001);
p1 = plot(tt,spline1D(0,tt)); hold on; axis([-3,12,0,1])
set(p1,'linewidth',3,'color','k')
p2 = plot(tt,spline1D(2,tt),'--');
p3 = plot(tt,spline1D(7,tt),'--');
set([p2;p3],'linestyle','--','linewidth',1.5,'color','k'); axis off;
%==============================================================================


function check = gamma_check ( a, b, c )

%*****************************************************************************80
%
%% GAMMA_CHECK checks the parameters of the Gamma PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GAMMA_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B <= 0.\n' );
    fprintf ( 1, '  B = %f\n', b );
    check = 0;
    return
  end

  if ( c <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GAMMA_CHECK - Fatal error!\n' );
    fprintf ( 1, '  C <= 0.\n' );
    fprintf ( 1, '  C = %f\n', c );
    check = 0;
    return
  end

  check = 1;

  return
end


function [out]=woody(x,varargin)
%
% [out]=woody(x,tol,max_it,est_mthd,xcorr_mthd)
%
% Weighted average using Woody average for a signal
% with jitter. Parameters:
%
% x             Signal measurements. Each COLUMN represents
%               and independent measure of the signal (or channel).
% tol           Tolerance paremeter to stop average (default is 0.1)
% max_it        Maximum number of iterations done on the average (default is 100).
% est_mthd      Estimation method to use. Options are:
%               'woody'     : classical approach (default)
%               'thornton'  : implements the Thornton approach that is also useful for different noise sources.
% xcorr_mthd    Determines what estimation method to use for the estimating the correlaation function using the
%               XCORR function. Options are:
%               'biased'   - scales the raw cross-correlation by 1/M.
%               'unbiased' - scales the raw correlation by 1/(M-abs(lags)). (Default)
% out           Final averaged waveform (time aligned).
%
%
%
% Written by Ikaro Silva
%
% Since 0.9.5
%
% %%%Example 1 %%%%
% t=[0:1/1000:1];
% N=1001;
% x=sin(2*pi*t)+sin(4*pi*t)+sin(8*pi*t);
% y=exp(0.01*[-1*[500:-1:1] 0 -1*[1:500]]);
% s=x.*y;
% sig1=0;
% sig2=0.1;
% M=100;
% S=zeros(N,M);
% center=501;
% TAU=round((rand(1,M)-0.5)*160);
% for i=1:M,
%     tau=TAU(i);
%
%     if(tau<0)
%         S(:,i)=[s(-1*tau:end)'; zeros(-1*(tau+1),1)];
%     else
%         S(:,i)=[zeros(tau,1);s(1:N-tau)'; ];
%     end
%     if(i<50)
%        S(:,i)=S(:,i) + randn(N,1).*sig1;
%     else
%         S(:,i)=S(:,i) + randn(N,1).*sig2;
%     end
% end
%
% [wood]=woody(S,[],[],'woody','biased');
% [thor]=woody(S,[],[],'thornton','biased');
% figure;
% subplot(211)
% plot(s,'b','LineWidth',2);grid on;hold on;plot(S,'r');plot(s,'b','LineWidth',2)
% legend('Signal','Measurements')
% subplot(212)
% plot(s);hold on;plot(mean(S,2),'r');plot(wood,'g');plot(thor,'k')
% legend('Signal','Normal Ave','Woody Ave','Thornton Ave');grid on

%endOfHelp
%Default parameter values
tol= 0.1;
max_it=100;
est_mthd='woody';
xcorr_mthd='unbiased';
thornton_sub=3;         %number of subaverages to use in the thornton procedure

if(nargin>1)
    if(~isempty(varargin{1}))
        tol=varargin{1};
    end
    if(nargin>2)
        if(~isempty(varargin{2}))
            max_it=varargin{2};
        end
        if(nargin>3)
            if(~isempty(varargin{3}))
                est_mthd=varargin{3};
            end
            if(nargin>4)
                if(~isempty(varargin{4}))
                    xcorr_mthd=varargin{4};
                end
            end
        end
    end
end

%Call repective averaging technique
switch est_mthd

    case 'woody'
        out=woody_core(x,tol,max_it,xcorr_mthd);

    case 'thornton'
        %Implement procedure from Thornton 2008
        [N,M]=size(x);
        K=floor(M/thornton_sub);

        %Call woody several times implementing the subaverages
        for k=1:K

            sub=thornton_sub*k;
            ind=round(linspace(1,M,sub+1));

            if((length(ind)-2) > (M/2))
                %Number of subaverages is equal to or just less than
                %half the number of trials, move to the final stage
                %and exit loop
                [out,est_lags]=woody_core(x,tol,max_it,xcorr_mthd);
                break
            end

            %Get woody average from the subaverages
            %procedure converges when there is no lag changes
            y=gen_subave(x,ind); %Generate sub averages
            y_old=y;
            err=1;
            while(err)
                [trash,est_lags]=woody_core(y,tol,max_it,xcorr_mthd);
                x=shift_data(x,est_lags,ind,N,M);
                y=gen_subave(x,ind); %Re-generate sub averages
                err=sum(abs(y(:)-y_old(:)));
                y_old=y;
            end

        end

    otherwise
        error(['Invalid option for est_mthd parameter: ' xcorr_mthd ' valid options are: woody, weighted, and thornton']);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%End of Maing Function%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%Helper Functions%%%%%%%%%%%%

function    x=shift_data(x,est_lags,ind,N,M)

%Shifts individual trials within each subaverage
K=length(est_lags);
for k=1:K
    lag=est_lags(k);
    if(lag)
        if(k~=K)
            sel_ind=[ind(k):ind(k+1)-1];
        else
            sel_ind=[ind(k):M];
        end
        pad=length(sel_ind);
        if(lag>0)
            x(:,sel_ind)=[zeros(lag-1,pad); x(lag:end,sel_ind)];
            %x(:,sel_ind)=[randn(lag-1,pad).*mean(std(x(:,sel_ind))).*0.001; x(lag:end,sel_ind)];
        elseif(lag<0)
            x(:,sel_ind)=[x(1:N+lag,sel_ind); zeros(lag*-1,pad)];
            %x(:,sel_ind)=[x(1:N+lag,sel_ind); randn(lag*-1,pad).*mean(std(x(:,sel_ind))).*0.001];
        end
    end
end

function [out,varargout]=woody_core(x,tol,max_it,xcorr_mthd)
[N,M]=size(x);
mx=mean(x,2);
p=zeros(N,1);
conv=1;
run=0;
sig_x=diag(sqrt(x'*x));
X=xcorr(mx);
ref=length(X)/2;
if(mod(ref,2))
    ref=ceil(ref);
else
    ref=floor(ref);
end

if(nargout>1)
    %In this case we output the lag of the trials as well
    lag_data=zeros(1,M);
end

while(conv*(run<max_it))

    z=zeros(N,1);
    w=ones(N,1);
    for i=1:M,

        y=x(:,i);
        xy=xcorr(mx,y,xcorr_mthd);
        [val,ind]=max(xy);
        if(ind>ref)
            lag=ref-ind-1;
        else
            lag=ref-ind;
        end
        if(lag>0)
            num=w(lag:end)-1;
            z(1:N-lag+1)=( z(1:N-lag+1).*num + y(lag:end))./w(lag:end);
            w(lag:end)=w(lag:end)+1;
        elseif(lag<0)
            num=w(lag*(-1)+1:end)-1;
            z(lag*(-1)+1:end)=( z(lag*(-1)+1:end).*num + y(1:N+lag) )./w(lag*(-1)+1:end);
            w(lag*(-1)+1:end)=w(lag*(-1)+1:end)+1;
        else
            z=z.*(w-1)./w + y./w;
            w=w+1;
        end
        if(exist('lag_data','var'))
            lag_data(i)=lag;
        end

    end

    old_mx=mx;
    mx=z;
    p_old=p;
    p=mx'*x./(sqrt(mx'*mx).*sig_x');
    p=sum(p)./M;
    err=abs(p-p_old);
    if(err<tol)
        conv=0;
    end
    run=run+1;

end

out=mx;

if(exist('lag_data','var'))
    varargout(1)={lag_data};
end

function [y]=gen_subave(x,ind)

[N,M]=size(x);
T=length(ind)-1;
y=zeros(N,T);

%Generate Subaverages
for i=1:T-1
    y(:,i)=mean(x(:,ind(i):ind(i+1)-1),2);
end

y(:,end)=mean(x(:,ind(T):end),2);


function [p,m,n,s] = nanstats( x, d, av1, av2)
% Copyright (c) 2014-present University of Illinois at Urbana-Champaign
% All rights reserved.
%
% Developed by:     Po-Sen Huang, Paris Smaragdis
%                   Department of Electrical and Computer Engineering
%                   Department of Computer Science
%
% Averaging?
if ~exist( 'av1', 'var')
	av1 = 1;
end
if ~exist( 'av2', 'var')
	av2 = 1;
end

% Start from higher dimensions
d = sort( d, 'descend');

% Did we get the whole thing?
if isstruct( x)
	% Get each case's stats
	for i = 1:length( x.sdr)
		[p1(i,:),m1(i,:),n1(i,:),s1(i,:)] = nanstats( x.sdr{i}, d, av1, av2);
		[p2(i,:),m2(i,:),n2(i,:),s2(i,:)] = nanstats( x.sir{i}, d, av1, av2);
		[p3(i,:),m3(i,:),n3(i,:),s3(i,:)] = nanstats( x.sar{i}, d, av1, av2);
		[p4(i,:),m4(i,:),n4(i,:),s4(i,:)] = nanstats( x.sto{i}, d, av1, av2);
	end

	% Put them together
	p = [p1 p2 p3 p4];
	m = [m1 m2 m3 m4];
	n = [n1 n2 n3 n4];
	s = [s1 s2 s3 s4];

	% Reshape
	if isvector( p) && size( av1, 2) > 1 && ~isscalar( av1)
		p = reshape( p, size( av1, 2), [])';
		m = reshape( m, size( av1, 2), [])';
		n = reshape( n, size( av1, 2), [])';
		s = reshape( s, size( av1, 2), [])';
	end
	if isvector( p) && size( av2, 1) > 1
		size( p)
		size( av2)
		p = reshape( p, [], size( av2, 1))';
		m = reshape( m, [], size( av2, 1))';
		n = reshape( n, [], size( av2, 1))';
		s = reshape( s, [], size( av2, 1))';
	end
	return
end

% Init
p = x;
m = x;
n = x;
s = x;

% Get each dimension stats
for i = 1:length( d)
	p = nanmean( p, d(i));
	m = nanmax( m, [], d(i));
	n = nanmin( n, [], d(i));
	s = nanstd( s, [], d(i));
end

% Thin out
if length( size( p)) > 2
	p = (av2' * squeeze( p)' * av1)';
	m = (av2' * squeeze( m)' * av1)';
	n = (av2' * squeeze( n)' * av1)';
	s = (av2' * squeeze( s)' * av1)';
else
	p = (av1' * p * av2);
	m = (av1' * m * av2);
	n = (av1' * n * av2);
	s = (av1' * s * av2);
end
return
p = vec( p);
m = vec( m);
n = vec( n);
s = vec( s);


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function tfrgabot
%TFRGABT Unit test for the function tfrgabor.

%	O. Lemoine - February 1996.

N1=256;
M=16;
N=16;

% Test of the biorthogonality between h and gam (dual frame window)
Nh=65;					% length of window h
h=amgauss(Nh); h=h/norm(h);		% h must be of odd length
n0=1;
m0=1;
sig=zeros(N1,1);
sig(((n0-1)*N+1):((n0-1)*N+Nh))=h.*fmconst(Nh,(m0-1)/M);
tfr=tfrgabor(sig,N,1,h);		% Critical sampling case
if abs(tfr(1,1)-1)>sqrt(eps),
  error('tfrgabor test 1 failed');
end
errors=find(any(tfr>eps));
if length(errors)~=1,
  error('tfrgabor test 2 failed');
end

% Localization
Nh=37;					% length of window h
h=amgauss(Nh); h=h/norm(h);		% h must be of odd length
n0=8;
m0=8;
sig=zeros(N1,1);
sig(((n0-1)*N+1):((n0-1)*N+Nh))=h.*fmconst(Nh,(m0-1)/M);
tfr=tfrgabor(sig,N,1,h);		% Critical sampling case
if abs(tfr(m0,n0)-1)>sqrt(eps),		% C(n0,m0)=1
  error('tfrgabor test 3 failed');
end
errors=find(any(tfr>eps));		% Only one non-zero coeff
if length(errors)~=1,
  error('tfrgabor test 4 failed');
end

% For another window and position of sig
Nh=19;					% length of window h
h=amexpo2s(Nh);	h=h/norm(h);		% h must be of odd length
n0=13;
m0=5;
sig=zeros(N1,1);
sig(((n0-1)*N+1):((n0-1)*N+Nh))=h.*fmconst(Nh,(m0-1)/M);
tfr=tfrgabor(sig,N,1,h);		% Critical sampling case
if abs(tfr(m0,n0)-1)>sqrt(eps),		% C(n0,m0)=1
  error('tfrgabor test 5 failed');
end
errors=find(any(tfr>eps));		% Only one non-zero coeff
if length(errors)~=1,
  error('tfrgabor test 6 failed');
end

% For another window and position of sig
Nh=17;					% length of window h
h=amexpo2s(Nh);	h=h/norm(h);		% h must be of odd length
n0=15;
m0=8;
sig=zeros(N1,1);
sig(((n0-1)*N+1):((n0-1)*N+Nh))=h.*fmconst(Nh,(m0-1)/M);
tfr=tfrgabor(sig,N,1,h);		% Critical sampling case
if abs(tfr(m0,n0)-1)>sqrt(eps),		% C(n0,m0)=1
  error('tfrgabor test 7 failed');
end
errors=find(any(tfr>eps));		% Only one non-zero coeff
if length(errors)~=1,
  error('tfrgabor test 8 failed');
end

% Synthesis for q=1
Nh=33;q=1;
h=amgauss(Nh); h=h/norm(h);
sig=zeros(N1,1);
sig=amgauss(N1);
sigr=zeros(N1,1);
[tfr,dgr]=tfrgabor(sig,N,q,h);
alpha=round((2*N1/N-1-Nh)/(2*q));
hN1=zeros(N1,1);
hN1((N1-(Nh-1))/2-alpha:(N1+Nh-1)/2-alpha)=h;
n=1:N;
som=M*ifft(dgr);
for k=1:N1,
  indice=modulo(k-round(n*M/q),N1);
  sigr(k)=som(modulo(k,M),:)*fftshift(hN1(indice));
end
if any(abs(sig-sigr)>sqrt(eps))~=0,
  error('tfrgabor test 9 failed');
end

% Synthesis for q=4
Nh=33;q=4;N=32;M=32;
h=amgauss(Nh); h=h/norm(h);
sig=zeros(N1,1);
sig=amgauss(N1);
sigr=zeros(N1,1);
[tfr,dgr]=tfrgabor(sig,N,q,h);
alpha=round((2*N1/N-1-Nh)/(2*q));
hN1=zeros(N1,1);
hN1((N1-(Nh-1))/2-alpha:(N1+Nh-1)/2-alpha)=h;
n=1:N;
som=M*ifft(dgr);
for k=1:N1,
  indice=modulo(k-round(n*M/q),N1);
  sigr(k)=som(modulo(k,M),:)*fftshift(hN1(indice));
end
if any(abs(sig-sigr)>sqrt(eps))~=0,
  error('tfrgabor test 10 failed');
end


function bk = shepard_basis_1d ( nd, xd, k, p, ni, xi )

%*****************************************************************************80
%
%% SHEPARD_BASIS_1D evaluates a 1D Shepard basis function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Shepard,
%    A two-dimensional interpolation function for irregularly spaced data,
%    ACM '68: Proceedings of the 1968 23rd ACM National Conference,
%    ACM, pages 517-524, 1969.
%
%  Parameters:
%
%    Input, integer ND, the number of data points.
%
%    Input, real XD(ND,1), the data points.
%
%    Input, integer K, the index of the desired basis function,
%    1 <= K <= ND.
%
%    Input, real P, the power.
%
%    Input, integer NI, the number of interpolation points.
%
%    Input, real XI(NI,1), the interpolation points.
%
%    Output, real BK(NI,1), the basis function at the interpolation points.
%
  bk = zeros ( ni, 1 );

  for i = 1 : ni

    if ( p == 0.0 )

      w(1:nd,1) = 1.0 / nd;

    else

      w = zeros ( nd, 1 );

      z = -1;
      for j = 1 : nd
        w(j) = abs ( xi(i) - xd(j) );
        if ( w(j) == 0.0 )
          z = j;
          break
        end
      end

      if ( z ~= -1 )
        w = zeros ( nd, 1 );
        w(z) = 1.0;
      else
        w(1:nd,1) = 1.0 ./ w(1:nd,1) .^ p;
        s = sum ( w );
        w(1:nd,1) = w(1:nd,1) / s;
      end

    end

    bk(i,1) = w(k,1);

  end

  return
end


function a_cr = r83_cr_fa ( n, a )

%*****************************************************************************80
%
%% R83_CR_FA decomposes a real tridiagonal matrix using cyclic reduction.
%
%  Discussion:
%
%    The R83 storage format is used for a real tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%    Once R83_CR_FA has decomposed a matrix A, then R83_CR_SL may be used to solve
%    linear systems A * x = b.
%
%    R83_CR_FA does not employ pivoting.  Hence, the results can be more
%    sensitive to ill-conditioning than standard Gauss elimination.  In
%    particular, R83_CR_FA will fail if any diagonal element of the matrix
%    is zero.  Other matrices may also cause R83_CR_FA to fail.
%
%    R83_CR_FA can be guaranteed to work properly if the matrix is strictly
%    diagonally dominant, that is, if the absolute value of the diagonal
%    element is strictly greater than the sum of the absolute values of
%    the offdiagonal elements, for each equation.
%
%    The algorithm may be illustrated by the following figures:
%
%    The initial matrix is given by:
%
%          D1 U1
%          L1 D2 U2
%             L2 R83 U3
%                L3 D4 U4
%                   L4 D5 U5
%                      L5 D6
%
%    Rows and columns are permuted in an odd/even way to yield:
%
%          D1       U1
%             R83    L2 U3
%                D5    L4 U5
%          L1 U2    D2
%             L3 U4    D4
%                L5       D6
%
%    A block LU decomposition is performed to yield:
%
%          D1      |U1
%             R83   |L2 U3
%                D5|   L4 U5
%          --------+--------
%                  |D2'F3
%                  |F1 D4'F4
%                  |   F2 D6'
%
%    For large systems, this reduction is repeated on the lower right hand
%    tridiagonal subsystem until a completely upper triangular system
%    is obtained.  The system has now been factored into the product of a
%    lower triangular system and an upper triangular one, and the information
%    defining this factorization may be used by R83_CR_SL to solve linear
%    systems.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Hockney,
%    A fast direct solution of Poisson's equation using Fourier Analysis,
%    Journal of the ACM,
%    Volume 12, Number 1, pages 95-113, January 1965.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A(3,N), the R83 matrix.
%
%    Output, real A_CR(3,2*N+1), factorization information.
%
  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R83_CR_FA - Fatal error!\n' );
    fprintf ( 1, '  Nonpositive N = %d\n', n );
    return
  end

  a_cr = zeros ( 3, 2 * n + 1 );

  if ( n == 1 )
    a_cr(1,1:3) = 0.0;
    a_cr(2,1) = 0.0;
    a_cr(2,2) = 1.0 / a(2,1);
    a_cr(2,3) = 0.0;
    a_cr(3,1:3) = 0.0;
    return
  end
%
%  Zero out the workspace entries.
%
  a_cr(1,1) = 0.0;
  a_cr(1,2:n) = a(1,2:n);
  a_cr(1,n+1:2*n+1) = 0.0;

  a_cr(2,1) = 0.0;
  a_cr(2,2:n+1) = a(2,1:n);
  a_cr(2,n+2:2*n+1) = 0.0;

  a_cr(3,1) = 0.0;
  a_cr(3,2:n) = a(3,1:n-1);
  a_cr(3,n+1:2*n+1) = 0.0;

  il = n;
  ipntp = 0;

  while ( 1 < il )

    ipnt = ipntp;
    ipntp = ipntp + il;
    if ( mod ( il, 2 ) == 1 )
      inc = il + 1;
    else
      inc = il;
    end

    incr = floor ( inc / 2 );
    il = floor ( il / 2 );
    ihaf = ipntp + incr + 1;
    ifulp = ipnt + inc + 2;

    for ilp = incr : -1 : 1
      ifulp = ifulp - 2;
      iful = ifulp - 1;
      ihaf = ihaf - 1;
      a_cr(2,iful+1) = 1.0 / a_cr(2,iful+1);
      a_cr(3,iful+1)  = a_cr(3,iful+1)  * a_cr(2,iful+1);
      a_cr(1,ifulp+1) = a_cr(1,ifulp+1) * a_cr(2,ifulp+2);
      a_cr(2,ihaf+1)  = a_cr(2,ifulp+1) - a_cr(1,iful+1)  * a_cr(3,iful+1) ...
                                  - a_cr(1,ifulp+1) * a_cr(3,ifulp+1);
      a_cr(3,ihaf+1) = -a_cr(3,ifulp+1) * a_cr(3,ifulp+2);
      a_cr(1,ihaf+1) = -a_cr(1,ifulp+1) * a_cr(1,ifulp+2);
    end

  end

  a_cr(2,ipntp+2) = 1.0 / a_cr(2,ipntp+2);

  return
end


function datapt = ma_GAAeroTasks(stateLogEntry, subTask, celBodyData)
%ma_GAAeroTasks Summary of this function goes here
%   Detailed explanation goes here

    bodyID = stateLogEntry(8);

    bodyInfo = getBodyInfoByNumber(bodyID, celBodyData);
    ut = stateLogEntry(1);
    rVectECI = stateLogEntry(2:4)';
    vVectECI = stateLogEntry(5:7)';

    switch subTask
        case 'dynPress'
            altitude = norm(rVectECI) - bodyInfo.radius;

            if(altitude <= bodyInfo.atmohgt && altitude >= 0)
                [lat, long, ~, ~, ~, ~, ~, vVectECEF] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
                density = getAtmoDensityAtAltitude(bodyInfo, altitude, lat, ut, long);
            elseif(altitude <= 0)
                density = 0;
                vVectECEF = [0;0;0];
            else
                density = 0;
                vVectECEF = [0;0;0];
            end

            vVectEcefMag = norm(vVectECEF);
            vVectEcefMagMS = vVectEcefMag*1000;

            dynP = density * (vVectEcefMagMS^2) / 2; %kg/m^3 * m^2 / s^2  = kg/(m*s^2)
            dynP_kPa = dynP/1000;

            datapt = dynP_kPa;
        case 'atmoPress'
            altitude = norm(rVectECI) - bodyInfo.radius;
            pressure = getPressureAtAltitude(bodyInfo, altitude);

            datapt = pressure;

        case 'atmoTemp'
            altitude = norm(rVectECI) - bodyInfo.radius;

            if(altitude <= bodyInfo.atmohgt && altitude >= 0)
                [lat, long, ~, ~, ~, ~, ~, ~] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
                temperature = getTemperatureAtAltitude(bodyInfo, altitude, lat, ut, long) ;
            elseif(altitude <= 0)
                temperature = 0;
            else
                temperature = 0;
            end

            datapt = temperature;

        case 'atmoDensity'
            altitude = norm(rVectECI) - bodyInfo.radius;

            if(altitude <= bodyInfo.atmohgt && altitude >= 0)
                [lat, long, ~, ~, ~, ~, ~, ~] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
                density = getAtmoDensityAtAltitude(bodyInfo, altitude, lat, ut, long);
            elseif(altitude <= 0)
                density = 0;
            else
                density = 0;
            end

            datapt = density;
        case 'machNumber'
            altitude = norm(rVectECI) - bodyInfo.radius;
            pressure = getPressureAtAltitude(bodyInfo, altitude);

            if(altitude <= bodyInfo.atmohgt && altitude >= 0)
                [lat, long, ~, ~, ~, ~, ~, vVectECEF, ~] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
                density = getAtmoDensityAtAltitude(bodyInfo, altitude, lat, ut, long);
            elseif(altitude <= 0)
                density = 0;
            else
                density = 0;
            end

            pressurePa = pressure*1000; %kPa -> Pa

            if(density > 0)
                speedSound = sqrt(1.4 * pressurePa / density);
                vECEFMag = norm(vVectECEF) * 1000; %km/s -> m/s

                datapt = vECEFMag/speedSound;
            else
                datapt = 0;
            end
    end
end


clear all; close all; clc;
%--------------------------------------------------------------------------
library_directory = '../fmlib/';
addpath(library_directory);
data_directory   = '../Data/';
addpath(data_directory);
matlab_directory   = '../matlab/';
addpath(matlab_directory);
%%
%==========================================================================
n = 500;
% create 2D vector field
s = [n n 1];
U = randn(n,n,2);
sigma = (n/200)*40;
for it=1:10
    U = perform_vf_normalization( perform_blurring(U, sigma) );
end;
U = perform_vf_normalization( U );
% test for various degree of anisotropy
aniso_list = [.01 .05 .1 .2 .5 1];

%% test for progressive propagation
aniso = 0.1;
V = cat(3, -U(:,:,2), U(:,:,1)); % orthogonal vector
T = perform_tensor_recomp(U,V, ones(n),ones(n)*aniso );
plot_tensor_field(T);
source_points = pick_start_points(T(:,:,1,1));
h = [1;1];
%%
tic
[U, dUx, dUy, V, L] = fm2dAniso(h, T, source_points);
toc
%%
figure; imshow(U, []); colormap(jet);
hold on;
%plot_tensor_field(T);


function Xi = NonlinearSparseRegression(x,u,dt,options_method,lambda)

Ninputs = min(size(u));
%% Compute Derivative
% compute derivative using fourth order central difference
% use TVRegDiff if more error
dx = zeros(length(x)-5,3);
for i=3:length(x)-3
    for k=1:size(x,2)
        dx(i-2,k) = (1/(12*dt))*(-x(i+2,k)+8*x(i+1,k)-8*x(i-1,k)+x(i-2,k));
    end
end
% concatenate
xaug = [x(3:end-3,:) u(3:end-3,:)];
dx(:,size(x,2)+1:size(x,2)+Ninputs) = repmat(0*dx(:,size(x,2)),[1,Ninputs]);

n = size(dx,2);

%% Sparse regression
clear Theta Xi
Theta = poolData(xaug,n,options_method.order,options_method.usesine);
Xi = sparsifyDynamics(Theta,dx,lambda,n);


 function [b, d, f, z, mm] = compute_rf_spsp_mgh(kp,rfp,gz,kz,kf)
%function [b, d, f, z, mm] = compute_rf_spsp_mgh(kp,rfp,gz,kz,kf)
%Function that computes the complex-valued SPSP RF pulse waveform,
%using parameters specified in structure rfp.
%Inputs:
%kp: k-space trajectory parameter structure, generated by kparameterSPSP.m
%rfp:RF waveform design parameter structure, generated by rfparameterSPSP.m
%gz: z gradient waveform vector
%kz,kf: SPSP k-space (kf-kz space) trajectory vectors
%Output:
%b:complex-value SPSP RF pulse waveform in units of Gauss (g).
%
%Chun-yu Yip, 4/1/2009

%Physical parameters
gam = 26751;                       %rad/s/g; gyromagnetic ratio
gambar = gam/2/pi;                 %Hz/g

%Create grid in SPSP space
dresz = rfp.dfovz/rfp.ddimz;       %cm
dresf = rfp.dfovf/rfp.ddimf;       %Hz
z = [-rfp.dfovz/2:dresz:rfp.dfovz/2-dresz];
f = [-rfp.dfovf/2:dresf:rfp.dfovf/2-dresf];
[Z,F] = ndgrid(z,f);

printm('Creating desired pattern for SPSP pulse...');

%phase pattern
TD = rfp.TE-kp.pointtime*kp.npnts/2; %pulse end to acquisition of DC sample
dphs = gam * rfp.alpha * TD * F .* (Z-rfp.zshift); %Eq. 5

%magnitude pattern
if streq(rfp.profileshape,'rect')

    dmag = zeros(size(Z));
    dmag(find((Z<(rfp.slthickz/2)) & (Z>(-rfp.slthickz/2)))) = 1;

elseif streq(rfp.profileshape,'gaussian')

    a = -4*log(2)/rfp.slthickz/rfp.slthickz;
    dmag = exp(a*(Z.^2));

else

    printm('Slice profile shape unknown.');

end

if rfp.smoothprofile

    kernel = gaussian(z,0,rfp.kernelstdv)';
    dmag=convn_fft(dmag,kernel);     %Smoothing implemented in Fourier domain

end;

dmag = dmag/max(max(dmag));
dmag = sin(rfp.flipangle/180 * pi) * dmag;
d = exp(1i*dphs).*dmag;

%Define matrices and vectors for conjugate gradient
b0 = zeros(length(kz),1);            %Initiation of CG
mask = ones(size(d));                %ROI in f-z space
%W = spdiag(mask); % weighting based on ROI definition
W = diag_sp(mask); % jf version

%total energy penalty
beta = rfp.beta;
% C = beta*speye(length(kz)); % original version
C = beta; % jf version

M = length(Z(:));
%precon =speye(length(kz));           %CG Preconditioner
precon = 1; % jf version
Niter = rfp.Niter;                   %Number of CG iterations

%Define matlab object "A" to represent the gigantic system matrix.
%"A" is of type "Fatrix" from Jeff Fessler's toolbox.
printm('Forming spatial-spectral fast object...');
A=spsp_Af(z,f,kz,kf,kp.pointtime,rfp.dfovf,rfp.dfovz);

%%%%%%%%%%%Conjugate Gradient%%%%%%%%%%%%%%%%%%%
printm('Running preconditioned conjugate gradient...');
[b_all, info] = qpwls_pcg(b0, A, W, d(:), zeros(M,1), sqrt(C), precon, Niter);
b = b_all(:,Niter);

if rfp.hamming
	scale = 2;          %This parameter controls how wide the window is.
	hamm=(0.54-0.46.*cos(pi*((kz/scale/max(kz))+1)));
	b= b.*hamm;         %Intended to smooth the excited slice profile.
end

m = A*b;            %Small-tip-angle approximation of excitation pattern
mm = reshape(m,size(d));

if 0 % no, superceded by ir_mri_rf_spsp_plot.m
	printm('Displaying SPSP pulse design results...');
	t = kp.pointtime*[0:1:length(gz)-1].';
	figure;
	subplot(2,1,1);
	plot(t*1000,gz);
	grid;
	x1=xlabel('Time (ms)');
	y1=ylabel('g/cm');
	t1=title('z gradient');
	set(t1,'Fontsize',14);
	set(x1,'Fontsize',12);
	set(y1,'Fontsize',12);

	subplot(2,1,2);
	hold on;
	plot(t*1000,real(b),'b');
	plot(t*1000,imag(b),'r');
	grid;
	x2=xlabel('Time (ms)');
	y2=ylabel('g');
	t2=title('SPSP pulse');
	l2=legend('Real','Imaginary');
	set(t2,'Fontsize',14);
	set(x2,'Fontsize',12);
	set(y2,'Fontsize',12);
	set(l2,'Fontsize',12);

	%Display desired pattern of the SPSP recovery pulse.
	figure;
	subplot(2,1,1);
	imagesc(f,z,abs(d));
	colormap default;colorbar;
	x1 = xlabel('Frequency offset (Hz)');
	y1 = ylabel('z (cm)');
	t1 = title('Desired SPSP pattern (magnitude)');

	set(x1,'Fontsize',12);
	set(y1,'Fontsize',12);
	set(t1,'Fontsize',12);

	subplot(2,1,2);
	imagesc(f,z,angle(d).*abs(d),[-pi,pi]);colorbar;
	x2 =xlabel('Frequency offset (Hz)');
	y2=ylabel('z (cm)');
	t2=title('Desired SPSP pattern (phase)');
	set(x2,'Fontsize',12);
	set(y2,'Fontsize',12);
	set(t2,'Fontsize',12);

	figure;
	subplot(2,1,1);
	imagesc(f,z,abs(mm));
	xlabel('frequency (Hz)');
	ylabel('z (cm)');
	title('Resulting pattern (linear regime prediction)');

	subplot(2,1,2);
	imagesc(f,z,angle(mm));
	xlabel('frequency (Hz)');
	ylabel('z (cm)');
	title('Resulting phase pattern (linear regime prediction)');
end


function SurfCov = surface_coverage2(Axis,Len,Vec,height,nl,ns)

% Computes surface coverage (number between 0 and 1) of points on cylinder
% surface defined by "Axis" and "Len". "Vec" are the vectors connecting
% points to the Axis and "height" are the heights of the points from
% the base of the cylinder

[U,W] = orthonormal_vectors(Axis);
Vec = Vec*[U W];
ang = atan2(Vec(:,2),Vec(:,1))+pi;
I = ceil(height/Len*nl);
I(I == 0) = 1;
I(I > nl) = nl;
J = ceil(ang/2/pi*ns);
J(J == 0) = 1;
K = [I J-1]*[1 nl]';
SurfCov = length(unique(K))/nl/ns;


begin=clock;
omega=1;
over_r=2.4; %note that over_r should be between 3 and 4 for this program.
rate=5.3; %this is approximatly the ratio N/r, here the ratio must be above 1
n=ceil(rate*over_r);%this is the number of unions of sets
p=ceil(2*n-over_r); %this is the degree of the fine mesh, use p=n+1, or larger
L=200/n; %the signal is sampled from -L*T:L*T
%L=0.5;
step=over_r/2/omega;

%R1=round(-2/3+n/3);
%L1=R1+1-n;
%L2=round(2/3-n/3);
%R2=n+L2-1;

%I have doubled over_r and halved L so that the
%interval is the same, i.e., width is the same
%as it was for the single sample case.

rand('state',sum(100*clock))
%rand('seed',4)
h=(rand(n,1)-1/2)*step;
%h=(-1/2:1/n:1/2-1/n)*step;

width=L*step;
y=-width:step/p:width;
ysize=size(y);
ysize=ysize(2);
% y is the sampling set for signal_origin and y+h for signal_shift
signal=zeros(n,ysize);
pad=zeros(n,ysize);

signal_origin=zeros(size(y));
signal_shift=zeros(size(y));
pad_origin=zeros(size(y));
pad_shift=zeros(size(y));

%rand('seed',0)
%this sets the seed to a fixed value, 0, so that I get reproducable results
rand('state',sum(100*clock))
R=100;
%the first two collumns are the amplitudes, real(first) and complex(second),
%i.e., signal_coef(1,q)+i*signal_coef(2,q), the third and fourth are the
%band limits, for example, min(signal_coef(3,q),signal_coef(4,q)) is
%the left bandwidth, the max will give the right bandwidth
signal_coef=rand(4,R);
signal_coef(1,:)=2*(signal_coef(1,:)-1/2);
signal_coef(1,:)=signal_coef(1,:)/norm(signal_coef(1,:),2)/2/pi;
signal_coef(2,:)=2*(signal_coef(2,:)-1/2);
signal_coef(2,:)=signal_coef(2,:)/norm(signal_coef(2,:),2)/2/pi;
signal_coef(3,:)=2*(signal_coef(3,:)-1/2);
signal_coef(3,:)=omega*signal_coef(3,:)/max(abs(signal_coef(3,:)));
signal_coef(4,:)=2*(signal_coef(4,:)-1/2);
signal_coef(4,:)=omega*signal_coef(4,:)/max(abs(signal_coef(4,:)));

%need an axis for the dual space that has the same number of
%elements as y does.

dual_axis=2*p*omega/over_r/(2*p*L+1)*(-p*L:1:p*L);

sample_dual=zeros(size(dual_axis));
dualsize=size(dual_axis);
dualsize=dualsize(2);

for q=1:R
  for j=1:dualsize
if dual_axis(j)>=min(signal_coef(3,q),signal_coef(4,q)) & dual_axis(j)<=max(signal_coef(3,q),signal_coef(4,q))
      sample_dual(j)=sample_dual(j)+signal_coef(1,q)+i*signal_coef(2,q);
    end
  end
end

tmp=0;
for q=1:R
  signal_origin=signal_origin+(signal_coef(1,q)+i*signal_coef(2,q))./(sqrt(2*pi)*2*pi*i*y).*(exp(2*pi*i*y*max(signal_coef(3,q),signal_coef(4,q)))-exp(2*pi*i*y*min(signal_coef(3,q),signal_coef(4,q))))*2*pi;
%  signal_shift=signal_shift+(signal_coef(1,q)+i*signal_coef(2,q))./(sqrt(2*pi)*2*pi*i*(y+h(2))).*(exp(2*pi*i*(y+h(2))*max(signal_coef(3,q),signal_coef(4,q)))-exp(2*pi*i*(y+h(2))*min(signal_coef(3,q),signal_coef(4,q))))*2*pi;
  tmp=tmp+(signal_coef(1,q)+i*signal_coef(2,q))*(max(signal_coef(3,q),signal_coef(4,q))-min(signal_coef(3,q),signal_coef(4,q)))/sqrt(2*pi)*2*pi;
end
  joe=0;
  for q=1:ysize
    if y(q)==0
      joe=q;
    end
  end
  if joe>0
    signal_origin(joe)=tmp;
  end

for j=1:n
tmp=0;
for q=1:R
  signal(j,:)=signal(j,:)+(signal_coef(1,q)+i*signal_coef(2,q))./(sqrt(2*pi)*2*pi*i*(y+h(j))).*(exp(2*pi*i*(y+h(j))*max(signal_coef(3,q),signal_coef(4,q)))-exp(2*pi*i*(y+h(j))*min(signal_coef(3,q),signal_coef(4,q))))*2*pi;
  tmp=tmp+(signal_coef(1,q)+i*signal_coef(2,q))*(max(signal_coef(3,q),signal_coef(4,q))-min(signal_coef(3,q),signal_coef(4,q)))/sqrt(2*pi)*2*pi;
end
  joe=0;
  for q=1:ysize
    if y(q)+h(j)==0
      joe=q;
    end
  end
  if joe>0
    signal(j,joe)=tmp;
  end
end

% p=n is the measure of difference between the fine and coarse mesh
for j=1:p:ysize
  pad(:,j)=signal(:,j);
end

pad_dual=zeros(n,ysize);

for j=1:dualsize
  for k=1:n
    pad_dual(k,j)=sum(pad(k,:).*exp(-2*pi*i*dual_axis(j)*y));
  end
end

pad_dual=pad_dual*(over_r/2/omega/p)/sqrt(2*pi);

%SHOULD THEY BE MULTIPLIED BY p SO THAT THEY MATCH sample_dual?

%this makes it so that the origin matches.
for j=1:n
  pad_dual(j,:)=pad_dual(j,:).*exp(-2*pi*i*h(j)*dual_axis);
end

kappa=max(2,min(n,floor((n+over_r+1)/(n-over_r+1))))

%b=zeros(2*kappa,1);
%B=zeros(2*kappa);
%B(1,1)=1; B(1,2)=1; B(1,3)=-1;
%B(kappa,end)=1; B(kappa,end-1)=1; B(kappa,end-2)=-1;
%
%for j=2:kappa-1
%  B(j,2*(j-1))=-1;
%  B(j,2*(j-1)+1)=1;
%  B(j,2*(j-1)+2)=1;
%  B(j,2*(j-1)+3)=-1;
%end
%b(1)=-1;
%b(kappa)=1;
%for j=1:kappa
%  B(j+kappa,2*j-1)=-1;
%  B(j+kappa,2*j)=1;
%  b(j+kappa)=n-1;
%end
%tmp1=B\b;

zones=zeros(kappa,2);
for j=1:kappa
%zones(j,1)=tmp1(2*j-1);
%zones(j,2)=tmp1(2*j);
zones(j,1)=round(j*(n+1)/(kappa+1)-n);
zones(j,2)=zones(j,1)+n-1;
end

%for j=1:kappa
%  if abs(zones(j,1)-round(zones(j,1)))<=abs(zones(j,2)-round(zones(j,2)))
%    zones(j,1)=round(zones(j,1));
%    zones(j,2)=n-1+zones(j,1);
%  else
%    zones(j,2)=round(zones(j,2));
%    zones(j,1)=zones(j,2)-n+1;
%  end
%end

A=zeros(n);
for j=1:n
  for k=1:n
    A(j,k)=exp(2*pi*i*h(k)*j/step);
  end
end
A_inverse=inv(A);

c=zeros(kappa,n);

for j=1:kappa
  e=zeros(n,1);
  e(1-zones(j,1))=1;
  R=diag(exp(2*pi*i*h*(zones(j,1)-1)/step));
  c(j,:)=(inv(R)*A_inverse*e)'; %'
end

%e=zeros(n,1);
%e(1-L2)=1;
%R=diag(exp(2*pi*i*h*(L2-1)/step));
%c(2,:)=(inv(R)*A_inverse*e)'; %'

partition=over_partitions(zones,omega,over_r,n,dual_axis);

%left_partition=zeros(size(dual_axis));
%right_partition=zeros(size(dual_axis));

%for j=1:length(dual_axis)
%  if dual_axis(j)<=(L1-1)/step+omega
%    left_partition(j)=0;
%  elseif (L1-1)/step+omega<dual_axis(j) & dual_axis(j)<-omega
%    left_partition(j)=rho((-dual_axis(j)-omega)/(-(L1-1)/step-2*omega));
%  elseif -omega<=dual_axis(j) & dual_axis(j)<=(L2-1)/step+omega
%    left_partition(j)=1;
%  elseif (L2-1)/step+omega<dual_axis(j) & dual_axis(j)<(R1+1)/step-omega
%    left_partition(j)=rho((dual_axis(j)-((L2-1)/step+omega))/((R1-L2+2)/step-2*omega));
%  else
%    left_partition(j)=0;
%  end
%end
%for j=1:length(dual_axis)
%  if dual_axis(j)<=(L2-1)/step+omega
%    right_partition(j)=0;
%  elseif (L2-1)/step+omega<dual_axis(j) & dual_axis(j)<(R1+1)/step-omega
%    right_partition(j)=1-rho((dual_axis(j)-((L2-1)/step+omega))/((R1-L2+2)/step-2*omega));
%  elseif (R1+1)/step-omega<=dual_axis(j) & dual_axis(j)<=omega
%    right_partition(j)=1;
%  elseif omega<dual_axis(j) & dual_axis(j)<(R2+1)/step-omega
%    right_partition(j)=rho((dual_axis(j)-omega)/((R2+1)/step-2*omega));
%  else
%    right_partition(j)=0;
%  end
%end

filters=zeros(n,length(dual_axis));
for j=1:n
  for k=1:kappa
    filters(j,:)=filters(j,:)+c(k,j)*partition(k,:);
  end
end

filtered_duals=zeros(size(filters));

for j=1:n
  filtered_duals(j,:)=filtered_duals(j,:)+filters(j,:).*pad_dual(j,:);
end

recon_dual=zeros(size(dual_axis));
for j=1:n
  recon_dual=recon_dual+filtered_duals(j,:);
end

recon_signal=zeros(size(y));

k=-p*L:1:p*L;

for j=1:dualsize
  recon_signal=recon_signal+recon_dual(j)*exp(2*pi*i*k/(2*p*L+1)*k(j));
end

recon_signal=recon_signal/(2*p*L+1)*sqrt(2*pi)*omega*p/over_r*2*p;

%hold off
semilogy(y,abs(recon_signal-signal_origin),'k');
%pause
%hold on
%plot(h,10^(-2),'o');

%h/step
cond(A)

done=clock;
time_cost=done-begin;
time_cost=time_cost(6)+60*time_cost(5)+360*time_cost(4)

%L
%step

%for j=1:n
%  hold off
%  plot(dual_axis,real(filters(j,:)))
%  hold on
%  plot(dual_axis,imag(filters(j,:)),'r')
%  hold off
%  pause
%end


function varargout = createMengerSponge()
%CREATEMENGERSPONGE Create a cube with an inside cross removed
%
%   [n, e, f] = createMengerSponge;
%   Main use is to test possibility of drawing polyhedra with complex faces
%   (polygonal faces with holes)
%
%   Example
%   [n, e, f] = createMengerSponge;
%   drawMesh(n, f);
%
%   See also
%   meshes3d, drawMesh
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2007-10-18
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

%   HISTORY
%   2008-10-17 finishes implementation

nodes =[...
    ... % main cube corners (1->8)
    0 0 0; ...
    3 0 0; ...
    0 3 0; ...
    3 3 0; ...
    0 0 3; ...
    3 0 3; ...
    0 3 3; ...
    3 3 3; ...
    ... % outer cube inner face corners
    1 1 0; ... % face z=0 (9->12)
    2 1 0; ...
    1 2 0; ...
    2 2 0; ...
    1 1 3; ... % face z=3 (13->16)
    2 1 3; ...
    1 2 3; ...
    2 2 3; ...
    1 0 1; ... % face y=0 (17->20)
    2 0 1; ...
    1 0 2; ...
    2 0 2; ...
    1 3 1; ... % face y=3 (21->24)
    2 3 1; ...
    1 3 2; ...
    2 3 2; ...
    0 1 1; ... % face x=0 (25->28)
    0 2 1; ...
    0 1 2; ...
    0 2 2; ...
    3 1 1; ... % face x=3 (29->32)
    3 2 1; ...
    3 1 2; ...
    3 2 2; ...
    ... % inner cube corners  (33->40)
    1 1 1; ...
    2 1 1; ...
    1 2 1; ...
    2 2 1; ...
    1 1 2; ...
    2 1 2; ...
    1 2 2; ...
    2 2 2; ...
    ];

edges = [...
    1 2;1 3;2 4;3 4;5 6;5 7;6 8;7 8;1 5;2 6;3 7;4 8;... % outer cube
    9 10;9 11;10 12;11 12;13 14;13 15;14 16;15 16; ...
    17 18;17 19;18 20;19 20; 21 22;21 23;22 24;23 24; ...
    25 26;25 27;26 28;27 28; 29 30;29 31;30 32;31 32; ...
    33 34;33 35;34 36;35 36; 37 38;37 39;38 40;39 40; ... % inner cube
    33 37;34 38;35 39;36 40; ...
     9 33;10 34;11 35;12 36; ... % parallel to xy
    13 37;14 38;15 39;16 40; ...
    17 33;18 34;19 37;20 38; ... % parallel to yz
    21 35;22 36;23 39;24 40; ...
    25 33;26 35;27 37;28 39; ... % parallel to xz
    29 34;30 36;31 38;32 40; ...
    ];

% Alternative definition for faces:
%     [1 2 4 3 NaN 9  11 12 10], ...
%     [5 6 8 7 NaN 13 15 16 14], ...
%     [1 5 7 3 NaN 25 26 28 27], ....
%     [2 6 8 4 NaN 29 30 32 31], ...
%     [1 2 6 5 NaN 17 18 20 19], ...
%     [3 4 8 7 NaN 21 22 24 23], ...

faces = {...
    ... % 6 square faces with a square hole
    [1 2 4 3 1 9  11 12 10  9], ...
    [5 6 8 7 5 13 15 16 14 13], ...
    [1 5 7 3 1 25 26 28 27 25], ....
    [2 6 8 4 2 29 30 32 31 29], ...
    [1 2 6 5 1 17 18 20 19 17], ...
    [3 4 8 7 3 21 22 24 23 21], ...
    ... % faces orthogonal to XY plane, parallel to Oz axis
    [ 9 10 34 33], [ 9 11 35 33], [10 12 36 34], [11 12 36 35], ...
    [13 14 38 37], [13 15 39 37], [14 16 40 38], [15 16 40 39], ...
    ... % faces orthogonal to YZ plane, parallel to Oy axis
    [17 18 34 33], [17 19 37 33], [18 20 38 34], [19 20 38 37], ...
    [21 22 36 35], [21 23 39 35], [22 24 40 36], [23 24 40 39], ...
    ...% faces orthogonal to the YZ plane, parallel to Ox axis
    [25 33 35 26], [25 33 37 27], [26 35 39 28], [27 37 39 28], ...
    [29 30 36 34], [29 31 38 34], [30 32 40 36], [31 32 40 38], ...
    };

% format output
varargout = formatMeshOutput(nargout, nodes, edges, faces);


function [mssim, ssim_map, mcs, cs_map] = ssim_index_new(img1, img2, K, window)

if (nargin < 2 | nargin > 4)
   ssim_index = -Inf;
   ssim_map = -Inf;
   return;
end

if (size(img1) ~= size(img2))
   ssim_index = -Inf;
   ssim_map = -Inf;
   return;
end

[M N] = size(img1);

if (nargin == 2)
   if ((M < 11) | (N < 11))
	   ssim_index = -Inf;
	   ssim_map = -Inf;
      return
   end
   window = fspecial('gaussian', 11, 1.5);	%
   K(1) = 0.01;										% default settings
   K(2) = 0.03;										%
end

if (nargin == 3)
   if ((M < 11) | (N < 11))
	   ssim_index = -Inf;
	   ssim_map = -Inf;
      return
   end
   window = fspecial('gaussian', 11, 1.5);
   if (length(K) == 2)
      if (K(1) < 0 | K(2) < 0)
		   ssim_index = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   ssim_index = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 4)
   [H W] = size(window);
   if ((H*W) < 4 | (H > M) | (W > N))
	   ssim_index = -Inf;
	   ssim_map = -Inf;
      return
   end
   if (length(K) == 2)
      if (K(1) < 0 | K(2) < 0)
		   ssim_index = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   ssim_index = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

C1 = (K(1)*255)^2;
C2 = (K(2)*255)^2;
window = window/sum(sum(window));

mu1   = filter2(window, img1, 'valid');
mu2   = filter2(window, img2, 'valid');
mu1_sq = mu1.*mu1;
mu2_sq = mu2.*mu2;
mu1_mu2 = mu1.*mu2;
sigma1_sq = filter2(window, img1.*img1, 'valid') - mu1_sq;
sigma2_sq = filter2(window, img2.*img2, 'valid') - mu2_sq;
sigma12 = filter2(window, img1.*img2, 'valid') - mu1_mu2;

if (C1 > 0 & C2 > 0)
   ssim_map = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2));
   cs_map = (2*sigma12 + C2)./(sigma1_sq + sigma2_sq + C2);
else
   numerator1 = 2*mu1_mu2 + C1;
   numerator2 = 2*sigma12 + C2;
	denominator1 = mu1_sq + mu2_sq + C1;
   denominator2 = sigma1_sq + sigma2_sq + C2;

   ssim_map = ones(size(mu1));
   index = (denominator1.*denominator2 > 0);
   ssim_map(index) = (numerator1(index).*numerator2(index))./(denominator1(index).*denominator2(index));
   index = (denominator1 ~= 0) & (denominator2 == 0);
   ssim_map(index) = numerator1(index)./denominator1(index);

   cs_map = ones(size(mu1));
   index = denominator2 > 0;
   cs_map(index) = numerator2(index)./denominator2(index);
end

mssim = mean2(ssim_map);
mcs = mean2(cs_map);

return


function result = x2stereo(x)
%X2STEREO Stereographic projection of Euclidean points
%
% result = x2stereo(x);

% Copyright 2004-2005 Paul Leopardi for the University of New South Wales.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

dim = size(x,1)-1;
mask = (x(dim+1,:) == 1);
scale = ones(dim,1)*(1-x(dim+1,~mask));
result = x(1:dim,~mask)./scale;
% end function


function combo_test23 ( )

%*****************************************************************************80
%
%% COMBO_TEST23 tests PART_SUCCESSOR and PART_SF_CONJUGATE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 8;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST23\n' );
  fprintf ( 1, '  PART_SUCCESSOR produces partitions of N,\n' );
  fprintf ( 1, '  PART_SF_CONJUGATE produces the conjugate of a partition.\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  Partitions of N = %d\n', n );
  fprintf ( 1, ' \n' );
%
%  List.
%
  npart = 0;
  t = [];
  rank = -1;

  while ( 1 )

    rank_old = rank;

    [ npart, t, rank ] = part_successor ( n, npart, t, rank );

    if ( rank <= rank_old )
      break
    end

    fprintf ( 1, '  %3d   ', rank );
    for i = 1 : npart
      fprintf ( 1, '%3d', t(i) );
    end
    fprintf ( 1, '\n' );

    [ npartb, b ] = part_sf_conjugate ( n, npart, t );

    fprintf ( 1, '  Con   ', rank );
    for i = 1 : npartb
      fprintf ( 1, '%3d', b(i) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


classdef recovery_problems

    methods(Static)

        function problem = problem_small_1()
            m = 64;
            n = 121;
            k = 4;
            dict = spx.dict.simple.gaussian_dict(m, n);
            gen = spx.data.synthetic.SparseSignalGenerator(n, k);
            % create a sparse vector
            rep =  gen.biGaussian();
            signal = dict*rep;
            problem.dictionary = dict;
            problem.representation_vector = rep;
            problem.sparsity_level = k;
            problem.signal_vector = signal;
        end

        function problem = problem_large_1()
            m = 1000;
            n = 2000;
            k = 100;
            dict = spx.dict.simple.gaussian_dict(m, n);
            gen = spx.data.synthetic.SparseSignalGenerator(n, k);
            % create a sparse vector
            rep =  gen.biGaussian();
            signal = dict*rep;
            problem.dictionary = dict;
            problem.representation_vector = rep;
            problem.sparsity_level = k;
            problem.signal_vector = signal;
        end

        function problem = problem_barbara_blocks()
            env = spx_get_env();
            images_dir = env.local_settings.standard_test_images_dir;
            image_path = fullfile(images_dir, 'barbara.png');
            image = spx.data.synthetic.recovery_problems.read_image(image_path);
            blkSize = 8;
            patches = im2col(image, [blkSize, blkSize], 'distinct');
            problem.signals = patches;
            problem.image = image;
            problem.blkSize = blkSize;
            problem.sparsity_level = 4;
            % Let's prepare a dictionary for the image
            N = 64;
            D = 121;
            problem.dictionary = spx.dict.simple.overcomplete2DDCT(N, D);
        end

        function problem = problem_test_image_blocks(image_name, block_type)
            if nargin < 2
                block_type = 'distinct';
            end
            env = spx_get_env();
            rootdir = env.local_settings.standard_test_images_dir;
            switch image_name
                case 'barbara'
                    fname = 'barbara.png';
                case 'cameraman'
                    fname = 'cameraman.tif';
                case 'house'
                    fname = 'house.tif';
                case 'jetplane'
                    fname = 'jetplane.tif';
                case 'lake'
                    fname = 'lake.tif';
                case 'lena'
                    fname = 'lena_gray_512.tif';
                case 'livingroom'
                    fname = 'livingroom.tif';
                case 'mandril'
                    fname = 'mandril_gray.tif';
                case 'peppers'
                    fname = 'peppers_gray.tif';
                case 'pirate'
                    fname = 'pirate.tif';
                case 'walkbridge'
                    fname = 'walkbridge.tif';
                case 'blonde'
                    fname = 'woman_blonde.tif';
                case 'darkhair'
                    fname = 'woman_darkhair.tif';
                otherwise
                    error('Unsupported test image');
            end
            filepath = fullfile(rootdir, fname);
            image = spx.data.synthetic.recovery_problems.read_image(filepath);
            blkSize = 8;
            patches = im2col(image, [blkSize, blkSize], block_type);
            problem.signals = patches;
            problem.image = image;
            problem.blkSize = blkSize;
        end

        function image = read_image(filepath)
            [image,~]=imread(filepath);
            image = im2double(image);
            % If the image is RGB, let us convert it to gray
            sz = size(image);
            if (length(sz) > 2)
                if sz(3) == 2
                    image = image(:, :, 1);
                elseif sz(3) == 3
                    image = rgb2gray(image);
                end
            end
            % Let us bring image to 0-255 range
            if max(image(:)) < 2
                image = image * 255;
            end
        end

    end

end


% TEST_MAXWELL_SQUARE_H_DRCHLT: data function for Dirichlet boundary condition.

function h = test_maxwell_square_h_drchlt (x, y, ind)

  h = zeros (size (x));
  switch (ind)
    case 1
      h = exp(x) .* cos(y);
    case 2
      h = -exp(x) .* cos(y);
    case 3
      h = -sin(y);
    case 4
      h = sin(y);
    otherwise
      error ('h_drchlt: unknown reference number')
  end

end


%ComputeJointDistribution Computes the joint distribution defined by a set
% of given factors
%
%   Joint = ComputeJointDistribution(F) computes the joint distribution
%   defined by a set of given factors
%
%   Joint is a factor that encapsulates the joint distribution given by F
%   F is a vector of factors (struct array) containing the factors
%     defining the distribution
%

function Joint = ComputeJointDistribution(F)

  % Check for empty factor list
  if (numel(F) == 0)
      warning('Error: empty factor list');
      Joint = struct('var', [], 'card', [], 'val', []);
      return;
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE:
% Compute the joint distribution defined by F
% You may assume that you are given legal CPDs so no input checking is required.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Joint = struct('var', [], 'card', [], 'val', []); % Returns empty factor. Change this.

for i = 1:length(F)
	Joint = FactorProduct(Joint,F(i));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function tetrahedron_arbq_rule_test03 ( degree, n, header )

%*****************************************************************************80
%
%% TETRAHEDRON_ARBQ_RULE_TEST03 gets a rule and creates GNUPLOT input files.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    10 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.  0 <= DEGREE <= 15.
%
%    Input, integer N, the number of nodes to be used by the rule.
%
%    Input, string HEADER, an identifier for the filenames.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_ARBQ_RULE_TEST03\n' );
  fprintf ( 1, '  Get a quadrature rule for the tetrahedron.\n' );
  fprintf ( 1, '  Set up GNUPLOT graphics input.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );
%
%  Retrieve a symmetric quadrature rule.
%
   [ x, w ] = tetrahedron_arbq ( degree, n );
%
%  Create files for input to GNUPLOT.
%
  tetrahedron_arbq_gnuplot ( n, x, header );

  return
end


a=[2,4;3,-5;1,2;2,1];
b=[11;3;6;7];
solution=a\b


    %This example generates NE elements for a rectangular structure of
    %length = Ly units and width = Lx units with Nx divisions on the x
    %axis using femTriangularMeshGenerator function

    %   Kehinde Orolu
    %   Systems Engineering
    %   University of Lagos, Nigeria
    %   olukeh@yahoo.com

    cla
    Lx=10;
    Ly=10;
    Nx=8;
    NE=144;

    [coords cT nNodes ]=femTriangularMeshGenerator(Lx,Ly,Nx,NE);

    disp(['Number of nodes =  ',num2str(nNodes)])
    disp('Connectivity Table')
    disp(cT)

    z=1;
    for i=1:NE
        figure(1),patch('Vertices',coords(z:z+2,:),'Faces',[1,2,3],'FaceColor','none','EdgeColor','g')
        hold on
        z=z+3;
    end

    figure(1),scatter(coords(:,1),coords(:,2),'MarkerFaceColor','r')

    hold off


function node_num = grid_q9_node_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_Q9_NODE_NUM counts the nodes in a grid of Q9 elements.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    NELEMX * NELEMY.
%
%    Output, integer NODE_NUM, the number of nodes in the grid.
%
  node_num = ( 2 * nelemx + 1 ) * ( 2 * nelemy + 1 );

  return
end


% geopdes_inv: the inverse matrix or, for codimension > 0 (rdim > ndim)
% the Moore-Penrose pseudo-inverse.
%
% To be used with msh.geo_map_jac, to compute DF^{-1}.
% In the case of codimension > 0, it computes
%   J = (DF^t * DF)^{-1} * DF^t
%   det = sqrt ( det(DF^t * DF)^{-1} )
%
%  [JinvT, det] = geopdes_invT__ (geo_map_jac)
%
% OUTPUT:
%   Jinv: the computed matrix evaluated at every point. Size (ndim x rdim x nqn x nel)
%   det:  the determinant evaluated at every point. Size (nqn x nel)
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2015 Rafael Vazquez
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [Jinv, det] = geopdes_inv__ (v)

  [JinvT, det] = geopdes_invT__ (v);
  Jinv = permute (JinvT, [2, 1, 3:numel(size(v))]);

end

%!test
%! A = [1 2; 3 4];
%! Ainv = geopdes_inv__ (A);
%! assert (Ainv * A, eye(2), 1e-14)
%!
%!test
%! A = [1 2 3; 4 5 6; 7 8 10];
%! Ainv = geopdes_inv__ (A);
%! assert (Ainv * A, eye(3), 1e-14)


% A demo of Bayesian CP factorization for image completion
% Written by Qibin Zhao 2014 RIKEN BSI
%
% In this demo, we provide two algorithms including BCPF_IC and BCPF_MP.
% BCPF_IC is a Bayesian CP for image completion; BCPF_MP is a Bayesian CP
% using mixture priors, which is particularly useful for natural image
% completion. For algorithm settings, please refer to the detailed help by
% >> help BCPF_MP

% The experimental data can be tested with
% 1) Different image files
% 2) Observation rate (1-missing rate)
% The predictive image can be online visualized during model learning.
% The performance of RSE, PSNR, SSIM, Time Cost are evaluated and reported.

close all; clear all;
randn('state',1); rand('state',1); %#ok<RAND>
%% Load image data
filename='./TestImages/peppers.bmp';    % Image file
ObsRatio = 0.1;                      % Observation rate

X = double(imread(filename));
DIM = size(X);

Omega = randperm(prod(DIM));
Omega = Omega(1:round(ObsRatio*prod(DIM)));
O = zeros(DIM);
O(Omega) = 1;
Y = O.*X;

% plot images
subplot = @(m,n,p) subtightplot (m, n, p, [0.01 0.01], [0.01 0.01], [0.01 0.01]);
row =1; col =2;
figure;
subplot(row,col,1);
imshow(uint8(X));
subplot(row,col,2);
imshow(uint8(Y));
drawnow;

% Initialization
TimeCost = zeros(2,1);
RSElist = zeros(2,3);
PSNRlist = zeros(2,1);
SSIMlist = zeros(2,1);
RankEst = zeros(2,1);

%% BCPF for low-rank images completion
tStart = tic;
fprintf('------Bayesian CP factorization for Image Completion---------- \n');
[model] = BCPF_IC(Y, 'obs', O, 'init', 'rand', 'maxRank', 100, 'maxiters', 20, ...
    'tol', 1e-4, 'dimRed', 1, 'verbose', 2);
X_FBCP = double(model.X);
RSElist(1,1) = perfscore(X_FBCP, X);
RSElist(1,2) = perfscore(X_FBCP(O==1), X(O==1));
RSElist(1,3) = perfscore(X_FBCP(O==0), X(O==0));

X_FBCP(O==1) = X(O==1);
PSNRlist(1) = PSNR_RGB(X_FBCP,X);
SSIMlist(1) = ssim_index(rgb2gray(uint8(X_FBCP)),rgb2gray(uint8(X)));
RankEst(1) = model.TrueRank;
TimeCost(1) = toc(tStart);
% figure; imshow(uint8(X_FBCP)); title('FBCP','FontWeight','bold'); drawnow;

%% BCPF-MP (mixture priors) for natural images
if ~isempty(strfind(filename,'facade.bmp'))
    nd=0.1;    % low-rank structural images
else
    nd=1;      % natural images
end
tStart = tic;
fprintf('------Bayesian CP with Mixture Priors for Image Completion---------- \n');
[model] = BCPF_MP(Y, 'obs', O, 'init', 'rand', 'maxRank', 100, 'maxiters', 30, ...
    'tol', 1e-4, 'dimRed', 1, 'verbose', 2, 'nd', nd);
X_FBCPS = double(model.X);

RSElist(2,1) = perfscore(X_FBCPS, X);
RSElist(2,2) = perfscore(X_FBCPS(O==1), X(O==1));
RSElist(2,3) = perfscore(X_FBCPS(O==0), X(O==0));

X_FBCPS(O==1) = X(O==1);
PSNRlist(2) = PSNR_RGB(X_FBCPS,X);
SSIMlist(2) = ssim_index(rgb2gray(uint8(X_FBCPS)),rgb2gray(uint8(X)));
RankEst(2) = model.TrueRank;
TimeCost(2) = toc(tStart);
% figure; imshow(uint8(X_FBCPS)); title('FBCP-MP','FontWeight','bold'); drawnow;

%%
RankEst
RSElist
PSNRlist
SSIMlist
TimeCost


function [ mO ] = ApplySideWindowFiltering( mI, boxRadius, numIterations )
% ----------------------------------------------------------------------------------------------- %
% [ mO ] = ApplySideWindowFiltering( mI, boxRadius, numIterations )
%   Applying Image Edge Preserving Filter using the Side Window Box
%   Filtering algorithm.
% Input:
%   - mI            -   Input Image.
%                       Structure: Image Matrix (Single Channel)
%                       Type: 'Single' / 'Double'.
%                       Range: [0, 1].
%   - boxRadius     -   Box Radius.
%                       Sets the radius of the box filter.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, 3, ...}.
%   - numIterations -   Number of Iterations.
%                       Sets the number of iterations to apply the filter.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, 3, ...}.
% Output:
%   - mO            -   Output Image.
%                       Structure: Image Matrix (Single Channel)
%                       Type: 'Single' / 'Double'.
%                       Range: [0, 1].
% References
%   1.  Side Window Filtering (https://arxiv.org/abs/1905.07177).
% Remarks:
%   1.  Can be much faster if written In Place.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     24/04/2021  Royi Avital     RoyiAvital@yahoo.com
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

dataType    = class(mI);
boxLength   = (2 * boxRadius) + 1;

% Atoms of the Filter Bank (Since any of the filters is separable)
vK  = ones(boxLength, 1, dataType) / boxLength;
vKU = ones(boxLength, 1, dataType) / (boxRadius + 1); %<! Up
vKD = ones(boxLength, 1, dataType) / (boxRadius + 1); %<! Down

vKU((boxRadius + 2):boxLength)  = 0;
vKD(1:boxRadius)                = 0;

mO = padarray(mI, [boxRadius, boxRadius], 'both', 'replicate');
mF = zeros(size(mO, 1), size(mO, 2), 8, dataType);
mM = zeros(size(mO), dataType); %<! Minimum Index

for kk = 1:numIterations
    % Written for clarity, not performance
    mF(:, :, 1) = conv2(vK, vKU, mO, 'same'); %<! Left Box
    mF(:, :, 2) = conv2(vK, vKD, mO, 'same'); %<! Right Vox
    mF(:, :, 3) = conv2(vKU, vK, mO, 'same'); %<! Up Box
    mF(:, :, 4) = conv2(vKD, vK, mO, 'same'); %<! Down Box
    mF(:, :, 5) = conv2(vKU, vKU, mO, 'same'); %<! NW Box
    mF(:, :, 6) = conv2(vKU, vKD, mO, 'same'); %<! NE Box
    mF(:, :, 7) = conv2(vKD, vKU, mO, 'same'); %<! SW Box
    mF(:, :, 8) = conv2(vKD, vKD, mO, 'same'); %<! SE Box

    [~, mM(:, :)]   = min(abs(mF - mO), [], 3); %<! Index which minimizes
    for jj = 1:size(mO, 2)
        for ii = 1:size(mO, 1)
            mO(ii, jj) = mF(ii, jj, mM(ii, jj));
        end
    end

end

% Removing boundary
mO = mO((boxRadius + 1):(end - boxRadius), (boxRadius + 1):(end - boxRadius));
mO = min(max(mO, 0), 1);

end


function [mach] = mph2mach(mph)
% Convert speed from miles per hour to mach number (at STP!)
% Chad A. Greene 2012
mach = mph*0.00130332361516;


% Stress recovery in plane stress analysis of plates
% Plane stress analysis of a thin plate under tension at its extremes
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% Warning : On running this the workspace memory will be deleted. Save any
% if any data present before running the code !!
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%--------------------------------------------------------------------------
% Code written by : Siva Srinivas Kolukula                                |
%                   Senior Research Fellow                                |
%                   Structural Mechanics Laboratory                       |
%                   Indira Gandhi Center for Atomic Research              |
%                   India                                                 |
% E-mail : allwayzitzme@gmail.com                                         |
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
%
% Variable descriptions
%   k = element matrix for stiffness
%   f = element vector
%   stiffness = system matrix
%   force = system vector
%   displacement = system nodal displacement vector
%   coordinates = coordinate values of each node
%   nodes = nodal connectivity of each element
%   index = a vector containing system dofs associated with each element
%   gausspoint = matrix containing sampling points for bending term
%   gaussweight = matrix containing weighting coefficients for bending term
%   bcdof = a vector containing dofs associated with boundary conditions
%   bcval = a vector containing boundary condition values associated with
%           the dofs in 'bcdof'
%   B = matrix for kinematic equation for plane stress
%   D = matrix for material property for plane stress
%   stressGP = stress at the Gauss points
%----------------------------------------------------------------------------

%--------------------------------------------------------------------------
%  input data
%--------------------------------------------------------------------------
clear
clc
disp('Please wait Programme is under Run')
%--------------------------------------------------------------------------
% Input data for nodal coordinate values
%--------------------------------------------------------------------------
load coordinates.dat ;
%--------------------------------------------------------------------------
% Input data for nodal connectivity for each element
%--------------------------------------------------------------------------
load nodes.dat ;

nel = length(nodes) ;                  % number of elements
nnel=4;                                % number of nodes per element
ndof=2;                                % number of dofs per node (UX,UY)
nnode = length(coordinates) ;          % total number of nodes in system
sdof=nnode*ndof;                       % total system dofs
edof=nnel*ndof;                        % degrees of freedom per element

% Units are in SI system
a = 1 ;                           % Length of the plate (along X-axes)
b = 1 ;                           % Length of the plate (along Y-axes)
elementsalongX = 10 ;             % Number of elements along X-axes
elementsalongY = 10 ;

E = 2.1*10^11;                      % Youngs modulus
nu = 0.3;                           % Poisson's ratio
t = 0.0254;                         % plate thickness
rho = 7840. ;                       % Density of the plate

nglx = 2; ngly = 2;         % 2x2 Gauss-Legendre quadrature
nglxy=nglx*ngly;            % number of sampling points per element

%--------------------------------------------------------------------------
% Input data for boundary conditions
%--------------------------------------------------------------------------
% (0,0) and (1,0) are fixed

  bcdof = [ 1 2 241 242] ;
  bcval = zeros(1,length(bcdof)) ;
%--------------------------------------------------------------------------
%  initialization of matrices and vectors
%--------------------------------------------------------------------------

force = zeros(sdof,1);                % system force vector
stiffness = zeros(sdof,sdof);         % system stiffness matrix
displacement = zeros(sdof,1);         % system displacement vector
eldepl = zeros(edof,1) ;              % element displacement vector
index = zeros(edof,1);                % index vector
B = zeros(3,edof);              % kinematic matrix for bending
D = zeros(3,3);                 % constitutive matrix for bending

%--------------------------------------------------------------------------
% force vector
%--------------------------------------------------------------------------
P = 1e5 ;       % Load
rightedge = find(coordinates(:,1)==a);
rightdof = 2*rightedge-ones(length(rightedge),1);
force(rightdof) = -P*b/(elementsalongY+1) ;
leftedge = find(coordinates(:,1)==0);
leftdof = 2*leftedge-ones(length(leftedge),1) ;
force(leftdof) = P*b/(elementsalongY+1) ;

%--------------------------------------------------------------------------
%  computation of element matrices and vectors and their assembly
%--------------------------------------------------------------------------
[Gausspoint,Gaussweight]=GaussQuadrature(nglx);     % sampling points & weights
D = E/(1-nu^2)*[1 nu 0 ; nu 1 0; 0 0 (1-nu)/2] ;    % Constituent Matrix for Plane stress

for iel=1:nel           % loop for the total number of elements

for i=1:nnel
nd(i)=nodes(iel,i);         % extract connected node for (iel)-th element
xx(i)=coordinates(nd(i),1);    % extract x value of the node
yy(i)=coordinates(nd(i),2);    % extract y value of the node
end

k = zeros(edof,edof);        % initialization of stiffness matrix

%--------------------------------------------------------------------------
%  numerical integration for stiffness matrix
%--------------------------------------------------------------------------

for intx=1:nglx
xi = Gausspoint(intx,1);                  % sampling point in x-axis
wtx = Gaussweight(intx,1);               % weight in x-axis
for inty=1:ngly
eta = Gausspoint(inty,1);                  % sampling point in y-axis
wty = Gaussweight(inty,1) ;              % weight in y-axis

[shape,dhdr,dhds] = shapefunctions(xi,eta);     % compute shape functions and
                                    % derivatives at sampling point
jacobian = Jacobian(nnel,dhdr,dhds,xx,yy);  % compute Jacobian

detjacob=det(jacobian);                 % determinant of Jacobian
invjacob=inv(jacobian);                 % inverse of Jacobian matrix

[dhdx,dhdy]=shapefunctionderivatives(nnel,dhdr,dhds,invjacob); % derivatives w.r.t.
                                               % physical coordinate

B=fekineps(nnel,dhdx,dhdy);          % kinematic matrix for stiffness

%--------------------------------------------------------------------------
%  compute element stiffness matrix
%--------------------------------------------------------------------------

k = k+B'*D*B*wtx*wty*detjacob;

end
end                      % end of numerical integration loop for bending term

index = elementdof(nd,nnel,ndof); % extract system dofs associated with element

stiffness = assemble(stiffness,k,index);    % assemble element stiffness matrices

end

%--------------------------------------------------------------------------
%   apply boundary conditions
%--------------------------------------------------------------------------

[stiffness,force] = constraints(stiffness,force,bcdof,bcval);

%--------------------------------------------------------------------------
% Solve the matrix equation
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% Solve the matrix equation
%--------------------------------------------------------------------------
displacement = stiffness\force ;

%--------------------------------------------------------------------------
% Stress Recovery
%--------------------------------------------------------------------------
% Calculaing the stress at the Gauss points
[vonmises,stressGP,stressEXP,stressPR,stressG] = StressRecovery(D,displacement);

% Calculating the Element nodal stresses

% Nodal Averaging
type = 'average' ;
% type = 'sum' ;
stressEXPavg = NodalAveraging(stressEXP,type);
stressPRavg = NodalAveraging(stressPR,type) ;

%#### OUTPUTS ####

display('=======SYSTEM MAIN PROPERTIES (isotropic material)=======');
fprintf('       Elasticity  constant  % .2e N/m**2 \n',E);
fprintf('       Poission ratio        % .5f         \n',nu);
fprintf('       thickness of plate    % .5f m       \n',t);
fprintf('       length of plate       % .5f m       \n',a);
fprintf('       breadth of plate      % .5f m       \n',b);
fprintf(' \n ')

display('======SYSTEM GEOMETRICALY PROPERTIES======');
fprintf('        System total element no   % .f \n  ',nel);
fprintf('      System total nodes        % .f \n  ',nnode);
fprintf('      System degree of freedom  % .f \n  ',sdof);

fprintf(' \n ')
% Output of displacements
mytable(displacement,1);

% Output of Stresses at Gauss points
display('======STRESS VALUES AT THE GAUSS POINTS=======')
mytable(stressGP,3);

% Output Von Mises stress
mytable(vonmises,2) ;

% Output of element nodal stresses
display('====ELEMENT NODAL STRESS USING EXTRAPOLATION====')
mytable(stressEXP,3) ;
display('====ELEMENT NODAL STRESS USING PATCH RECOVERY====')
mytable(stressPR,3) ;

% Output of Nodal averaged stresses
display('====NODAL AVERAGED STRESSES OF EXTRAPOLATION====')
mytable(stressEXPavg,4);
display('====NODAL AVERAGED STRESSES OF PATCH RECOVERY====')
mytable(stressPRavg,4);


classdef latticeType < int32
% class representing the different Bravais lattices

  enumeration
    triclinic    (1)
    monoclinic   (2)
    orthorhombic (3)
    trigonal     (4)
    tetragonal   (5)
    hexagonal    (6)
    cubic        (7)
    none         (8)
  end

  methods

    function abg = defaultAngles(this)

      switch this
        case {'trigonal','hexagonal'}
          abg = [90 90 120] * degree;

        otherwise
          abg = [90 90 90] * degree;
      end

    end

    function out = isTriHex(this)

      out = this == latticeType.trigonal || this == latticeType.hexagonal;

    end

    function out = isEucledean(this)

      out = this == latticeType.orthorhombic || ...
        this == latticeType.tetragonal || this == latticeType.cubic;

    end

  end

end


function x = inv_posdef(A)
% INV_POSDEF        Invert positive definite matrix.
% INV_POSDEF(A) is like INV(A) but faster and more numerically stable.
% See test_inv_posdef for a timing test.

% Written by Tom Minka

U = cholproj(A);
iU = inv_triu(U);
x = iU*iU';