function [u,w,AE,AI,isExteriorTElem,isExteriorSElem] = interfacefittedPoisson(node,telem,selem,pde,interfaceEdge,bdEdge,option)
%% INTERFACEFITTEDPOISSON Poisson equation: P1 linear element.
%
%   u = INTERFACEPOISSON(node,telem,selem,pde,E) produces the linear finite element
%   approximation of the interface Poisson equation
%
%   input:
%       node, N*2 matrix, node(i,:) are the xy coordinates of i-th node;
%       telem, NTT*3 matrix, telem(j,:) are the three  global indices of the
%              vertices ofj-th triangle element;
%       selem,NTS*4 matrix, selem(j,:) are the four global indices of the
%             vertices of j-th quad element;
%
%

[N,Dim] = size(node);
Ndof = N;

%% Geometry structures of interface meshes

NTS = size(selem,1);
isExteriorSElem = false(NTS,1);
c = (node(selem(:,1),:) + node(selem(:,2),:) + node(selem(:,3),:)+node(selem(:,4),:))/4;
isExteriorSElem(pde.phi(c)>0) = true;

NTT = size(telem,1);
isExteriorTElem = false(NTT,1);
c = (node(telem(:,1),:) + node(telem(:,2),:) + node(telem(:,3),:))/3;
isExteriorTElem(pde.phi(c)>0) = true;

[sAE,sbE] = getstiffmatrixandrhsonquad(node,selem(isExteriorSElem,:));
[sAI,sbI] = getstiffmatrixandrhsonquad(node,selem(~isExteriorSElem,:));
[tAE,tbE] = getstiffmatrixandrhsontri(node,telem(isExteriorTElem,:));
[tAI,tbI] = getstiffmatrixandrhsontri(node,telem(~isExteriorTElem,:));

A = sAE + sAI + tAE + tAI;
b = sbE + sbI + tbE + tbI;

AI = sAI + tAI;
AE = sAE + tAE;

flux = getfluxconditiononinterface(node,interfaceEdge);
b = b - flux;

%% Extend w to the whole domain
isInterfaceNode = false(Ndof,1);
isInterfaceNode(interfaceEdge(:)) = true;
interfaceNode = find(isInterfaceNode);

isInNode = false(Ndof,1);
interiorSElem = selem(~isExteriorSElem,:);
interiorTElem = telem(~isExteriorTElem,:);
isInNode([interiorSElem(:);interiorTElem(:)]) = true;
inNode = find(isInNode & ~isInterfaceNode);

w = zeros(Ndof,1);
FI= zeros(Ndof,1);
w(interfaceNode) = pde.exactw(node(interfaceNode,:));
FI = FI - AI*w;
extensionoption.printlevel = 0;
w(inNode) = amg(AI(inNode, inNode),FI(inNode),extensionoption);

%% Dirichlet boundary condition
isBdNode = false(Ndof,1);
isBdNode(bdEdge(:)) = true;
bdNode = find(isBdNode);

u = zeros(Ndof,1);
u(bdNode) = pde.g_D(node(bdNode,:));
b = b - A*u+AI*w;
b(bdNode) = u(bdNode);

freeNode = find(~isBdNode);
bdidx = zeros(Ndof,1);
bdidx(bdNode) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
AD = T*A*T + Tbd;

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option);
end

%%
eqn = struct('A',AD,'b',b,'freeNode',freeNode);

   %% Assemble stiffness matrix and rhs on quad mesh
    function [A,b] = getstiffmatrixandrhsonquad(node,elem)
        [NT,NV] = size(elem);
        % generate sparse pattern
        ii = zeros(10*NT,1); jj = zeros(10*NT,1);
        index = 0;
        for i = 1:4
            for j = i:4
                ii(index+1:index+NT) = double(elem(:,i));
                jj(index+1:index+NT) = double(elem(:,j));
                index = index + NT;
            end
        end
        % quadrature points
        if ~isfield(pde,'d'), pde.d = []; end
        if ~isfield(option,'dquadorder')
            option.dquadorder = 2;        % default order is exact for quadratic function
        end
        [pts, weight] = quadptsquad(option.dquadorder);
        nQuad = size(pts,1);
        % compute non-zeros
        sA = zeros(10*NT,nQuad);
        for p = 1:nQuad
            % Dphi at quadrature points
            [phi, Dphip, J] = quadbasis(node,elem,pts(p,:));
            index = 0;
            for i = 1:4
                for j = i:4
                    Aij = 0;
                    if isempty(pde.d) || isnumeric(pde.d)
                        Aij = Aij + weight(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
                    else
                        pxy = zeros(NT, Dim);
                        for ip = 1:Dim
                            xi = node(:,ip);
                            pxy(:,ip) = xi(elem)*phi;
                        end
                        Aij = Aij + weight(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
                    end
                    if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
                        Aij = pde.d.*Aij;
                    end
                    Aij = Aij.*J;
                    sA(index+1:index+NT,p) = Aij;
                    index = index + NT;
                end
            end
        end
        sA = sum(sA,2);
        % assemble the matrix
        diagIdx = (ii == jj);   upperIdx = ~diagIdx;
        A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
        AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
        A = A + AU + AU';
        clear Aij ii jj Dphip

        %% Assemble the right hand side
        b = zeros(Ndof,1);
        if ~isfield(option,'fquadorder')
            option.fquadorder = 3;   % default order
        end
        if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
            pde.f = [];
        end

        if ~isempty(pde.f)
            [pts, weight] = quadptsquad(option.fquadorder);
            nQuad = size(pts,1);
            bt = zeros(NT,NV);
            for p = 1:nQuad
                % quadrature points in the x-y coordinate
                [phi, ~, J] = quadbasis(node,elem, pts(p,:));
                pxy = zeros(NT, Dim);
                for i = 1:Dim
                    xi = node(:,i);
                    pxy(:,i) = xi(elem)*phi; % ? questionable
                end
                fp = pde.f(pxy);
                bt = bt + (weight(p)*fp.*J)*phi';
            end
            b = accumarray(elem(:),bt(:),[Ndof 1]);
        end

    end

    %% Assemble the stiffmatrix and right hand side on triangle mesh
    function [A,b] = getstiffmatrixandrhsontri(node,elem)
        NT = size(elem,1);
        % quadrature points
        center = (node(elem(:,1),:) + node(elem(:,2),:) + node(elem(:,3),:))/3;
       % Diffusion coefficient
        if isfield(pde,'d') && ~isempty(pde.d)
            if isnumeric(pde.d)
                K = pde.d;                   % d is an array
            else                            % d is a function
                K = pde.d(center);
            end
        else
            K = [];
        end
        [Dphi,area] = gradbasis(node,elem);
        A = sparse(Ndof,Ndof);
        for i = 1:3
            for j = i:3
                Aij = (Dphi(:,1,i).*Dphi(:,1,j)+Dphi(:,2,i).*Dphi(:,2,j)).*area;
                if ~isempty(K)
                    Aij = K.*Aij;
                end
                if (j==i)
                    A = A + sparse(elem(:,i),elem(:,j),Aij,Ndof,N);
                else
                    A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],[Aij; Aij],Ndof,Ndof);
                end
            end
        end

        b = zeros(Ndof,1);
        if ~isfield(option,'fquadorder')
            option.fquadorder = 3;   % default order
        end
        if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
            pde.f = [];
        end
        if ~isempty(pde.f)
            [lambda,weight] = quadpts(option.fquadorder);
            nQuad = size(lambda,1);
            ft = zeros(NT,3);
            for p = 1:nQuad
                % quadrature points in the x-y coordinate
                pxy = lambda(p,1)*node(elem(:,1),:) ...
                    + lambda(p,2)*node(elem(:,2),:) ...
                    + lambda(p,3)*node(elem(:,3),:);
                % function values at quadrature points
                fp = pde.f(pxy);
                % evaluate fp outside.
                for j = 1:3
                    ft(:,j) = ft(:,j) + lambda(p,j)*weight(p)*fp;
                end
            end
            ft = ft.*[area,area,area];
            b = accumarray(elem(:),ft(:),[Ndof 1]);
        end
    end

    %% Neumann boundary condition on interface edges
    function b = getfluxconditiononinterface(node,interfaceEdge)
        ve = node(interfaceEdge(:,1),:) - node(interfaceEdge(:,2),:);
        ve = [-ve(:,2), ve(:,1)];
        edgeLen = sqrt(sum(ve.^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 5;   % default order
        end
        [lambda,weight] = quadpts1(option.gNquadorder);
        nQuad = length(weight);
        ge = zeros(size(interfaceEdge,1),2);
        for i = 1:nQuad
            pxy=lambda(i,1)*node(interfaceEdge(:,1),:)+lambda(i,2)*node(interfaceEdge(:,2),:);
            leftpt = pxy - ve/2;
            rightpt = pxy + ve/2;
            pxy = findintersectbisect(pde.phi,leftpt,rightpt);
            q = pde.exactq(pxy);
            ge(:,1)=ge(:,1)+weight(i)*lambda(i,1)*q;
            ge(:,2)=ge(:,2)+weight(i)*lambda(i,2)*q;
        end
        ge = ge.*[edgeLen,edgeLen];
        b = accumarray(interfaceEdge(:), ge(:),[Ndof,1]);
    end

end


function pos2 = nutate (tjd, pos1)

% this function nutates equatorial rectangular coordinates from
% the mean dynamical equator and equinox of epoch to the true
% equator and equinox of epoch. see explanatory supplement to the
% astronomical almanac, pp. 114-115.

% input

%  tjd  = tdb julian date of epoch

%  pos1 = position vector, geocentric equatorial rectangular
%         coordinates, referred to mean dynamical equator and
%         equinox of epoch

% output

%  pos2 = position vector, geocentric equatorial rectangular
%         coordinates, referred to true equator and equinox of epoch

% note:  if tjd is negative, inverse nutation (true to mean) is applied.

% ported from NOVAS 3.0

%%%%%%%%%%%%%%%%%%%%%%%

seccon = 180.d0 * 3600.d0 / pi;

tjd1 = abs(tjd);

[oblm, oblt, eqeq, dpsi, deps] = etilt (tjd1);

oblm = oblm * 3600.d0 / seccon;

oblt = oblt * 3600.d0 / seccon;

dpsi = dpsi / seccon;

deps = deps / seccon;

cobm = cos(oblm);

sobm = sin(oblm);

cobt = cos(oblt);

sobt = sin(oblt);

cpsi = cos(dpsi);

spsi = sin(dpsi);

% compute elements of nutation rotation matrix

xx =  cpsi;
yx = -spsi * cobm;
zx = -spsi * sobm;

xy =  spsi * cobt;
yy =  cpsi * cobm * cobt + sobm * sobt;
zy =  cpsi * sobm * cobt - cobm * sobt;

xz =  spsi * sobt;
yz =  cpsi * cobm * sobt - sobm * cobt;
zz =  cpsi * sobm * sobt + cobm * cobt;

if (tjd < 0.0d0)

    % perform rotation from true to mean

    pos2(1) = xx * pos1(1) + xy * pos1(2) + xz * pos1(3);

    pos2(2) = yx * pos1(1) + yy * pos1(2) + yz * pos1(3);

    pos2(3) = zx * pos1(1) + zy * pos1(2) + zz * pos1(3);

else

    % perform rotation from mean to true

    pos2(1) = xx * pos1(1) + yx * pos1(2) + zx * pos1(3);

    pos2(2) = xy * pos1(1) + yy * pos1(2) + zy * pos1(3);

    pos2(3) = xz * pos1(1) + yz * pos1(2) + zz * pos1(3);

end


% A Thin Plate Subjected to Uniform Traction
% T3 Implementation
% 2 elements
% clear memory
clear all;
clc;
close all;
% materials
E  = 30e6;     poisson = 0.30;  thickness = 1;

% matrix D
D=E/(1-poisson^2)*[1 poisson 0;poisson 1 0;0 0 (1-poisson)/2];

% trivial preprocessing
% numberElements: number of elements
numberElements=2;
% numberNodes: number of nodes
numberNodes=4;
% coordinates and connectivities
elementNodes=[1 3 2; 1 4 3];
nodeCoordinates=[0, 0; 0, 10; 20, 10; 20, 0];
drawingMesh(nodeCoordinates,elementNodes,'T3','k-o');

% GDof: global number of degrees of freedom
GDof=2*numberNodes;

% boundary conditions
prescribedDof=[1 2 3 4]';
% force vector
force=zeros(GDof,1);
force(5)=5000; force(7) =5000;

% calculation of the system stiffness matrix
stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,D,thickness);

% solution
displacements=solution(GDof,prescribedDof,stiffness,force);

% output displacements
outputDisplacements(displacements, numberNodes, GDof);

outputStress(displacements,numberElements,...
    elementNodes,nodeCoordinates,D)


function area = polySgnArea(x,y)
% area of an orientation with sign depending of orientation

area = 0.5*sum((y(2:end)-y(1:end-1)) .* (x(2:end)+x(1:end-1)));

end


function res = imGeodesicPropagation(img, varargin)
%IMGEODESICPROPAGATION Compute geodesic propagation for each foreground pixel
%
%   RES = imGeodesicPropagation(IMG);
%   IMG is a binary image. For each foreground pixel, the geodesic
%   progagation is defined as the maximum geodesic distance to another
%   pixel of the foreground. If the foreground is not connected this
%   distance equals infinity.
%
%   RES = imGeodesicPropagation(IMG, WEIGHTS);
%   use different weights for the computation of distances. See
%   imChamferDistance for further details.
%
%   Note:
%   * As the algorithm propagates geodesic distances from each foreground
%       pixel, the computation time may be expensive.
%   * the function is defined for both 2D and 3D images
%
%
%   Example
%     % Compute geodesic propagation in a L-shape
%     img = zeros(20, 20);
%     img(4:16, 4:9) = 1;
%     img(11:16, 4:16) = 1;
%     prop = imGeodesicPropagation(img);
%     imagesc(prop);
%     colormap([1 1 1 ; jet]);
%
%     % Compute geodesic propagation in a set of particles
%     prop = zeros(size(img));
%     lbl = bwlabel(img);
%     for i = 1:max(lbl(:))
%         prop = max(prop, imGeodesicPropagation(lbl==i));
%     end
%     imagesc(prop);
%     colormap([1 1 1 ; jet]);
%
%   See also
%   imGeodesics, imGeodesicDistanceMap, imGeodesicRadius, imGeodesicExtremities

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2009-05-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

img = img > 0;
res = zeros(size(img));

dim = size(img);
if length(dim) == 2
    for i = 1:dim(1)
        for j = 1:dim(2)
            if ~img(i,j)
                continue;
            end

            marker = false(size(img));
            marker(i, j) = true;

            dist = imGeodesicDistanceMap(img, marker, varargin{:});
            res(i, j) = max(dist(img));
        end
    end

elseif length(dim) == 3
    for k = 1:dim(3)
        for j = 1:dim(2)
            for i = 1:dim(1)
                if ~img(i,j,k)
                    continue;
                end

                marker = false(size(img));
                marker(i, j, k) = true;

                dist = imGeodesicDistanceMap3d(img, marker, varargin{:});
                res(i, j, k) = max(dist(img));
            end
        end
    end

else
    error('Requires a 2D or a 3D image');
end


function [cepstra,aspectrum,pspectrum] = melfcc(samples, sr, varargin)
%[cepstra,aspectrum,pspectrum] = melfcc(samples, sr[, opts ...])
%  Calculate Mel-frequency cepstral coefficients by:
%   - take the absolute value of the STFT
%   - warp to a Mel frequency scale
%   - take the DCT of the log-Mel-spectrum
%   - return the first <ncep> components
%  This version allows a lot of options to be controlled, as optional
%  'name', value pairs from the 3rd argument on: (defaults in parens)
%    'wintime' (0.025): window length in sec
%    'hoptime' (0.010): step between successive windows in sec
%    'numcep'     (13): number of cepstra to return
%    'lifterexp' (0.6): exponent for liftering; 0 = none; < 0 = HTK sin lifter
%    'sumpower'    (1): 1 = sum abs(fft)^2; 0 = sum abs(fft)
%    'preemph'  (0.97): apply pre-emphasis filter [1 -preemph] (0 = none)
%    'dither'      (0): 1 = add offset to spectrum as if dither noise
%    'minfreq'     (0): lowest band edge of mel filters (Hz)
%    'maxfreq'  (4000): highest band edge of mel filters (Hz)
%    'nbands'     (40): number of warped spectral bands to use
%    'bwidth'    (1.0): width of aud spec filters relative to default
%    'dcttype'     (2): type of DCT used - 1 or 2 (or 3 for HTK or 4 for feac)
%    'fbtype'  ('mel'): frequency warp: 'mel','bark','htkmel','fcmel'
%    'usecmp'      (0): apply equal-loudness weighting and cube-root compr.
%    'modelorder'  (0): if > 0, fit a PLP model of this order
%    'broaden'     (0): flag to retain the (useless?) first and last bands
%    'useenergy'   (0): overwrite C0 with true log energy
% The following non-default values nearly duplicate Malcolm Slaney's mfcc
% (i.e. melfcc(d,16000,opts...) =~= log(10)*2*mfcc(d*(2^17),16000) )
%       'wintime': 0.016
%     'lifterexp': 0
%       'minfreq': 133.33
%       'maxfreq': 6855.6
%      'sumpower': 0
% The following non-default values nearly duplicate HTK's MFCC
% (i.e. melfcc(d,16000,opts...) =~= 2*htkmelfcc(:,[13,[1:12]])'
%  where HTK config has PREEMCOEF = 0.97, NUMCHANS = 20, CEPLIFTER = 22,
%  NUMCEPS = 12, WINDOWSIZE = 250000.0, USEHAMMING = T, TARGETKIND = MFCC_0)
%     'lifterexp': -22
%        'nbands': 20
%       'maxfreq': 8000
%      'sumpower': 0
%        'fbtype': 'htkmel'
%       'dcttype': 3
% For more detail on reproducing other programs' outputs, see
% http://www.ee.columbia.edu/~dpwe/resources/matlab/rastamat/mfccs.html
%
% 2005-04-19 dpwe@ee.columbia.edu after rastaplp.m.
% Uses Mark Paskin's process_options.m from KPMtools
% $Header: /Users/dpwe/matlab/rastamat/RCS/melfcc.m,v 1.3 2012/09/03 14:01:26 dpwe Exp dpwe $

if nargin < 2;   sr = 16000;    end

% Parse out the optional arguments
[wintime, hoptime, numcep, lifterexp, sumpower, preemph, dither, ...
 minfreq, maxfreq, nbands, bwidth, dcttype, fbtype, usecmp, modelorder, ...
 broaden, useenergy] = ...
    process_options(varargin, 'wintime', 0.025, 'hoptime', 0.010, ...
          'numcep', 13, 'lifterexp', 0.6, 'sumpower', 1, 'preemph', 0.97, ...
	  'dither', 0, 'minfreq', 0, 'maxfreq', 4000, ...
	  'nbands', 40, 'bwidth', 1.0, 'dcttype', 2, ...
	  'fbtype', 'mel', 'usecmp', 0, 'modelorder', 0, ...
          'broaden', 0, 'useenergy', 0);

if preemph ~= 0
  samples = filter([1 -preemph], 1, samples);
end

% Compute FFT power spectrum
[pspectrum,logE] = powspec(samples, sr, wintime, hoptime, dither);

aspectrum = audspec(pspectrum, sr, nbands, fbtype, minfreq, maxfreq, sumpower, bwidth);

if (usecmp)
  % PLP-like weighting/compression
  aspectrum = postaud(aspectrum, maxfreq, fbtype, broaden);
end

if modelorder > 0

  if (dcttype ~= 1)
    disp(['warning: plp cepstra are implicitly dcttype 1 (not ', num2str(dcttype), ')']);
  end

  % LPC analysis
  lpcas = dolpc(aspectrum, modelorder);

  % convert lpc to cepstra
  cepstra = lpc2cep(lpcas, numcep);

  % Return the auditory spectrum corresponding to the cepstra?
%  aspectrum = lpc2spec(lpcas, nbands);
  % else return the aspectrum that the cepstra are based on, prior to PLP

else

  % Convert to cepstra via DCT
  cepstra = spec2cep(aspectrum, numcep, dcttype);

end

cepstra = lifter(cepstra, lifterexp);

if useenergy
  cepstra(1,:) = logE;
end


function [ m, p, t ] = naca4_mpt ( code )

%*****************************************************************************80
%
%% NACA4_MPT returns the parameters stored in a NACA 4 digit airfoil code.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer CODE, the NACA4 code.
%    0 <= CODE <= 9999.
%
%    Output, real M, the maximum camber, as a percentage of the chord length.
%    0 <= M <= 1.0.
%
%    Output, real P, the relative distance of the occurrence of the maximum
%    camber from the beginning of the chord.
%    0 <= P <= 1.0.
%
%    Output, real T, the maximum thickness relative to the chord length.
%    0 <= T <= 1.0.
%
  if ( code < 0 || 9999 < code )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'NACA4_MPT - Fatal error!\n' );
    fprintf ( 1, '  CODE should be an integer between 0 and 9999.\n' );
    error ( 'NACA4_MPT - Fatal error!' );
  end

  m = floor ( code / 1000 );
  code = code - m * 1000;
  m = m / 100.0;

  p = floor ( code / 100 );
  code = code - p * 100;
  p = p / 10.0;

  t = code / 100.0;

  return
end


function eps = eps_RCUs( i_s, n, r )
eps = mean( exp( - max(0, i_s - log(2^(n*r)-1) ) ) ); % avg pr err
% eps = mean( exp( - max(0, i_s - log(2^(n*r)-1)-log(2)) ) ); % max pr err
end


%%
% \documentclass[12pt]{article}
%
% \title{ODEbox: A Toolbox for Ordinary Differential Equations\\
% Sturm Liouville Example 2\\
% The Mathieu Differential Equation}
%
% \author{Matthew Harker and Paul O'Leary\\
% Institute for Automation\\
% University of Leoben\\
% A-8700 Leoben,
% Austria\\
% URL: automation.unileoben.ac.at\\
% \\
% Original: January 9, 2013\\
% $\copyright$ 2013\\
% \\
% Last Modified: \today}
%%
% \section{The Mathieu Differential Equation}
%
% This example addresses the solution of the Mathieu differential equation.
% This equation occurs in conjunction with the modelling of the vibration
% of a ellptical membrane. The problem has no known analytical solution.
% %
% \begin{equation}
%    -\ddot{y} + 2 \,r \, \cos( 2 \, x )= \lambda \, y
%    \hspace{5mm}
%    \text{with}
%    \hspace{5mm}
%    y(0) = 0}
%   \hspace{5mm}
%   \text{and}
%   \hspace{5mm}
%   \dot{y}(\pi) = 0
% \end{equation}
% %
% in the closed interval $0 \leq x \leq \pi$.
%
%%
% \section{tidy up the Workspace}
%
clear all;
close all;
setUpGraphics;
%%
% \section{Define the Nodes and Compute the Basis Functions}
%
% Define the number of nodes and the number of basis functions used.
%
noPts = 1000;
noBfs = round( noPts/2);
%%
% Compute the Chebyshev points, but scaled to form a closed interval
% $0 \leq x \leq \pi$.
%
x = dopNodes( noPts, 'chebyends' );
x = pi * (x + 1)/2;
%%
% Synthesize a complete set of basis functions
%
B = dop(x);
%%
% \subsection{Generate the Differentiating Matrix}
%
% Synthesize the differentiating matrix with support length $l_s$
%
ls = 13;
D = dopDiffLocal( x, ls, ls );
%%
% \subsection{Define the Constraints and Compute the Admissible Functions}
%
% Define the constraints
%
C = zeros( noPts, 2 );
C(1,1) = 1;
C(end,end) = 1;
%%
% Compute the set of constrained basis functions, i.e., admissible
% functions.
%
Bc = dopConstrain( C, B );
%%
% Trucate the basis functions
%
Bc = Bc(:,1:noBfs);
%
%%
% \section{Setup the Sturm-Liouville Matrix Linear Differential Operator}
%
% Setup the Linear differential operator for the Mathieu differential
% equation.
%
c1 = 0;
c2 = -50;
gx = diag(c1 + c2 * cos(2*x)) ;
L = Bc' * (D * D - gx )* Bc;
%
%%
% \section{Solve the Eigenvector Problem}
%
% Solve the eigenvalue problem
%
[Vec, Val] = eig( L );
vals = -diag( Val );
%%
% and sort the solutions.
%
[vals, inds] = sort(vals);
Vec = Vec(:,inds);
%%
% and compute the final eigenfunctions.
%
sols = Bc * Vec;
%%
% \section{Close Pair of Eigenvalues}
%
% The Mathieu differential equation as paramatized here is known to produce
% a close pair of eigenvalues. We now show this pair.
%
noEVals = 2;
for k=1:noEVals
    numStr = num2str(vals(k),'%2.10E') ;
    str = ['\lambda_{', int2str(k-1), '} = ', numStr]
end;
%noV = length(vals);
%n = [1:noV]';
%valsT = n.^2;
%
%%
% \section{Plot a Few Eigenfunctions}
%
setUpGraphics(10)
FigureSize=[1 1 10 6];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.18 0.17 0.8 0.8];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig1 = figure;
plot( x, sols(:,1), 'r');
hold on
plot( x, sols(:,2), 'g');
plot( x, sols(:,3), 'b');
plot( x, sols(:,4), 'k');
grid on;
xlabel('$$x$$');
ylabel('$$y8x)$$');
axis([0,pi,-0.1,0.1]);
%
% \caption{The first $4$ eigen functions of the Mathieu differential equation.}
%%
%
setUpGraphics(10)
FigureSize=[1 1 10 10];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.16 0.17 0.8 0.8];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig2 = figure;
P1 = [0.16 0.4 0.8 0.55] ;
A = axes('position',P1);
imagesc( log10(abs(Vec) ));
colorbar;
ylabel(['Basis function number $$m$$, $$n_b = ',int2str(noBfs),'$$']);
P2 = [0.16 0.1 0.615 0.25] ;
A = axes('position',P2);
plot( vals(1:noBfs), 'k');
range = axis;
axis([0,noBfs,-1000,900000]);
grid on;
xlabel('Eigenvector number $$n$$');
ylabel('$$\lambda$$');
%
% \caption{The Rayleigh-Ritz Spectrum of the Eigenfunvtions with respect to
% the admissible functions and the eigenvalues for the Mathieu differential
% equation.}
%%
% \section{Save the Figure to Disk.}
fileType = 'eps';
printFigure( fig1, ['SLEx2EigFnsNb',int2str(noBfs)], fileType);
printFigure( fig2, ['SLEx2SpectrumNb',int2str(noBfs)], fileType);


function [ n_data, mu, sigma, x, fx ] = cauchy_cdf_values ( n_data )

%*****************************************************************************80
%
%% CAUCHY_CDF_VALUES returns some values of the Cauchy CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = CauchyDistribution [ mu, sigma ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the standard deviation of the distribution.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  fx_vec = [ ...
     0.5000000000000000E+00, ...
     0.8524163823495667E+00, ...
     0.9220208696226307E+00, ...
     0.9474315432887466E+00, ...
     0.6475836176504333E+00, ...
     0.6024163823495667E+00, ...
     0.5779791303773693E+00, ...
     0.5628329581890012E+00, ...
     0.6475836176504333E+00, ...
     0.5000000000000000E+00, ...
     0.3524163823495667E+00, ...
     0.2500000000000000E+00 ];

  mu_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01 ];

  sigma_vec = [ ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01 ];

  x_vec = [ ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    mu = 0.0;
    sigma = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    mu = mu_vec(n_data);
    sigma = sigma_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c)

%compute_extrinsic
%
%[omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c)
%
%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
%Works with planar and non-planar structures.
%
%INPUT: x_kk: Feature locations on the images
%       X_kk: Corresponding grid coordinates
%       fc: Camera focal length
%       cc: Principal point coordinates
%       kc: Distortion coefficients
%       alpha_c: Skew coefficient
%
%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
%        Tckk: 3D translation vector attached to the grid positions in space
%        Rckk: 3D rotation matrices corresponding to the omc vectors
%
%Method: Computes the normalized point coordinates, then computes the 3D pose
%
%Important functions called within that program:
%
%normalize_pixel: Computes the normalize image point coordinates.
%
%pose3D: Computes the 3D pose of the structure given the normalized image projection.
%
%project_points.m: Computes the 2D image projections of a set of 3D points

if nargin < 6,
   alpha_c = 0;
	if nargin < 5,
   	kc = zeros(5,1);
   	if nargin < 4,
      	cc = zeros(2,1);
      	if nargin < 3,
         	fc = ones(2,1);
         	if nargin < 2,
            	error('Need 2D projections and 3D points (in compute_extrinsic.m)');
            	return;
         	end;
      	end;
   	end;
	end;
end;

%keyboard;

% Compute the normalized coordinates:

xn = normalize_pixel_fisheye(x_kk,fc,cc,kc,alpha_c);

Np = size(xn,2);

%% Check for planarity of the structure:
%keyboard;

X_mean = mean(X_kk')';

Y = X_kk - (X_mean*ones(1,Np));

YY = Y*Y';

[U,S,V] = svd(YY);

r = S(3,3)/S(2,2);

%keyboard;

if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity

   %fprintf(1,'Planar structure detected: r=%f\n',r);

   % Transform the plane to bring it in the Z=0 plane:

   R_transform = V';

   %norm(R_transform(1:2,3))

   if norm(R_transform(1:2,3)) < 1e-6,
      R_transform = eye(3);
   end;

   if det(R_transform) < 0, R_transform = -R_transform; end;

	T_transform = -(R_transform)*X_mean;

	X_new = R_transform*X_kk + T_transform*ones(1,Np);

   % Compute the planar homography:

   H = compute_homography(xn,X_new(1:2,:));

   % De-embed the motion parameters from the homography:

   sc = mean([norm(H(:,1));norm(H(:,2))]);

   H = H/sc;

   % Extra normalization for some reasons...
   %H(:,1) = H(:,1)/norm(H(:,1));
   %H(:,2) = H(:,2)/norm(H(:,2));

   if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!!

       % Case#1: no opposite sign:

       omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
       Rckk1 = rodrigues(omckk1);
       Tckk1 = H(:,3);

       Hs1 = [Rckk1(:,1:2) Tckk1];
       xn1 = Hs1*[X_new(1:2,:);ones(1,Np)];
       xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)];
       e1 = xn1 - xn;

       % Case#2: opposite sign:

       omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]);
       Rckk2 = rodrigues(omckk2);
       Tckk2 = -H(:,3);

       Hs2 = [Rckk2(:,1:2) Tckk2];
       xn2 = Hs2*[X_new(1:2,:);ones(1,Np)];
       xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)];
       e2 = xn2 - xn;

       if 1, %norm(e1) < norm(e2),
           omckk = omckk1;
           Tckk = Tckk1;
           Rckk = Rckk1;
       else
           omckk = omckk2;
           Tckk = Tckk2;
           Rckk = Rckk2;
       end;

   else

       u1 = H(:,1);
       u1 = u1 / norm(u1);
       u2 = H(:,2) - dot(u1,H(:,2)) * u1;
       u2 = u2 / norm(u2);
       u3 = cross(u1,u2);
       RRR = [u1 u2 u3];
       omckk = rodrigues(RRR);

       %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
       Rckk = rodrigues(omckk);
       Tckk = H(:,3);

   end;

   %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform

   Tckk = Tckk + Rckk* T_transform;
   Rckk = Rckk * R_transform;

   omckk = rodrigues(Rckk);
   Rckk = rodrigues(omckk);

else

   %fprintf(1,'Non planar structure detected: r=%f\n',r);

   % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!):
   % The DLT method is applied here!!

   J = zeros(2*Np,12);

	xX = (ones(3,1)*xn(1,:)).*X_kk;
	yX = (ones(3,1)*xn(2,:)).*X_kk;

	J(1:2:end,[1 4 7]) = -X_kk';
	J(2:2:end,[2 5 8]) = X_kk';
	J(1:2:end,[3 6 9]) = xX';
	J(2:2:end,[3 6 9]) = -yX';
	J(1:2:end,12) = xn(1,:)';
	J(2:2:end,12) = -xn(2,:)';
	J(1:2:end,10) = -ones(Np,1);
	J(2:2:end,11) = ones(Np,1);

	JJ = J'*J;
	[U,S,V] = svd(JJ);

   RR = reshape(V(1:9,12),3,3);

   if det(RR) < 0,
      V(:,12) = -V(:,12);
      RR = -RR;
   end;

   [Ur,Sr,Vr] = svd(RR);

   Rckk = Ur*Vr';

   sc = norm(V(1:9,12)) / norm(Rckk(:));
   Tckk = V(10:12,12)/sc;

	omckk = rodrigues(Rckk);
   Rckk = rodrigues(omckk);

end;


%-------------------------------------------------------------------------
% Coupling Kinetic and Fluid System of Euler Equations
% To solve Shock Tube Problem
%
% Kinetic equation use SBBGK and Hydrodynamic equation use Roe Euler
%
% Based on:
%  [1] Pierre Degond, Giacomo Dimarco and Luc Mieussens
%      A moving interface method for dynamic kinetic-fluid coupling
%      Journal of Computation Physics 227(2007)1176-1208
%
% By Manuel Diaz and Yun-Da Tsai
% 007@IAM 25.01.2013
%-------------------------------------------------------------------------

clear all; clc; close all;

%% Global Variables
global CFL r_time theta dt dtdx nx
global w k nv
global gamma etpfix

%% Controling Parameters
name        ='SBBGK1d'; % Simulation Name
CFL         = 0.05;     % CFL condition
r_time      = 1/10000;  % Relaxation time
tEnd        = 0.04;      % End time
theta       = 0;        % {-1} BE, {0} MB, {1} FD.
quad        = 2;        % for NC = 1 , GH = 2
method      = 1;        % for TVD = 1, WENO3 = 2, WENO5 = 3
IC_case     = 1;        % IC: {1}Sod's, {2}LE, {3}RE, {4}DS, {5}SS, {6}Cavitation
plot_figs   = 1;        % 0: no, 1: yes please!
write_ans   = 0;        % 0: no, 1: yes please!
gamma       = 2;      % Ratio of specific heats
flxtype     = 2;        % {1} Roe, {2} LF, {3} LLF, {4} Upwind <-non-conservative!
etpfix      = 0.90;     % {#} Harten's sonic entropy fix value, {0} no entropy fix

%% Space Discretization
nx  = 100;                      % number of cells
x   = linspace(0,1,nx);         % Physical domain -x
dx  = max(x(2:end)-x(1:end-1)); % delta x

%% Load Initial Condition
[z0,ux0,t0,p0,rho0,E0] = SSBGK_IC1d(x,IC_case);

%% Load Initial Cut Function
 xa = 0.5;   % buffer left boundary
 xb = 0.5125;   % buffer right boundary
 h0  = cutfunc(x,xa,xb);  % Physical Cut Function

%  h0 = ones(size(x));
%  h0 = zeros(size(x));

%% Discretization of the Velocity Space
% Microscopic Velocity Discretization (using Discrete Ordinate Method)
% that is to make coincide discrete values of microscopic velocities with
% values as the value points for using a quadrature method, so that we can
% integrate the velocity probability distribution to recover our
% macroscopics properties.
switch quad

    case{1} % Newton Cotes Quadrature:
    V  = [-20,20];  % range: a to b
    nv = 200;       % nodes desired (may not the actual value)
    [c,w,k] = cotes_xw(V(1),V(2),nv,5); % Using Netwon Cotes Degree 5

    case{2} % Gauss Hermite Quadrature:
    nv = 100;          % nodes desired (the actual value)
    [c,w] = GaussHermite(nv); % for integrating range: -inf to inf
    k = 1;            % quadrature constant.
    w = w.*exp(c.^2); % weighting function of the Gauss-Hermite quadrature

    otherwise
        error('Order must be between 1 and 2');
end

%% Applying DOM
% The actual nv value will be computed using 'lenght' vector function:
nv = length(c);
% Remap velocity points
    c = repmat(c,1,nx);     w = repmat(w,1,nx);
% Remap classical IC
    [rho0,ux0,p0] = apply_DOM(rho0,ux0,p0,nv);
% Remap Semiclassical IC
    [z0,t0,E0] = apply_DOM(z0,t0,E0,nv);
% Remap h coeficient
  h0 = repmat(h0,nv,1);

%% Semi0-classical Equilibrium Distribution Function
M0 = f_equilibrium_1d(z0,ux0,c,t0,theta);

%% Load initial Conditions and Spliting of information
M = M0; rho = rho0; ux = ux0; t = t0; p = p0; z = z0; h = h0; E = E0;

%% Split ICs in R and L
rhor = h.*rho0;     rhol = (1-h).*rho0;
uxr = h.*ux0;       uxl = (1-h).*ux0;
pr = h.*p0;         pl = (1-h).*p0;

% [zr,~,tr,~] = macroproperties1d(rhor,rhor.*uxr,pr+0.5*rhor.*uxr.^2,nx,nv,theta);
% [zl,~,tl,~] = macroproperties1d(rhol,rhol.*uxl,pl+0.5*rhol.*uxl.^2,nx,nv,theta);
% Ml = f_equilibrium_1d(zl,uxl,c,tl,theta) ;
% fr = f_equilibrium_1d(zr,uxr,c,tr,theta);
 fr=h.*M;
 Ml=(1-h).*M;
%% Main Loop
% Compute next time step
 dt = dx*CFL/max(c(:,1));

% dt =1/20000;
time  = 0:dt:tEnd;
dtdx = dt/dx;
% Ml(isnan(Ml)) = 0;     fr(isnan(fr)) = 0;
f = fr + Ml; % computed here for ploting purposes

count = 1; % iteration counter
tic;
for tsteps = time

    CFL=dtdx*max(c(:,1));
% Plot IC
   if plot_figs == 1; surf(f); end;

% Compute vector 'q'
   q = [rhol(1,:) ; rhol(1,:).*uxl(1,:) ; pl(1,:)+0.5*rhol(1,:).*uxl(1,:).^2];

% Update Physical cut function 'h'
     h_next = h; % this means: fixed buffer assumption

%     Evaluate Modified Boltzmann BGK
    [fr_next] = ModSBBGK(h,h_next,M,fr,Ml,c,flxtype);

% Evaluate Modificed Roe Euler Solver
    [rhol,rhoul,El] = ModEuler(h,h_next,q,fr_next,c);

%     plot partial result
%     if plot_figs == 1;
%         subplot(1,3,1); plot(x,rhol,'o'); title('Density');
%         subplot(1,3,2); plot(x,rhoul./rhol,'o'); title('Velocity');
%         subplot(1,3,3); plot(x,El,'o'); title('Energy');
%     end

%     macroscopic properties
    [zl,uxl,tl,pl] = macroproperties1d(rhol,rhoul,El,nx,nv,gamma,theta);

    % Apply DOM
    [tl,zl,uxl] = apply_DOM(tl,zl,uxl,nv);

    % Compute Ml_next
     Ml_next = f_equilibrium_1d(zl,uxl,c,tl,theta) ;
%  ****************************************************

    % New time step info: sum left and righ values with 'NaN' filter sum
    % function.
    Ml_next(isnan(Ml_next)) = 0;     fr_next(isnan(fr_next)) = 0;
    % Total f
     f  = fr_next + Ml_next;

     % Update New macroquantity
   [rho,rhou,E] = macromoments1d(k,w,f,c);

    %Apply Neumann BC's in total variables
    f(:,1)  = f(:,2);                 f(:,end)  = f(:,end-1);
    rho(:,1)= rho(:,2);            rho(:,end)= rho(:,end-1);
    rhou(:,1) = rhou(:,2);       rhou(:,end) = rhou(:,end-1);
    E(:,1)  = E(:,2);                 E(:,end)  = E(:,end-1);

    [rho,rhou,E] = apply_DOM(rho,rhou,E,nv);

    % Recover Semiclassical Conditions for next time step
     [z,ux,t,p] = macroproperties1d(rho,rhou,E,nx,nv,gamma,theta);

%  update New equilibrium
     M=f_equilibrium_1d(z,ux,c,t,theta);
     fr=h.*M;

%  preparing new Ml
     rhol = (1-h).*rho;
     uxl = (1-h).*ux;
     pl = (1-h).*p;
     [zl,~,tl,~] = macroproperties1d(rhol,rhol.*uxl,pl+0.5*rhol.*uxl.^2,nx,nv,gamma,theta);
% update New Ml
     Ml=   f_equilibrium_1d(zl,uxl,c,tl,theta) ;
     Ml(isnan(Ml)) = 0;

% update counter
    count = count+1;

% update plot
    drawnow
end
toc;

% write/plot final output
%plot(x,r_total,'o');


%% This script is used to generate the Brain Network for user to perform further analysis by themselves.

clear all
addpath(genpath(pwd));
input_folder='./data/'; % this is the directory of the time course data for all subjects, each file for each subject, similar as you prepared for GUI
output_dir='./Generated_BrainNet/'; %this is the output directory;
meth_Net='aHOFC'; %Here you can set the brain network construction method;
%["PC","aHOFC","tHOFC","SR","WSR","SLR","SGR","WSGR","GSR","SSGSR","dHOFC"];
switch meth_Net
    case {'SR','WSR','GSR'}
        lambda=0.01:0.01:0.1; %User can change the parameter range by themselves;
    case {'SLR','SGR','WSGR','SSGSR'}
        lambda_1=0.01:0.01:0.1; %User can change the parameter range by themselves;
        lambda_2=0.01:0.01:0.1;
    case 'dHOFC'
        C=100:100:800;
        W=20:10:70;
        s=1; %step size is usually set to 1 or 2;
    case {'PC','tHOFC','aHOFC'}

end

dirOutput=dir(fullfile(input_folder,'*.txt'));
fileName={dirOutput.name}';
%folder={dirOutput.folder}';
for i=1:length(fileName)
    BOLD{i,1}=load([input_folder,'/',fileName{i}]);
end
%label=importdata(label_input);

[~,nROI]=size(BOLD{1});
nSubj=length(BOLD);

fprintf('Begin network construction\n');

switch meth_Net
    case 'PC'       % Pearson's correlation
        BrainNet{1}=PC(BOLD);
    case 'tHOFC'    % Topographical high-order FC
        BrainNet{1}=tHOFC(BOLD);
    case 'aHOFC'    % Associated high-order FC
        BrainNet{1}=aHOFC(BOLD);
    case 'SR'       % Sparse representation
        parfor i=1:length(lambda)
            BrainNet{i}=SR(BOLD,lambda(i));
        end

    case 'WSR'      % PC weighted SR
        parfor i=1:length(lambda)
            BrainNet{i}=WSR(BOLD,lambda(i));
        end

    case 'SLR'      % Sparse low-rank representation
        lambda1=lambda_1;
        lambda2=lambda_2;
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);
        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=SLR(BOLD,lambda1(i),lambda2(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'SGR'      % Sparse group representation
        lambda1=lambda_1;
        lambda2=lambda_2;
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);
        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=SGR(BOLD,lambda1(i),lambda2(j));
            end
        end

        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'WSGR'     % PC weighted SGR
        lambda1=lambda_1; % parameter for sparsity
        lambda2=lambda_2; % parameter for group sparsity
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);

        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=WSGR(BOLD,lambda1(i),lambda2(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'GSR'      % Group sparse representation
        parfor i=1:length(lambda)
            BrainNet{i}=GSR(BOLD,lambda(i));
        end

    case 'SSGSR'    % Strength and Similarity guided GSR
        %lambda1=lambda_1(1:6); % parameter for group sparsity
        lambda1=lambda_1;
        lambda2=lambda_2; % parameter for inter-subject LOFC-pattern similarity
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);
        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=SSGSR(BOLD,lambda1(i),lambda2(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'dHOFC'    % Dynamic high-order FC
        num_W=length(W);
        num_C=length(C);
        parfor i=1:num_W % number of clusters
            for j=1:num_C
                [BrainNet{i,j},IDX{i,j}]=dHOFC(BOLD,W(i),s,C(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_W*num_C);
end
save(char(strcat(output_dir,meth_Net,'.mat')),'BrainNet','-v7.3');
% All the generated networks (and those resulted from different parameters) are saved as a cell array, nROIxnROIxnSubject in each cell, different cells for results with different combinations of parameters
fprintf('Network construction finished\n');


function [ySqrtEst, PInvSqrtEst]=sqrtInfoBatchSmoother(ySqrtPred,PInvSqrtPred,z,u,H,F,SR,SQ,Gamma,kD)
%%SQRTINFOBATCHSMOOTHER Run the square root information smoother for linear
%                  dynamic and measurement models on a batch of
%                  measurements. The smoothed result at one time step or
%                  along the entire batch are available. The initial
%                  predicted states can not be uninformative.
%
%INPUTS: ySqrtPred The xDimX1 predicted square root information state at
%                 the time of the initial measurement in z. The predicted
%                 information state is always PInvSqrtPred times the
%                 predicted target state estimate.
%    PInvSqrtPred The inverse square root information matrix associated
%                 with the predicted information state at the time of the
%                 initial measurement in z. If P is the covariance matrix
%                 of a Gaussian state x, then P=PSqrt*PSqrt' and
%                 PInvSqrtPred=inv(PSqrt). This can be either upper
%                 triangular or lower triangular.
%               z The zDim X N matrix of measurements for the whole batch.
%               u The xDim X(N-1) matrix of control inputs for the whole
%                 batch. If there are no control inputs, then set u=[];
%               H The zDim X xDim X N hypermatrix of measurement matrices
%                 such that H(:,:,k)*x+w is the measurement at time k,
%                 where x is the state and w is zero-mean Gaussian noise
%                 with covariance matrix R (:,:,k). Alternatively, if all
%                 of the measurement matrices are the same, one can just
%                 pass a single zDim X xDim matrix.
%               F The xDim X xDim X (N-1) hypermatrix of state transition
%                 matrices. The state at discrete-time k+1 is modeled as
%                 F(:,:,k) times the state at time k plus zero-mean
%                 Gaussian process noise with covariance matrix Q(:,:,k).
%                 Alternatively, if all of the state transition matrices
%                 are the same, one can just pass a single xDim X xDim
%                 matrix.
%              SR The zDim X zDim X N hypermatrix of lower-triangular
%                 square-root of the measurement covariance matrices. The
%                 matrices must be invertible. Alternatively, if all of the
%                 measurement covariance matrices are the same, one can
%                 just pass a single zDim X zDim matrix.
%              SQ The xDim X xDim X (N-1) hypermatrix of lower-triangular
%                 square-root of the process noise covariance matrices.
%                 The matrices must be invertible. Alternatively, if all of
%                 the measurement covariance matrices are the same, one can
%                 just pass a single xDim X xDim matrix.
%           Gamma An optional xDim X xDim X (N-1) hypermatrix of matrices
%                 that transform the process noise to the state domain if
%                 the process noise covariance matrix is singular.
%                 Alternatively, if all of the transform matrices are the
%                 same, one can just pass a single xDim X xDim matrix. If
%                 this is omitted entirely, an identity matrix is used
%                 (i.e. there is no Gamma).
%              kD The discrete time-step at which the smoothed state
%                 estimate is desired, where z(:,1) is at discrete
%                 time-step 1 (not 0). If kD is omitted ot an empty matrix
%                 is passed, then results along the entire batch are
%                 obtained.
%
%OUTPUTS: ySqrtEst The xDimXN smoothed square root information state
%                  estimates at all steps if kD is not provided or the
%                  xDimX1 smoothed information state estimate at step kD if
%                  kD is provided.
%      PSqrtInvEst The inverse square root covariance matrices associated
%                  with the smoothed information state estimates. This is
%                  xDimXxDimXN for the whole batch if kD is not provided
%                  and is xDimXxDim if kD is provided.
%
%The algorithm is that of the linear square root information filter and
%smoother that is described in the book [1]. The forward pass steps can be
%found in Chapter IV, and the backwards smoother in Chapter X.
%
%The algorithm works by running a square root information filter forward
%in time and then smoothing backwards. The smoothing step depends on
%information stored during the forward pass. The smoothing step here is
%performed on a state residual rather than the state itself and applies the
%smoothed output to the previous updated measurement. This was based off of
%NASA's Orbit Determination Toolbox.
%
%z*(j+1)=PInvSqrt*(j+1)(x*(j+1)-xPred(j+1)
%A=[Rw(j)+Rwx(j),   Rwx(j)F(j+1,j),         u(j)
%   PInvSqrt*(j+1), PInvSqrt*(j+1)F(j+1,j), z*(j+1)]
%qr(A)=[Rw*(j),   Rwx*(j),      u*(j)
%       0,        PInvSqrt*(j), z*(j)]
%x*(j)=xUpd(j)+PInvSqrt*(j)\z*(j)
%
%where xPred is the forward predicted state, xUpd is the forward updated
%state, Rw and Rwx are outputs from the forward prediction step, u is the
%the control input (often zero), and x* and PInvSqrt* are the smoothed
%states. x*(N)=xUpd(N) and PInvSqrt*(N)=PInvSqrtUpd(N)
%
%REFERENCES:
%[1] G. J. Bierman, "Factorization Methods for Discrete Sequential
%    Estimation. Academic Press, New York, 1977.
%
%March 2015, David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(H,2);
N=size(z,2);

if(nargin<9||isempty(Gamma))
    Gamma=eye(xDim);
end
if(nargin<10||isempty(kD))
    kD=[];
end
if(isempty(ySqrtPred))
    ySqrtPred=zeros(xDim,1);
end
if(isempty(PInvSqrtPred))
    PInvSqrtPred=zeros(xDim,xDim);
end
if(isempty(u))
    u=zeros(xDim,N-1);
end
if(size(H,3)==1)
    H=repmat(H,[1,1,N]);
end
if(size(F,3)==1)
    F=repmat(F,[1,1,N-1]);
end
if(size(SR,3)==1)
    SR=repmat(SR,[1,1,N]);
end
if(size(SQ,3)==1)
    SQ=repmat(SQ,[1,1,N-1]);
end
if(size(Gamma,3)==1)
    Gamma=repmat(Gamma,[1,1,N-1]);
end

%Run the SRIF forward until we have the predictions of step kD|kD-1 (going forwards).
ySqrtFwdPred=zeros(xDim,N);
PInvSqrtFwdPred=zeros(xDim,xDim,N);
ySqrtFwdUpd=zeros(xDim,N);
PInvSqrtFwdUpd=zeros(xDim,xDim,N);
Rw=zeros(xDim,xDim,N);
Rwx=zeros(xDim,xDim,N);

%The first step uses the priors
ySqrtFwdPred(:,1)=ySqrtPred;
PInvSqrtFwdPred(:,:,1)=PInvSqrtPred;

%The rest of the steps
for curStep=1:(N-1)
    [ySqrtFwdUpd(:,curStep),PInvSqrtFwdUpd(:,:,curStep)]=sqrtInfoFilterUpdate(ySqrtFwdPred(:,curStep),PInvSqrtFwdPred(:,:,curStep),z(:,curStep),SR(:,:,curStep),H(:,:,curStep));
    [ySqrtFwdPred(:,curStep+1),PInvSqrtFwdPred(:,:,curStep+1),Rw(:,:,curStep+1),Rwx(:,:,curStep+1)]=sqrtInfoFilterDiscPred(ySqrtFwdUpd(:,curStep),PInvSqrtFwdUpd(:,:,curStep),F(:,:,curStep),SQ(:,:,curStep),u(:,curStep),Gamma(:,:,curStep));
end

%Run the SRIS backwards until we have the updated information state of
%step kD|kD (going backwards).
ySqrtEst=zeros(xDim,N);
PInvSqrtEst=zeros(xDim,xDim,N);

%ODTBX method
[ySqrtEst(:,end),PInvSqrtEst(:,:,end)]=sqrtInfoFilterUpdate(ySqrtFwdPred(:,N),PInvSqrtFwdPred(:,:,N),z(:,N),SR(:,:,N),H(:,:,N));
for curStep=(N-1):-1:1
    xPred=PInvSqrtFwdPred(:,:,curStep+1)\ySqrtFwdPred(:,curStep+1); %Forward predicted state_j+1
    xStar=PInvSqrtEst(:,:,curStep+1)\ySqrtEst(:,curStep+1); %Backward smoothed state_j+1
    zStar=PInvSqrtEst(:,:,curStep+1)*(xStar-xPred);

    [zSmooth,PInvSqrtEst(:,:,curStep)]=sqrtInfoSmoothStep(zStar,PInvSqrtEst(:,:,curStep+1),F(:,:,curStep),Rw(:,:,curStep),Rwx(:,:,curStep),u(:,curStep),Gamma(:,:,curStep));

    xUpd=PInvSqrtFwdUpd(:,:,curStep)\ySqrtFwdUpd(:,curStep); %Forward updated state_j
    ySqrtEst(:,curStep)=PInvSqrtEst(:,:,curStep)*(xUpd+PInvSqrtEst(:,:,curStep)\zSmooth);
end

if(~isempty(kD))
    ySqrtEst=ySqrtEst(:,kD);
    PInvSqrtEst=PInvSqrtEst(:,:,kD);
end
end

function [ySqrtPred, PInvSqrtPred]=sqrtInfoSmoothStep(ySqrtPrev,PInvSqrtPrev,F,Ru,Rux,u,Gamma)
%This is the basic Smoothing step shown in Bierman's book
xDim=size(ySqrtPrev,1);

A=[Ru+Rux*Gamma, Rux*F, u;
    PInvSqrtPrev*Gamma,   PInvSqrtPrev*F,   ySqrtPrev];
[~,T]=qr(A);

ySqrtPred=T((xDim+1):end,end);
PInvSqrtPred=T((xDim+1):end,(end-xDim):(end-1));
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [TW] = Btuph2TW(Btuph)
% Convert power from British thermal units per hour to terawatts.
% Chad A. Greene 2012
TW = Btuph*2.930710702e-13;


function [fb_sim] = acc_gen (ref, imu)
% acc_gen: generates simulated accelerometers measurements from reference
%  data and IMU error profile.
%
% INPUT
%	ref: data structure with true trajectory.
%	imu: data structure with IMU error profile.
%
% OUTPUT
%	fb_sim: Nx3 matrix with simulated accelerations in the
%		body frame [X Y Z] (m/s^2, m/s^2, m/s^2).
%
%   Copyright (C) 2014, Rodrigo González, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%	R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Sec. 2.2.
%
%   Aggarwal, P. et al. MEMS-Based Integrated Navigation. Artech
% House. 2010.
%
%   Thinking about accelerometers and gravity by Dave Redell
% http://www.lunar.org/docs/LUNARclips/v5/v5n1/Accelerometers.html
%
% Version: 012
% Date:    2022/08/22
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

N = max(size(ref.t));
M = [N, 3];

%% SIMULATION OF ACC

% If true, accelerations are provided...
if (isfield(ref, 'fb'))

    acc_b = ref.fb;

% If not, accelerations are obtained from velocity
elseif (isfield(ref, 'vel'))

    acc_raw = (diff(ref.vel)) ./ [diff(ref.t) diff(ref.t) diff(ref.t)];
    acc_raw = [ 0 0 0; acc_raw; ];

    % Noise introduced by derivatives should be smoothed
    acc_ned = my_sgolayfilt(acc_raw);
    acc_b = acc_nav2body(acc_ned, ref.DCMnb_m);

% If not, accelerations are obtained from position
else

    % Method: LLH > ECEF > NED
    [~, acc_ned] = pllh2vned (ref);
    acc_b = acc_nav2body(acc_ned, ref.DCMnb_m);
end

%% SIMULATION OF GRAVITY AND CORIOLIS

% Gravity and Coriolis in nav-ref
g_n = -gravity(ref.lat, ref.h);              % Accelerometer in Z axis senses an
                                                % acceleration of 1.0 G straight up
cor_n = coriolis(ref.lat, ref.vel, ref.h);

% Gravity and Coriolis from nav-ref to body-ref
g_b = zeros(M);
cor_b = zeros(M);
for i = 1:N
    dcmnb = reshape(ref.DCMnb_m(i,:), 3, 3);
    gb = dcmnb * g_n(i,:)';
    corb =  dcmnb * cor_n(i,:)';
    g_b(i,:) = gb';
    cor_b(i,:) = corb';
end

%% SIMULATION OF NOISES

% -------------------------------------------------------------------------
% Simulation of static bias as a constant random variable

ab_sta = noise_b_sta (imu.ab_sta, N);

% -------------------------------------------------------------------------
% Simulation of white noise

wn = randn(M);
acc_wn = zeros(M);

for i=1:3

    acc_wn(:, i) = imu.a_std(i).* wn(:,i);
end

% -------------------------------------------------------------------------
% Simulation of dynamic bias (bias instability) as a first-order Gauss-Markov model

dt = 1/ref.freq;
ab_dyn = noise_b_dyn (imu.ab_corr, imu.ab_dyn, dt, M);

% -------------------------------------------------------------------------
% Simulation of rate random walk

acc_rrw = noise_rrw (imu.vrrw, dt, M);

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

fb_sim = acc_b + cor_b + g_b + acc_wn + ab_sta + ab_dyn + acc_rrw;

end


function [mh,mw]=tropmapf(time,pos,azel)

if pos(3)<-1000||pos(3)>20000
    mh=0; mw=0; return;
end

% hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75
coef=[ 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3;...
       2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3;...
       62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3;...

       0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5;...
       0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5;...
       0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5;...

       5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4;...
       1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3;...
       4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2];

% height correction
aht=[2.53E-5, 5.49E-3, 1.14E-3];
ah=zeros(3,1); aw=zeros(3,1);

if azel(2)<=0,mh=0; mw=0; return;end

lat=pos(1)*180/pi;
if lat<0,yy=0.5;else,yy=0;end
y=(time2doy(time)-28.0)/365.25+yy;
cosy=cos(2.0*pi*y);
lat=abs(lat);

for i=1:3
    ah(i)=interpc(coef(i,:),lat)-interpc(coef(i+3,:),lat)*cosy;
    aw(i)=interpc(coef(i+6,:),lat);
%     j=fix(lat/15);
%     ah(i)=(coef(i,j)+(coef(i,j+1)-coef(i,j))*(lat/15-j))-...
%           (coef(i+3,j)+(coef(i+3,j+1)-coef(i+3,j))*(lat/15-j))*cosy;
%     aw(i)=(coef(i+6,j)+(coef(i+6,j+1)-coef(i+6,j))*(lat/15-j));
end

el=azel(2);
sinel=sin(el);

mh1=(1+ah(1)/(1+ah(2)/(1+ah(3))))/(sinel+(ah(1)/(sinel+ah(2)/(sinel+ah(3)))));
mh2=(1+aht(1)/(1+aht(2)/(1+aht(3))))/(sinel+(aht(1)/(sinel+aht(2)/(sinel+aht(3)))));
mh=mh1+(1/sinel-mh2)*pos(3)/1000;

mw=(1+aw(1)/(1+aw(2)/(1+aw(3))))/(sinel+(aw(1)/(sinel+ah(2)/(sinel+aw(3)))));

return


seed = 0;
rand('state', seed);
randn('state', seed);

nrows = 3;
ncols = 3;
npixels = nrows*ncols;

% we number pixels in transposed raster scan order (top to bottom, left to right)

% hidden var
HV = reshape(1:npixels, nrows, ncols);
% observed var
OV = reshape(1:npixels, nrows, ncols) + length(HV(:));

% observed factor
OF = reshape(1:npixels, nrows, ncols);
% vertical edge factor VEF(i,j) is the factor for edge HV(i,j) - HV(i+1,j)
VEF = reshape((1:(nrows-1)*ncols), nrows-1, ncols) + length(OF(:));
% horizontal edge factor HEF(i,j) is the factor for edge HV(i,j) - HV(i,j+1)
HEF = reshape((1:nrows*(ncols-1)), nrows, ncols-1) + length(OF(:)) + length(VEF(:));

nvars = length(HV(:))+length(OV(:));
assert(nvars == 2*npixels);
nfac = length(OF(:)) + length(VEF(:)) + length(HEF(:));

K = 2; % number of discrete values for the hidden vars
%O = 1; % each observed pixel is a scalar
O = 2; % each observed pixel is binary

factors = cell(1,3);

% hidden states generate observed 0 or 1 plus noise
%factors{2} = cond_gauss1_kernel(K, O, 'mean', [0 1], 'cov', [0.1 0.1]);
pnoise = 0.2;
factors{1} = tabular_kernel([K O], [1-pnoise pnoise; pnoise 1-pnoise]);
ofactor = 1;

% encourage compatibility between neighboring vertical pixels
factors{2} = tabular_kernel([K K], [0.8 0.2; 0.2 0.8]);
vedge_factor = 2;

%% no constraint between neighboring horizontal pixels
%factors{3} = tabular_kernel([K K], [0.5 0.5; 0.5 0.5]);

factors{3} = tabular_kernel([K K], [0.8 0.2; 0.2 0.8]);
hedge_factor = 3;

factor_ndx = zeros(1, 3);
G = zeros(nvars, nfac);
ns = [K*ones(1,length(HV(:))) O*ones(1,length(OV(:)))];

N = length(ns);
%cnodes = OV(:);
cnodes = [];
dnodes = 1:N;

for i=1:nrows
  for j=1:ncols
    G([HV(i,j), OV(i,j)], OF(i,j)) = 1;
    factor_ndx(OF(i,j)) = ofactor;

    if i < nrows
      G(HV(i:i+1,j), VEF(i,j)) = 1;
      factor_ndx(VEF(i,j)) = vedge_factor;
    end

    if j < ncols
      G(HV(i,j:j+1), HEF(i,j)) = 1;
      factor_ndx(HEF(i,j)) = hedge_factor;
    end

  end
end

fg = mk_fgraph(G, ns, factors, 'discrete', dnodes, 'equiv_class', factor_ndx);

if 1
  % make image with vertical stripes
  I = zeros(nrows, ncols);
  for j=1:2:ncols
    I(:,j) = 1;
  end
else
  % make image with square in middle
  I = zeros(nrows, ncols);
  I(3:6,3:6) = 1;
end

% corrupt image
O = mod(I + (rand(nrows,ncols)> (1-pnoise)), 2);

maximize = 1;
engine = belprop_fg_inf_engine(fg, 'maximize', maximize, 'max_iter', npixels*5);

evidence = cell(1, nvars);
onodes = OV(:);
evidence(onodes) = num2cell(O+1); % values must be in range {1,2}

engine = enter_evidence(engine, evidence);

for i=1:nrows
  for j=1:ncols
    m = marginal_nodes(engine, HV(i,j));
    Ihat(i,j) = argmax(m.T)-1;
  end
end

Ihat


function [A_hat E_hat iter svp elapsed] = ialm_rpca(D, lambda, tol, maxIter, blk)

% Oct 2009
% This matlab code implements the inexact augmented Lagrange multiplier
% method for Robust PCA.
%
% D - m x n matrix of observations/data (required input)
%
% lambda - weight on sparse error term in the cost function
%
% tol - tolerance for stopping criterion.
%     - DEFAULT 1e-7 if omitted or -1.
%
% maxIter - maximum number of iterations
%         - DEFAULT 1000, if omitted or -1.
% blk - indicate whether to use BLWS
%
% Initialize A,E,Y,u
% while ~converged
%   minimize (inexactly, update A and E only once)
%     L(A,E,Y,u) = |A|_* + lambda * |E|_1 + <Y,D-A-E> + mu/2 * |D-A-E|_F^2;
%   Y = Y + \mu * (D - A - E);
%   \mu = \rho * \mu;
% end

%addpath PROPACK;

%elapsed = tic;

[m n] = size(D);

if nargin < 2
    lambda = 1 / sqrt(max(m,n));
elseif lambda == -1
    lambda = 1 / sqrt(max(m,n));
end

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

if nargin < 4
    maxIter = 1000;
elseif maxIter == -1
    maxIter = 1000;
end

if nargin < 5
    blk = 0;
elseif blk == -1
    blk = 0;
end

% initialize
Y = D;
norm_two = lansvd(Y, 1, 'L');
norm_inf = norm( Y(:), inf) / lambda;
dual_norm = max(norm_two, norm_inf);
Y = Y / dual_norm;

A_hat = zeros( m, n);
E_hat = zeros( m, n);
mu = 1.25/norm_two % this one can be tuned
mu_bar = mu * 1e7
rho = 1.2         % this one can be tuned
d_norm = norm(D, 'fro');

iter = 0;
total_svd = 0;
converged = false;
stopCriterion = 1;
sv = 10;
d = min(m, n);

mark = false;
Block_mark = false ;

while ~converged
    iter = iter + 1;

    temp_T = D - A_hat + (1/mu)*Y;
    E_hat = max(temp_T - lambda/mu, 0);
    E_hat = E_hat+min(temp_T + lambda/mu, 0);

%     if Block_mark==true
%         uv_temp=[U_temp; V_temp]/sqrt(2);
%         [U S V]=BL_SVD(D - E_hat + (1/mu)*Y, U_temp, V_temp, 1);
%     else
          [U S V] = lansvd(D - E_hat + (1/mu)*Y, sv, 'L');
%     end

    if mark==true
        U_temp = U;
        V_temp = V;
        Block_mark=true;
    end

    diagS = diag(S);
    svp = length(find(diagS > 1/mu));

    len = length(diagS);
    ratio = diagS(1:len-1) ./ diagS(2:len);
%     ind = find(ratio > 2);
%     if blk && length(ind) > 0
%         svp = min(svp, ind(1));
%         mark = true;
%     end

    if svp < sv
        sv = min(svp + 1, d);
    else
        sv = min(svp + round(0.05*d), d);
    end

    A_hat = U(:, 1:svp) * diag(diagS(1:svp) - 1/mu) * V(:, 1:svp)';

    total_svd = total_svd + 1;

    Z = D - A_hat - E_hat;

    Y = Y + mu*Z;
    mu = min(mu*rho, mu_bar);

    %% stop Criterion
    stopCriterion = norm(Z, 'fro') / d_norm;
    if stopCriterion < tol
        converged = true;
    end

    if mod( total_svd, 10) == 0
        disp(['#svd ' num2str(total_svd) ' r(A) ' num2str(svp)...
            ' |E|_0 ' num2str(length(find(abs(E_hat)>0)))...
            ' stopCriterion ' num2str(stopCriterion)]);
    end

    if ~converged && iter >= maxIter
        disp('Maximum iterations reached') ;
        converged = 1 ;
    end
end

%elapsed = toc(elapsed);


function [dSf_dV1, dSf_dV2, dSt_dV1, dSt_dV2, Sf, St] = dSbr_dV(branch, Yf, Yt, V, vcart)
%DSBR_DV   Computes partial derivatives of branch power flows w.r.t. voltage.
%
%   The derivatives can be taken with respect to polar or cartesian coordinates
%   of voltage, depending on the 5th argument.
%
%   [DSF_DVA, DSF_DVM, DST_DVA, DST_DVM, SF, ST] = DSBR_DV(BRANCH, YF, YT, V)
%   [DSF_DVA, DSF_DVM, DST_DVA, DST_DVM, SF, ST] = DSBR_DV(BRANCH, YF, YT, V, 0)
%
%   Returns four matrices containing partial derivatives of the complex
%   branch power flows at "from" and "to" ends of each branch w.r.t voltage
%   magnitude and voltage angle, respectively (for all buses).
%
%   [DSF_DVR, DSF_DVI, DST_DVR, DST_DVI, SF, ST] = DSBR_DV(BRANCH, YF, YT, V, 1)
%
%   Returns four matrices containing partial derivatives of the complex
%   branch power flows at "from" and "to" ends of each branch w.r.t real and
%   imaginary parts of voltage, respectively (for all buses).
%
%   If YF is a sparse matrix, the partial derivative matrices will be as well.
%   Optionally returns vectors containing the power flows themselves. The
%   following explains the expressions used to form the matrices:
%
%   If = Yf * V;
%   Sf = diag(Vf) * conj(If) = diag(conj(If)) * Vf
%
%   Polar coordinates:
%     Partials of V, Vf & If w.r.t. voltage angles
%       dV/dVa  = j * diag(V)
%       dVf/dVa = sparse(1:nl, f, j * V(f)) = j * sparse(1:nl, f, V(f))
%       dIf/dVa = Yf * dV/dVa = Yf * j * diag(V)
%
%     Partials of V, Vf & If w.r.t. voltage magnitudes
%       dV/dVm  = diag(V./abs(V))
%       dVf/dVm = sparse(1:nl, f, V(f)./abs(V(f))
%       dIf/dVm = Yf * dV/dVm = Yf * diag(V./abs(V))
%
%     Partials of Sf w.r.t. voltage angles
%       dSf/dVa = diag(Vf) * conj(dIf/dVa)
%                       + diag(conj(If)) * dVf/dVa
%               = diag(Vf) * conj(Yf * j * diag(V))
%                       + conj(diag(If)) * j * sparse(1:nl, f, V(f))
%               = -j * diag(Vf) * conj(Yf * diag(V))
%                       + j * conj(diag(If)) * sparse(1:nl, f, V(f))
%               = j * (conj(diag(If)) * sparse(1:nl, f, V(f))
%                       - diag(Vf) * conj(Yf * diag(V)))
%
%     Partials of Sf w.r.t. voltage magnitudes
%       dSf/dVm = diag(Vf) * conj(dIf/dVm)
%                       + diag(conj(If)) * dVf/dVm
%               = diag(Vf) * conj(Yf * diag(V./abs(V)))
%                       + conj(diag(If)) * sparse(1:nl, f, V(f)./abs(V(f)))
%
%   Cartesian coordinates:
%     Partials of V, Vf & If w.r.t. real part of complex voltage
%       dV/dVr  = diag(ones(n,1))
%       dVf/dVr = Cf
%       dIf/dVr = Yf
%     where Cf is the connection matrix for line & from buses
%
%     Partials of V, Vf & If w.r.t. imaginary part of complex voltage
%       dV/dVi  = j * diag(ones(n,1))
%       dVf/dVi = j * Cf
%       dIf/dVi = j * Yf
%
%     Partials of Sf w.r.t. real part of complex voltage
%       dSf/dVr = conj(diag(If)) * Cf + diag(Vf) * conj(Yf)
%
%     Partials of Sf w.r.t. imaginary part of complex voltage
%       dSf/dVi = j * (conj(diag(If)) * Cf - diag(Vf) * conj(Yf))
%
%   Derivations for "to" bus are similar.
%
%   Examples:
%       [Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch);
%       [dSf_dVa, dSf_dVm, dSt_dVa, dSt_dVm, Sf, St] = ...
%           dSbr_dV(branch, Yf, Yt, V);
%       [dSf_dVr, dSf_dVi, dSt_dVr, dSt_dVi, Sf, St] = ...
%           dSbr_dV(branch, Yf, Yt, V, 1);
%
%   For more details on the derivations behind the derivative code used
%   in MATPOWER, see:
%
%   [TN2]  R. D. Zimmerman, "AC Power Flows, Generalized OPF Costs and
%          their Derivatives using Complex Matrix Notation", MATPOWER
%          Technical Note 2, February 2010. [Online]. Available:
%          https://matpower.org/docs/TN2-OPF-Derivatives.pdf
%          doi: 10.5281/zenodo.3237866
%   [TN4]  B. Sereeter and R. D. Zimmerman, "AC Power Flows and their
%          Derivatives using Complex Matrix Notation and Cartesian
%          Coordinate Voltages," MATPOWER Technical Note 4, April 2018.
%          [Online]. Available: https://matpower.org/docs/TN4-OPF-Derivatives-Cartesian.pdf
%          doi: 10.5281/zenodo.3237909

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

%% define named indices into bus, gen, branch matrices
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

%% default input args
if nargin < 5
    vcart = 0;      %% default to polar coordinates
end

%% define
f = branch(:, F_BUS);       %% list of "from" buses
t = branch(:, T_BUS);       %% list of "to" buses
nl = length(f);
nb = length(V);

%% compute intermediate values
Yfc = conj(Yf);
Ytc = conj(Yt);
Vc = conj(V);
Ifc = Yfc * Vc;     %% conjugate of "from" current
Itc = Ytc * Vc;     %% conjugate of "to" current

if issparse(Yf)             %% sparse version (if Yf is sparse)
    diagVf  = sparse(1:nl, 1:nl, V(f), nl, nl);
    diagVt  = sparse(1:nl, 1:nl, V(t), nl, nl);
    diagIfc = sparse(1:nl, 1:nl, Ifc, nl, nl);
    diagItc = sparse(1:nl, 1:nl, Itc, nl, nl);
    if ~vcart
        Vnorm       = V ./ abs(V);
        diagVc      = sparse(1:nb, 1:nb, Vc, nb, nb);
        diagVnorm   = sparse(1:nb, 1:nb, Vnorm, nb, nb);
        CVf  = sparse(1:nl, f, V(f), nl, nb);
        CVnf = sparse(1:nl, f, Vnorm(f), nl, nb);
        CVt  = sparse(1:nl, t, V(t), nl, nb);
        CVnt = sparse(1:nl, t, Vnorm(t), nl, nb);
    end
else                        %% dense version
    diagVf  = diag(V(f));
    diagVt  = diag(V(t));
    diagIfc = diag(Ifc);
    diagItc = diag(Itc);
    if ~vcart
        Vnorm       = V ./ abs(V);
        diagVc      = diag(Vc);
        diagVnorm   = diag(Vnorm);
%         CVf        = zeros(nl, nb);    CVf(sub2ind([nl,nb], (1:nl)', f)) = V(f);
%         CVnf       = zeros(nl, nb);    CVnf(sub2ind([nl,nb], (1:nl)', f)) = Vnorm(f);
%         CVt        = zeros(nl, nb);    CVt(sub2ind([nl,nb], (1:nl)', t)) = V(t);
%         CVnt       = zeros(nl, nb);    CVnt(sub2ind([nl,nb], (1:nl)', t)) = Vnorm(t);
        CVf  = full(sparse(1:nl, f, V(f), nl, nb));
        CVnf = full(sparse(1:nl, f, Vnorm(f), nl, nb));
        CVt  = full(sparse(1:nl, t, V(t), nl, nb));
        CVnt = full(sparse(1:nl, t, Vnorm(t), nl, nb));
    end
end
if vcart
    Cf = sparse(1:nl, f, ones(nl, 1), nl, nb);      %% connection matrix for line & from buses
    Ct = sparse(1:nl, t, ones(nl, 1), nl, nb);      %% connection matrix for line & to buses
    Af = diagIfc * Cf;
    Bf = diagVf * Yfc;
    At = diagItc * Ct;
    Bt = diagVt * Ytc;

    dSf_dV1 = Af + Bf;          %% dSf_dVr
    dSf_dV2 = 1j * (Af - Bf);   %% dSf_dVi
    dSt_dV1 = At + Bt;          %% dSt_dVr
    dSt_dV2 = 1j * (At - Bt);   %% dSt_dVi
else
    dSf_dV1 = 1j * (diagIfc * CVf - diagVf * Yfc * diagVc);     %% dSf_dVa
    dSf_dV2 = diagVf * conj(Yf * diagVnorm) + diagIfc * CVnf;   %% dSf_dVm
    dSt_dV1 = 1j * (diagItc * CVt - diagVt * Ytc * diagVc);     %% dSt_dVa
    dSt_dV2 = diagVt * conj(Yt * diagVnorm) + diagItc * CVnt;   %% dSt_dVm
end

if nargout > 4
    Sf = V(f) .* Ifc;
    St = V(t) .* Itc;
end


function [ value, ifault ] = xinbta ( p, q, beta, alpha )

%*****************************************************************************80
%
%% XINBTA computes inverse of the incomplete Beta function.
%
%  Discussion:
%
%    The accuracy exponent SAE was loosened from -37 to -30, because
%    the code would not otherwise accept the results of an iteration
%    with p = 0.3, q = 3.0, alpha = 0.2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2014
%
%  Author:
%
%    Original FORTRAN77 version by GW Cran, KJ Martin, GE Thomas.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    GW Cran, KJ Martin, GE Thomas,
%    Remark AS R19 and Algorithm AS 109:
%    A Remark on Algorithms AS 63: The Incomplete Beta Integral
%    and AS 64: Inverse of the Incomplete Beta Integeral,
%    Applied Statistics,
%    Volume 26, Number 1, 1977, pages 111-114.
%
%  Parameters:
%
%    Input, real P, Q, the parameters of the incomplete
%    Beta function.
%
%    Input, real BETA, the logarithm of the value of
%    the complete Beta function.
%
%    Input, real ALPHA, the value of the incomplete Beta
%    function.  0 <= ALPHA <= 1.
%
%    Output, real VALUE, the argument of the incomplete
%    Beta function which produces the value ALPHA.
%
%    Output, integer IFAULT, error flag.
%    0, no error occurred.
%    nonzero, an error occurred.
%
%  Local Parameters:
%
%    Local, real SAE, requests an accuracy of about 10^SAE.
%
  sae = -30.0;

  fpu = 10.0 ^ sae;

  ifault = 0;
  value = alpha;
%
%  Test for admissibility of parameters.
%
  if ( p <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'XINBTA - Fatal error!\n' );
    fprintf ( 1, '  P <= 0.0\n' );
    ifault = 1;
    error ( 'XINBTA - Fatal error!' );
  end

  if ( q <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'XINBTA - Fatal error!\n' );
    fprintf ( 1, '  Q <= 0.0\n' );
    ifault = 1;
    error ( 'XINBTA - Fatal error!' );
  end

  if ( alpha < 0.0 || 1.0 < alpha )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'XINBTA - Fatal error!\n' );
    fprintf ( 1, '  ALPHA not between 0 and 1.\n' );
    ifault = 2;
    error ( 'XINBTA - Fatal error!' );
  end
%
%  If the answer is easy to determine, return immediately.
%
  if ( alpha == 0.0 )
    value = 0.0;
    return
  end

  if ( alpha == 1.0 )
    value = 1.0;
    return
  end
%
%  Change tail if necessary.
%
  if ( 0.5 < alpha )
    a = 1.0 - alpha;
    pp = q;
    qq = p;
    indx = 1;
  else
    a = alpha;
    pp = p;
    qq = q;
    indx = 0;
  end
%
%  Calculate the initial approximation.
%
  r = sqrt ( - log ( a * a ) );

  y = r - ( 2.30753 + 0.27061 * r ) ...
    / ( 1.0 + ( 0.99229 + 0.04481 * r ) * r );

  if ( 1.0 < pp && 1.0 < qq )

    r = ( y * y - 3.0 ) / 6.0;
    s = 1.0 / ( pp + pp - 1.0 );
    t = 1.0 / ( qq + qq - 1.0 );
    h = 2.0 / ( s + t );
    w = y * sqrt ( h + r ) / h - ( t - s ) ...
      * ( r + 5.0 / 6.0 - 2.0 / ( 3.0 * h ) );
    value = pp / ( pp + qq * exp ( w + w ) );

  else

    r = qq + qq;
    t = 1.0 / ( 9.0 * qq );
    t = r * ( 1.0 - t + y * sqrt ( t ) )^3;

    if ( t <= 0.0 )
      value = 1.0 - exp ( ( log ( ( 1.0 - a ) * qq ) + beta ) / qq );
    else

      t = ( 4.0 * pp + r - 2.0 ) / t;

      if ( t <= 1.0 )
        value = exp ( ( log ( a * pp ) + beta ) / pp );
      else
        value = 1.0 - 2.0 / ( t + 1.0 );
      end

    end

  end
%
%  Solve for X by a modified Newton-Raphson method,
%  using the function BETAIN.
%
  r = 1.0 - pp;
  t = 1.0 - qq;
  yprev = 0.0;
  sq = 1.0;
  prev = 1.0;

  if ( value < 0.0001 )
    value = 0.0001;
  end

  if ( 0.9999 < value )
    value = 0.9999;
  end

  iex = max ( - 5.0 / pp / pp - 1.0 / a ^ 0.2 - 13.0, sae );

  acu = 10.0 ^ iex;
%
%  Iteration loop.
%
  while ( 1 )

    [ y, ifault ] = betain ( value, pp, qq, beta );

    if ( ifault ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'XINBTA - Fatal error!\n' );
      fprintf ( 1, '  BETAIN returns IFAULT = %d\n', ifault );
      error ( 'XINBTA - Fatal error!' );
      ifault = 3;
      return
    end

    xin = value;
    y = ( y - a ) * exp ( beta + r * log ( xin ) + t * log ( 1.0 - xin ) );

    if ( y * yprev <= 0.0 )
      prev = max ( sq, fpu );
    end

    g = 1.0;

    while ( 1 )

      while ( 1 )

        adj = g * y;
        sq = adj * adj;

        if ( sq < prev )

          tx = value - adj;

          if ( 0.0 <= tx && tx <= 1.0 )
            break
          end

        end

        g = g / 3.0;

      end
%
%  Check whether current estimate is acceptable.
%  The change "VALUE = TX" was suggested by Ivan Ukhov.
%
      if ( prev <= acu && y * y <= acu )
        value = tx;
        if ( indx )
          value = 1.0 - value;
        end
        return
      end

      if ( tx ~= 0.0 && tx ~= 1.0 )
        break
      end

      g = g / 3.0;

    end

    if ( tx == value )
      break
    end

    value = tx;
    yprev = y;

  end

  if ( indx )
    value = 1.0 - value;
  end

  return
end


function [M] = spm_nwcov (M)
% Get second moments of Normal-Wishart
% FORMAT [M] = spm_nwcov (M)
%
% .mean_prior_cov    Prior covariance of mean
% .sample_prior_cov  Prior covariance of samples
% .mean_post_cov     Posterior covariance of mean
% .sample_pred_cov   Predictive covariance of samples
%
% The latter quantity is also the covariance of the predictive density
% The marginal distributions of the mean and of the samples
% are multivariate-T, not Gaussian.
%
% See J. Bernardo and A. Smith (2000)
% Bayesian Theory, Wiley (page 435)
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_nwcov.m 6548 2015-09-11 12:39:47Z will $

% Get prior covariances
M.mean_prior_cov=M.B0/(M.n0*(M.a0-1));

alpha=2*M.a0-M.P+1;
w_s=(1+1/M.n0)/(0.5*alpha-1);
M.sample_prior_cov=w_s*M.B0;

% Get posterior covariances
M.mean_post_cov=M.BN/(M.nN*(M.aN-1));

alpha=2*M.aN-M.P+1;
w_s=(1+1/M.nN)/(0.5*alpha-1);
M.sample_pred_cov=w_s*M.BN;


function Q = ForwardStepBC2D(xin, yin, nxin, nyin, mapI, mapO, mapW, mapC, Q, time);

%  function [Q] = ForwardStepBC2D(xin, yin, nxin, nyin, mapI, mapO, mapW, mapC, Q, time);
% Purpose: Impose channel boundary conditions on 2D Euler equations on weak form

% Example is Mach ** 0.3 ** flow in wind tunnel
gamma = 1.4;

% extract conserved variables
rho = Q(:,:,1); rhou = Q(:,:,2); rhov = Q(:,:,3); Ener = Q(:,:,4);

% Inflow conditions -- uniform inflow
rhoin = gamma; uin = 3.0; vin = 0.0; pin = 1.0;
Ein = pin/(gamma-1.0) + 0.5*rhoin*(uin^2+vin^2);

rho(mapI) = rhoin; rhou(mapI) = rhoin*uin; rhov(mapI) = rhoin*vin; Ener(mapI) = Ein;

% Outflow conditions -- supersonic outflow ( do nothing )

% Wall conditions -- reflective, isothermal, i.e., n.u=0, T=T(t=0)
rhoW = rho(mapW); rhouW = rhou(mapW); rhovW = rhov(mapW);
nxW = nxin(mapW);   nyW = nyin(mapW);

% reverse flow in normal direction in ghost elements
rhou(mapW) = rhouW - 2*nxW.*(nxW.*rhouW + nyW.*rhovW);
rhov(mapW) = rhovW - 2*nyW.*(nxW.*rhouW + nyW.*rhovW);

% pack modified conserved variables
Q(:,:,1) = rho; Q(:,:,2) = rhou; Q(:,:,3) = rhov; Q(:,:,4) = Ener;
return


function W = nullspaceLUSOLapply2Modes(mode, m, n, V, nullS)
% Computes the matrix vector product with the operator nullspace
% function handle of the form `y = pdMat(mode, m, n, x)`
%
% USAGE:
%
%    W = nullspaceLUSOLapply2Modes(mode, m, n, V, nullS)
%
% INPUTS:
%    mode:     :math:`mode = 1` returns :math:`W = Z V`, `mode = 2` returns :math:`W = Z^T V`
%    m:        first dimension of the matrix
%    n:        second dimension of the matrix
%    V:        one of the components of the multiplication
%    nullS:    structure `nullS` from the function `nullspaceLUSOLform(S)`;
%              where `m x n` sparse matrix `S` (:math:`m < n`).
%
% OUTPUT:
%    W:        Matrix vector product
%
% .. 16 May 2008: (MAS) First version of nullspaceLUSOLapply.m. See nullspaceLUSOLtest.m for testing.
% .. 13 Mar 2009: (RF) Tried to add matrix vector product of the transpose for use with pdco

if mode==1
    %returns   W = Z*V (mode=1)

    % Second, if V is an (n-m) x k sparse matrix (k >= 1),
    %        W = nullspaceLUSOLapply(nullS,V);
    % computes an n x k sparse matrix W from V such that S*W = 0.
    %
    % This is an operator form of finding an n x (n-m) matrix Z
    % such that S*Z = 0 and then computing W = Z*V.
    % The aim is to obtain W without forming Z explicitly.

    % 16 May 2008: (MAS) First version of nullspaceLUSOLapply.m.
    %              See nullspaceLUSOLtest.m for testing.

    Cinv    = nullS.Cinv; % Column scales
    L       = nullS.L;    % Strictly triangular L
    p       = nullS.p;    % Column permutation for S
    rankS   = nullS.rank; % rank(S)

    [n ,n ] = size(L);
    [mV,nV] = size(V);

    B       = [sparse(rankS,nV)
                V        ];
    Z       = (L')\B;
    W       = Z;
    W(p,:)  = Z;
    W       = Cinv*W;
else
    %returns  W = Z'*V (mode=2).

    % This is an operator form of finding an n x (n-m) matrix Z
    % such that S*Z = 0 and then computing W = Z'*V.
    % The aim is to obtain W without forming Z explicitly.

    % When S is scaled as S = R*A*C, we have Z = Cinv*P'*inv(L')[0;I]

    % This was the maths I tried to follow -RF
    %     W   =   A'*V
    %         =   (Cinv*P'*inv(L')[0;I])'*V
    %         =   [0,I]'*inv(L)*P*Cinv'*V

    Cinv    = nullS.Cinv; % Column scales
    L       = nullS.L;    % Strictly triangular L
    p       = nullS.p;    % Column permutation for S
    rankS   = nullS.rank; % rank(S)

    [n ,n ] = size(L);

    V = Cinv'*V;

    % I am not sure how to use these permutations
    % e.g. if P*A = B then is it B(p,:)=A or B=A(p,:)
    V(p,:) = V;
%     V(fliplr(p),:) = V;

    W = L\V;

    W = W(rankS+1:n,:);

end

return


function dt_stability_limit = checkStability(kgrid, medium)
% CHECKSTABILITY   Compute maximum stable timestep for k-space models
%
% DESCRIPTION:
%       checkStability calculates the maximum time step for which the
%       k-space propagation models kspaceFirstOrder1D, kspaceFirstOrder2D
%       and kspaceFirstOrder3D are stable. These models are unconditionally
%       stable when the reference sound speed is equal to or greater than
%       the maximum sound speed in the medium and there is no absorption.
%       However, when the reference sound speed is less than the maximum
%       sound speed the model is only stable for sufficiently small time
%       steps. The criterion is more stringent (the time step is smaller)
%       in the absorbing case.
%
%       The time steps given are accurate when the medium properties are
%       homogeneous. For a heterogeneous media they give a useful, but not
%       exact, estimate.
%
%       The timesteps given are accurate when the medium properties are
%       homogeneous. For a heterogeneous media they give a useful, but not
%       exact, estimate.
%
% USAGE:
%       dt_stability_limit = checkStability(kgrid, medium)
%
% INPUTS:
%
%       kgrid              - structure returned by makeGrid holding grid parameters
%       medium             - structure holding the medium properties
%
% OUTPUTS:
%
%       dt_stability_limit - maximum timestep for stability. (Set to Inf
%                            when the model is unconditionally stable.)
%
% ABOUT:
%       author             - Ben Cox
%       date               - 12th August 2014
%       last update        - 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also kspaceFirstOrder1D, kspaceFirstOrder2D, kspaceFirstOrder3D,
% makeGrid, makeTime

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

% Find the maximum wavenumber
kmax = max(kgrid.k(:));

% calculate the reference sound speed for the fluid code, using the
% maximum by default which ensures the model is unconditionally stable
if isfield(medium, 'sound_speed_ref')
    if isnumeric(medium.sound_speed_ref)
        c_ref = medium.sound_speed_ref;
    elseif strcmp(medium.sound_speed_ref, 'min')
        c_ref = min(medium.sound_speed(:));
    elseif strcmp(medium.sound_speed_ref, 'mean')
        c_ref = mean(medium.sound_speed(:));
    else strcmp(medium.sound_speed_ref, 'max')
        c_ref = max(medium.sound_speed(:));
    end
else
    c_ref = max(medium.sound_speed(:));
end

% calculate the timesteps required for stability
if ~isfield(medium, 'alpha_coeff') || all(medium.alpha_coeff(:) == 0)

    % =====================================================================
    % NON-ABSORBING CASE
    % =====================================================================

    if c_ref >= max(medium.sound_speed(:))

        % set the timestep to Inf when the model is unconditionally stable.
        dt_stability_limit = Inf;

    else

        % set the timestep required for stability when c_ref~=max(medium.sound_speed(:))
        dt_stability_limit = 2/(c_ref * kmax) * asin(c_ref/max(medium.sound_speed(:)));

    end

else

    % =====================================================================
    % ABSORBING CASE
    % =====================================================================

    % convert the absorption coefficient to nepers.(rad/s)^-y.m^-1
    medium.alpha_coeff = db2neper(medium.alpha_coeff, medium.alpha_power);

    % calculate the absorption constant
    if ~(isfield(medium, 'alpha_mode') && strcmp(medium.alpha_mode, 'no_absorption'))
        absorb_tau = -2*medium.alpha_coeff.*medium.sound_speed.^(medium.alpha_power - 1);
    else
        absorb_tau = 0;
    end

    % calculate the dispersion constant
    if ~(isfield(medium, 'alpha_mode') && strcmp(medium.alpha_mode, 'no_dispersion'))
        absorb_eta = 2*medium.alpha_coeff.*medium.sound_speed.^(medium.alpha_power)*tan(pi*medium.alpha_power/2);
    else
        absorb_eta = 0;
    end

    % Estimate the timestep required for stability in the absorbing case by
    % assuming the k-space correction factor, kappa = 1;
    % (Note that absorb_tau and absorb_eta are negative quantities.)
    temp1 = max(medium.sound_speed(:)) * min(absorb_tau(:)) * kmax^(medium.alpha_power-1);
    temp2 = 1 - min(absorb_eta(:)) * kmax^(medium.alpha_power-1);
    dt_estimate = (temp1 + sqrt(temp1^2 + 4*temp2))/(temp2 * kmax * max(medium.sound_speed(:)));

    % Use a fixed point iteration to find the correct timestep, assuming
    % now that kappa = kappa(dt), using the previous estimate as a starting
    % point

    % First define the function to iterate
    kappa = @(dt) sinc(c_ref*kmax*dt/2);
    temp3 = @(dt) max(medium.sound_speed(:))*min(absorb_tau(:))*kappa(dt)*kmax^(medium.alpha_power-1);
    func_to_solve = @(dt) (temp3(dt) + sqrt((temp3(dt))^2 + 4*temp2 ))/(temp2*kmax*kappa(dt)*max(medium.sound_speed(:)));

    % run the fixed point iteration
    dt_stability_limit = dt_estimate;
    dt_old = 0;
    accuracy = 1e-12;
    while abs(dt_stability_limit-dt_old)>accuracy
        dt_old = dt_stability_limit;
        dt_stability_limit = func_to_solve(dt_stability_limit);
    end

end


function [logp, yhat, res] = tapas_beta_obs(r, infStates, ptrans)
% Calculates the log-probability of responses representing probabilities on the unit interval
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform nu-prime to its native space
nupr = exp(ptrans(1));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Predictions or posteriors?
if strcmp(r.c_prc.model,'tapas_rw_binary')
    mu = tapas_sgm(infStates(:,1), 1);
else
    mu = infStates(:,1,1); % Default: predictions (ie, mu1hat)
    if r.c_obs.predorpost == 2
        mu = tapas_sgm(infStates(:,2,3), 1); % Alternative: posteriors (ie, sgm(mu2))
    end
end

% Special cases
if strcmp(r.c_prc.model,'hgf_whichworld')
    mu = tapas_sgm(infStates(:,2,1,3), 1);
end
if strcmp(r.c_prc.model,'ph_binary')
    mu = infStates(:,2);
end

% Weed irregular trials out from inferred states and responses
mu(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];

% y has to be in the *open* unit interval
%y(y==0) = 1e-4;
%y(y==1) = 1-1e-4;
y = 0.95.*(y-0.5)+0.5; % Shrink all y values toward 1/2 by a factor of 0.95

% Nu is nu-prime plus two (sometimes)
%nu = nupr+2;
nu = nupr;

% Calculate alpha and beta from mu and nu
al = mu.*nu;
be = nu - al;

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = log(betaDens(y,al,be));
yhat(reg) = mu;
res(reg) = y-mu;

end

function p = betaDens(x,alpha,beta)
% Check whether x is in the unit interval
if any(x(:)<0) || any(x(:)>1)
    error('tapas:hgf:BetaObs:ArgNotInUnitIntrv', 'Error: first argument to betaDens must be in the unit interval.');
end
% Check whether alpha and beta are greater than 0
if any(alpha(:)<0) || any(beta(:)<0)
    error('tapas:hgf:BetaObs:AlphaOrBetaNeg', 'Error: alpha and beta have to be non-negative.');
end
% Calculate beta density
p = gamma(alpha+beta)./(gamma(alpha).*gamma(beta)).*x.^(alpha-1).*(1-x).^(beta-1);
end


function [IPPEPoses,refinedPoses,HHat,U,Q] = featureBasedPlanePoseFromImage(planarTemplate,inputImage,K,kc,featureOpts,ransacOpts,IPPEOpts,plottingOpts)
%featureBasedPlanePoseFromImage: An example of using IPPE to solve a plane's pose given a frontal 'template' image and a single input image. This follows the following pipeline:
%
%1. Detect features in the template and input image
%2. Match features based on their descriptor similarity
%3. Use the matched features to robustly estimate the homography between the template image and the input image.
%4. From the inlier matches, use IPPE to estimate the camera's pose.
%5. (Optionally) Refine the camera's pose with Levenberg–Marquardt.
%
% The camera should be intrinsically calibrated a priori. We use a perspective camera model with lens distortion, which
% is the standard model and used in OpenCV and Bouguet's Matlab camera calibration toolbox
%(http://www.vision.caltech.edu/bouguetj/calib_doc/). This is parameterised by a 3x3 calibration matrix K and a 5x1 distortion vector
%kc. You can determine K and kc by running Bouguet's calibration toolbox using a checkerboard calibration target.
%
%Inputs
%
%planarTemplate: a structure holding the planar object used to compute the
%camera's pose. See makePlanarTemplate.m for a description of its fields.
%
%inputImage: a 2D unit8 input image (grayscale or rgb). This is an image of the plane viewed
%from the camera.
%
%K: The input image's 3x3 intrinsic matrix (5x1 double).
%
%kc: The input image's distortion parameters (5x1 double)
%
%Note that when using ASIFT, it is best to use an undistorted image (i.e.
%the effects of lens distortion are undone). This is because ASIFT does som
%match filtering based on epipolar geometry, and does not handle lens
%distortion.
%
%featureOpts: Options for feature detection and matching. See
%detectAndMatchFeatures.m for details.
%
%ransacOpts: Options for ransac. See basicHomographyRansac.m for details.
%detectAndMatchFeatures.m for details.
%
%IPPEOpts: Options for IPPE pose estimation. See perspectiveIPPE.m for details.
%
%plottingOpts: Options for plotting the fitted homography. This has two
%fields:
%    plottingOpts.doPlot (true or false): true if we want to visualise
%    results, false otherwise
%    plottingOpts.figId (positive integer): the figure number for plotting the results.
%
%Outputs:
%IPPEPoses: structure containing the two pose solutions from IPPE. See
%perspectiveIPPE.m for details.
%
%refinedPoses: structure containing the two refined pose solutions using
%Levenberg–Marquardt (same format as IPPEPoses). See
%perspectiveIPPE.m for details.
%
%HHat is the homography matrix outputted from ransac (3x3 double). Note
%that this is the mapping from the template's image to the input image's
%normalised pixel coordinates.
%
%poseResults.U is the set of inlier correspondences detected in the template image (in mm) (2xN double)
%
%poseResults.Q is the set of inlier correspondences detected in the input image (in normalised pixels) (2xN double)
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of the IPPE package for fast plane-based pose
% estimation from the paper "Infinitesimal Plane-based Pose Estimation" by Toby Collins and Adrien Bartoli,
% published in the International Journal of Computer Vision, September
% 2014. A copy of the author's pre-print version can be found here:
%
% http://isit.u-clermont1.fr/~ab/Publications/Collins_Bartoli_IJCV14.pdf
%
% This package is free and covered by the BSD licence without any warranty. We hope you find this code useful and please cite our paper in your work:
% (c) Toby Collins 2015
%
%
%
%@article{
%year={2014},
%issn={0920-5691},
%journal={International Journal of Computer Vision},
%volume={109},
%number={3},
%doi={10.1007/s11263-014-0725-5},
%title={Infinitesimal Plane-Based Pose Estimation},
%url={http://dx.doi.org/10.1007/s11263-014-0725-5},
%publisher={Springer US},
%keywords={Plane; Pose; SfM; PnP; Homography},
%author={Collins, Toby and Bartoli, Adrien},
%pages={252-286},
%language={English}
%}
%
%
%basic argument checking:
assert(isa(inputImage,'uint8'));
assert(size(K,1)==3);
assert(size(K,2)==3);
assert(size(kc,1) == 5);
assert(size(kc,2) == 1);

%first detect and match features:
[p,q] = detectAndMatchFeatures(planarTemplate.rectifiedImage_g,planarTemplate.roi,...
    inputImage,[],featureOpts);
qNormalised = normaliseImagePoints(q,K,kc);

%now estimate the homography with RANSAC:
[HHat,inliers] = basicHomographyRansac(p,qNormalised,ransacOpts);

%lets plot the results:
if plottingOpts.doPlot
    visualiseHomography(planarTemplate.rectifiedImage_g,planarTemplate.roi,inputImage,HHat,K,kc,p,q,inliers,plottingOpts.figId);
end

%Now use IPPE to estimate the camera's pose.

%First get the points on the template image into mm, and reject outliers:
U = planarTemplate.templatImgPixelSize*p(:,inliers);
Q = qNormalised(:,inliers);

[IPPEPoses,refinedPoses] = perspectiveIPPE(U,Q,[],IPPEOpts);


function [U_final, V_final, nIter_final, objhistory_final] = LCCF(X, k, W, options, U, V)
% Locally Consistant Concept Factorization (LCCF)
%
% 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
%
% You only need to provide the above four inputs.
%
% X = X*U*V'
%
% References:
% [1] Deng Cai, Xiaofei He, Jiawei Han, "Locally Consistent Concept
%     Factorization for Document Clustering", IEEE Transactions on Knowledge
%     and Data Engineering, Vol. 23, No. 6, pp. 902-913, 2011.
%
%
%   version 2.0 --April/2010
%   version 1.0 --Dec./2008
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

nSmp=size(X,2);

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

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

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

K = constructKernel(X',[],options);

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

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

switch lower(options.Optimization)
    case {lower('Multiplicative')}
        [U_final, V_final, nIter_final,objhistory_final] = LCCF_Multi(K, k, W, options, U, V);
    otherwise
        error('optimization method does not exist!');
end