function [S,D,J] = sample_discrete(D,n)
  % SAMPLE_DISCRETE  Sample a discrete distribution given by D n times. This
  % employs the "Alias Table Method". Ensuring that the running time is O(n +
  % m)
  %
  % Inputs:
  %   D  m list of probabilities summing to 1
  %   n  number of samples to output
  % Outputs:
  %   S  n list of indices into D
  %   D  m list of alias table heights
  %   J  m list of "upper half" reindices
  %

  % Assume column vector
  D = D(:);
  % https://hips.seas.harvard.edu/blog/2013/03/03/the-alias-method-efficient-sampling-with-many-discrete-outcomes/
  m = numel(D);
  assert(abs(sum(D) - 1)<1e-15);
  % We want to sort based on above or below 1/n
  D = D*m;
  % now we can sort based on above or below 1
  % Below queue: http://www.alecjacobson.com/weblog/?p=3933
  % and preallocate up to 2*m (extra m to be safe, I don't think it's needed)
  B = find(D<1);
  Bl = numel(B);
  B(end+1:2*m) = 0;
  % After queue, and preallocate up to m
  A = find(D>=1);
  Al = numel(A);
  A(end+1:m) = 0;
  % Upper half list (initialize with this index because above includes "equal")
  J = (1:m)';

  % While below and above still have elements
  while Bl>0 && Al>0
    % pop below
    b = B(Bl);
    Bl = Bl - 1;
    % Look above
    a = A(Al);
    % a going to be b's upper half
    J(b) = a;
    % lob off enough to fit D(b) to 1
    %D(a) = D(a) - (1.0 - D(b));
    % Reduce rounding error (see comments above)
    D(a) = (D(a) + D(b)) - 1.0;
    if D(a) < 1
      % pop off above
      Al = Al - 1;
      % push onto below
      Bl = Bl+1;
      B(Bl) = a;
    % else still on above
    end
  end

  R1 = floor(rand(n,1)*m)+1;
  R2 = rand(n,1);
  I = (1:m)';
  % select those that are in upper half
  upper = R2>=D(R1);
  S = I(R1);
  S(upper) = J(R1(upper));

end


function UNew = fluidDirichlet2D(varargin);
% fluidDirichlet2D: solve fluid registraion in 2D with Dirichlet
%        boundary conditions
%
%
% author: Nathan D. Cahill
% email: nathan.cahill@rit.edu
% affiliation: Rochester Institute of Technology
% date: January 2014
% licence: GNU GPL v3
%
% Copyright Nathan D. Cahill
% Code available from https://github.com/tomdoel/npReg
%
%

% parse input arguments
[DU,F,mu,lambda,PixSize,NumPix,HX,HY] = parse_inputs(varargin{:});

% construct filters that implement discretized Navier-Lame equations
d1 = [1;-2;1]/(PixSize(1)^2);
d2 = [1 -2 1]/(PixSize(2)^2);
d12 = [1 0 -1;0 0 0;-1 0 1]/(4*PixSize(1)*PixSize(2));

[A11,A22] = deal(zeros(3,3));
A11(:,2) = A11(:,2) + (lambda+2*mu)*d1;
A11(2,:) = A11(2,:) + mu*d2;
A22(:,2) = A22(:,2) + mu*d1;
A22(2,:) = A22(2,:) + (lambda+2*mu)*d2;

A12 = d12*(lambda+mu)/4;
A21 = A12;

% multiply force field by adjoint of Navier-Lame equations
Fnew = zeros(NumPix(1),NumPix(2),2);
Fnew(:,:,1) = imfilter(F(:,:,1),A22,'replicate') - imfilter(F(:,:,2),A12,'replicate');
Fnew(:,:,2) = imfilter(F(:,:,2),A11,'replicate') - imfilter(F(:,:,1),A21,'replicate');

% compute sine transform of new force field
FnewF1 = imag(fft(imag(fft(Fnew(:,:,1),2*NumPix(1)-2,1)),2*NumPix(2)-2,2));
FnewF2 = imag(fft(imag(fft(Fnew(:,:,2),2*NumPix(1)-2,1)),2*NumPix(2)-2,2));
FnewF1 = FnewF1(1:NumPix(1),1:NumPix(2));
FnewF2 = FnewF2(1:NumPix(1),1:NumPix(2));

% construct images of coordinates scaled by pi/(N or M)
[alpha,beta] = ndgrid(pi*(0:(NumPix(1)-1))/(NumPix(1)-1),pi*(0:(NumPix(2)-1))/(NumPix(2)-1));

% construct LHS factor
LHSfactor = mu.*(lambda+2*mu).*(2*cos(alpha) + 2*cos(beta) - 4).^2;

% set origin term to 1, as DC term does not matter
LHSfactor(1,1) = 1;

% solve for FFT of V
VF1 = FnewF1./LHSfactor;
VF2 = FnewF2./LHSfactor;

% perform inverse DST
V1 = imag(ifft(imag(ifft(VF1,2*NumPix(1)-2,1)),2*NumPix(2)-2,2));
V2 = imag(ifft(imag(ifft(VF2,2*NumPix(1)-2,1)),2*NumPix(2)-2,2));

% crop and concatenate
V = cat(3,V1(1:NumPix(1),1:NumPix(2)),V2(1:NumPix(1),1:NumPix(2)));

% construct estimate of transformation Jacobian
J = zeros(NumPix(1),NumPix(2),2,2);
J(:,:,1,1) = 1 - imfilter(V(:,:,1),HX,'replicate','same');
J(:,:,2,1) = -imfilter(V(:,:,1),HY,'replicate','same');
J(:,:,1,2) = -imfilter(V(:,:,2),HX,'replicate','same');
J(:,:,2,2) = 1 - imfilter(V(:,:,2),HY,'replicate','same');

% now perform Euler integration to construct new displacements
UNew = zeros(NumPix(1),NumPix(2),2);
UNew(:,:,1) = J(:,:,1,1).*V(:,:,1) + J(:,:,1,2).*V(:,:,2);
UNew(:,:,2) = J(:,:,2,1).*V(:,:,1) + J(:,:,2,2).*V(:,:,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [DU,F,mu,lambda,PixSize,NumPix,HX,HY] = parse_inputs(varargin);

% get arguments
F = varargin{2};
PixSize = varargin{4}(1:2);
NumPix = [varargin{5} varargin{6}];
mu = varargin{8};
lambda = varargin{9};
DU = varargin{11};
HX = varargin{12};
HY = varargin{13};
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Performs fast detrended fluctuation analysis on a nonstationary input signal to
% obtain an estimate for the scaling exponent.
%
% Useage:
% [alpha, intervals, flucts] = fastdfa(x)
% [alpha, intervals, flucts] = fastdfa(x, intervals)
% Inputs
%    x          - input signal: must be a column vector
% Optional inputs
%    intervals  - List of sample interval widths at each scale
%                 (If not specified, then a binary subdivision is constructed)
%
% Outputs:
%    alpha      - Estimated scaling exponent
%    intervals  - List of sample interval widths at each scale
%    flucts     - List of fluctuation amplitudes at each scale
%
% (c) 2006 Max Little. If you use this code, please cite:
% M. Little, P. McSharry, I. Moroz, S. Roberts (2006),
% Nonlinear, biophysically-informed speech pathology detection
% in Proceedings of ICASSP 2006, IEEE Publishers: Toulouse, France.
%

function [alpha, intervals, flucts] = ML_fastdfa(x, varargin)

[xpts, ypts] = ML_fastdfa_core(x, varargin{:});

% Sort the intervals, and produce a log-log straight line fit
datapts   = sortrows([xpts, ypts],1);
intervals = datapts(:,1);
flucts    = datapts(:,2);

coeffs    = polyfit(log10(xpts), log10(ypts), 1);
alpha     = coeffs(1);

end


%% Set up paths

path(path,'mesh_functions/');
path(path,'../data');

%% Read shape

[X,T] = readOff('../data/meshes/moomoo_s0.off');
M = getMeshData(X,T,10); % compute 10 LB eigenfunctions for fun

%% Set up Gaussian blur function

blurTime = .001; % if this gets too small, distances get noisy
blurSteps = 3;

% h = blurTime/blurSteps;

blur = @(x) blurOnMesh(x,M,blurTime,blurSteps); % faster than pre-factored?
blurTranspose = @(x) blurOnMesh(x,M,blurTime,blurSteps,1);

%% Design a few functions to average

centerVerts = [300 100 600];
nFunctions = length(centerVerts);

distributions = zeros(M.numVertices,nFunctions);

for i=1:nFunctions
    distributions(centerVerts(i),i) = 1./M.areaWeights(centerVerts(i));
end

distributions = blur(distributions);

close all
for i=1:nFunctions
    f = subplot(1,nFunctions,i);
    showDescriptor(M,distributions(:,i),[],[],[],f);
    colorbar off;
    title(sprintf('Distribution %d',i));
end

%% Take the barycenter

euclideanBarycenter = sum(distributions,2)/nFunctions;

f = subplot(1,2,1);
showDescriptor(M,euclideanBarycenter,[],[],[],f);
title('Euclidean barycenter');
colorbar off;

alpha = [1 1 1];
barycenter = convolutionalBarycenter(distributions,alpha,M.areaWeights,blur,blurTranspose);

f = subplot(1,2,2);
showDescriptor(M,barycenter,[],[],[],f);
title('Wasserstein barycenter');
colorbar off;

%% Test different entropy limits

averageEntropy = -mean(sum(bsxfun(@times,distributions.*log(distributions),M.areaWeights),1));

entropyChanges = [-1.5 -1 -.5 0 .5 1 1.5];

euclideanBarycenter = sum(distributions,2)/nFunctions;

f = subplot(1,length(entropyChanges)+1,1);
showDescriptor(M,euclideanBarycenter,[],[],[],f);
title('Euclidean barycenter');
colorbar off;

for i=1:length(entropyChanges)
    targetEntropy = averageEntropy + entropyChanges(i);

    alpha = [1 1 1];
    barycenter = convolutionalBarycenter(distributions,alpha,M.areaWeights,blur,blurTranspose,targetEntropy);

    f = subplot(1,length(entropyChanges)+1,i+1);
    showDescriptor(M,barycenter,[],[],[],f);
    title(sprintf('entropy < average+(%g)',entropyChanges(i)));
    colorbar off;

    drawnow;
end

%% Try displacement interpolation

nTimeSteps = 100;

p1 = distributions(:,1);
p2 = distributions(:,2);

interp = zeros(M.numVertices,nTimeSteps);
for i=1:nTimeSteps
    fprintf('i = %d/%d\n',i,nTimeSteps);
    t = (i-1)/(nTimeSteps-1);
    alpha = [t 1-t]; % is this right?
    interp(:,i) = convolutionalBarycenter([p1 p2],alpha,M.areaWeights,blur,blurTranspose,averageEntropy);
end

%% Animate the result

f = figure;
for i=1:nTimeSteps
    clf;
    showDescriptor(M,interp(:,i),[],[],[],f);
    colorbar off;
    drawnow;
end


function subRoPS =  subRoPSFunc(projNeighbor,binSize)
%get the distribution matrix
neighbNum = length(projNeighbor);
distrMatrix = zeros(binSize,binSize);
minX = min(projNeighbor(:,1));
stepX = (max(projNeighbor(:,1))-minX)/binSize;
minY = min(projNeighbor(:,2));
stepY = (max(projNeighbor(:,2))-minY)/binSize;

if stepX==0 || stepY==0
    subRoPS = [0,0,0,0,0];
    return;
end

for k=1:neighbNum
    idxX = ceil((projNeighbor(k,1) - minX)/stepX);
    idxY = ceil((projNeighbor(k,2) - minY)/stepY);
    if idxX>binSize      idxX = binSize;  end
    if idxX<1                idxX = 1;            end
    if idxY>binSize     idxY = binSize;   end
    if idxY<1                idxY = 1;            end
    distrMatrix(idxX,idxY) = distrMatrix(idxX,idxY)+1;
end
distrMatrix = distrMatrix/neighbNum;%normalization
%calculate the moment of this distribution matrix
meanX = 0;
meanY = 0;
pde = 0;
for idxX = 1:binSize
    for idxY = 1:binSize
        meanX = meanX+idxX*distrMatrix(idxX,idxY);
        meanY = meanY+idxY*distrMatrix(idxX,idxY);
        if distrMatrix(idxX,idxY)>0
            pde = pde - distrMatrix(idxX,idxY)*log2(distrMatrix(idxX,idxY));
        end
    end
end
u11 = 0;
u21 = 0;
u12 = 0;
u22 = 0;

for idxX = 1:binSize
    for idxY = 1:binSize
        u11 = u11+(idxX-meanX)*(idxY-meanY)*distrMatrix(idxX,idxY);
        u21 = u21+(idxX-meanX)^2*(idxY-meanY)*distrMatrix(idxX,idxY);
        u12 = u12+(idxX-meanX)*(idxY-meanY)^2*distrMatrix(idxX,idxY);
        u22 = u22+(idxX-meanX)^2*(idxY-meanY)^2*distrMatrix(idxX,idxY);
    end
end
 subRoPS = [u11,u21,u12,u22,pde];


% This library is coded for the double pendulum example
% Last Updated: 2019/07/30
% Coded By: K.Kahirman

function [Data,Sym_Struct]=SINDyLib(X,dX,iter,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order)
%% First get the size of the X matrix, determin the data length and the number of variables we have.
[Data_Length,Variable_Number]=size(X);
[~,Variable_Number_dX]=size(dX);
[~,Variable_Number_u]=size(u);
%Also create the symbolic variable
Symbol=sym('z',[Variable_Number,1]);
Symbol_dX=sym('dz',[Variable_Number,1]);
Symbol_u=sym('u',[Variable_Number_u,1]);

%Now according the Highest Polynomial Order entered, we will calculate the data matrix.
Data=[];
Index=1;

%% First calculate the polynomial term

% Form basis vector
Basis=[X(:,1) X(:,2) X(:,1)-X(:,2) X(:,1)-2*X(:,2) 2*X(:,1)-X(:,2) 2*X(:,1)-2*X(:,2)];
Basis_Sym=[Symbol(1) Symbol(2) Symbol(1)-Symbol(2) Symbol(1)-2*Symbol(2) 2*Symbol(1)-Symbol(2) 2*Symbol(1)-2*Symbol(2)];

% Add the trigonometric form
for i=1:size(Basis,2)
   Data(:,Index)=sin(Basis(:,i));
   Sym_Struct{1,Index}=sin(Basis_Sym(1,i));
   Index=Index+1;
end

for i=1:size(Basis,2)
   Data(:,Index)=cos(Basis(:,i));
   Sym_Struct{1,Index}=cos(Basis_Sym(1,i));
   Index=Index+1;
end

for i=3:size(Basis,2)
   Data(:,Index)=cos(Basis(:,i)).^2;
   Sym_Struct{1,Index}=cos(Basis_Sym(1,i))^2;
   Index=Index+1;
end

% Adding following terms will reduce the noise robustness
% for i=3:size(Basis,2)
%    Data(:,Index)=sin(Basis(:,1)).*cos(Basis(:,i));
%    Sym_Struct{1,Index}=sin(Basis_Sym(1,1))*cos(Basis_Sym(1,i));
%    Index=Index+1;
% end
%
% for i=3:size(Basis,2)
%    Data(:,Index)=cos(Basis(:,1)).*cos(Basis(:,i));
%    Sym_Struct{1,Index}=cos(Basis_Sym(1,1))*cos(Basis_Sym(1,i));
%    Index=Index+1;
% end
%
% for i=3:size(Basis,2)
%    Data(:,Index)=sin(Basis(:,2)).*cos(Basis(:,i));
%    Sym_Struct{1,Index}=sin(Basis_Sym(1,2))*cos(Basis_Sym(1,i));
%    Index=Index+1;
% end
%
% for i=3:size(Basis,2)
%    Data(:,Index)=cos(Basis(:,2)).*cos(Basis(:,i));
%    Sym_Struct{1,Index}=cos(Basis_Sym(1,2))*cos(Basis_Sym(1,i));
%    Index=Index+1;
% end

% Add polynomial term
Data(:,Index)=X(:,3);
Sym_Struct{1,Index}=Symbol(3);
Index=Index+1;

Data(:,Index)=X(:,4);
Sym_Struct{1,Index}=Symbol(4);
Index=Index+1;

for i=3:size(Basis,2)
   Data(:,Index)=X(:,3).*sin(Basis(:,i));
   Sym_Struct{1,Index}=Symbol(3)*sin(Basis_Sym(1,i));
   Index=Index+1;
end

for i=3:size(Basis,2)
   Data(:,Index)=X(:,4).*sin(Basis(:,i));
   Sym_Struct{1,Index}=Symbol(4)*sin(Basis_Sym(1,i));
   Index=Index+1;
end

for i=3:size(Basis,2)
   Data(:,Index)=X(:,3).^2.*sin(Basis(:,i));
   Sym_Struct{1,Index}=Symbol(3)^2*sin(Basis_Sym(1,i));
   Index=Index+1;
end

for i=3:size(Basis,2)
   Data(:,Index)=X(:,4).^2.*sin(Basis(:,i));
   Sym_Struct{1,Index}=Symbol(4)^2*sin(Basis_Sym(1,i));
   Index=Index+1;
end

% Add dx term
Data(:,Index)=dX(:,1);
Sym_Struct{1,Index}=Symbol_dX(iter);
Index=Index+1;

for i=3:size(Basis,2)
   Data(:,Index)=dX(:,1).*cos(Basis(:,i)).^2;
   Sym_Struct{1,Index}=Symbol_dX(iter)*cos(Basis_Sym(1,i)).^2;
   Index=Index+1;
end

% Add constant
%Order zero:
Data(:,Index)=ones(Data_Length,1);
Sym_Struct{1,Index}=1;


% =========================================================================
% NCSR for image denoising, Version 1.0
% Copyright(c) 2013 Weisheng Dong, Lei Zhang, Guangming Shi, and Xin Li
% All Rights Reserved.
%
% ----------------------------------------------------------------------
% Permission to use, copy, or modify this software and its documentation
% for educational and research purposes only and without fee is here
% granted, provided that this copyright notice and the original authors'
% names appear on all copies and supporting documentation. This program
% shall not be used, rewritten, or adapted as the basis of a commercial
% software or hardware product without first obtaining permission of the
% authors. The authors make no representations about the suitability of
% this software for any purpose. It is provided "as is" without express
% or implied warranty.
%----------------------------------------------------------------------
%
% This is an implementation of the algorithm for image interpolation
%
% Please cite the following paper if you use this code:
%
% Weisheng Dong, Lei Zhang, Guangming Shi, and Xin Li.,"Nonlocally
% centralized sparse representation for image restoration", IEEE Trans. on
% Image Processing, vol. 22, no. 4, pp. 1620-1630, Apr. 2013.
%
%--------------------------------------------------------------------------
function [pos_arr, wei_arr]  =  Block_matching(im, par)
S         =  30;
f         =  par.win;
f2        =  f^2;
nv        =  par.nblk;
s         =  par.step;
hp        =  max(12*par.nSig, par.hp);

N         =  size(im,1)-f+1;
M         =  size(im,2)-f+1;
r         =  [1:s:N];
r         =  [r r(end)+1:N];
c         =  [1:s:M];
c         =  [c c(end)+1:M];
L         =  N*M;
X         =  zeros(f*f, L, 'single');

k    =  0;
for i  = 1:f
    for j  = 1:f
        k    =  k+1;
        blk  =  im(i:end-f+i,j:end-f+j);
        X(k,:) =  blk(:)';
    end
end

I     =   (1:L);
I     =   reshape(I, N, M);
N1    =   length(r);
M1    =   length(c);
pos_arr   =  zeros(nv, N1*M1 );
wei_arr   =  zeros(nv, N1*M1 );
X         =  X';

for  i  =  1 : N1
    for  j  =  1 : M1

        row     =   r(i);
        col     =   c(j);
        off     =  (col-1)*N + row;
        off1    =  (j-1)*N1 + i;

        rmin    =   max( row-S, 1 );
        rmax    =   min( row+S, N );
        cmin    =   max( col-S, 1 );
        cmax    =   min( col+S, M );

        idx     =   I(rmin:rmax, cmin:cmax);
        idx     =   idx(:);
        B       =   X(idx, :);
        v       =   X(off, :);

        dis     =   (B(:,1) - v(1)).^2;
        for k = 2:f2
            dis   =  dis + (B(:,k) - v(k)).^2;
        end
        dis   =  dis./f2;
        [val,ind]   =  sort(dis);
        dis(ind(1))  =  dis(ind(2));

        wei         =  exp( -dis(ind(1:nv))./hp );
        wei         =  wei./(sum(wei)+eps);
        indc        =  idx( ind(1:nv) );
        pos_arr(:,off1)  =  indc;
        wei_arr(:,off1)  =  wei;
    end
end
pos_arr  = pos_arr';
wei_arr  = wei_arr';


%% FUNCTION bsa_ihb
%    Singular Projection
%
%% OBJECTIVE
%   min 1/2*||x - a||_2^2
%    s.t. b'*x = r, 0<= x <= u,  b > 0
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% Related papers
%
% [1] KC. Kiwiel. On linear-time algorithms for the continuous
%     quadratic knapsack problem, Journal of Optimization Theory
%     and Applications, 2007
%

function [x_star,t_star,iter] = bsa_ihb(a,b,r,u)

% initilization
break_flag = 0;
t_l = a./b; t_u = (a - u)./b;
T = [t_l;t_u];
t_L = -inf; t_U = inf;
g_tL = 0; g_tU = 0;

iter = 0;
while ~isempty(T)
    iter = iter + 1;
    g_t = 0;
    t_hat = median(T);

    U = t_hat < t_u;
    M = (t_u <= t_hat) & (t_hat <= t_l);

    if sum(U)
       g_t = g_t + b(U)'*u(U);
    end
    if sum(M)
        g_t = g_t + sum(b(M).*(a(M) - t_hat*b(M)));
    end

    if g_t > r
        t_L = t_hat;
        T = T(T > t_hat);
        g_tL = g_t;
    elseif g_t < r
        t_U = t_hat;
        T = T(T < t_hat);
        g_tU = g_t;
    else
        t_star = t_hat;
        break_flag = 1;
        break;
    end
end
if ~break_flag
     t_star = t_L - (g_tL -r)*(t_U - t_L)/(g_tU - g_tL);
end
x_star = min(max(0,a - t_star*b),u);
end


function MS = createMesh2D(varargin)
% MeshStructure = createMesh2D(Nx, Ny, Lx, Ly)
% MeshStructure = createMesh2D(facelocationX, facelocationY)
% creates a uniform 2D mesh:
% Nx is the number of cells in x (horizontal) direction
% Ny is the number of cells in y (vertical) direction
% Lx is the domain length in x direction
% Ly is the domain length in y direction
%
% SYNOPSIS:
%   MeshStructure = createMesh2D(Nx, Ny, Lx, Ly)
%
% PARAMETERS:
%   Nx: number of cells in the x direction
%   Lx: domain length in x direction
%   Ny: number of cells in the y direction
%   Ly: domain length in y direction
%
% RETURNS:
%   MeshStructure.
%                 dimensions=2 (2D problem)
%                 numbering: shows the indexes of cellsn from left to right
%                 and top to bottom
%                 cellsize: x and y elements of the cell size =[Lx/Nx,
%                 Ly/Ny]
%                 cellcenters.x: location of each cell in the x direction
%                 cellcenters.y: location of each cell in the y direction
%                 facecenters.x: location of interface between cells in the
%                 x direction
%                 facecenters.y: location of interface between cells in the
%                 y direction
%                 numberofcells: [Nx, Ny]
%
%
% EXAMPLE:
%   Nx = 5;
%   Ny = 7;
%   Lx = 10;
%   Ly = 20;
%   m = createMesh2D(Nx, Ny, Lx, Ly);
%   [X, Y] = ndgrid(m.cellcenters.x, m.cellcenters.y);
%   [Xf,Yf]=ndgrid(m.facecenters.x, m.facecenters.y);
%   plot(X, Y, 'or', ...
%        Xf, Yf, '-b', Xf', Yf', '-b');
%
% SEE ALSO:
%     createMesh1D, createMesh3D, createMeshCylindrical1D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==4
  % uniform 1D mesh
  Nx=varargin{1};
  Ny=varargin{2};
  Width=varargin{3};
  Height=varargin{4};
  % cell size is dx
  dx = Width/Nx;
  dy = Height/Ny;
  G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
  CellSize.x= dx*ones(Nx+2,1);
  CellSize.y= dy*ones(Ny+2,1);
  CellSize.z= [0.0];
  CellLocation.x= [1:Nx]'*dx-dx/2;
  CellLocation.y= [1:Ny]'*dy-dy/2;
  CellLocation.z= [0.0];
  FaceLocation.x= [0:Nx]'*dx;
  FaceLocation.y= [0:Ny]'*dy;
  FaceLocation.z= [0.0];
elseif nargin==2
  % nonuniform 1D mesh
  facelocationX=varargin{1};
  facelocationY=varargin{2};
  facelocationX=facelocationX(:);
  facelocationY=facelocationY(:);
  Nx = length(facelocationX)-1;
  Ny = length(facelocationY)-1;
  G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
  CellSize.x= [facelocationX(2)-facelocationX(1); ...
    facelocationX(2:end)-facelocationX(1:end-1); ...
    facelocationX(end)-facelocationX(end-1)];
  CellSize.y= [facelocationY(2)-facelocationY(1); ...
    facelocationY(2:end)-facelocationY(1:end-1); ...
    facelocationY(end)-facelocationY(end-1)];
  CellSize.z= [0.0];
  CellLocation.x= 0.5*(facelocationX(2:end)+facelocationX(1:end-1));
  CellLocation.y= 0.5*(facelocationY(2:end)+facelocationY(1:end-1));
  CellLocation.z= [0.0];
  FaceLocation.x= facelocationX;
  FaceLocation.y= facelocationY;
  FaceLocation.z= [0.0];
end
c=G([1,end], [1,end]);
MS=MeshStructure(2, ...
  [Nx,Ny], ...
  CellSize, ...
  CellLocation, ...
  FaceLocation, ...
  c(:), ...
  [1]);


function model = kpca_train(X,varargin)
% DESCRIPTION
% Kernel principal component analysis (KPCA)
%
%       model = kpca_train(X,varargin)
%
% INPUT
%   X            Training samples (N*d)
%                N: number of samples
%                d: number of features
%
% OUTPUT
%   model        KPCA model
%
%
% Created on 18th April 2019, by Kepeng Qiu.
%-------------------------------------------------------------%

% Default Parameters setting
options.sigma = 10;   % kernel width
options.dims  = 2;    % output dimension (dimensionality reduction)
                      % 2D data is easy to visualize
options.type  = 0;    % 0: dimensionality reduction or feature extraction
                      % 1: fault detection using KPCA
                      % 2: fault detection using dynamic KPCA(DKPCA)
options.beta  = 0.9;  % corresponding probabilities (for fault detection)
                      %
options.pcr   = 0.65; % principal contribution rate (for fault detection)
options.fd    = 0;    % 0: No fault diagnosis (fd)
                      % 1: fault diagnosis (fd)
options.lag   = 4;    % time lag (for DKPCA)
%

if rem(nargin-1,2)
    error('Parameters to kpca_train should be pairs')
end
numParameters = (nargin-1)/2;

for n =1:numParameters
    Parameters = varargin{(n-1)*2+1};
    value	= varargin{(n-1)*2+2};
    switch Parameters
            %
        case 'type'
            options.type = value;
            %
        case 'dims'
            options.dims = value;
            %
        case 'sigma'
            options.sigma = value;
            %
        case 'fd'
            options.fd = value;
            %
        case 'lag'
            options.lag = value;
            %
        case 'pcr'
            options.pcr = value;
            %
        case 'beta'
            options.beta = value;
    end
end

% number of training samples
L = size(X,1);

% DPCA
if options.type == 2
    %  Construct the augmented matrix
    X = constructAM(X,options.lag);
    L = size(X,1);
    model.lag = options.lag; % time lag
end

% Compute the kernel matrix
K = computeKM(X,X,options.sigma);

% Centralize the kernel matrix
unit = ones(L,L)/L;
K_c = K-unit*K-K*unit+unit*K*unit;

% Solve the eigenvalue problem
[V,D] = eigs(K_c/L);
% [V,D] = eig(K_c/L);
lambda = diag(D);

% Normalize the eigenvalue
V_s = V ./ sqrt(L*lambda)';
%  Lower version of MATLAB may report an error at line 70,
%  please replace it with the following code.
% ------------------------------
% for i = 1:size(lambda,1)
%     V_s(:,i) = V(:,i)/sqrt(L*lambda(i,1));
% end
% ------------------------------

% Compute the numbers of principal component
if options.type  == 1 || options.type  == 2  % fault detection
    dims = find(cumsum(lambda/sum(lambda)) >= ...
        options.pcr,1, 'first');
else % dimensionality reduction or feature extraction
    dims = options.dims;
end

% Extract the nonlinear component
mappedX  = K_c* V_s(:,1:dims) ;

% Store the results
model.mappedX =  mappedX ;
model.V_s = V_s;
model.lambda = lambda;
model.K_c = K_c;
model.L = L;
model.dims = dims;
model.X = X;
model.K = K;
model.unit = unit;
model.sigma = options.sigma;
model.diagnosis = options.fd;
model.type = options.type;

% Compute the threshold for fault detection
if options.type  == 1 || options.type  == 2
    model.beta = options.beta; % corresponding probabilities
    [SPE_limit,T2_limit] = comtupeLimit(model);
    model.SPE_limit = SPE_limit;
    model.T2_limit = T2_limit;
end

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [ D ] = dctn( A, varargin )
%
% N-D Discrete Cosine Transform
%
% Computes the N-dimensional dct of an array, by applying the
% one-dimnesional dct along all dimensions.
%
% Input:
%
%   A        - full N-dimensional array
%   varargin - (optional flag in which dimensions to apply the dct)
%
% Output:
%
%   D        - discrete cosine transform of A
%
%==============================================================================

function [ D ] = dctn( A, varargin )

if nargin==0
    help(mfilename)
    return;
end

% size if the input array
m = size(A);
dim = length(m);

% default parameters
dimFlag = ones(1,dim);  % apply dct in all dimensions by default

% overwrites default parameter
for k=1:2:length(varargin),
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

m = size(A);
D = A;
md = m;
P = circshift(1:dim,[0 -1]);

for d=1:dim
    if dimFlag(d)
        D = reshape(D,md(1),prod(md(2:end)));
        D = dct(D);
        D = reshape(D,md);
    end
    md = circshift(md,[0 -1]);
    D = permute(D,P);
end


function y = SSST (x, p)
% SSST  Spherical Surface Spline in Tension
%
%   y = SSST (x, p)
%
%   x is cos(theta) in range -1 <= x <= 1
%   p is tension (p >= 0)
%
% Returns the Green's' function for a spherical surface spline
% in tension, following Wessel and Becker [2008].
% If p == 0 or not given then we return Parker's [1994] minimum
% curvature solution instead.

% $Id: SSST.m,v 1.1.1.1 2008/05/09 21:34:52 myself Exp $
% P. Wessel, SOEST, U of Hawaii, April 2008 (pwessel@hawaii.edu)

if nargin == 1
	p = 0;
end
if (p == 0)     % Just do Parker's dilog solution
    y = dilog (0.5 - 0.5 * x);
    return
end

% Here we have tension
v = tension2nu (p);

y = zeros(size(x));     % Initialize output array
A = pi / sin(v*pi);     % Constant scale
k = find (abs(x) < (1-eps));  % Where Pv solution works
if (~isempty(k))
    y(k) = A*Pv(-x(k),v) - log(1-x(k));
end
% Deal with special case x == -1
k = find ((x-eps) <= -1);
if (~isempty(k))
    y(k) = A - log(2);
end
% Deal with special case x == +1
k = find ((x+eps) >= +1);
if (~isempty(k))
    y(k) = pi*cot(v*pi) + 2*(0.577215664901 + psi(1+v)) - log(2);
end
y = real(y);    % Knock off any insignificant imaginary noise

% Sub-functions used by SSST
function P = Pv (x, v)

P = zeros(size(x));
for i = 1:length(x)
    if (x(i) == -1)
        p = inf;
    else
        [p q k] = PvQv (x(i), v);
    end
    P(i) = p;
end

function [Pv Qv iter] = PvQv (x, v)
% Based on recipe in "An Atlas of Functions" by
% Spanier and Oldham, 1987
iter = 0;
if (x == -1)
    Pv = -inf;
    Qv = -inf;
    return
end
if (x == +1)
    Pv = 1;
    Qv = inf;
    return
end
a = 1;
R = 1;
K = 4 * sqrt (abs(v - v^2));
if (abs(1+v) + floor (1+v)) == 0
	a = 1.0e99;
	v = -1 - v;
end
s = sin (0.5*pi*v);
c = cos (0.5*pi*v);
w = (0.5 + v)^2;
while v <= 6.0
	v = v + 2;
	R = R * (v - 1)/v;
end
X = 1.0 / (4 + 4*v);
g = 1 + 5*X*(1 - 3*X*(0.35+6.1*X));
R = R*(1 - X*(1 - g*X/2))/sqrt (8*X);
g = 2*x;
u = g;
f = 1;
t = 1;
k = 0.5;
X = 1 + (1e8/(1 - x.^2));

t = t .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
k = k + 1;
f = f + t;
u = u .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
k = k + 1;
g = g + u;
while (k < K || abs (X*t) > abs(f))
        iter = iter + 1;
	t = t .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
	k = k + 1;
	f = f + t;
	u = u .* x.^2 * (k^2 - w) / ((k + 1)^2 - 0.25);
	k = k + 1;
	g = g + u;
end
f = f + (x.^2.*t ./ (1 - x.^2));
g = g + (x.^2.*u ./ (1 - x.^2));
Pv = ((s*g*R) + (c*f/R))/sqrt(pi);
Qv = a*sqrt(pi)*((c*g*R) - (s*f/R))/2;

function [f] = psi(z)
%Psi     Psi (or Digamma) function valid in the entire complex plane.
%
%                 d
%        Psi(z) = --log(Gamma(z))
%                 dz
%
%usage: [f] = psi(z)
%
%        Z may be complex and of any size.
%
%        This program uses the analytical derivative of the
%        Log of an excellent Lanczos series approximation
%        for the Gamma function.
%
%References: C. Lanczos, SIAM JNA  1, 1964. pp. 86-96
%            Y. Luke, "The Special ... approximations", 1969 pp. 29-31
%            Y. Luke, "Algorithms ... functions", 1977
%            J. Spouge,  SIAM JNA 31, 1994. pp. 931
%            W. Press,  "Numerical Recipes"
%            S. Chang, "Computation of special functions", 1996
%
siz = size(z);
z=z(:);
zz=z;

f = zeros(size(z));

%reflection point
p=find(real(z)<0.5);
if ~isempty(p)
   z(p)=1-z(p);
end

%Lanczos approximation for the complex plane

g=607/128; % best results when 4<=g<=5

c = [  0.99999999999999709182;
      57.156235665862923517;
     -59.597960355475491248;
      14.136097974741747174;
      -0.49191381609762019978;
        .33994649984811888699e-4;
        .46523628927048575665e-4;
       -.98374475304879564677e-4;
        .15808870322491248884e-3;
       -.21026444172410488319e-3;
        .21743961811521264320e-3;
       -.16431810653676389022e-3;
        .84418223983852743293e-4;
       -.26190838401581408670e-4;
        .36899182659531622704e-5];

n=0;
d=0;
for k=size(c,1):-1:2
    dz=1./(z+k-2);
    dd=c(k).*dz;
    d=d+dd;
    n=n-dd.*dz;
end
d=d+c(1);
gg=z+g-0.5;
%log is accurate to about 13 digits...

f = log(gg) + (n./d - g./gg) ;

if ~isempty(p)
   f(p) = f(p)-pi*cot(pi*zz(p));
end

p=find(round(zz)==zz & real(zz)<=0 & imag(zz)==0);
if ~isempty(p)
   f(p) = Inf;
end

f=reshape(f,siz);

function y = dilog (x)
% DILOG   The dilogarithm
%
%   y = dilog (x)
%
% Compute dilog(x) (defined for x >= 0) by the method of Parker's
% Appendix A of his Geophysical Inverse Theory.  The function
% is needed for x in the range 0 <= x <= 1 when solving the
% spherical spline interpolation in section 2.07 of Parker.

y = zeros (size (x));

pisqon6 = pi * pi / 6.0;
k = find (x <= 0.0);
if (~isempty(k))
    y(k) = pisqon6;
end
k = find (x > 0.0 & x < 0.5);
if (~isempty(k))
	y(k) = -log (1.0 - x(k));
	ysq = y(k) .* y(k);
	z = y(k) .* (1.0 + y(k) .* (-0.25 + y(k) .* (0.027777777777213 + ...
        ysq .* (-2.7777776990e-04 + ysq .* (4.724071696e-06 + ...
        ysq .* (-9.1764954e-08 + 1.798670e-09 .* ysq))))));
	y(k) = pisqon6 - z + y(k) .* log (x(k));
end
k = find (x >= 0.5 & x < 2.0);
if (~isempty(k))
    y(k) = -log (x(k));
	ysq = y(k) .* y(k);
	z = y(k) .* (1.0 + y(k) .* (-0.25 + y(k) .* (0.027777777777213 + ...
        ysq .* (-2.7777776990e-04 + ysq .* (4.724071696e-06 + ...
        ysq .* (-9.1764954e-08 + 1.798670e-09 .* ysq))))));
    y(k) = z;
end
k = find (x >= 2.0);
if (~isempty(k))
    y(k) = log (x(k));
	ysq = y(k) .* y(k);
	z = y(k) .* (1.0 + y(k) .* (-0.25 + y(k) .* (0.027777777777213 + ...
        ysq .* (-2.7777776990e-04 + ysq .* (4.724071696e-06 + ...
        ysq .* (-9.1764954e-08 + 1.798670e-09 .* ysq))))));
        y(k) = -z - 0.5 * ysq;
end

function nu = tension2nu (p)
nu = (-1 + sqrt (1 - 4*p^2))/2;


function value = r8_besk0 ( x )

%*****************************************************************************80
%
%% R8_BESK0 evaluates the Bessel function K of order 0 of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the Bessel function K of order 0 of X.
%
  persistent bk0cs
  persistent ntk0
  persistent xmax
  persistent xsml

  if ( isempty ( ntk0 ) )

    bk0cs = [ ...
      -0.353273932339027687201140060063153E-01, ...
      +0.344289899924628486886344927529213, ...
      +0.359799365153615016265721303687231E-01, ...
      +0.126461541144692592338479508673447E-02, ...
      +0.228621210311945178608269830297585E-04, ...
      +0.253479107902614945730790013428354E-06, ...
      +0.190451637722020885897214059381366E-08, ...
      +0.103496952576336245851008317853089E-10, ...
      +0.425981614279108257652445327170133E-13, ...
      +0.137446543588075089694238325440000E-15, ...
      +0.357089652850837359099688597333333E-18, ...
      +0.763164366011643737667498666666666E-21, ...
      +0.136542498844078185908053333333333E-23, ...
      +0.207527526690666808319999999999999E-26, ...
      +0.271281421807298560000000000000000E-29, ...
      +0.308259388791466666666666666666666E-32 ]';

    ntk0 = r8_inits (bk0cs, 16, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( 4.0 * r8_mach ( 3 ) );
    xmax = - log ( r8_mach ( 1 ) );
    xmax = xmax - 0.5 * xmax * log ( xmax ) / ( xmax + 0.5 );

  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BESK0 = Fatal error!\n' );
    fprintf ( 1, '  X <= 0.\n' );
    error ( 'R8_BESK0 = Fatal error!' )
  elseif ( x <= xsml )
    y = 0.0;
    value = - log ( 0.5 * x ) * r8_besi0 ( x ) ...
      - 0.25 + r8_csevl ( 0.5 * y - 1.0, bk0cs, ntk0 );
  elseif ( x <= 2.0 )
    y = x * x;
    value = - log ( 0.5 * x ) * r8_besi0 ( x ) ...
      - 0.25 + r8_csevl ( 0.5 * y - 1.0, bk0cs, ntk0 );
  elseif ( x <= xmax )
    value = exp ( - x ) * r8_besk0e ( x );
  else
    value = 0.0;
  end

  return
end


function [cost, gradf, hessian, hessianinv]=costgradf(px,py,pz,rho,u,param)
% compute the cost function

e=1e-6;

h     = param.h;
dim   = param.dim;
gamma = param.gamma;
A1x   = param.A1x;
A1y   = param.A1y;
A1z   = param.A1z;
A2    = param.A2;
A3    = param.A3;
a     = param.a;
c     = param.c;
F1    = param.F1;
F2    = param.F2;

n  = dim(1);
nx = dim(2);
ny = dim(3);
nz = dim(4);
nt = dim(5);
hx = h(1);
hy = h(2);
hz = h(3);
ht = h(4);

%compute p^2 and u^2

pxsq  = px.^2;
pysq  = py.^2;
pzsq  = pz.^2;
usq   = u.^2;

% inverse of rho
rhoinv  = 1./rho;
rhoinv1 = 1./(F1'*rho(:));
rhoinv2 = 1./(F2'*rho(:));

E=eye(n);
E(n,n)=e;
W1=kron(speye((nx-1)*ny*nz*nt),E);
W2=kron(speye((ny-1)*nx*nz*nt),E);
W3=kron(speye((nz-1)*nx*ny*nt),E);

% cost f
%cost = (A1x*W1*pxsq(:)+A1y*W2*pysq(:)+A1z*W3*pzsq(:))'*(A2*rhoinv(:) + a(:))*hx*hy*hz*ht + usq(:)'*(A3*rhoinv1(:)+A3*rhoinv2(:)+ c(:))*hx*hy*hz*ht*gamma;
cost = (A1x*W1*pxsq(:)+A1y*W2*pysq(:)+A1z*W3*pzsq(:))'*(A2*rhoinv(:) + a(:))*hx*hy*hz*ht + usq(:)'*(A3*rhoinv2(:)+ c(:))*hx*hy*hz*ht*gamma;
% compute gradient of f
if nargout > 1
    % derivative wrt px
    A11xe    = 2*W1*A1x'*(A2*rhoinv(:)+a(:));
    nablapxf = px(:).*A11xe(:);

    % derivative wrt py
    A11ye    = 2*W2*A1y'*(A2*rhoinv(:)+a(:));
    nablapyf = py(:).*A11ye(:);

    % derivative wrt pz
    A11ze    = 2*W3*A1z'*(A2*rhoinv(:)+a(:));
    nablapzf = pz(:).*A11ze(:);

    % derivative wrt rho
    A22e     = A2'*(A1x*W1*pxsq(:) + A1y*W2*pysq(:)+ A1z*W3*pzsq(:));
    nablarhof = -A22e.*rhoinv(:).^2-gamma.*F2*((A3'*usq(:)).*rhoinv2(:).^2);
                %-gamma.*F1*((A3'*usq(:)).*rhoinv1(:).^2);

    % derivative wrt u
    %A33e    = 2*gamma*(A3*(rhoinv2(:)+rhoinv1(:))+c(:));
    A33e    = 2*gamma*(A3*(rhoinv2(:))+c(:));
    nablauf = u(:).*A33e(:);

    % gradient wrt p rho u
    gradf   = [nablapxf; nablapyf; nablapzf; nablarhof; nablauf];

    % compute Hessian
    if nargout > 2
        % Hessian over px
        A11x    = A11xe;

        % Hessian over py
        A11y    = A11ye;

        % Hessian over py
        A11z    = A11ze;

         % hessian over rho
        temp1   = 2.*A22e.*rhoinv(:).^3;
        temp2   = 2.*gamma.*(F2*((A3'*usq(:)).*rhoinv2(:).^3));
        %temp3   = 2.*gamma.*(F1*((A3'*usq(:)).*rhoinv1(:).^3));
        A22     = temp1(:)+temp2(:)...+temp3(:)
        +1e-4*ones(n*nx*ny*nz*(nt-1),1);

        % hessian over u
        A33  = A33e;

        % hessian
        hessian = [A11x(:);A11y(:);A11z(:);A22(:);A33(:)];

        % compute the inverse of Hessian
        if nargout > 3
            % inverse of hessian
            hessianinv = spdiags(1./(hessian),0,numel(hessian),numel(hessian));
        end
    end
end

end


function varargout = drawEllipse3d(varargin)
%DRAWELLIPSE3D Draw a 3D ellipse
%
%   Possible calls for the function :
%   drawEllipse3d([XC YC ZC A B THETA PHI])
%   drawEllipse3d([XC YC ZC A B THETA PHI PSI])
%   drawEllipse3d([XC YC ZC A B], [THETA PHI])
%   drawEllipse3d([XC YC ZC A B], [THETA PHI PSI])
%   drawEllipse3d([XC YC ZC A B], THETA, PHI)
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI)
%   drawEllipse3d([XC YC ZC A B], THETA, PHI, PSI)
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI, PSI)
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI)
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI, PSI)
%
%   where XC, YC, ZY are coordinate of ellipse center, A and B are the
%   half-lengths of the major and minor axes of the ellipse,
%   PHI and THETA are 3D angle (in degrees) of the normal to the plane
%   containing the ellipse (PHI between 0 and 360 corresponding to
%   longitude, and THETA from 0 to 180, corresponding to angle with
%   vertical).
%
%   H = drawEllipse3d(...)
%   return handle on the created LINE object
%
%   ------
%   Author: David Legland
%   e-mail: david.legland@grignon.inra.fr
%   Created: 2008-05-07
%   Copyright 2008 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY

%   Possible calls for the function, with number of arguments :
%   drawEllipse3d([XC YC ZC A B THETA PHI])             1
%   drawEllipse3d([XC YC ZC A B THETA PHI PSI])         1
%   drawEllipse3d([XC YC ZC A B], [THETA PHI])          2
%   drawEllipse3d([XC YC ZC A B], [THETA PHI PSI])      2
%   drawEllipse3d([XC YC ZC A B], THETA, PHI)           3
%   drawEllipse3d([XC YC ZC A B], THETA, PHI, PSI)      4
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI)         5
%   drawEllipse3d([XC YC ZC], A, B, THETA, PHI, PSI)    6
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI)         7
%   drawEllipse3d(XC, YC, ZC, A, B, THETA, PHI, PSI)    8

% extract drawing options
ind = find(cellfun(@ischar, varargin), 1, 'first');
options = {};
if ~isempty(ind)
    options = varargin(ind:end);
    varargin(ind:end) = [];
end

if length(varargin)==1
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);
    a  = ellipse(:,4);
    b  = ellipse(:,5);

    % get colatitude of normal
    if size(ellipse, 2)>=6
        theta = ellipse(:,6);
    else
        theta = zeros(size(ellipse, 1), 1);
    end

    % get azimut of normal
    if size(ellipse, 2)>=7
        phi     = ellipse(:,7);
    else
        phi = zeros(size(ellipse, 1), 1);
    end

    % get roll
    if size(ellipse, 2)==8
        psi = ellipse(:,8);
    else
        psi = zeros(size(ellipse, 1), 1);
    end

elseif length(varargin)==2
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);
    a  = ellipse(:,4);
    b  = ellipse(:,5);

    % get angle of normal
    angle = varargin{2};
    theta   = angle(:,1);
    phi     = angle(:,2);

    % get roll
    if size(angle, 2)==3
        psi = angle(:,3);
    else
        psi = zeros(size(angle, 1), 1);
    end

elseif length(varargin)==3
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);
    a  = ellipse(:,4);
    b  = ellipse(:,5);

    % get angle of normal and roll
    theta   = varargin{2};
    phi     = varargin{3};
    psi     = zeros(size(phi, 1), 1);

elseif length(varargin)==4
    % get center and radius
    ellipse = varargin{1};
    xc = ellipse(:,1);
    yc = ellipse(:,2);
    zc = ellipse(:,3);

    if size(ellipse, 2)==5
        a  = ellipse(:,4);
        b  = ellipse(:,5);
    end

    theta   = varargin{2};
    phi     = varargin{3};
    psi     = varargin{4};

elseif length(varargin)==5
    % get center and radius
    ellipse = varargin{1};
    xc      = ellipse(:,1);
    yc      = ellipse(:,2);
    zc      = ellipse(:,3);
    a       = varargin{2};
    b       = varargin{3};
    theta   = varargin{4};
    phi     = varargin{5};
    psi     = zeros(size(phi, 1), 1);

elseif length(varargin)==6
    ellipse = varargin{1};
    xc      = ellipse(:,1);
    yc      = ellipse(:,2);
    zc      = ellipse(:,3);
    a       = varargin{2};
    b       = varargin{3};
    theta   = varargin{4};
    phi     = varargin{5};
    psi     = varargin{6};

elseif length(varargin)==7
    xc      = varargin{1};
    yc      = varargin{2};
    zc      = varargin{3};
    a       = varargin{4};
    b       = varargin{5};
    theta   = varargin{6};
    phi     = varargin{7};
    psi     = zeros(size(phi, 1), 1);

elseif length(varargin)==8
    xc      = varargin{1};
    yc      = varargin{2};
    zc      = varargin{3};
    a       = varargin{4};
    b       = varargin{5};
    theta   = varargin{6};
    phi     = varargin{7};
    psi     = varargin{8};

else
    error('drawEllipse3d: please specify center and radius');
end

% uses 60 intervals
t = linspace(0, 2*pi, 61)';

% polyline approximation of ellipse, centered and parallel to main axes
x       = a * cos(t);
y       = b * sin(t);
z       = zeros(length(t), 1);
base    = [x y z];

% compute transformation from local basis to world basis
trans   = localToGlobal3d(xc, yc, zc, theta, phi, psi);

% transform points composing the ellipse
ellipse = transformPoint3d(base, trans);

% draw the curve
h = drawPolyline3d(ellipse, options{:});

if nargout > 0
    varargout = {h};
end


function x_new = r8sd_cg ( n, ndiag, offset, a, b, x )

%*****************************************************************************80
%
%% R8SD_CG uses the conjugate gradient method on a R8SD linear system.
%
%  Discussion:
%
%    The R8SD storage format is for symmetric matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0, and
%    each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%
%    Assuming there are NDIAG nonzero diagonals (ignoring subdiagonals!),
%    we then create an array B that has N rows and NDIAG columns, and simply
%    "collapse" the matrix A to the left:
%
%    For the conjugate gradient method to be applicable, the matrix A must
%    be a positive definite symmetric matrix.
%
%    The method is designed to reach the solution to the linear system
%      A * x = b
%    after N computational steps.  However, roundoff may introduce
%    unacceptably large errors for some problems.  In such a case,
%    calling the routine a second time, using the current solution estimate
%    as the new starting guess, should result in improved results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals that are stored.
%    NDIAG must be at least 1 and no more than N.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8SD matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, real X(N), an estimate for the solution, which may be 0.
%
%    Output, real X_NEW(N), the approximate solution vector.  Note that
%    repeated calls to this routine, with the output X_NEW from the
%    previous call used as the input value of X, MAY improve the solution.
%
  x_new(1:n) = x(1:n);
%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap(1:n) = r8sd_mxv ( n, ndiag, offset, a, x_new );

  r(1:n) = b(1:n) - ap(1:n);
  p(1:n) = b(1:n) - ap(1:n);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP = A*P.
%
    ap(1:n) = r8sd_mxv ( n, ndiag, offset, a, p );
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p(1:n) * ap(1:n)';
    pr =  p(1:n) * r(1:n)';

    if ( pap == 0.0E+00 )
      return;
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x_new(1:n) = x_new(1:n) + alpha * p(1:n);
    r(1:n) = r(1:n) - alpha * ap(1:n);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r(1:n) * ap(1:n)';

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n) = r(1:n) + beta * p(1:n);

  end

  return
end


% homodyne_recon_demo.m
%
% - Demonstrates use of homodyne_recon.m, which implements the homodyne
% reconstruction method outlined in Doug Noll's 1991 IEEE T-MI paper
% "Homodyne detection in magnetic resonance imaging"
% - Figure shows the decrease in RMSE that results from sampling more.
% - Reconstructed image can be rectangular, with odd or even dimensions.
%
% 2012-06-15, Mai Le
% 2012-09-26 Jeff Fessler tweaked

% set up "true" brain image
%f.dir = [path_find_dir('mri') '/../data/mri/'];
%f.xtrue = [f.dir 'brainweb_t1.jpg'];
%mag_true = double(imread(f.xtrue))'; % true image magnitude
mag_true = 25*ellipse_im(256);
mag_true = mag_true(16:end-16,3:end-2); % make it rectangle to stress test
dims = size(mag_true);

% simulate linear phase over object
[xx yy] = ndgrid(-dims(1)/2:dims(1)/2-1,-dims(2)/2:dims(2)/2-1);
ph_max = pi/2;
ph = ph_max*xx/(dims(1)/2) + ph_max*yy/(dims(2)/2);
image = mag_true .* exp(1i*ph); % modulate to make it complex
im_fft = fftshift(fft2(image));

im plc 2 3
im(1, mag_true)

for homodyne_direction = [1 2] % partial k-space can be in direction 1 or 2
	% # of samples in "half" kspace:
	nhalf = ceil((dims(homodyne_direction)+1)/2);
	overlaps = dims(homodyne_direction) - nhalf; % maximum # of extra rows
	overlaps = [0:5:(overlaps-1) overlaps];
	nover = numel(overlaps);

	nrms_im = zeros(nover,1);
	nrms_ph_demod = zeros(nover,1);

	for ii = 1:nover
		overlap = overlaps(ii);
		if (homodyne_direction == 1)
			k1 = 1:min(dims(1), nhalf+overlap);
			partial_kspace = im_fft(k1,:);
		elseif (homodyne_direction == 2)
			k2 = 1:min(dims(2), nhalf+overlap);
			partial_kspace = im_fft(:,k2);
		else
			fail 'bug'
		end

		f.sigma = 800; % complex AWGN std dev
		noisy_partial_kspace = partial_kspace + f.sigma * ...
			(randn(size(partial_kspace)) + 1i*randn(size(partial_kspace)));

		[recon_ph_demod, recon_im, lp_im, full_kspace] = ...
			homodyne_recon(noisy_partial_kspace, dims(1), dims(2), ...
				overlap, 'direction', homodyne_direction);

		nrms_fun = @(x,y) 100 * nrms(x(:), y(:));
		nrms_im(ii) = nrms_fun(abs(recon_im), mag_true);
		nrms_ph_demod(ii) = nrms_fun(abs(recon_ph_demod), mag_true);

		if overlap <= 15
			clim = minmax(mag_true)';
			im(2, abs(recon_im), clim)
			titlef('Conventional, overlap=%d', overlap)
			im(3, abs(recon_ph_demod), clim)
			titlef('Demodulated, overlap=%d', overlap)
			im(4, log(abs(partial_kspace)), clim)
			im(5, abs(recon_im) - mag_true, [-9 9], 'Error')
			im(6, abs(recon_ph_demod) - mag_true, [-9 9], 'Error')
			drawnow
	%		prompt
		end
	end

	f.snr = sqrt(mean(abs(partial_kspace(:)).^2)) / ...
		sqrt(mean(abs(noisy_partial_kspace(:) - partial_kspace(:)).^2));
	f.snr = 20*log10(f.snr);
	pr f.snr

	im subplot 4
	plot(overlaps, nrms_im, 'g-+', overlaps, nrms_ph_demod, 'b-o')
	axis tight
	xtick([0 15 max(overlaps)])
	title('NRMSE (%)')
	legend('without demodulation', 'phase demodulated');
	xlabel('rows acquired beyond 1/2 of kspace');
	ylabel('NRMS error of reconstructed image (%)');
prompt
end


classdef DOC4 < PROBLEM
% <multi> <real> <constrained>
% Benchmark MOP with constraints in both decision and objective spaces

%------------------------------- Reference --------------------------------
% Z. Liu and Y. Wang, Handling constrained multiobjective optimization
% problems with constraints in both the decision and objective spaces. IEEE
% Transactions on Evolutionary Computation, 2019, 23(5): 870-884.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            obj.D = 8;
            obj.lower    = [0 -10 -10 -10 -10 -10 -10 -10];
            obj.upper    = [ 1 10 10 10 10 10 10 10];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            g_temp = (X(:, 2) - 10).^2 + 5 * (X(:, 3) - 12).^2 + X(:, 4).^4 + 3 * (X(:, 5) - 11).^2 + 10 * X(:, 6).^6 + ...
            7 * X(:, 7).^2 + X(:, 8).^4 - 4 * X(:, 7).* X(:, 8) - 10 * X(:, 7) - 8 * X(:, 8);
            g = g_temp-680.6300573745 +1;
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1))./g);
            % Constraints in objective space
            c(:,1) = max( -(PopObj(:,1) + PopObj(:,2)-1), 0);
            c(:,2) = max(- ( PopObj(:,1)+ PopObj(:,2) - 1 - abs(sin(10*pi*(PopObj(:,1) - PopObj(:,2) + 1) ))), 0);
            % Constraints in decision space
            c(:,3) = -127 + 2 * X(:, 2).^2 + 3 * X(:, 3).^4 + X(:, 4) + 4 * X(:, 5).^2 + 5 * X(:, 6);
            c(:,4) = -282 + 7 * X(:, 2) + 3 * X(:, 3) + 10 * X(:, 4).^2 + X(:, 5) - X(:, 6);
            c(:,5) = -196 + 23 * X(:, 2) + X(:, 3).^2 + 6 * X(:, 7).^2 - 8 * X(:, 8);
            c(:,6) = 4 * X(:, 2).^2 + X(:, 3).^2 - 3 * X(:, 2).* X(:, 3) + 2 * X(:, 4).^2 + 5 * X(:, 7) - 11 * X(:, 8);
            Population = SOLUTION(X,PopObj,c,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = (0:20)/20;
            R(:,2) = 1 - R(:,1);
        end
    end
end


function La = YeePattanaikLuminanceAdaptation(img, maxLayers)
%
%
%       La = YeePattanaikLuminanceAdaptation(img, maxLayers)
%
%
%       Input:
%           -img: HDR image
%           -maxLayers: the number of layers in [16,96]
%
%       Output:
%           -La: luminance adaptation
%
%     Copyright (C) 2010-2020  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "Segmentation and Adaptive Assimilation for Detail-Preserving Display of High-Dynamic Range Images"
% 	  by Hector Yee, Sumanta N. Pattanaik
%     in Elsevier The Visual Computer 2003
%

%is it a three color channels image?
check13Color(img);

checkNegative(img);

%check parameters
if(~exist('maxLayers', 'var'))
    maxLayers = 32;
end

if((maxLayers < 16) || (maxLayers > 96))
    maxLayers = 32;
end

maxLayers = round(maxLayers);

%these could be parameters
bin_size1 = 0.5;
bin_size2 = 2.0;

%compute luminance channel
L = lum(img);

%compute the adaptation
L_log = log10(L + 1e-6);
minL_Log = min(L_log(:));

La = zeros(size(L));

for i=0:(maxLayers - 1)
    bin_size = bin_size1 + (bin_size2 - bin_size1) * i / (maxLayers - 1);
    category = round((L_log - minL_Log) / bin_size) + 1;

    %compute layers
    [imgLabel, ~, ~] = computeConnectedComponents(category, 8);
    labels = unique(imgLabel);

    for j=1:length(labels) %adaptation group
        indx = find(imgLabel == labels(j));
        La(indx) = La(indx) + mean(L_log(indx));
    end
end

La = 10.^(La / maxLayers);
La(La < 0.0) = 0.0;

end


function [ R ] = vl_nnr2R( r,dCdR )
%r2R Axis angle to rotation matrix layer
% Forwards mode:
% R = vl_nnr2R(r);
%   r is of size 1 x 1 x 3 x nbatch containing axis angle vectors
%   R is of size 1 x 3 x 3 x nbatch containing rotation matrices
% Backwards mode:
% dCdr = vl_nnr2R(r,dCdR);
%   dCdR is derivative of the cost function with respect to R and is of
%   size 1 x 3 x 3 x nbatch
%   dCdr is derivative of the cost function with respect to r and is of
%   size 1 x 1 x 3 x nbatch
%
% Useful reference for formula:
% [1] Gallego, Guillermo, and Anthony Yezzi. "A compact formula for the
%     derivative of a 3-D rotation in exponential coordinates." Journal
%     of Mathematical Imaging and Vision 51.3 (2015): 378-384.

nbatch = size(r,4);
if nargin<2
    % Forwards

    % Vectors with zero length are a special case - they just have an
    % identity rotation matrix
    idmask = squeeze(all(r==0,3));

    R = zeros(1,3,3,nbatch,'single');

    % Fill in the identity cases
    R(1,1,1,idmask)=1;
    R(1,2,2,idmask)=1;
    R(1,3,3,idmask)=1;

    theta = sqrt(sum(r(:,:,:,~idmask).^2,3)); % 1 x 1 x 1 x non-zero nbatch rotation angles
    k1 = r(:,:,1,~idmask)./theta; % rotation axis unit vectors
    k2 = r(:,:,2,~idmask)./theta; % rotation axis unit vectors
    k3 = r(:,:,3,~idmask)./theta; % rotation axis unit vectors

    % Fill in the non-identity cases using the following formula (see [1]):
    % K = [0 -k(3) k(2); k(3) 0 -k(1); -k(2) k(1) 0];
    % R = eye(3) + sin(theta).*K + (1-cos(theta)).*K*K;

    R(1,1,1,~idmask) = (cos(theta) - 1).*k2.^2 + (cos(theta) - 1).*k3.^2 + 1;
    R(1,1,2,~idmask) =  -k3.*sin(theta) - k1.*k2.*(cos(theta) - 1);
    R(1,1,3,~idmask) = k2.*sin(theta) - k1.*k3.*(cos(theta) - 1);
    R(1,2,1,~idmask) = k3.*sin(theta) - k1.*k2.*(cos(theta) - 1);
    R(1,2,2,~idmask) = (cos(theta) - 1).*k1.^2 + (cos(theta) - 1).*k3.^2 + 1;
    R(1,2,3,~idmask) = -k1.*sin(theta) - k2.*k3.*(cos(theta) - 1);
    R(1,3,1,~idmask) = -k2.*sin(theta) - k1.*k3.*(cos(theta) - 1);
    R(1,3,2,~idmask) = k1.*sin(theta) - k2.*k3.*(cos(theta) - 1);
    R(1,3,3,~idmask) = (cos(theta) - 1).*k1.^2 + (cos(theta) - 1).*k2.^2 + 1;
else
    % Backwards

	idmask = squeeze(all(r==0,3));

    % Handle identity case
    dRdr1 = zeros(size(dCdR));
    dRdr1(1,2,3,idmask)=-1;
    dRdr1(1,3,2,idmask)=1;
    dRdr2 = zeros(size(dCdR));
    dRdr2(1,1,3,idmask)=1;
    dRdr2(1,3,1,idmask)=-1;
    dRdr3 = zeros(size(dCdR));
    dRdr3(1,1,2,idmask)=-1;
    dRdr3(1,2,1,idmask)=1;

    % Call forwards version to get R
    [ R ] = vl_nnr2R( r );

    % Subselect and precompute some values
    R11 = R(1,1,1,~idmask); R12 = R(1,1,2,~idmask); R13 = R(1,1,3,~idmask);
    R21 = R(1,2,1,~idmask); R22 = R(1,2,2,~idmask); R23 = R(1,2,3,~idmask);
    R31 = R(1,3,1,~idmask); R32 = R(1,3,2,~idmask); R33 = R(1,3,3,~idmask);
    r1 = r(1,1,1,~idmask); r2 = r(1,1,2,~idmask); r3 = r(1,1,3,~idmask);
    sqnorm = (r1.^2 + r2.^2 + r3.^2);

    % For all other cases, find 3x3 derivatives of R with respect to each
    % element of r
    dRdr1(1,1,1,~idmask) = (R31.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm - (R21.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm;
    dRdr1(1,1,2,~idmask) = (R32.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm - (R22.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm;
    dRdr1(1,1,3,~idmask) = (R33.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm - (R23.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm;
    dRdr1(1,2,1,~idmask) = (R11.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm - (R31.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm;
    dRdr1(1,2,2,~idmask) = (R12.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm - (R32.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm;
    dRdr1(1,2,3,~idmask) = (R13.*(r1.*r3 - R21.*r1 + r2.*(R11 - 1)))./sqnorm - (R33.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm;
    dRdr1(1,3,1,~idmask) = (R21.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm - (R11.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm;
    dRdr1(1,3,2,~idmask) = (R22.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm - (R12.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm;
    dRdr1(1,3,3,~idmask) = (R23.*(r1.^2 + R21.*r3 - R31.*r2))./sqnorm - (R13.*(R31.*r1 + r1.*r2 - r3.*(R11 - 1)))./sqnorm;

    dRdr2(1,1,1,~idmask) = (R31.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm - (R21.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm;
    dRdr2(1,1,2,~idmask) = (R32.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm - (R22.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm;
    dRdr2(1,1,3,~idmask) = (R33.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm - (R23.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm;
    dRdr2(1,2,1,~idmask) = (R11.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm - (R31.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm;
    dRdr2(1,2,2,~idmask) = (R12.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm - (R32.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm;
    dRdr2(1,2,3,~idmask) = (R13.*(R12.*r2 + r2.*r3 - r1.*(R22 - 1)))./sqnorm - (R33.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm;
    dRdr2(1,3,1,~idmask) = (R21.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm - (R11.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm;
    dRdr2(1,3,2,~idmask) = (R22.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm - (R12.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm;
    dRdr2(1,3,3,~idmask) = (R23.*(r1.*r2 - R32.*r2 + r3.*(R22 - 1)))./sqnorm - (R13.*(r2.^2 - R12.*r3 + R32.*r1))./sqnorm;

    dRdr3(1,1,1,~idmask) = (R31.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm - (R21.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm;
    dRdr3(1,1,2,~idmask) = (R32.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm - (R22.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm;
    dRdr3(1,1,3,~idmask) = (R33.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm - (R23.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm;
	dRdr3(1,2,1,~idmask) = (R11.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm - (R31.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm;
    dRdr3(1,2,2,~idmask) = (R12.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm - (R32.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm;
    dRdr3(1,2,3,~idmask) = (R13.*(r3.^2 + R13.*r2 - R23.*r1))./sqnorm - (R33.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm;
    dRdr3(1,3,1,~idmask) = (R21.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm - (R11.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm;
    dRdr3(1,3,2,~idmask) = (R22.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm - (R12.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm;
    dRdr3(1,3,3,~idmask) = (R23.*(R23.*r3 + r1.*r3 - r2.*(R33 - 1)))./sqnorm - (R13.*(r2.*r3 - R13.*r3 + r1.*(R33 - 1)))./sqnorm;

    dCdr(1,1,1,:) = sum(sum(dCdR.*dRdr1,2),3);
    dCdr(1,1,2,:) = sum(sum(dCdR.*dRdr2,2),3);
    dCdr(1,1,3,:) = sum(sum(dCdR.*dRdr3,2),3);

    R = dCdr;
end

end


function [ n_data, alpha, beta, x, fx ] = extreme_values_cdf_values ( n_data )

%*****************************************************************************80
%
%% EXTREME_VALUES_CDF_VALUES returns some values of the Extreme Values CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = ExtremeValuesDistribution [ alpha, beta ]
%      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 ALPHA, the first parameter of the distribution.
%
%    Output, real BETA, the second parameter of the distribution.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  alpha_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 ];

  beta_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 ];

  fx_vec = [ ...
     0.3678794411714423E+00, ...
     0.8734230184931166E+00, ...
     0.9818510730616665E+00, ...
     0.9975243173927525E+00, ...
     0.5452392118926051E+00, ...
     0.4884435800065159E+00, ...
     0.4589560693076638E+00, ...
     0.4409910259429826E+00, ...
     0.5452392118926051E+00, ...
     0.3678794411714423E+00, ...
     0.1922956455479649E+00, ...
     0.6598803584531254E-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;
    alpha = 0.0;
    beta = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    alpha = alpha_vec(n_data);
    beta = beta_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function a = r8mat_geinverse ( a, n, pivot )

%*****************************************************************************80
%
%% R8MAT_GEINVERSE computes the inverse of a matrix factored by R8MAT_GEFA.
%
%  Discussion:
%
%    R8MAT_GEINVERSE is a modified version of the LINPACK routine DGEDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input/output, real A(N,N).
%    On input, the factor information computed by R8MAT_GEFA.
%    On output, the inverse matrix.
%
%    Input, integer N, the order of the matrix A.
%
%    Input, integer PIVOT(N), the pivot vector from R8MAT_GEFA.
%

%
%  Compute Inverse(U).
%
  for k = 1 : n

    a(k,k) = 1.0 / a(k,k);

    a(1:k-1,k) = - a(1:k-1,k) * a(k,k);

    for j = k + 1 : n

      temp = a(k,j);
      a(k,j) = 0.0;

      a(1:k,j) = a(1:k,j) + temp * a(1:k,k);

    end

  end
%
%  Form Inverse(U) * Inverse(L).
%
  for k = n - 1 : -1 : 1

    work(k+1:n) = a(k+1:n,k);
    a(k+1:n,k) = 0.0;

    for j = k + 1 : n
      a(1:n,k) = a(1:n,k) + work(j) * a(1:n,j);
    end

    if ( pivot(k) ~= k )

      for i = 1 : n
        temp = a(i,k);
        a(i,k) = a(i,pivot(k));
        a(i,pivot(k)) = temp;
      end

    end

  end

  return
end


function [ C , coord , MN ] = ccv ( A , N , F )
% Generates a crystal consists of lattice sites and bases, Calculates
% connecting vectors from point to point, Finds the number of nearest
% neighbors of each atomic site.
%
% function [ C , coord , MN ] = ccv ( A , N , F )
%
% arguments: ( input )
%
%  A - ( class - double ) a ( 3 * 3 ) matrix that each row is a vector that
%  shows primary generator vectors of a lattice in three dimensions.
%
%  N - ( class - double ) a matrix with three possitive integers that shows
%  the number of atoms in each dimensions.
%
%  F - ( class - double ) a ( m * 3 ) matrix that show the bases
%  coordinates. ( m is the number of bases )
%
% arguments: ( output )
%
%  C - ( class - double ) A ( N ( 1 ) * N ( 2 ) * N ( 3 ) ) * ( N ( 1 ) *
%  N ( 2 ) * N ( 3 ) ) * 3 matrix connecting vector between each two atomic
%  sites
%
%  coord - ( class - double ) Coordination number ( Number of nearest
%  neighbors ) of each atomic site
%
%  MN - ( class - double ) Mean distance between the nearest neighbor of
%  each atomic site
%
% Example:
%  A = [ 2 3 3 ; 6 5 3 ; 2 5 3 ] ;
%  N = [ 4 3 5 ] ;
%  F = [ 0 0 0 ; 0 0 1 ; 0 -2 0 ] ;
%  [ CV , COORD , MN ] = ccv ( A , N , F )
%
% See also rcv.
%
% Copyright 2009
%
% Release Date: 2009-10-12

% check for simple errors

if nargin < 3
    F = [ 0 0 0 ] ;
end % end of if loop

if nargin < 2
    N = [ 3 3 3 ] ;
end % end of if loop

if nargin < 1
    A = [ 1 0 0 ; 0 1 0 ; 0 0 1 ] ;
end % end of if loop

if ( N ( 1 ) <= 0 ) || ( N ( 2 ) <= 0 ) || ( N ( 3 ) <= 0 ) % condition of negative numbers
    error ' Enterd demensions must be positive integers. ' % error message
end % end of if loop

V = dot ( A ( 1 , : ) , cross ( A ( 2 , : ) , A ( 3 , : ) ) ) ; % Primitive Cell Volume

if V == 0 % condition of same plane vectors
    error ' Vectors must not be in same plane. ' % error message
end % end of if loop

% end of error checking

n = ( N ( 1 ) * N ( 2 ) ) * N ( 3 ) ; % calculates the total number of atomic sites
xy = zeros ( n , 3 ) ; % Preallocating

for i = 0 : N ( 1 ) - 1 % i is the numerator of for loop
    for j = 0 : N ( 2 ) - 1 % j is the numerator of for loop
        for k = 0 : N ( 3 ) - 1 % k is the numerator of for loop
            xy ( ( ( N ( 1 ) * j ) + ( i + 1 ) ) + ( N ( 1 ) * N ( 2 ) ) * k , : ) = A ( 1 , : ) * i + A ( 2 , : ) * j + A( 3 , : ) * k ;
        end % end of for loop
    end % end of for loop
end % end of for loop

xyold = xy ;
f = size ( F ) ;
xyo = [ ] ;

for q = 1 : f ( 1 ) % q is the numerator of for loop
    for p = 1 : 3 % p is the numerator of for loop
        xy ( : , p ) = xyold ( : , p ) + F ( q , p ) ;
    end % end of for loop
    xyo = [ xyo ; xy ] ;
end % end of for loop

n = length ( xyo ) ;
xy = xyo ;

C = zeros ( n , n , 3 ) ; % Preallocating
D = zeros ( n , n , 3 ) ; % Preallocating
E = zeros ( n , 3 ) ; % Preallocating
coord = zeros ( 1 , n ) ; % Preallocating
MIN = zeros ( n-1 , n ); % Preallocating
k = 1 ; % Preallocating

for P = 1 : n % P is the numerator of for loop
    for j = 1 : 3 % j is the numerator of for loop
        C ( P , 1 : n , j ) = xy ( 1 : n , j ) - xy ( P , j ) ; % Connecting vector elements between to lattice atomic sites
        D ( P , 1 : n , j ) = ( xy ( 1 : n , j ) - xy ( P , j ) ) .^ 2 ;
    end % end of for loop
    E ( P , 1 : n ) = sqrt ( D ( P , 1 : n , 1 ) + D ( P , 1 : n , 2 ) + D ( P , 1 : n , 3 ) ) ;
end % end of for loop

for P = 1 : n % P is the numerator of for loop
    for i = 1 : n % i is the numerator of for loop
        if E ( P , i ) ~= 0
            MIN ( k ) = E ( P , i ) ; % Eliminates zero value elements
            k = k + 1 ;
        end % end of if loop
    end % end of for loop
end % end of for loop

MN = min ( MIN ) ; % Nearest neighbors distance

for P = 1 : n % P is the numerator of for loop
    t = 0 ; % Preallocating
    for i = 1 : n % i is the numerator of for loop
        if E ( P , i ) == MN ( P )
            t = t + 1 ; % Counts number of nearest neighbors
        end % end of if loop
    end % end of for loop
    coord ( P ) = t ;
end % end of for loop

% With special thanks to John D'Errico
% By Ali Mohammad Razeghi
% My Email ( am_razeghi@yahoo.com ) is also ready to get full detailed
% commentation.


%% Tracking of rotating point using Kalman filter
%
% Tracking of rotating point.
% Rotation speed is constant.
% Both state and measurements vectors are 1D (a point angle),
% Measurement is the real point angle + gaussian noise.
% The real and the estimated points are connected with yellow line segment,
% the real and the measured points are connected with red line segment.
% (if Kalman filter works correctly,
% the yellow segment should be shorter than the red one).
%
% Pressing any key will reset the tracking with a different speed.
% Close the window to stop the program.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/kalman.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/kalman.py>
%

%%
% set up display window
img = zeros(500, 500, 3, 'uint8');
hFig = figure('KeyPressFcn',@(o,e)setappdata(o, 'flag',true), ...
    'Menubar','none', 'Name','Kalman Filter demo');
setappdata(hFig, 'flag',false);
hImg = imshow(img);

%%
% helper anonymous functions
calcPoint = @(center,R,angle) center + [cos(angle), -sin(angle)]*R;
drawCross = @(img,center,clr,d) cv.line(...
    cv.line(img, center-d, center+d, 'Color',clr, ...
        'Thickness',1, 'LineType','AA'), ...
    center+[d -d], center+[-d d], 'Color',clr, ...
        'Thickness',1, 'LineType','AA');

%%
% create and initialize Kalman filter
KF = cv.KalmanFilter(2, 1);
state = zeros(2,1);  % [phi; delta_phi]
processNoise = zeros(2,1);
measurement = zeros(1,1);

%%
% keep repeating until figure is closed
while ishghandle(hFig)
    setappdata(hFig, 'flag',false);

    % initialize KF
    state = randn(size(state))*0.1;
    KF.transitionMatrix = [1 1; 0 1];
    KF.measurementMatrix = eye(size(KF.measurementMatrix));
    KF.processNoiseCov = eye(size(KF.processNoiseCov))*1e-5;
    KF.measurementNoiseCov = eye(size(KF.measurementNoiseCov))*1e-1;
    KF.errorCovPost = eye(size(KF.errorCovPost));
    KF.statePost = randn(size(KF.statePost))*0.1;

    % main loop
    while ishghandle(hFig)
        center = [size(img,2) size(img,1)]/2;
        R = size(img,2)/3;
        stateAngle = state(1);
        statePt = calcPoint(center, R, stateAngle);

        prediction = KF.predict();
        predictAngle = prediction(1);
        predictPt = calcPoint(center, R, predictAngle);

        measurement = randn(size(measurement))*KF.measurementNoiseCov(1);

        % generate measurement
        measurement = measurement + KF.measurementMatrix*state;

        measAngle = measurement(1);
        measPt = calcPoint(center, R, measAngle);

        % plot points
        img(:) = 0;
        img = drawCross(img, statePt, [255 255 255], 3);
        img = drawCross(img, measPt, [255 0 0 0], 3);
        img = drawCross(img, predictPt, [0 255 0], 3);
        img = cv.line(img, statePt, measPt, 'Color',[255 0 0], ...
            'Thickness',3, 'LineType','AA');
        img = cv.line(img, predictPt, measPt, 'Color',[255 255 0], ...
            'Thickness',3, 'LineType','AA');

        if rand > 0.75
            KF.correct(measurement);
        end

        processNoise = randn(size(processNoise))*sqrt(KF.processNoiseCov(1,1));
        state = KF.transitionMatrix*state + processNoise;

        % update display
        set(hImg, 'CData',img);

        % break of inner loop on any key press
        flag = getappdata(hFig, 'flag');
        if isempty(flag)||flag, break; end
        pause(0.1)
    end
end


function logpdf = logmvtpdf(t,mu,f,Sigma);
% function logpdf = logmvtpdf(t,mu,f,Sigma);
% log pdf of multivariate t-student dist.
% t : D by N

[d,n] = size(t);

c = gammaln((d+f)*0.5) - (d*0.5)*log(f*pi) - gammaln(f*0.5) - 0.5*detln(Sigma);
diff = t - repmat(mu,1,n); % d by n
logpdf = c - (f+d)*0.5 * log(1 + sum(diff.*(inv(f*Sigma)*diff),1));

% Local Variables: ***
% mode: matlab ***
% End: ***


function [u,v,px,py] = perform_tv_hilbert_projection(f,kernel,lam,options)

% Aujol & Chambolle projection for solving TV-K regularization
%
%   [u,v,px,py] = perform_tv_hilbert_projection(f,kernel,lam,options);
%
%   Solve the image separation problem :
%       u = argmin_u |f-u|_K^2 + lam*|u|_TV
%
%   f = u+v
%   u is the cartoon part.
%   v is the texture part.
%
%   where |.|_K is an hilber space norm defined by some operator K
%       <f,g>_K = <f,K*g>
%       |f|_K^2 = <f,f>_K
%
%   f is the input image.
%   kernel is a callback function of the kernel operator, should be like
%       f = kernel(f, options);
%
%	Set options.use_gabor==1 to setup the Gabor Kernel automatically
%	and options.use_gabor==-1 to set up the L2 identity kernel.
%
% Original code by Guy Gilboa, modified by Gabriel Peyre
%
%   Copyright (c) 2006 Gabriel Peyre

[ny,nx]=size(f);

options.null = 0;
if isfield(options, 'niter')
    niter = options.niter;
else
    niter = 30;
end
if isfield(options, 'dt')
    dt = options.dt;
else
    dt = 0.01;
end
if isfield(options, 'p0x') && ~isempty(options.p0x)
    p0x = options.p0x;
else
    p0x=zeros(ny,nx);
end
if isfield(options, 'p0y') && ~isempty(options.p0y)
    p0y = options.p0y;
else
    p0y=p0x;
end

use_gabor = 1;
if isfield(options, 'use_gabor')
    use_gabor = options.use_gabor;
end

px=p0x; py=p0y;
lami=1/lam;  % here the algorithm works with inverse of lambda

if use_gabor==1
    sig_k=1; freq=0.25; bias=0;
    n=11; %kernel size
    x=ones(n,1)*(-(n-1)/2:(n-1)/2); y=x';
    gs=exp(-((x/sig_k).^2+(y/sig_k).^2)/2);  % 2D gaussian
    gs=gs/sum(sum(gs)); % normalize
    r=sqrt(x.^2+y.^2); cs=cos(2*pi*r*freq); %radially symmetric
    iKx = cs.*gs; %% "Gabor filter" for inverse K
    iKx=iKx-mean(mean(iKx)); % zero mean filter
    cp=(n+1)/2; % center of kernel
    iKx(cp,cp) = iKx(cp,cp)-bias;
elseif use_gabor==-1
    iKx = 1;
end

%% perform iterations
if use_gabor~=0
    for i=1:niter,  %% do niterations
        progressbar(i,niter);
        %% compute projection
        km1div = filter2(iKx,div(px,py))-f*lam;
        %[Gx,Gy] = grad(km1div);
        Gx=gradx(km1div); Gy=grady(km1div);
        aG = sqrt(Gx.^2+Gy.^2); %abs(G)
        px = (px + dt*Gx)./(1+dt*aG);
        py = (py + dt*Gy)./(1+dt*aG);
    end % for i
    v = filter2(iKx,div(px,py))/lam;
else
    for i=1:niter,  %% do niterations
        progressbar(i,niter);
        % compute projection
        km1div = feval( kernel, div(px,py), options ) - f*lam;
        % compute gradient
        Gx=gradx(km1div); Gy=grady(km1div);
%        [Gx,Gy] = grad(km1div);
        aG = sqrt(Gx.^2+Gy.^2); %abs(G)
        px = (px + dt*Gx)./(1+dt*aG);
        py = (py + dt*Gy)./(1+dt*aG);
    end % for i
    v = lami*feval( kernel, div(px,py), options );
end

u=f-v;

%% additional functions
%%%%%%%%%%%%%%%%%%%%%%%%%%% Gradient (forward difference)
function [fx,fy] = grad(P)
error('Should not be used.');
fx = P(:,[2:end end])-P;
fy = P([2:end end],:)-P;
%%%%%%%%%%%%%%%%%%%%%%%%%%% Divergence (backward difference)
function M=div(px,py)
[m,n]=size(px);
M=zeros(m,n);
Mx=M; My=M;
Mx(2:m-1,1:n)=px(2:m-1,1:n)-px(1:m-2,1:n);
Mx(1,:)=px(1,:);
Mx(m,:)=-px(m-1,:);
My(1:m,2:n-1)=py(1:m,2:n-1)-py(1:m,1:n-2);
My(:,1)=py(:,1);
My(:,n)=-py(:,n-1);
M=Mx+My;
%%%%%%%%%%%%%%%%%%%%%%%%%%% Laplacian (central difference)
function fl = lap(P)
[gx,gy]=grad(P);
fl=div(gx,gy);
%%%%%%%%%%%%%%%%%%%%%%%%%%% Gradient on X (forward difference)
function M=gradx(I)
[m,n]=size(I);
M=zeros(m,n);
M(1:m-1,1:n)=-I(1:m-1,:)+I(2:m,:);
M(m,1:n)=zeros(1,n);
%%%%%%%%%%%%%%%%%%%%%%%%%%% Gradient on Y (forward difference)
function M=grady(I)
[m,n]=size(I);
M=zeros(m,n);
M(1:m,1:n-1)=-I(:,1:n-1)+I(:,2:n);
M(1:m,n)=zeros(m,1);


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Orbital Mechanics with MATLAB

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

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function [var_prc,pci,Nneeded,pcitarget] = var_prctile(p,n_in_sample,varargin)
% :Usage:
% ::
%
%     [var_prc,pci,Nneeded,pcitarget] = var_prctile(p,n_in_sample,['nboot',nboot],['x',data])
%
%     [var_prc,pci,Nneeded,pcitarget] = var_prctile(p,50,'nboot',5000)
%
% :Inputs:
%
%   **p:**
%        is desired prctile of data (threshold)
%
%   **nboot:**
%        is number of bootstrap samples (if bootstrapping)
%
%   **n_in_sample:**
%        is number of obs. in original sample
%
%   **x:**
%        is data sample of distribution of interest (empirical pdf based on
%        this)
%
%        Note: using empirical PDF/CDF depends a great deal on choice of h (see
%        code).
%
% :Examples:
% ::
%
%    Nneeded = [];
%    for p = [.05:-.001:.001]
%        [var_prc,pci,Nneeded(end+1),pcitarget] = var_prctile(x,p);
%    end
%    figure;
%    plot([.05:-.001:.001],Nneeded)
%
% ..
%    tor wager, jan 2007
% ..

nboot = Inf;
norm_model = 1;
x = [];
for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            case 'nboot', nboot = varargin{i+1};                    % # bootstrap samples
            case {'x','data'}, x = varargin{i+1}; norm_model = 0;   % use empirical PDF

            otherwise
                error('Unknown string option');
        end
    end
end

% ------------------------------------------------------------
% * get PDF
% ------------------------------------------------------------

if norm_model
    % get normal PDF at p-th percentile
    % -------------------------------------------
    prc = norminv(p);
    pdfval = normpdf(prc);
else
    % empirical estimate of PDF from x (data)
    % -------------------------------------------
    % prc is x-score at prctile of interest
    prc = prctile(x,100*p);

    % now we need pdf at that prctile.
    % pick some unit h, and differentiate around prc

    h = max(.01,1000 ./ length(x));  % enough so we have reasonable idea
    h = min(h,p);
    h = max(h,1000 ./ length(x));

    pdfval = 0;

    while pdfval == 0
        pdfval = emppdf(x,prc,h);

        h = h + .01;
    end
end

% ------------------------------------------------------------
% * get variance
% ------------------------------------------------------------

%for normal: fit = ( 1./ (normpdf(norminv(p)).^2) ) .* (p *(1-p)./N);
% from Brown and Wolfe, asymptotic
%var_prc = ( 1./ (pdfval.^2) ) .* (p *(1-p)./N);

% from Martin's bootstrap book, Ch 19, Eq. 19.7, p. 275
var_prc = ( 1./ (pdfval.^2) ) .* (p *(1-p)) .* (1./nboot + 1./n_in_sample);

if nargout < 2, return, end

% ------------------------------------------------------------
% * get confidence interval
% ------------------------------------------------------------

halfci_prc = 1.96 .* sqrt(var_prc);

% lower and upper bounds on p-value derived from distribution of x
pci = get_pval_ci(x,prc,halfci_prc);

if nargout < 3, return, end

% now get nboot needed to achieve target
% ---------------------------------------

% expression for tolerance: if upper bound of p-value is within 10% of p-value
maxN = 50000;               % maximum number of iterations
Nneeded = 500;              % starting estimate for nboot needed (should be fairly low)
Nstep = 500;                % increase Nneeded in units of Nstep until satisfied
p_upper_bound = p + .1 * p; % upper bound for p desired; now 10% larger than p
pcitarget = Inf;            % target conf. interval for p-values with Nneeded boot samples

% computations we don't have to repeat.  Divide by sqrt(N) to get halfci of prc
halfci_squared_determiner = 1.96 .* sqrt( ( 1./ (pdfval.^2) ) .* p *(1-p) );

while ( max(pcitarget) > p_upper_bound ) && ( Nneeded < maxN )

    Nneeded = Nneeded + Nstep;
    halfci_prc = halfci_squared_determiner ./ sqrt(Nneeded);
    pcitarget = get_pval_ci(x,prc,halfci_prc);

end

end

function pci = get_pval_ci(x,prc,halfci_prc)
% get confidence interval for p-value, based on data x, x-axis-value prc,
% and confidence half-interval for prc

if isempty(x)
    % normal CDF
    pci = [max(0,normcdf(prc - halfci_prc)) min(1,normcdf(prc + halfci_prc))];
else
    % empirical CDF
    pci = [max(0,empcdf(x,prc - halfci_prc)) min(1,empcdf(x,prc + halfci_prc))];
end

end

function pdfval = emppdf(x,prc,h)
% empirical PDF of data numerically differentiated in a window h
pdfval = ( sum(x <= prc + h) - sum(x <= prc - h) ) ./ (length(x) * 2 * h);

end

function p = empcdf(x,prc)
% empirical CDF of data x at prctile prc

p = sum(x <= prc) ./ length(x);
end


function [output, fs] = sp_johansson_impl_n3(inp_fs, input, time_skew)
% Reconstruction of a signal from non-uniform samples
% Based on Sindhi and Prabhu's implementation of Johansson's
% method

% index_mapping is used to reorded filters in case the first input
% signal is actually delayed comparing to the second and not vice versa
N = length(time_skew); % number of samplers
fs = inp_fs * N; % full sampling frequency

T_inp = 1/inp_fs;

% index_mapping is used to reorded filters in case the first input
% signal is actually delayed comparing to the second and not vice versa
[time_skew, index_mapping] = sort(time_skew);
x11 = cell2mat(input')';
x11 = x11(index_mapping, :);

TQ = 1; % Nyquist sampling period

% Decimation Periods - in our case all ADCs sample with the same rate
T = [3*TQ 3*TQ 3*TQ];

K = 0.5*lcm(2*T(1), 2*T(2))/TQ; % number of samples in recurrent period
K = 0.5*lcm(2*K, 2*T(3))/TQ;
capT = K*TQ; % the full sampling period - of all samplers
M = capT./T;
capM = lcm(M(1), M(2));
capM = lcm(capM, M(3));
excess = ceil((K-1)/capM);

maxf = K/(excess*capM+1);
K1 = K/maxf;

ML = min(cellfun('length', input)); % number of slices
w_c = 0.85;

LF = capM*2*K1+1;  %% min length of LF should be capM*2*K1
n = -(LF-1)/2:1:(LF-1)/2;

taus = time_skew / T_inp * capT; % ADC delays in seconds
tausI = sort([taus(1) taus(2) taus(3)]);
tauI = zeros(K,ML);
for p = 1:K
    tauI(p,:) = tausI(p)+(0:ML-1)*capT;
end;

r = tausI-TQ*(0:K-1);
r = r.';
w_o = w_c*pi*TQ;
hJ = zeros(K,LF);
C = zeros(1,LF);
Nt = (LF-1)/2;
for i = 1:K
    C = -2*sin(w_o*(n-r(1+(mod(i-1-n,K)))'))./(pi*(n-r(1+(mod(i-1-n,K)))'));
    C(isnan(C))=-2*w_o/pi;
    C = C.';
    S = zeros(LF,LF);
    for k = 1:LF
        S(k,:) = sin(w_o*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')))./(pi*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')));
    end;
    S(isnan(S))=w_o/pi;
    hJ(i,:) = -0.5*S\C;
end;

x1 = reshape(x11,1,size(x11,2)*K);
y1 = zeros(K,length(x1));
for j=1:K
    y1(j,:) = filter(hJ(j,:),1,x1);
    y1(j,:) = upsample(downsample(y1(j,:),K,j-1),K)/K;
    y1(j,:) = filter([zeros(1,j-1),1],1,y1(j,:));
end;
y = K*0.25*sum(y1,1);
delayJ = (size(hJ,2)-1)/2;
y = real(y(1+delayJ:end));

output = y;
end


%%********************************************************************
%% minEpts: minimum volume ellipsoid containing given
%%          points, V(:,1),...,V(:,m).
%%
%% max log(det(B))
%% s.t. || Bx + d || <= 1, for all x = V(:,1),...,V(:,m)
%%
%% [blk,At,C,b,OPTIONS,B,d] = minEpts(V,solve)
%%
%% For problem formulation, see
%% Vandenberghe, Boyd, Wu, Determinant maximization with linear
%% matrix inequalities, SIAM J. Matrix Analysis and Applications,
%% 19 (1998), pp. 499--533.
%%
%%********************************************************************

  function  [blk,At,C,b,OPTIONS,B,d] = minEpts(V,solve)

  if (nargin < 2); solve = 0; end;
%%
%% form data matrices
%%
  [p,m] =  size(V); N = (p+1)*m;
  blk{1,1} = 's'; blk{1,2} = (p+1)*ones(1,m);
  blk{2,1} = 's'; blk{2,2} = p;
%%
  count = 0;
  for j = 1:p
     s1 = V(j,:)'; i1 = [j:(p+1):(p+1)*(m-1)+j]; j1 = [p+1:p+1:(p+1)*m];
     tmp = sparse(i1,j1,s1,N,N);
     tmp = tmp + tmp';
     count = count + 1;
     F{1,count} = -tmp;
     F{2,count} = sparse(j,j,-1,p,p);
  end
  for j = 2:p
     for k = 1:j-1
        s1 = V(k,:)'; i1 = [j:(p+1):(p+1)*(m-1)+j]; j1 = [p+1:p+1:(p+1)*m];
        s2 = V(j,:)'; i2 = [k:(p+1):(p+1)*(m-1)+k]; j2 = [p+1:p+1:(p+1)*m];
        tmp  = sparse(i1,j1,s1,N,N);
        tmp  = tmp + sparse(i2,j2,s2,N,N);
        tmp = tmp + tmp';
        count = count + 1;
        F{1,count} = -tmp;
        F{2,count} = sparse([j k],[k j],[-1 -1],p,p);
     end
  end
  for j = 1:p
     s1 = ones(m,1);
     i1 = [j:(p+1):(p+1)*(m-1)+j]; j1 = [p+1:p+1:(p+1)*m];
     tmp = sparse(i1,j1,s1,N,N);
     tmp = tmp + tmp';
     count = count + 1;
     F{1,count} = -tmp;
     F{2,count} = sparse(p,p);
  end
  At = svec(blk,F,ones(2,1));
  C{1,1} = speye(N);
  C{2,1} = zeros(p);
  b = zeros(p*(p+1)/2+p,1);
  parbarrier{1,1} = 0;
  parbarrier{2,1} = 1;
  OPTIONS.parbarrier = parbarrier;
%%
%% || Bx + d || <= 1.
%% x'*(B'*B)*x + 2(B'*d)'*x  + d'*d <= 1.
%%
  if (solve)
     [obj,X,y,Z,info] = sqlp(blk,At,C,b,OPTIONS);
     if (length(y) ~= p*(p+3)/2)
        error('length of y not compatible with p');
     end
     B = diag(y(1:p));
     tmp = p;
     for k = 1:p-1
         B(k+1:p,k) = y(tmp + [1:p-k]);
         B(k,k+1:p) = B(k+1:p,k)';
         tmp = tmp + p-k;
     end;
     d = y(tmp+1:length(y));
  else
     B = []; d = [];
  end
%%********************************************************************


% Computes gain using the covariance matrix.
% Copyrights Farhat Masood (NUST) Pakistan

function [k,s] = kfilter(A,C,V1,V2,V12)%function [k,s] = kfilter(A,C,V1,V2,V12)%KFILTER can have arguments: (A,C,V1,V2) if there are no cross% products, V12=0.%     KFILTER calculates the kalman gain, k, and the stationary%     covariance matrix, s, using the Kalman filter for:%  %		x[t+1] = Ax[t] + Bu[t] + w1[t+1]%               y[t] = Cx[t] + Du[t] + w2[t]%%               E [w1(t+1)] [w1(t+1)]' =  [V1   V12;%                 [ w2(t) ] [ w2(t) ]      V12' V2 ]%%  where x is the mx1 vector of states, u is the nx1 vector of controls, y is%  the px1 vector of observables, A is mxm, B is mxn, C is pxm, V1 is mxm,%  V2 is pxp, V12 is mxp.%m=max(size(A));[rc,cc]=size(C);if nargin==4; V12=zeros(m,rc); end;if (rank(V2)==rc);  A=A-(V12/V2)*C;  V1=V1-V12*(V2\V12');  [k,s]=doubleo(A,C,V1,V2);  k=k+(V12/V2);else;  s0=.01*eye(m);  dd=1;  it=1;  maxit=1000;  while (dd>1e-8 & it<=maxit);    k0= (A*s0*C'+V12)/(V2+C*s0*C');    s1= A*s0*A' + V1 -(A*s0*C'+V12)*k0';    k1= (A*s1*C'+V12)/(V2+C*s1*C');    dd=max(max(abs(k1-k0)));    it=it+1;    s0=s1;  end;  k=k1;s=s0;  if it>=maxit;     disp('WARNING: Iteration limit of 1000 reached in KFILTER.M');   end;end;


function fourthorder
%%
%     laplace^2 u = f;   [0,1]^2
%               u = g_D;
%       Du\cdot n = g_N.
%
%more information , you can check fourorderdata.m,biharmonicP1.m,biharmonicP2.m
%biharmonicP3.m.
% Created by Jie Zhou.
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.
close all
%% Parameters
%maxN = 2e3;     theta = 0.5;
maxIt = 6;
N = zeros(maxIt,1);     errL2 = zeros(maxIt,1);     errH1 = zeros(maxIt,1);

%%  Generate an initial mesh
node = [0,0; 0,1; 1,0;1,1];        % nodes
elem = [1,3,2; 4,2,3];                 % elements
elem = label(node,elem);               % label the mesh
bdEdge = setboundary(node,elem,'Dirichlet');    % Dirichlet boundary condition

% showmesh(node,elem);                            % plot mesh
% findelem(node,elem,'all','index','color','g');  % plot element indices
% findnode(node,'all','index','color','r');       % plot node indices

%%  Get a fine mesh by uniform bisection
for k = 1:2
     [node,elem,bdEdge] = uniformbisect(node,elem,bdEdge);
%    [node,elem,bdEdge] = bisect(node,elem,'all',bdEdge);
end

showmesh(node,elem);
findnode(node,'all','index','color','r');
findelem(node,elem,'all','index','color','r');

%% Set up PDE data
pde = fourorderdata;

%%       Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*
 for k = 1:maxIt
%                   [w,u] = biharmonicP1(node,elem,pde,bdEdge);
                    [w,u] = biharmonicP2(node,elem,pde,bdEdge);
%                   [w,u] = biharmonicP3(node,elem,pde,bdEdge);
               errL2(k) = getL2error(node,elem,pde.exactu,u);
               errH1(k) = getH1error(node,elem,pde.Du,u);
                   N(k) = size(w,1)+size(u,1);
     [node,elem,bdEdge] = uniformbisect(node,elem,bdEdge);
 end

%  Plot convergence rates
N= N(1:k);  errH1 = errH1(1:k);     errL2 = errL2(1:k);
figure;
showrate2(N,errH1,3,'-*','||Du-Du_h||',...
          N,errL2,3,'k-+','||u-u_h||');


function fe2d_r_fast_test ( )

%*****************************************************************************80
%
%% FE2D_R_FAST_TEST tests the FE2D_R_FAST code.
%
%  Discussion:
%
%    This function sets all parameter values and initial condition information
%    necessary to execute the "fast" version of the fe2d_r algorithm.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Reference:
%
%    Marcus R Garvie, John Burkardt, Jeff Morgan,
%    Simple Finite Element Methods for Approximating Predator-Prey Dynamics
%    in Two Dimensions using MATLAB,
%    Submitted to Bulletin of Mathematical Biology, 2014.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2D_R_FAST_TEST:\n' );
  fprintf ( 1, '  Test the FE2D_R_FAST function\n' );
  fprintf ( 1, '  which applies Robin boundary conditions as it\n' );
  fprintf ( 1, '  approximates a solution to a predator-prey system.\n' );
%
%  Set the parameters.
%
  alpha = 0.4;
  beta = 2.0;
  gamma = 0.6;
  delta = 1.0;
%
%  Use T=150.0 for normal run.
%  Use T=0.50 for a "quick" run that might take 15 minutes of computing.
%
% T = 150.0;
  T = 0.50;
  delt = 1.0 / 384.0;
  k1 = 0.01;
  k2 = 0.01;

  t = tic;
  fe2d_r_fast ( alpha, beta, gamma, delta, T, delt, @u0f, @v0f, k1, k2 );
  t = toc ( t );

  fprintf ( 1, '  Execution took %10.2g minutes \n', t / 60.0 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2D_R_FAST_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end

function value = u0f ( x, y )

%*****************************************************************************80
%
%% U0F evaluates the initial condition for U.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for U at (X,Y).
%
  value = 6.0 / 35.0 - 2.0E-07 * ( x - 0.1 * y - 225.0 ) * ( x - 0.1 * y - 675.0 );

  return
end

function value = v0f ( x, y )

%*****************************************************************************80
%
%% V0F evaluates the initial condition for V.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for V at (X,Y).
%
  value = 116.0 / 245.0 - 3.0E-05 * ( x - 450.0 ) - 1.2E-04 * ( y - 150.0 );

  return
end


function [W,Idx] = barycentricWeights(X,xBnd,N)
% [W,Idx] = barycentricWeights(X,xBnd,N)
%
% This function computes the weights and indicies such that
%
% INPUTS:
%   X = [d , n] = n points of interest, each is d-dimensional
%   xBnd = [d , 2] = [min,max] along each dimension
%   N = [1, d] = number of grid points along each dimension
%
% OUTPUTS:
%   W = [d+1, n] weight to apply to each index
%   Idx = [d+1, n] linear index corresponding to each weight
%
% NOTES:
%   --> y = f(x), where x is d-dimensional, and y is scalar
%   --> Y = Y(x1,x2,...,xd)
%   --> n = size(Y);
%   --> y(X(:,i)) = dot(W(:,i)*Y(Idx(:,i));
%
% ASSUME:
%   --> all data is uniformly spaced:
%       {xi} = linspace(xBnd(i,1), xBnd(i,2), n(i));
%
% REFERENCE:
%
% Based on paper: "Multidimensional Triangulation and Interpolation for
% Reinforcement Learning" by Scott Davies, NIPS 1996
%
% See Also: barycentricInterpolate

[d, n] = size(X);
W = zeros(d+1,n);
Idx = zeros(d+1,n);
for i=1:n
    [W(:,i),Idx(:,i)] = barycentricWeightsCore(X(:,i),xBnd,N);
end

end

function [w,idx] = barycentricWeightsCore(x,xBnd,N)
% [w,idx] = barycentricWeights(x,xBnd,n)
%
% This function computes the weights and indicies such that y = Y(idx)*w,
% where Y is some data, stored on a length(N) dimensional grid.
%
% INPUTS:
%   x = [d , 1] point of interest
%   xBnd = [d , 2] = [min,max] along each dimension
%   N = [1, d] = number of grid points along each dimension
%
% OUTPUTS:
%   w = [d+1,1] weight to apply to each index
%   idx = [d+1,1] linear index corresponding to each weight
%
% NOTES:
%   --> y = f(x), where x is d-dimensional, and y is scalar
%   --> Y = Y(x1,x2,...,xd)
%   --> n = size(Y);
%   --> y = dot(Y(idx),w);
%
% ASSUME:
%   --> all data is uniformly spaced:
%       {xi} = linspace(xBnd(i,1), xBnd(i,2), n(i));
%
% REFERENCE:
%
% Based on paper: "Multidimensional Triangulation and Interpolation for
% Reinforcement Learning" by Scott Davies, NIPS 1996

% Coerce any data that is out of range:
checkLow = x<xBnd(:,1); x(checkLow) = xBnd(checkLow,1);
checkUpp = x>xBnd(:,2); x(checkUpp) = xBnd(checkUpp,2);

% Translate and scale such that each grid cell is a unit cube:
x = x-xBnd(:,1);
x = x./(xBnd(:,2)- xBnd(:,1));
x = x.*(N-1);

% Seperate out into bin number and fractional length:
xBaseIdx = floor(x);  %Figure out the lower index (bin) number
checkCeil = xBaseIdx==(N-1); newCeil = N-2;
xBaseIdx(checkCeil) = newCeil(checkCeil);  %Fix special case for points on upper grid
x = x-xBaseIdx; %Keep the fraction along each dimension:

% Figure out which dimensions to build simplex along:
[~,I] = sort(x); %x must be a column vector!

% Build simplex by walking along each successive dimension
% The matrix X stores the verticies of the simplex, where each vertex is on
% the unit hypercube. The first vertex is always zeros(n,1) and the last is
% always ones(n,1). The intermediate verticies are selected based on the
% sort step. Since X will be triangular, and all entries are unity, it is
% trivial to solve the system: x = X*w for the unknown weights w by row
% reduction.
n = length(N);
X = zeros(n,n+1);
w = zeros(n+1,1);
wSum = 0;
for i=1:n
    X(I(i), (n+2-i):(n+1)) = 1;
    w(n+2-i) = x(I(i)) - wSum;
    wSum = wSum + w(n+2-i);
end
w(1) = 1-wSum;  %Final convex combination

% Compute the linear indices:
idx = zeros(n+1,1);
for i=1:(n+1)
    idx(i) = sub2idx(N, X(:,i)+xBaseIdx+1, n);
end

end

%%%% Custom version of sub2ind, that takes a vector input for the desired
%%%% input dimension, rather than a comma seperated list.
function idx = sub2idx(n,idxVec,d)
%
% Based on Matlab's sub2ind command
% Modified by Matthew Kelly, April 8, 2015
%
% CHANGES:
%   - must explicitly enter all dimensions: length(n) == length(idxVec)
%   - pass through for scalar: length(n)==1 is valid input
%   - must pass desired subscripts as a vector (not a comma-seperated-list)
%   - disabled most error checking
%
% INPUTS:
%   n = size(dataMatrix)
%   idxVec = list of desired indices. For example A(2,1) -> [2,1]
%
% OUTPUTS:
%   idx = linear index, such that:
%
%       A(idx) = A(idxVec(1), ... , idxVec(length(n));
%

if d == 1
    idx = idxVec;
elseif d ==2
    idx = idxVec(1) + (idxVec(2) - 1).*n(1);
else
    %Compute linear indices
    k = [1 cumprod(n(1:end-1))'];
    idx = 1;
    for i = 1:d
        v = idxVec(i);
        idx = idx + (v-1)*k(i);
    end
end

end


function [Xhat] = aprxMAPGMM(Y,patchSize,noiseSD,imsize,GS,excludeList,SigmaNoise)
% approximate GMM MAP estimation - a single iteration of the "hard version"
% EM MAP procedure (see paper for a reference)
%
% Inputs:
%   Y - the noisy patches (in columns)
%   noiseSD - noise standard deviation
%   imsize - size of the original image (not used in this case, but may be
%   used for non local priors)
%   GS - the gaussian mixture model structure
%   excludeList - used only for inpainting, misleading name - it's a list
%   of patch indices to use for estimation, the rest are just ignored
%   SigmaNoise - if the noise is non-white, this is the noise covariance
%   matrix
%
% Outputs:
%   Xhat - the restore patches

% handle exclusion list - used for inpainting
if ~exist('excludeList','var')
    excludeList = [];
end

% Supports general noise covariance matrices
if (~exist('SigmaNoise','var'))
    SigmaNoise = noiseSD^2*eye(patchSize^2);
end

if ~isempty(excludeList)
    T = Y;
    Y = Y(:,excludeList);
end

% remove DC component
meanY = mean(Y);
Y = bsxfun(@minus,Y,meanY);

% calculate assignment probabilities for each mixture component for all
% patches
GS2 = GS;
PYZ = zeros(GS.nmodels,size(Y,2));
for i=1:GS.nmodels
    GS2.covs(:,:,i) = GS.covs(:,:,i) + SigmaNoise;
    PYZ(i,:) = log(GS.mixweights(i)) + loggausspdf2(Y,GS2.covs(:,:,i));
end

% find the most likely component for each patch
[~,ks] = max(PYZ);

% and now perform weiner filtering
Xhat = zeros(size(Y));
for i=1:GS.nmodels
    inds = find(ks==i);
    Xhat(:,inds) = ((GS.covs(:,:,i)+SigmaNoise)\(GS.covs(:,:,i)*Y(:,inds) + SigmaNoise*repmat(GS.means(:,i),1,length(inds))));
end

% handle exclusion list stuff (inpainting only)
if ~isempty(excludeList)
    tt = T;
    tt(:,excludeList) = bsxfun(@plus,Xhat,meanY);
    Xhat = tt;
else
    Xhat = bsxfun(@plus,Xhat,meanY);
end