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

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

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

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

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

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

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

factors = cell(1,3);

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

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

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

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

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

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

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

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

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

  end
end

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

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

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

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

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

engine = enter_evidence(engine, evidence);

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

Ihat


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

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

%addpath PROPACK;

%elapsed = tic;

[m n] = size(D);

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

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

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

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

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

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

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

mark = false;
Block_mark = false ;

while ~converged
    iter = iter + 1;

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

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

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

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

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

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

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

    total_svd = total_svd + 1;

    Z = D - A_hat - E_hat;

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

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

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

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

%elapsed = toc(elapsed);


%% This file is the ODE file used to simulate the Michaelis-Menten kinetics.
% Date: 04/24/2019
% Coded By: K

% Here Vmax is rhe maximum rate of reaction time
% Km is the concentration of half-maximal reaction rate
%

%% ODE functions
function dx=MMK_ODE(t,x,jx,Vmax,Km)
dx=jx-(Vmax*x(1,:)./(Km+x(1,:)));


function [ x, know ] = p05_sol ( m, know )

%*****************************************************************************80
%
%% P05_SOL returns known solutions for problem 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input/output, integer KNOW.
%    On input, KNOW is 0, or the index of the previously returned solution.
%    On output, KNOW is 0 if there are no more solutions, or it is the
%    index of the next solution.
%
%    Output, real X(M), the solution.
%
  if ( know == 0 )
    know = 1;
    x(1:m,1) = [ ...
       3.0 / 11.0, ...
       6.0 / 13.0, ...
      12.0 / 23.0, ...
       8.0 / 37.0 ]';
  else
    know = 0;
    x = zeros ( m, 1 );
  end

  return
end


function [AD,Rank] = UpdateDiversityArchive(AD,Population,ND,R,Z,Xre,sigma_niche,Problem)
% Update the Diversity Archive

%------------------------------- Copyright --------------------------------
% Copyright 2017-2018 Yiping Liu
% Please contact {yiping0liu@gmail.com} if you have any problem.
%--------------------------------------------------------------------------

    %% Non-dominated sorting
    Population = [Population,AD];
    [FrontNo,MaxFNo] = NDSort(Population.objs,ND);

    %% Select the solutions in the last front
    if length(find(FrontNo<=MaxFNo))== ND
       Next = FrontNo<=MaxFNo;
    else
       Next = FrontNo < MaxFNo ;
       NQ     = ND-sum(Next);
       Last   = find(FrontNo==MaxFNo);
       Choose = LastSelection(Population(Last).objs,Population(Last).decs,NQ,R,Z,Xre,sigma_niche,Problem); %Algorithm 4, lines 5-30
       Next(Last(Choose)) = true;
    end

    %% Population for next generation
    AD = Population(Next);
    Rank = FrontNo(Next);
end

function Choose = LastSelection(PopObj,PopDec,NQ,R,Z,Xre,sigma_niche,Problem)
% Select solutions with good diversity in the last front
    N      = size(PopObj,1);
    NR     = size(R,1);

    %% Normalize
    PopObj = PopObj - repmat(Z,N,1);
    PopDec = (PopDec - repmat(Problem.lower,N,1))./repmat(Problem.upper-Problem.lower,N,1);

    %% Calculate distance between every solution and reference point in the objective space
    theta = pdist2(R,PopObj,'cosine');

    %% Calculate distance between every two solutions in the reminder decsion subspace
    d = pdist2(PopDec(:,Xre),PopDec(:,Xre),'chebychev');

    %% Cluster
    [thmin,label] = min(theta,[],1);
    C1 = false(NR,N);
    C2 = false(NR,N);
    for j = 1:NR
        member = find(label==j);
        member1 = label==j;
        [~,temp] = sort(thmin(member));
        for i = temp
           if any(d(member(i),and(C1(j,:),member1))<sigma_niche)
               C2(j,member(i))=true;
           else
               C1(j,member(i))=true;
           end
        end
    end

    %% Make selected solution == NQ
    while sum(sum(C1))>NQ
        cmax = max(sum(C1,2));
        jmax = sum(C1,2)== cmax;
        temp1 = find(sum(C1(jmax,:),1)>0);
        [~,xmax] = max(thmin(temp1));
        xmax=temp1(xmax);
        C1(label(xmax),xmax) = false;
    end
    while sum(sum(C1))<NQ
        c2 = sum(C2,2)>0;
        cmin = min(sum(C1(c2,:),2));
        jmin = and(sum(C1,2)==cmin,c2);
        temp1 = find(sum(C2(jmin,:),1)>0);
        [~,xmin] = min(thmin(temp1));
        xmin=temp1(xmin);
        C2(label(xmin),xmin) = false;
        C1(label(xmin),xmin) = true;
    end
    Choose = sum(C1)>0;
end


function pass = test_ultrapts(pref)

% Choose a tolerance:
tol = 1e-14;

% Test a small n (using REC)
n = 42;
lambda = .3;
x = ultrapts(n, lambda);
pass(1) = all(size(x) == [n, 1]);
[x, w, v] = ultrapts(n, lambda);
pass(2) = all(size(x) == [n, 1]) && all(size(w) == [1, n]) && all(size(v) == [n, 1]);
pass(3) = abs(w*x) < tol && abs(w*x.^2 - 0.8843414686338345) < tol;
pass(4) = abs(x(37) - 9.131896381993957E-01) < tol;
pass(5) = abs(w(37) - 4.332670514309510E-02) < tol;
pass(6) = abs(v(37) - 3.115587460502451E-01) < tol;

% Test on [0, 10]:
[x, w, v] = ultrapts(n, lambda, [0, 10]);
pass(7) = abs(w*x - 30.1957169274024) < 31*tol && abs(w*x.^2 - 209.0472710358626) < 300*tol;
pass(8) = abs(x(38) - 9.704505777068543E+00) < tol;
pass(9) = abs(w(38) - 1.018229378664342E-01) < tol;
pass(10) = abs(v(38) + 2.449177929215358E-01) < tol;

% Test a larger n
lambda = .8;
n = 251;
x = ultrapts(n, lambda);
pass(11) = all(size(x) == [n, 1]);
[x, w, v] = ultrapts(n, lambda);
pass(12) = all(size(x) == [n, 1]) && all(size(w) == [1, n]) && all(size(v) == [n, 1]);
pass(13) = abs(w*x) < tol && abs(w*x.^2 - 0.4744211549960596) < tol;
pass(14) = abs(x(37) + 8.958806879214126E-01) < tol;
pass(15) = abs(w(37) - 3.406945649865882E-03) < tol;
pass(16) = abs(v(37) - 2.321704534650446E-01) < tol;

% Test on [0, 10]:
[x, w, v] = ultrapts(n, lambda, [0, 10]);
pass(17) = abs(w*x - 112.1472319135050) < 100*tol && abs(w*x.^2 - 716.4962038918374) < 100*tol;
pass(18) = abs(x(38) - 5.486606034997460E-01 ) < tol;
pass(19) = abs(w(38) - 4.655102134393607E-02) < tol;
pass(20) = abs(v(38) + 2.427562206703888E-01 ) < tol;

% Test a larger n with a larger lambda
lambda = 7;
n = 551;
x = ultrapts(n, lambda,'asy');
pass(21) = all(size(x) == [n, 1]);
[x, w, v] = ultrapts(n, lambda,'asy');
pass(22) = all(size(x) == [n, 1]) && all(size(w) == [1, n]) && all(size(v) == [n, 1]);
pass(23) = abs(w*x) < tol && abs(w*x.^2 - (429/32768)*pi) < tol;
pass(24) = abs(x(37) + 9.748144265829347E-01) < tol;
pass(25) = abs(w(37) - 4.244751593204416E-12) < tol;
pass(26) = abs(v(37) - 6.123401324799126E-06) < tol;

% Test a larger n with a larger lambda
lambda = 25.5;
n = 5e3;
x = ultrapts(n, lambda,'asy');
pass(27) = all(size(x) == [n, 1]);
[x, w, v] = ultrapts(n, lambda,'asy');
pass(28) = all(size(x) == [n, 1]) && all(size(w) == [1, n]) && all(size(v) == [n, 1]);
pass(29) = abs(w*x) < tol && abs(w*x.^2 - 281474976710656/42710983650155457) < tol;
pass(30) = abs(sum(w) - 281474976710656/805867616040669 ) < tol;

% Test n = 1:
lambda = .6;
[x, w] = ultrapts(1, lambda);
pass(31) = (x == 0);
pass(32) = abs(w - 1.887181162535959) < tol;
[x, w] = ultrapts(1, lambda, [-10, 3]);
pass(33) = abs( (x + 10) + (x-3) ) < tol ; % midpoint
pass(34) = abs( w*x + 62.42774750787926 ) < 2*tol;

% Test n = 2:
[x, w] = ultrapts(2, lambda);
pass(35) = all(abs(x - [-sqrt(5)/4; sqrt(5)/4]) < tol);
pass(36) = all(abs(w - [.5*gamma(lambda+.5)*sqrt(pi)/gamma(lambda+1),...
    .5*gamma(lambda+.5)*sqrt(pi)/gamma(lambda+1)]) < tol);
[x, w] = ultrapts(2, lambda, [-10, 3]);
pass(37) = abs(sum(w) - 17.83649928796550) < tol;
pass(38) = abs( w*x + 62.42774750787926 ) < 10*tol;
pass(39) = abs( w*x.^2 - 453.9946459389969 ) < 100*tol;
pass(40) = abs( w*x.^3 + 3237.463968416426 ) < 100*tol;
end


% KATAMA_SSH
% grab time series of water levels from Chen's GOM3 NECOFS forecast model archive
% for points north and south of Katama Bay, using NCTOOLBOX
%
% Quick NCTOOLBOX Install:
% 1. Grab and unzip: https://github.com/acrosby/nctoolbox_recent/zipball/master
% 2. Run "setup_nctoolbox"

% OPeNDAP URL for Chen's Archive of NECOFS GOM3 Forecast
url='http://www.smast.umassd.edu:8080/thredds/dodsC/fvcom/archives/necofs_gom3';
nc=ncgeodataset(url);
var='zeta';
zvar=nc.geovariable(var);
start=[2011 6 1 0 0 0];
stop=[2011 9 15 0 0 0];

tdat=zvar.timewindowij(start,stop);
% picked these two spots (north, and south of Katama Bay)
loni=[ -70.4893  -70.4873];
lati=[  41.3973   41.3339];
disp(['Reading time series data from ' url '...'])
dat=nj_tseries(nc,'zeta',loni,lati,'method','nearest',...
      'itime',tdat.index,'ele',1);

save katama_ssh.mat dat
% plot whole time series of water levels
figure(1);
set(gcf,'pos',[40 100 900 400]);
plot(dat.time,dat.vals);datetick
legend('north','south');
title('Water levels from NECOFS GOM3')
grid;set(gcf,'color','white');

% zoom into Irene
figure(2)
set(gcf,'pos',[40 150 900 400]);
ii=date_index(dat.time,[2011 8 23 0 0 0],[2011 9 3 0 0 0]);
plot(dat.time(ii),dat.vals(ii,:));datetick
legend('north','south');
title('Water levels from NECOFS GOM3')
grid;set(gcf,'color','white');

%% plot the model bathymetry and locations
figure(3);
lon = nc.data(zvar.getlonname);
lat = nc.data(zvar.getlatname);
depth=nc{'h'}(:);
gvar=zvar.attribute('mesh'); % find mesh variable
grd=nc{gvar}(:); % get the mesh (connectivity array)
[m,n]=size(grd);
if m==3,grd=grd.';elseif n~=3;disp('Error:triangles only');return;end
%zeta=nc{var}(tdat.index(1),:);
%%
trisurf(grd,lon,lat,zeros(size(depth)),-depth);view(2);...
  shading interp;colorbar;dasp(lat(1));
axis([-70.55 -70.40 41.30  41.44]);
caxis([-20 0])
line(loni,lati,'marker','x','linestyle','none','color','black','markersize',14)
title('Katama Bay area bathymetry in NECOFS GOM3 forecast model (m)')
set(gcf,'color','white');
set(gca,'tickdir','out');


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

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

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

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

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

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

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

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

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


classdef gnonomicProjection < sphericalProjection
  % gnonomic projection

  methods

     function proj = gnonomicProjection(varargin)
      proj = proj@sphericalProjection(varargin{:});
    end

    function [x,y] = project(sP,v,varargin)
      % compute polar angles

      [rho,theta] = project@sphericalProjection(sP,v,varargin{:});

      % map to upper hemisphere
      ind = find(theta > pi/2+10^(-10));
      theta(ind)  = pi - theta(ind);

      % turn around antipodal vectors
      sP.sR.antipodal = false; v.antipodal = false;
      ind = ~sP.sR.checkInside(v);
      rho(ind) = rho(ind) + pi;

      % formula for stereographic projection
      r =  tan(theta);

      % compute coordinates
      x = reshape(cos(rho) .* r,size(v));
      y = reshape(sin(rho) .* r,size(v));

    end

    function v = iproject(sP,x,y)
      rho = atan2(y,x);
      theta = atan(sqrt(x.^2 + y.^2));
      v = vector3d('theta',theta,'rho',rho);
    end

  end

end


function [obj,objs] = dcc_inference_objective(parameters,data,dataAsym,m,l,n,univariate) %#ok<INUSL>
% Objective function used by DCC, CCC and related multivariate volatility models to perform
% inference.  It is not used (and cannot be used) to estimate parameters.
%
% USAGE:
%  [OBJ,OBJS] = dcc_inference_objective(PARAMETERS,DATA,DATAASYM,M,L,N,UNIVARIAT)
%
% INPUTS:
%   PARAMETERS   - Vector of ADCC parameters including possibly volatility and intercepts
%   DATA         - A T by K matrix of zero mean residuals -OR-
%                    K by K by T array of covariance estimators (e.g. realized covariance)
%   DATAASYM     - [OPTIONAL] K by K by T array of asymmetric covariance estimators only needed if
%                    DATA is 3-dimensional and O>0 or L>0
%   M            - Order of symmetric innovations in DCC model
%   L            - Order of asymmetric innovations in ADCC model
%   N            - Order of lagged correlation in DCC model
%   UNIVARIATE   - Cell array of structures containing information needed to compute volatilities.
%
% OUTPUTS:
%   OBJ          - "As if" objective for estimating correlation intercept parameters
%   OBJS         - T by 1 vector of "as if" objectives for estimating correlation intercept parameters
%
% COMMENTS:
%
% See also DCC, CCC_MVGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 4/13/2012

[k,~,T] = size(data);
% Parse parameters
count = 0;
for i=1:k
    u = univariate{i};
    count = count + u.p+u.o+u.q+1;
end
garchParameters = parameters(1:count);
offset = count;
% R is next
count = k*(k-1)/2;
R = corr_ivech(parameters(offset + (1:count)));
offset = offset + count;
if l>0
    count = k*(k+1)/2;
    N = ivech(parameters(offset+(1:count)));
end

H = dcc_reconstruct_variance(garchParameters,univariate);
stdData = zeros(k,k,T);
stdDataAsym = zeros(k,k,T);
for t=1:T
    h = sqrt(H(t,:));
    stdData(:,:,t) = data(:,:,t)./(h'*h);
    stdDataAsym(:,:,t) = dataAsym(:,:,t)./(h'*h);
end

scales = diag(mean(stdData,3));
objs = zeros(T,1);
for j=1:k-1 % Cols
    for i=j+1:k % Rows
        scale = sqrt(scales(i)*scales(j));
        errors = squeeze(stdData(i,j,:))/scale - R(i,j);
        objs = objs + 0.5*(errors.^2);
    end
end

if l>0
    for j=1:k
        for i= j:k
            errors = squeeze(stdDataAsym(i,j,:)) - N(i,j);
            objs = objs + 0.5*(errors.^2);
        end
    end
end

obj = sum(objs);


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

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

  fpu = 10.0 ^ sae;

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

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

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

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

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

  if ( 1.0 < pp && 1.0 < qq )

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

  else

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

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

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

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

    end

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

  if ( value < 0.0001 )
    value = 0.0001;
  end

  if ( 0.9999 < value )
    value = 0.9999;
  end

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

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

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

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

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

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

    g = 1.0;

    while ( 1 )

      while ( 1 )

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

        if ( sq < prev )

          tx = value - adj;

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

        end

        g = g / 3.0;

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

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

      g = g / 3.0;

    end

    if ( tx == value )
      break
    end

    value = tx;
    yprev = y;

  end

  if ( indx )
    value = 1.0 - value;
  end

  return
end


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

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

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

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

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

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


function [psfMatData, p] = constructMatrix_new( PSF, center, boundary, imsize )
%
%       psfMatData = constructMatrix_new( PSF, center, boundary, imsize );
%
%  Construct psfMatrix data.
%
%  Given a PSF and the locatio of the corresponding point source,
%  this function sets up the data needed to do efficient matrix-vector
%  multiplication.
%
%  This is meant only for spatially invariant PSFs.
%
%  Input:
%         PSF  -  PSF image array
%      center  -  location of point source
%    boundary  -  desired boundary condition
%      imsize  -  size of restored image
%
%  Output:
%   psfMatData -  array containing the (complex) data needed to
%                 do efficient matrix-vector multiplications.
%
switch boundary
    case 'periodic'
        padSize = imsize - size(PSF);
        PSF = padarray(PSF, padSize, 'post');
        psfMatData = fft2(circshift(PSF, 1-center));
        p = [];
    otherwise
        [U, S, V] = svd(PSF);
        [m, n] = size(PSF);
        imsize_pad = 2.^nextpow2(2*imsize);
        minU = abs(min(min(U(:,1))));
        maxU = max(max(abs(U(:,1))));
       if minU == maxU
           U = -U;
           V = -V;
       end
       %
       %  Find number of Kronecker terms to represent the matrix.
       %
       Rtol = 10*eps;
       MaxTerms = sum(diag(S)/S(1,1) >= Rtol);

       %
       %  Get quantities needed to multiply by the Toeplitz pieces:
       %
       bidx_top1 = 1:n-center(2)+1;
       bidx_top2 = center(2):n;
       bidx_bot1 = imsize_pad(2)-center(2)+2:imsize_pad(2);
       bidx_bot2 = 1:center(2)-1;
       cidx_top1 = 1:m-center(1)+1;
       cidx_top2 = center(1):m;
       cidx_bot1 = imsize_pad(1)-center(1)+2:imsize_pad(1);
       cidx_bot2 = 1:center(1)-1;

       BT = V(:,1:MaxTerms)*sqrt(S(1:MaxTerms,1:MaxTerms));
       CT = U(:,1:MaxTerms)*sqrt(S(1:MaxTerms,1:MaxTerms));

       BT_hat = zeros(imsize_pad(2), MaxTerms);
       CT_hat = zeros(imsize_pad(1), MaxTerms);

       BT_hat(bidx_top1,:) = BT(bidx_top2,:);
       BT_hat(bidx_bot1,:) = BT(bidx_bot2,:);
       CT_hat(cidx_top1,:) = CT(cidx_top2,:);
       CT_hat(cidx_bot1,:) = CT(cidx_bot2,:);
       lambdaT = fft(CT_hat);
       deltaT = fft(BT_hat);

       %
       %  Get quantities needed to multiply by the Hankel pieces:
       %
       bidx_top1 = 1:n-center(2);
       bidx_top2 = center(2)+1:n;
       bidx_bot1 = imsize_pad(2)-center(2)+1:imsize_pad(2)-1;
       bidx_bot2 = 1:center(2)-1;
       cidx_top1 = 1:m-center(1);
       cidx_top2 = center(1)+1:m;
       cidx_bot1 = imsize_pad(1)-center(1)+1:imsize_pad(1)-1;
       cidx_bot2 = 1:center(1)-1;

       BH_hat = zeros(imsize_pad(2), MaxTerms);
       CH_hat = zeros(imsize_pad(1), MaxTerms);
       BH_hat(bidx_top1,:) = BT(bidx_top2,:);
       BH_hat(bidx_bot1,:) = BT(bidx_bot2,:);
       CH_hat(cidx_top1,:) = CT(cidx_top2,:);
       CH_hat(cidx_bot1,:) = CT(cidx_bot2,:);
       lambdaH = fft(CH_hat);
       deltaH = fft(BH_hat);

       %
       %  Need to permute these with a shift matrix:
       %
       pB = [1;(imsize_pad(2):-1:2)'];
       pC = [1;(imsize_pad(1):-1:2)'];

       lambdaH = lambdaH(pC,:);
       deltaH = deltaH(pB,:);

       psfMatData = zeros(imsize_pad(1), imsize_pad(2), 4);
       psfMatData(:,:,1) = lambdaT*deltaT';
       psfMatData(:,:,2) = lambdaT*deltaH';
       psfMatData(:,:,3) = lambdaH*deltaT';
       psfMatData(:,:,4) = lambdaH*deltaH';
       p = [pB, pC];

end


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

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

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

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

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

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

% Outflow conditions -- supersonic outflow ( do nothing )

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

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

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


%
%  Generate an equal ripple minimum phase filter starting with a linear
%  phase filter.
%
%  Called by dzmp.

%  written by John Pauly, 1992
%  (c) Board of Trustees, Leland Stanford Junior University

function hmp = fmp(h)

l = length(h);
if rem(l,2) == 0,
   disp('filter length must be odd');
   return;
end;
lp = 8*exp(ceil(log(l)/log(2))*log(2));
hp = [zeros(1,ceil((lp-l)/2)) h zeros(1,floor((lp-l)/2))];
hpf = fftc(hp);
hpfs = hpf-min(real(hpf))*1.000001;
hpfmp = mag2mp(sqrt(abs(hpfs)));
hpmp = ifft(fftshift(conj(hpfmp)));
hmp = hpmp(1:(l+1)/2);


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

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

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

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

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

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

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

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

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

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

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

    [n ,n ] = size(L);

    V = Cinv'*V;

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

    W = L\V;

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

end

return


function WtW = pairwise_dists(WU, WUinit)

WU = reshape(WU, [], size(WU,3));
WUinit = reshape(WUinit, [], size(WUinit,3));
WtW = WU' * WUinit;

mu = sum(WU.^2,1);
mu = mu(:);

muinit = sum(WUinit.^2,1);
muinit = muinit(:);

mu     = repmat(mu, 1, size(WUinit,2));
muinit = repmat(muinit', size(WU,2), 1);

WtW = 1 - 2*WtW ./ (muinit + mu);


function plot_3d_2( data1, data2, theta )

    data1 = rotate(data1, theta);
    x1 = data1(:, 1);
    y1 = data1(:, 2);
    z1 = data1(:, 3);

    data2 = rotate(data2, theta);
    x2 = data2(:, 1);
    y2 = data2(:, 2);
    z2 = data2(:, 3);

    figure();
    scatter3(x1, y1, z1, 'b');
    hold on;
    scatter3(x2, y2, z2, 'r');
    hold off;

end


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

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

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

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

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

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

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

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

    else

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

    end

else

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

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

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

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

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

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

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

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

end


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

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

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

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

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

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

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

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

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

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

end

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


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

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

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

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

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

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

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


function [U_final, V_final, nIter_final, objhistory_final] = LCCF(X, k, W, options, U, V)
% Locally Consistant Concept Factorization (LCCF)
%
% where
%   X
% Notation:
% X ... (mFea x nSmp) data matrix
%       mFea  ... number of words (vocabulary size)
%       nSmp  ... number of documents
% k ... number of hidden factors
% W ... weight matrix of the affinity graph
%
% options ... Structure holding all settings
%
% You only need to provide the above four inputs.
%
% X = X*U*V'
%
% References:
% [1] Deng Cai, Xiaofei He, Jiawei Han, "Locally Consistent Concept
%     Factorization for Document Clustering", IEEE Transactions on Knowledge
%     and Data Engineering, Vol. 23, No. 6, pp. 902-913, 2011.
%
%
%   version 2.0 --April/2010
%   version 1.0 --Dec./2008
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

nSmp=size(X,2);

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

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

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

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

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

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

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

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

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

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

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


% TEST_MAXWELL_CUBE_G_NMNN: data function for Neumann boundary condition.

function g = test_maxwell_cube_g_nmnn (x, y, z, ind)

  g = zeros ([3, size(x)]);
  switch (ind)
    case 1
      g(2,:,:) = -(exp(x) .* cos(y) - z .* cos(y));
      g(3,:,:) = sin(y) + exp(z) .* sin(x);
    case 2
      g(2,:,:) = exp(x) .* cos(y) - z .* cos(y);
      g(3,:,:) = -(sin(y) + exp(z) .* sin(x));
    case 3
      g(1,:,:) = exp(x) .* cos(y) - z .* cos(y);
    case 4
      g(1,:,:) = -(exp(x) .* cos(y) - z .* cos(y));
    case 5
      g(1,:,:) = -(sin(y) + exp(z) .* sin(x));
    case 6
      g(1,:,:) = sin(y) + exp(z) .* sin(x);
    otherwise
      error ('g_nmnn: unknown reference number')
  end

end


function [km2] = cm22km2(cm2)
% Convert area from square centimeters to square kilometers.
% Chad A. Greene 2012
km2 = cm2*1E-10;


%    The following is an implementation of "the HMSD fusion based on the guided filter (HMSD-GF)"
%    (without visibility enhancement before the fusion).
%    Ref: "Fusion of infrared and visible images for night-vision context
%    enhancement", Applied Optics, 55(23), 2016
%
%    The HMSD (Hybrid-MSD) fusion method was originally proposed in:
%    Zhiqiang Zhou et al. "Perceptual fusion of infrared and visible images through a hybrid
%    multi-scale decomposition with Gaussian and bilateral filters", Information Fusion, 30, 2016
%
%    Some of the test images were obtained at
%      http://www.imagefusion.org
%      http://www.ece.lehigh.edu/SPCRL/IF/image_fusion.htm
%
%    Zhiqiang Zhou, Beijing Institute of Technology
%    Apr. 2016

clear all;
% close all;
nLevel = 4;

%  path_Vis = '.\image\b01_1.tif';      path_IR = '.\image\b01_2.tif';
 path_Vis = '.\image\Camp_Vis.jpg';      path_IR = '.\image\Camp_IR.jpg';
% path_Vis = '.\image\Trees4906_Vis.jpg'; path_IR = '.\image\Trees4906_IR.jpg';
% path_Vis = '.\image\Octec_Vis.jpg';     path_IR = '.\image\Octec_IR.jpg';
% path_Vis = '.\image\Road_Vis.jpg';      path_IR = '.\image\Road_IR.jpg';
% path_Vis = '.\image\Kayak_Vis.jpg';     path_IR = '.\image\Kayak_IR.jpg';
% path_Vis = '.\image\Steamboat_Vis.jpg'; path_IR = '.\image\Steamboat_IR.jpg';
% path_Vis = '.\image\Trees4917_Vis.jpg'; path_IR = '.\image\Trees4917_IR.jpg';
% path_Vis = '.\image\Dune_Vis.jpg';      path_IR = '.\image\Dune_IR.jpg';

[img1, img2, para.name] = PickName(path_Vis, path_IR);
paraShow1.fig = 'Visible image';
paraShow2.fig = 'Infrared image';
ShowImageGrad(img2, paraShow2);
ShowImageGrad(img1, paraShow1);

% %% ---------- Visibility enhancement for visible image--------------
% img1E = Ehn_GF(img1);
% img1 = img1E;
% %% ---------- Infrared image normalization--------------
% mi = min(img2(:));
% ma = max(img2(:));
% img2 = (img2-mi)/(ma-mi)*255;

%% ---------- Automatic parameter selection --------------
Rs = Relative_PS(img2, img1);
if Rs<0.8
    lambda = 100
else
    if Rs>1.6
        lambda = 2000
    else
        lambda = 2500*Rs - 1900
    end
end

%% ---------- Hybrid multiscale decomposition based on guided filter--------------
sigma = 2;  k = 2;
r0 = 2;     eps0 = 0.1;
l = 2;

M1 = cell(1, nLevel+1);
M1L = cell(1, nLevel+1);
M1{1} = img1/255;
M1L{1} = M1{1};
M1D = cell(1, nLevel+1);
M1E = cell(1, nLevel+1);
sigma0 = sigma;
r = r0;
eps = eps0;
for ii = 2:nLevel+1,

%     % ***using fast guided filter, which has the potential to achieve real-time performance when codes are fully optimized
%     % ***NOTE: large subsampling ratio may cause problem for fusion of some source images.
%     s = max(1, r/2); % subsampling ratio
%     M1{ii} = fastguidedfilter_md(M1{ii-1}, M1{ii-1}, r, 100^2, s);
%     M1L{ii} = fastguidedfilter_md(M1L{ii-1}, M1L{ii-1}, r, eps^2, s);

    M1{ii} = guidedfilter(M1{ii-1}, M1{ii-1}, r, 100^2);
    M1L{ii} = guidedfilter(M1L{ii-1}, M1L{ii-1}, r, eps^2);

    M1D{ii} = M1{ii-1} - M1L{ii};
    M1E{ii} = M1L{ii} - M1{ii};

    sigma0 = k*sigma0;
    r = k*r;
    eps = eps/l;
end

M2 = cell(1, nLevel+1);
M2L = cell(1, nLevel+1);
M2{1} = img2/255;
M2L{1} = M2{1};
M2D = cell(1, nLevel+1);
M2E = cell(1, nLevel+1);
sigma0 = sigma;
r = r0;
eps = eps0;
for ii = 2:nLevel+1,
%     s = max(1, r/2);
%     M2{ii} = fastguidedfilter_md(M2{ii-1}, M2{ii-1}, r, 100^2, s);
%     M2L{ii} = fastguidedfilter_md(M2L{ii-1}, M2L{ii-1}, r, eps^2, s);
    M2{ii} = guidedfilter(M2{ii-1}, M2{ii-1}, r, 100^2);
    M2L{ii} = guidedfilter(M2L{ii-1}, M2L{ii-1}, r, eps^2);

    M2D{ii} = M2{ii-1} - M2L{ii};
    M2E{ii} = M2L{ii} - M2{ii};

    sigma0 = k*sigma0;
    r = k*r;
    eps = eps/l;
end

%% ---------- Fusion --------------

for j = nLevel+1:-1:3
D2 = abs(M2E{j});
D1 = abs(M1E{j});
R = max(D2-D1, 0);
Rmax = max(R(:));
P = R/Rmax;

Cj = atan(lambda*P)/atan(lambda);

sigma_b = 2*sigma0;
if j == nLevel+1
    w = floor(3*sigma_b);
    h = fspecial('gaussian', [2*w+1, 2*w+1], sigma_b);
    lambda0 = lambda;
    Cb = atan(lambda0*P)/atan(lambda0);
    Cb = imfilter(Cb, h, 'symmetric');
    MB = Cb.*M2{nLevel+1} + (1-Cb).*M1{nLevel+1};
end

sigma_c = 1;
w = floor(3*sigma_c);
h = fspecial('gaussian', [2*w+1, 2*w+1], sigma_c);
Cj = imfilter(Cj, h, 'symmetric');

md = Cj.*M2E{j}+ (1-Cj).*M1E{j};
MB = MB + md;
md = Cj.*M2D{j}+ (1-Cj).*M1D{j};
MB = MB + md;
end

sigma_t = 1;
w = floor(3*sigma_t);
h = fspecial('gaussian', [2*w+1, 2*w+1], sigma_t);
C11 = double(abs(M1E{2}) < abs(M2E{2}));
C11 = imfilter(C11, h, 'symmetric');
md = C11.*M2E{2}+ (1-C11).*M1E{2};
MB = MB + md;
C10 = double(abs(M1D{2}) < abs(M2D{2}));
md = C10.*M2D{2}+ (1-C10).*M1D{2};
MB = MB + md;
FI = min(round(MB*275), 255);
FI = max(FI, 0);

paraShow.fig = 'Result';
ShowImageGrad(FI, paraShow);


function abram2_values_test ( )

%*****************************************************************************80
%
%% ABRAM2_VALUES_TEST demonstrates the use of ABRAM2_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ABRAM2_VALUES_TEST:\n' );
  fprintf ( 1, '  ABRAM2_VALUES stores values of \n' );
  fprintf ( 1, '  the Abramowitz function of order 2.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = abram2_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function [ r, z, rho, c, s ] = dchud ( r, ldr, p, x, z, ldz, nz, y, rho )

%*****************************************************************************80
%
%% DCHUD updates an augmented Cholesky decomposition.
%
%  Discussion:
%
%    DCHUD can also update the triangular part of an augmented QR
%    decomposition.
%
%    Specifically, given an upper triangular matrix R of order P, a row vector
%    X, a column vector Z, and a scalar Y, DCHUD determines a unitary matrix
%    U and a scalar ZETA such that
%
%           (R  Z)     (RR   ZZ )
%      U  * (    )  =  (        ),
%           (X  Y)     ( 0  ZETA)
%
%    where RR is upper triangular.
%
%    If R and Z have been obtained from the factorization of a least squares
%    problem, then RR and ZZ are the factors corresponding to the problem
%    with the observation (X,Y) appended.  In this case, if RHO is the
%    norm of the residual vector, then the norm of the residual vector of
%    the updated problem is sqrt ( RHO * RHO + ZETA * ZETA ).  DCHUD will
%    simultaneously update several triplets (Z, Y, RHO).
%
%    For a less terse description of what DCHUD does and how
%    it may be applied, see the LINPACK guide.
%
%    The matrix U is determined as the product U(P)*...*U(1),
%    where U(I) is a rotation in the (I,P+1) plane of the form
%
%      (     C(I)      S(I) )
%      (                    ).
%      (    -S(I)      C(I) )
%
%    The rotations are chosen so that C(I) is real.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and 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, real R(LDR,P), the upper triangular matrix to be
%    updated.  The part of R below the diagonal is not referenced.
%
%    Input, integer LDR, the leading dimension of the array R.
%    LDR must be at least equal to P.
%
%    Input, integer P, the order of the matrix R.
%
%    Input, real X(P), the row to be added to R.
%
%    Input, real Z(LDZ,NZ), contains NZ P-vectors to be updated with R.
%
%    Input, integer LDZ, the leading dimension of the array Z.
%    LDZ must be at least P.
%
%    Input, integer NZ, the number of vectors to be updated.  NZ may be
%    zero, in which case Z, Y, and RHO are not referenced.
%
%    Input, real Y(NZ), the scalars for updating the vectors Z.
%
%    Input, real RHO(NZ), the norms of the residual vectors to be updated.
%    If RHO(J) is negative, it is left unaltered.
%
%    Output, real R(LDR,P), the updated matrix.
%
%    Output, real Z(LDZ,NZ), the updated vectors.
%
%    Output, real RHO(NZ), the updated norms of the residual vectors.
%
%    Output, real C(P), S(P), the cosines and sines of the
%    transforming rotations.
%

%
%  Update R.
%
  for j = 1: p

    xj = x(j);
%
%  Apply the previous rotations.
%
    for i = 1 : j-1
      t = c(i) * r(i,j) + s(i) * xj;
      xj = c(i) * xj - s(i) * r(i,j);
      r(i,j) = t;
    end
%
%  Compute the next rotation.
%
    [ c(j), s(j), r(j,j), xj ] = drotg ( r(j,j), xj );

  end
%
%  If required, update Z and RHO.
%
  for j = 1 : nz

    zeta = y(j);

    for i = 1 : p
      t =    c(i) * z(i,j) + s(i) * zeta;
      zeta = c(i) * zeta   - s(i) * z(i,j);
      z(i,j) = t;
    end

    azeta = abs ( zeta );

    if ( azeta ~= 0.0 & 0.0 <= rho(j) )
      scale = azeta + rho(j);
      rho(j) = scale * sqrt ( ( azeta / scale )^2 + ( rho(j) / scale )^2 );
    end

  end

  return
end


%% Calculate mean angular error between source and target images
%
% Copyright (c) 2018-present, Mahmoud Afifi
% York University, Canada
% mafifi@eecs.yorku.ca | m.3afifi@gmail.com
%
% This source code is licensed under the license found in the
% LICENSE file in the root directory of this source tree.
% All rights reserved.
%
% Please cite the following work if this program is used:
% Mahmoud Afifi, Brian Price, Scott Cohen, and Michael S. Brown,
% "When color constancy goes wrong: Correcting improperly white-balanced
% images", CVPR 2019.
%
% Input:
%   -source: image A
%   -target: image B
%   -color_chart_area: If there is a color chart in the image (that is
%   masked out from both images, this variable represents the number of
%   pixels of the color chart.
%
% Output:
%   -f: the mean angular error between image A and image B.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
function f = calc_mae(source, target,color_chart_area)

if size(source,1)<=color_chart_area
    error('Color chart area should be less than the image area');
end
source=double(source);
target=double(target);
target_norm = sqrt(sum(target.^2,2));
source_mapped_norm = sqrt(sum(source.^2,2));
angles=dot(source,target,2)./(source_mapped_norm.*target_norm);
angles(angles>1)=1;
f=acosd(angles);
f(isnan(f))=0;
f=sum(f)/(size(source,1)-color_chart_area);
end


% VOCLABELCOLORMAP Creates a label color map such that adjacent indices have different
% colors.  Useful for reading and writing index images which contain large indices,
% by encoding them as RGB images.
%
% CMAP = VOCLABELCOLORMAP(N) creates a label color map with N entries.
function cmap = labelcolormap(N)

if nargin==0
    N=256
end
cmap = zeros(N,3);
for i=1:N
    id = i-1; r=0;g=0;b=0;
    for j=0:7
        r = bitor(r, bitshift(bitget(id,1),7 - j));
        g = bitor(g, bitshift(bitget(id,2),7 - j));
        b = bitor(b, bitshift(bitget(id,3),7 - j));
        id = bitshift(id,-3);
    end
    cmap(i,1)=r; cmap(i,2)=g; cmap(i,3)=b;
end
cmap = cmap / 255;


%% Interactive Perspective Transformation
% This program demonstrates Perspective Transformation.
%
% In this sample you will learn how to use the following OpenCV functions:
%
% * <matlab:doc('cv.getPerspectiveTransform') cv.getPerspectiveTransform>
% * <matlab:doc('cv.warpPerspective') cv.warpPerspective>
% * <matlab:doc('cv.perspectiveTransform') cv.perspectiveTransform>
%

function varargout = perspective_transform_gui(im)
    % load source image
    if nargin < 1
        img = imread(fullfile(mexopencv.root(),'test','fruits.jpg'));
    elseif ischar(im)
        img = imread(im);
    else
        img = im;
    end

    % create the UI
    h = buildGUI(img);
    if nargout > 0, varargout{1} = h; end
end

function onHelp(~,~)
    %ONHELP  Display usage help dialog

    helpdlg({
        'This program demonstrates Perspective Transformation.'
        ''
        'Drag the image corners using the mouse: Move the pointer over a'
        'vertex. The pointer changes to a circle. Click and drag the vertex'
        'to its new position.'
        ''
        'You can also drag the image itself: move the pointer inside the'
        'quadilateral. The pointer changes to a fleur shape. Click and drag'
        'the mouse to move the image.'
        ''
        'Note: you must not add or remove points from the polygon.'
        'The cv.getPerspectiveTransform function requires exactly'
        '4 points to estimate the homography.'
        'Also the function works best if no three points are collinear.'
    });
end

function onDrag(newpos, handles)
    %ONDRAG  Event handler for impoly

    % compute the perspective transform matrix from matching corners
    H = cv.getPerspectiveTransform(handles.pos, newpos);

    % trigger redraw with the new homography
    redraw(handles, H);
end

function redraw(handles, H)
    %REDRAW  Warp and repaint image

    % apply the perspective transformation on the source image
    img2 = cv.warpPerspective(handles.img, H);

    % display warped image and homography matrix
    set(handles.hImg, 'CData',img2);
    set(handles.hTxt, 'String',mat2str_latex(H));
    drawnow limitrate;
end

function showModalDialog(~,~,handles)
    %SHOWMODELDIALOG  Display dialog to edit matrix

    % prompt for matrix using a model dialog
    d = dialog('Position',[50 50 280 140], 'Resize','off', 'Name','Homography');
    movegui(d, 'center');
    uicontrol('Parent',d, 'Style','push', 'Position',[80 10 60 20], ...
        'String','Ok', 'Callback',{@onDialogClose,true});
    uicontrol('Parent',d, 'Style','push', 'Position',[140 10 60 20], ...
        'String','Cancel', 'Callback',{@onDialogClose,false});
    t = uitable(d, 'Position',[15 40 250 90], ...
        'Data',eye(3), 'ColumnWidth',{70}, 'ColumnEditable',true, ...
        'TooltipString','fill the 3x3 homography matrix');

    % wait for dialog to close before returning
    uiwait(d);

    function onDialogClose(~,~,flag)
        H = get(t, 'Data');  % get entered matrix
        delete(gcf);         % close dialog
        if flag && ~any(isnan(H(:)))
            % update impoly position
            newpos = cv.perspectiveTransform(handles.pos, H);
            setPosition(handles.hPoly, newpos);
            % transform image
            redraw(handles, H);
        end
    end
end

function str = mat2str_latex(M)
    %MAT2STR_LATEX  Convert numeric matrix to a latex table for display

    %str = mat2str(M,3);
    M = round(M, 9);  % nicely rounded numbers (for stuff like 1e-15)
    str = ['$$H = \left[\begin{array}{ccc}' ...
        sprintf('%.3g & %.3g & %.3g \\\\ ',M(1,:)) ...
        sprintf('%.3g & %.3g & %.3g \\\\ ',M(2,:)) ...
        sprintf('%.3g & %.3g & %.3g ',M(3,:)) ...
        '\end{array}\right]$$'];
end

function img = print_instructions(img)
    %PRINT_INSTRUCTIONS  Show help text on top of image

    if nargin < 1, img = zeros([512 512 3], 'uint8'); end
    opts = {'Color',[255 0 0], 'Thickness',3, 'FontScale',1.7};
    img = cv.putText(img, 'Drag the image', [50 200], opts{:});
    img = cv.putText(img, 'corners using', [50 300], opts{:});
    img = cv.putText(img, 'the mouse.', [50 400], opts{:});
end

function handles = buildGUI(img)
    %BUILDGUI  Creates the UI

    handles = struct();
    handles.img = print_instructions(img);

    % initial quadilateral (image corners), from top-left in clockwise order
    [h,w,~] = size(handles.img);
    handles.pos = [1 1; w 1; w h; 1 h];

    % display image and homography matrix
    handles.hImg = imshow(handles.img);
    handles.hTxt = text(10, 10, mat2str_latex(eye(3)), ...
        'Interpreter','latex', 'FontSize',20, 'Color','y', ...
        'HorizontalAlignment','left', 'VerticalAlignment','top');

    % create draggable polygon
    handles.hPoly = impoly(get(handles.hImg,'Parent'), handles.pos, 'Closed',true);
    setColor(handles.hPoly, 'y');
    if false
        % restrict polygon inside image limits
        setPositionConstraintFcn(hPoly, ...
            makeConstrainToRectFcn('impozy',[1 w], [1 h]));
    end

    uicontrol('Style','pushbutton', 'Position',[20 20 60 20], 'String','Help', ...
        'Callback',@onHelp);
    uicontrol('Style','pushbutton', 'Position',[80 20 60 20], 'String','Reset', ...
        'Callback',@(~,~) setPosition(handles.hPoly, handles.pos));
    uicontrol('Style','pushbutton', 'Position',[140 20 60 20], 'String','Preset 1', ...
        'Callback',@(~,~) setPosition(handles.hPoly, [100 100; w-100 100; w h; 1 h]));
    uicontrol('Style','pushbutton', 'Position',[200 20 60 20], 'String','Preset 2', ...
        'Callback',@(~,~) setPosition(handles.hPoly, [w+25 -25; 25 200; 175 h-10; w-100 h-50]));
    uicontrol('Style','pushbutton', 'Position',[260 20 60 20], 'String','Manual', ...
        'Callback',{@showModalDialog,handles});

    % set callback when dragging points
    addNewPositionCallback(handles.hPoly, @(p) onDrag(p, handles));
end


classdef prtClassFld < prtClass
 %prtClassFld Fisher linear discriminant classifier
 %
 %    CLASSIFIER = prtClassFld returns a Fisher linear discriminant classifier
 %
 %    CLASSIFIER = prtClassFld(PROPERTY1, VALUE1, ...) constructs a
 %    prtClassFld object CLASSIFIER with properties as specified by
 %    PROPERTY/VALUE pairs.
 %
 %    A prtClassFld object inherits all properties from the abstract class
 %    prtClass. In addition is has the following properties:
 %
 %    w                  - regression weights, estimated during training
 %    plotBasis          - Flag indicating whether to plot the basis
 %                         functions when the PLOT function is called
 %    plotProjections    - Flag indicating whether to plot the projection
 %                         of points to the basis when the PLOT function is
 %                         called
 %
 %    For information on the Fisher Linear Discriminant algorithm, please
 %    refer to the following URL:
 %
 %    http://en.wikipedia.org/wiki/Linear_discriminant_analysis#Fisher.27s_linear_discriminant
 %
 %    A prtClassFld object inherits the TRAIN, RUN, CROSSVALIDATE and
 %    KFOLDS methods from prtAction. It also inherits the PLOT method from
 %    prtClass.
 %
 %    Example:
 %
 %    ds1 = prtDataGenUnimodal;       % Create some test and
 %    ds2 = prtDataGenUnimodal;   % training data
 %    classifier = prtClassFld;           % Create a classifier
 %    classifier = classifier.train(ds1);    % Train
 %    classified = run(classifier, ds2);         % Test
 %    subplot(2,1,1);
 %    classifier.plot;
 %    subplot(2,1,2);
 %    [pf,pd] = prtScoreRoc(classified);
 %    h = plot(pf,pd,'linewidth',3);
 %    title('ROC'); xlabel('Pf'); ylabel('Pd');
 %
 %   See also prtClass, prtClassLogisticDiscriminant, prtClassBagging,
 %   prtClassMap, prtClassCap, prtClassBinaryToMaryOneVsAll, prtClassDlrt,
 %   prtClassPlsda, prtClassKnn, prtClassRvm, prtClassGlrt,  prtClassSvm,
 %   prtClassTreeBaggingCap, prtClassKmsd, prtClassKnn

% Copyright (c) 2013 New Folder Consulting
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the
% "Software"), to deal in the Software without restriction, including
% without limitation the rights to use, copy, modify, merge, publish,
% distribute, sublicense, and/or sell copies of the Software, and to permit
% persons to whom the Software is furnished to do so, subject to the
% following conditions:
%
% The above copyright notice and this permission notice shall be included
% in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
% OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
% MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN
% NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
% DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
% OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
% USE OR OTHER DEALINGS IN THE SOFTWARE.

    properties (SetAccess=private)

        name = 'Fisher Linear Discriminant' % Fisher Linear Discriminant
        nameAbbreviation = 'FLD'            % FLD
        isNativeMary = false;  % False
    end

    properties (SetAccess = protected)
        % w is a dataSet.nDimensions x 1 vector of projection weights
        % learned during Fld.train(dataSet)

        w = []; % The vector of weights, learned during training

        % plotting options
        plotBasis = false; % Flag indicating whether or not to plot the basis
        plotProjections = false; % Flag indicating whether or not to plot the projections
    end

    methods

               % Allow for string, value pairs
        function self = prtClassFld(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end

        function self = set.plotProjections(self,value)
            if islogical(value) || (isnumeric(value) && (value == 1 || value == 0))
                self.plotProjections = value;
            else
                error('prt:prtClassFld:plotProjections','plotProjections can only take true or false (boolean or 0/1) values; user speficied value %d',value);
            end
        end
    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,dataSet)

            n = dataSet.nObservations;
            p = dataSet.nFeatures;

            if p > n
                warning('prt:prtClassFld:train:illconditioned','dataSet has n (%d) < p (%d); prtClassFld may not be stable',n,p);
            end
            if ~dataSet.isBinary
                error('prtClassFld:nonBinaryTraining','Input dataSet for prtClassFld.train must be binary');
            end

            dataH0 = dataSet.getObservationsByClassInd(1);
            dataH1 = dataSet.getObservationsByClassInd(2);

            mean0 = mean(dataH0,1);
            mean1 = mean(dataH1,1);

            cov0 = cov(dataH0);
            cov1 = cov(dataH1);
            covW = cov1 + cov0;

            self.w = covW\(mean1-mean0)'; %w = covW^-1 * (mean1-mean0)'; But better
            self.w = self.w./norm(self.w);

        end

        function dataSet = runAction(self,dataSet)
            %dataSet = prtDataSetClass((self.w'*dataSet.getObservations()')');
            dataSet.X = (self.w'*dataSet.getObservations()')';
        end

        function imageHandle = plotGriddedEvaledClassifier(self, DS, linGrid, gridSize, cMap)

            % Call the original plot function
            imageHandle = plotGriddedEvaledClassifier@prtClass(self, DS, linGrid, gridSize, cMap);

            W = self.w;
            limits = axis;
            nDims = length(W);

            if self.plotBasis
                hold on
                switch nDims
                    case 1
                        % Nothing
                    case 2
                        distances = zeros(4,1);
                        distances(1) = sqrt(sum([limits(2); limits(4)].^2));
                        distances(2) = sqrt(sum([limits(1); limits(3)].^2));
                        distances(3) = sqrt(sum([limits(2); limits(3)].^2));
                        distances(4) = sqrt(sum([limits(1); limits(4)].^2));

                        highPoint =  max(distances).*W;
                        lowPoint =  -max(distances).*W;

                        h = plot([lowPoint(1),highPoint(1)],[lowPoint(2),highPoint(2)],'k');
                        set(h,'linewidth',3);
                    case 3
                        distances = zeros(8,1);
                        distances(1) = sqrt(sum([limits(1); limits(3); limits(5)].^2));
                        distances(2) = sqrt(sum([limits(1); limits(3); limits(6)].^2));
                        distances(3) = sqrt(sum([limits(1); limits(4); limits(5)].^2));
                        distances(4) = sqrt(sum([limits(1); limits(4); limits(6)].^2));
                        distances(5) = sqrt(sum([limits(2); limits(3); limits(5)].^2));
                        distances(6) = sqrt(sum([limits(2); limits(3); limits(6)].^2));
                        distances(7) = sqrt(sum([limits(2); limits(4); limits(5)].^2));
                        distances(8) = sqrt(sum([limits(2); limits(4); limits(6)].^2));

                        highPoint =  max(distances).*W;
                        lowPoint =  -max(distances).*W;

                        h = plot3([lowPoint(1),highPoint(1)],[lowPoint(2),highPoint(2)],[lowPoint(3), highPoint(3)],'k');
                        set(h,'linewidth',3);
                    otherwise
                        error('prt:prtClassFld:tooManyDimensions','Too many dimensions for plotting.')
                end
            end

            if self.plotProjections && ~isempty(self.dataSet)
                OutputDataSet = run(self, self.dataSet);
                hold on;
                switch nDims
                    case 2
                        for i = 1:double(self.plotProjections):self.dataSet.nObservations
                            cX = self.dataSet.X(i,:);
                            cYout = OutputDataSet.X(i,:);
                            plot([cX(1),cYout*W(1)],[cX(2),cYout*W(2)],'k');
                        end
                    case 3
                        for i = 1:double(self.plotProjections):self.dataSet.nObservations
                            cX = self.dataSet.X(i,:);
                            cYout = OutputDataSet.X(i,:);
                            plot3([cX(1),cYout*W(1)],[cX(2),cYout*W(2)],[cX(3),cYout*W(3)],'k');
                        end
                end
                axis(limits);
            end
            hold off;
        end

    end

end


function [B,Theta_diag,C_diag,Alpha_diag,Fit_diag,...
    C_offdiag,Theta_offdiag,Alpha_offdiag,Fit_offdiag phi_t]=smooth_data(C,K,Q,nloop_diag,nloop_offdiag)

%% This function is a wrapper for the smooth_mcmc function, which does the actual smoothing
%% This was written to smoot the diagonal and off-diagonal conectivity time series separately
%% and to save the coefficents and basis functions from each

N=size(C,1);
M_i=zeros(N,1);
for i=1:N
    M_i(i)=size(C{i},1);
end

%% Get smoothed time-varying connectivity coefficients

% smooth diagonal connectivities
C_diag=cell(sum(M_i),1);
ind=0;
for i=1:N
    for j=1:M_i(i)
        ind=ind+1;
        C_diag{ind}=squeeze(C{i}(j,j,:));
    end
end
[ALPHA ALPHA_BAR THETA FIT phi_t]=smooth_mcmc(C_diag,K,Q,nloop_diag);
first=round(nloop_diag/2); last=nloop_diag+1;
Alpha_diag=mean(ALPHA(:,:,first:last),3);
Theta_diag=mean(THETA(:,:,first:last),3);
Fit_diag=phi_t'*Theta_diag*Alpha_diag;

B=cell(N,1);
ind=0;
for i=1:N
    B{i}=cell(M_i(i));
    for j=1:M_i(i)
        ind=ind+1;
        B{i}{j,j}=Alpha_diag(:,ind)';
    end
end

% smooth off-diagonal connectivities
M_offdiag=M_i(1)^2-M_i(1);
for i=2:N
    M_offdiag=M_offdiag+M_i(i)^2-M_i(i);
end

C_offdiag=cell(M_offdiag,1);
ind=0;
for i=1:N
    for j1=1:M_i(i)
        for j2=1:M_i(i)
            if(not(j1==j2))
                ind=ind+1;
                C_offdiag{ind}=squeeze(C{i}(j1,j2,:));
            end
        end
    end
end
[ALPHA ALPHA_BAR THETA FIT phi_t]=smooth_mcmc(C_offdiag,K,Q,nloop_offdiag);
first=round(nloop_offdiag/2); last=nloop_offdiag+1;
Alpha_offdiag=mean(ALPHA(:,:,first:last),3);
Theta_offdiag=mean(THETA(:,:,first:last),3);
Fit_offdiag=phi_t'*Theta_offdiag*Alpha_offdiag;

ind=0;
for i=1:N
    for j1=1:M_i(i)
        for j2=1:M_i(i)
            if not(j1==j2)
                ind=ind+1;
                B{i}{j1,j2}=Alpha_offdiag(:,ind)';
            end
        end
    end
end


function zernike_poly_coef_test ( )

%*****************************************************************************80
%
%% ZERNIKE_POLY_COEF_TEST tests ZERNIKE_POLY_COEF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ZERNIKE_POLY_COEF_TEST\n' );
  fprintf ( 1, '  ZERNIKE_POLY_COEF determines the Zernike \n' );
  fprintf ( 1, '  polynomial coefficients.\n' );

  for m = 0 : n

    c = zernike_poly_coef ( m, n );

    r8poly_print ( n, c, '  Zernike polynomial' );

  end

  return
end


function AnalyzePersistence(Data,AggregationPersistence,LagsSamplAutCorr,Name)

for n=1:length(AggregationPersistence)
    Series_Changes(n).Lag=AggregationPersistence(n);
    SamplAutoCorrs=[];

    for s=1:AggregationPersistence(n)

        Sparse=Data(s:AggregationPersistence(n):end);
        Changes=diff(Sparse);
        SamplAutoCorrs = autocorr(Changes, LagsSamplAutCorr);
        SamplAutoCorrs(1)=[];
    end
    Series_Changes(n).SampleAutocorrelation=mean(SamplAutoCorrs,2);
end

SampleAutocorrelations=[];
for n=1:length(AggregationPersistence)
    SampleAutocorrelations=[SampleAutocorrelations Series_Changes(n).SampleAutocorrelation];
end

figure
bar3(SampleAutocorrelations,'detached')
set(gca,'xlim',[AggregationPersistence(1) AggregationPersistence(end)],'ylim',[1 LagsSamplAutCorr],...
    'zlim',[-.2 .4])
xlabel('aggregation size (days)')
ylabel('lag')
zlabel('autocorrelation')
colormap(.5*(1+gray));
set(gcf,'Name',['        persistence properties of ' Name ' par swap rate'])


function scatter3sph(X,Y,Z,varargin)
%SCATTER3SPH (X,Y,Z) Plots a 3d scatter plot with 3D spheres
%	SCATTER3SPH is like scatter3 only drawing spheres with volume, instead
%	of flat circles, at coordinates specified by vectors X, Y, Z. All three
%	vectors have to be of the same length.
%	SCATTER3SPH(X,Y,Z) draws the spheres with the default size and color.
%	SCATTER3SPH(X,Y,Z,'size',S) draws the spheres with sizes S. If length(S)= 1
%	the same size is used for all spheres.
%	SCATTER3SPH(X,Y,Z,'color',C) draws the spheres with colors speciffied in a
%	N-by-3 matrix C as RGB values.
%	Parameter names can be abreviated to 3 letters. For example: 'siz' or
%	'col'. Case is irrelevant.
%
% Example
% %Coordinates
%  X= 100*rand(9,1); Y= 100*rand(9,1); Z= 100*rand(9,1);
%
% %Colors: 3 blue, 3 red and 3 green
% C= ones(3,1)*[0 0 1];
% C= [C;ones(3,1)*[1 0 0]];
% C= [C;ones(3,1)*[0 1 0]];
%
% %Spheres sizes
% S= 5+10*rand(9,1);
%
% figure(1);
% scatter3sph(X,Y,Z,'size',S,'color',C);
% axis equal
% axis tight
% view(125,20);
% grid ON

%-- Some checking...
if nargin < 3 error('Need at least three arguments'); return; end
if mean([length(X),length(Y),length(Z)]) ~= length(X) error ('Imput vectors X, Y, Z are of different lengths'); return; end

%-- Defaults
C= ones(length(X),1)*[0 0 1];
S= 0.1*max([X;Y;Z])*ones(length(X),1);

%-- Extract optional arguments
for j= 1:2:length(varargin)
	string= lower(varargin{j});
	switch string(1:min(3,length(string)))
		case 'siz'
			S= varargin{j+1};
			if length(S) == 1
				S= ones(length(X),1)*S;
			elseif length(S) < length(X)
				error('The vector of sizes must be of the same length as coordinate vectors (or 1)');
				return
			end

		case 'col'
			C= varargin{j+1};
			if size(C,2) < 3	error('Colors matrix must have 3 columns'); return; end
			if size(C,1) == 1
				C= ones(length(X),1)*C(1:3);
			elseif size(C,1) < length(X)
				error('Colors matrix must have the same number of rows as length of coordinate vectors (or 1)');
				return
			end

		otherwise
			error('Unknown parameter name. Allowed names: ''size'', ''color'' ');
	end
end

%-- Sphere facets
[sx,sy,sz]= sphere(20);

%-- Plot spheres
hold on
for j= 1:length(X)
	surf(sx*S(j)+X(j), sy*S(j)+Y(j), sz*S(j)+Z(j),...
		'LineStyle','none',...
		'AmbientStrength',0.4,...
		'FaceColor',C(j,:),...
		'SpecularStrength',0.8,...
		'DiffuseStrength',1,...
		'FaceAlpha',0.65,...
		'SpecularExponent',2);
end
light('Position',[0 0 1],'Style','infinit','Color',[1 1 1]);
lighting gouraud
view(30,15)


function [centres, options, post, errlog] = kmeans(centres, data, options)
%KMEANS	Trains a k means cluster model.
%
%	Description
%	 CENTRES = KMEANS(CENTRES, DATA, OPTIONS) uses the batch K-means
%	algorithm to set the centres of a cluster model. The matrix DATA
%	represents the data which is being clustered, with each row
%	corresponding to a vector. The sum of squares error function is used.
%	The point at which a local minimum is achieved is returned as
%	CENTRES.  The error value at that point is returned in OPTIONS(8).
%
%	[CENTRES, OPTIONS, POST, ERRLOG] = KMEANS(CENTRES, DATA, OPTIONS)
%	also returns the cluster number (in a one-of-N encoding) for each
%	data point in POST and a log of the error values after each cycle in
%	ERRLOG.    The optional parameters have the following
%	interpretations.
%
%	OPTIONS(1) is set to 1 to display error values; also logs error
%	values in the return argument ERRLOG. If OPTIONS(1) is set to 0, then
%	only warning messages are displayed.  If OPTIONS(1) is -1, then
%	nothing is displayed.
%
%	OPTIONS(2) is a measure of the absolute precision required for the
%	value of CENTRES at the solution.  If the absolute difference between
%	the values of CENTRES between two successive steps is less than
%	OPTIONS(2), then this condition is satisfied.
%
%	OPTIONS(3) is a measure of the precision required of the error
%	function at the solution.  If the absolute difference between the
%	error functions between two successive steps is less than OPTIONS(3),
%	then this condition is satisfied. Both this and the previous
%	condition must be satisfied for termination.
%
%	OPTIONS(14) is the maximum number of iterations; default 100.
%
%	See also
%	GMMINIT, GMMEM
%

%	Copyright (c) Ian T Nabney (1996-2001)

[ndata, data_dim] = size(data);
[ncentres, dim] = size(centres);

if dim ~= data_dim
  error('Data dimension does not match dimension of centres')
end

if (ncentres > ndata)
  error('More centres than data')
end

% Sort out the options
if (options(14))
  niters = options(14);
else
  niters = 100;
end

store = 0;
if (nargout > 3)
  store = 1;
  errlog = zeros(1, niters);
end

% Check if centres and posteriors need to be initialised from data
if (options(5) == 1)
  % Do the initialisation
  perm = randperm(ndata);
  perm = perm(1:ncentres);

  % Assign first ncentres (permuted) data points as centres
  centres = data(perm, :);
end
% Matrix to make unit vectors easy to construct
id = eye(ncentres);

% Main loop of algorithm
for n = 1:niters

  % Save old centres to check for termination
  old_centres = centres;

  % Calculate posteriors based on existing centres
  d2 = dist2(data, centres);
  % Assign each point to nearest centre
  [minvals, index] = min(d2', [], 1);
  post = id(index,:);

  num_points = sum(post, 1);
  % Adjust the centres based on new posteriors
  for j = 1:ncentres
    if (num_points(j) > 0)
      centres(j,:) = sum(data(find(post(:,j)),:), 1)/num_points(j);
    end
  end

  % Error value is total squared distance from cluster centres
  e = sum(minvals);
  if store
    errlog(n) = e;
  end
  if options(1) > 0
    fprintf(1, 'Cycle %4d  Error %11.6f\n', n, e);
  end

  if n > 1
    % Test for termination
    if max(max(abs(centres - old_centres))) < options(2) & ...
        abs(old_e - e) < options(3)
      options(8) = e;
      return;
    end
  end
  old_e = e;
end

% If we get here, then we haven't terminated in the given number of
% iterations.
options(8) = e;
if (options(1) >= 0)
  disp(maxitmess);
end


function prob = survProbStdModel(t,b,time)
% survProbStdModel: Computes survival probability using the standard model

prob = ones(size(t));
time0 = [0;time];
dtime = diff(time0);

for jdx=1:length(time)
   if jdx < length(time)
      tmpidx = t > time0(jdx) & t <= time0(jdx+1);
   else
      tmpidx = t > time0(jdx);
   end
   H = 0;
   if (jdx>1)
      H = dtime(1:jdx-1)'*b(1:jdx-1);
   end
   H = H + (t(tmpidx) - time0(jdx))*b(jdx);
   prob(tmpidx) = exp(-H);
end


function classifier = train_boosted_dt_mc(features, cat_features, labels, ...
    num_iterations, num_nodes, stopval, init_weights, varargin)
% Train a classifier based on boosted decision trees.  Boosting done by the
% logistic regression version of Adaboost (Adaboost.L - Collins, Schapire,
% Singer 2002).  At each
% iteration, a set of decision trees is created for each class, with
% confidences equal to 1/2*ln(P+/P-) for that class, according to the
% weighted distribution.  Final classification is based on the largest
% confidence label (possibly incorporating a prior as h0(c) =
% 1/2*ln(Pc/(1-Pc)).  Weights are assigned as
% w(i,j) = 1 / (1+exp(sum{t in iterations}[yij*ht(xi, j)])).

if length(varargin) == 1  % class names supplied
    gn = varargin{1};
    gid = zeros(size(labels));
    for c = 1:length(gn)
        ind = find(strcmp(labels, gn{c}));
        gid(ind) = c;
        if ~isempty(init_weights)
            disp([gn{c} ': ' num2str(sum(init_weights(ind)))]);
        else
            disp([gn{c} ': ' num2str(length(ind))]);
        end
    end
    ind = find(gid==0);
    gid(ind) = [];
    labels(ind) = [];
    features(ind, :) = [];
else
    [gid, gn] = grp2idx(labels);
    gn
end

classifier.names = gn;

num_classes = length(gn);
num_data = length(gid);

if ~exist('init_weights', 'var') || isempty(init_weights)
    init_weights = ones(num_data, 1)/num_data;
else
    init_weights = init_weights(:) / sum(init_weights);
end

% if no examples from a class are present, create one dummy example for
% that class with very small weight
for c = 1:numel(gn)
    if ~any(gid==c)
        disp(['warning: no examples from class ' gn(c)])
        gid(end+1) = c;
        features(end+1, :) = zeros(size(features(end, 1)));
        num_data = num_data + 1;
        init_weights(end+1) = min(init_weights)/2;
    end
end

all_conf = zeros(num_data, num_classes);
for c = 1:num_classes

    disp(['class: ' num2str(gn{c})]);
    y = (gid == c)*2-1;
    cl = [-1 1];
    nc = 2;
    w = zeros(num_data, 1);
    cw = zeros(num_classes, 1);
    for i = 1:2
        indices = find(y==cl(i));
        %count = sum(init_weights(indices));
        %w(indices) = init_weights(indices) / count / 2;
        w(indices) = init_weights(indices);

        if cl(i)==1
            %classifier.h0(c) = log(count / (1-count));
            classifier.h0(c) = 0;
        end

    end

    data_confidences = zeros(num_data, 1);
    aveconf = [];

    for t = 1:num_iterations
        % learn decision tree based on weighted distribution
        dt = treefitw(features, y, w, 1/num_data/2, 'catidx', cat_features, 'method', 'classification', 'maxnodes', num_nodes*4);
        [tmp, level] = min(abs(dt.ntermnodes-num_nodes));
        dt = treeprune(dt, 'level', level-1);

        % assign partition confidences
        pi = (strcmp(dt.classname{1},'1')) + (2*strcmp(dt.classname{2},'1'));
        ni = (strcmp(dt.classname{1},'-1')) + (2*strcmp(dt.classname{2},'-1'));
        classprob = dt.classprob;
        confidences = 1/2*(log(classprob(:, pi)) - log(classprob(:, ni)));

        % assign weights
        [class_indices, nodes, classes] = treeval(dt, features);
        data_confidences = data_confidences + confidences(nodes);

        w = 1 ./ (1+exp(y.*data_confidences));
        % was w = 1 ./ (1+exp(y.*data_confidences));
        w = w / sum(w);

%         disp(['c: ' num2str(sum(init_weights ./ (1+exp(-y.*data_confidences)))) ...
%             '  e: ' num2str(sum(init_weights .* (y.*data_confidences < 0))) ...
%             '   w: ' num2str(max(w))]);

        classifier.wcs(t, c).dt = dt;
        classifier.wcs(t, c).confidences = confidences;

        %aveconf(t) = mean(1 ./ (1+exp(-y.*data_confidences)));
        aveconf(t) = sum(1 ./ (1+exp(-y.*data_confidences)).*init_weights);
        if t>10 && (aveconf(t)-aveconf(t-10) < stopval)
            disp(num2str(aveconf))
            disp(['Stopping after ' num2str(t) ' trees'])
            break;
        end

    end

    finalconf = 1 ./ (1+exp(-y.*data_confidences)) .* init_weights;
    finalerr = (y.*data_confidences < 0);
    disp(['confidence:: mean: ' num2str(sum(finalconf)) ...
        '  pos: ' num2str(sum(finalconf(y==1))/sum(init_weights(y==1))) ...
        '  neg: ' num2str(sum(finalconf(y~=1))/sum(init_weights(y~=1)))]);
    disp(['training error:: mean: ' num2str(sum(init_weights.*finalerr)) ...
        '  pos: ' num2str(sum(init_weights(y==1).*finalerr(y==1))/sum(init_weights(y==1))) ...
        '  neg: ' num2str(sum(init_weights(y~=1).*finalerr(y~=1))/sum(init_weights(y~=1)))]);
    all_conf(:, c) = data_confidences+classifier.h0(c);

end

% compute and display training error
[tmp, assigned_label] = max(all_conf, [], 2);
conf_matrix = zeros(num_classes, num_classes);
for c = 1:num_classes
    indices = find(gid==c);
    for c2 = 1:num_classes
        conf_matrix(c, c2) = sum(init_weights(indices).*(assigned_label(indices)==c2))/sum(init_weights(indices));
    end
    disp([gn{c} ' error: ' num2str(sum(init_weights(indices).*(assigned_label(indices)~=c))/sum(init_weights(indices)))]);
end
disp('Confusion Matrix: ');
disp(num2str(conf_matrix));
disp(['total error: ' num2str(sum(init_weights.*(assigned_label~=gid)))]);


function demo_RecPF

close all
clear all

addpath('utilities');
addpath('solver');
addpath('images');

% load images
Ims = cell(6,1);
for k = 1:6
    filename = ['ortho' int2str(k) '.jpg'];
    I = single(imread(filename))/255;
    if ndims(I) == 3
        Ims{k} = rgb2gray(I);
    end
end

% call tester
tester(Ims);

function tester(Ims)

%
% min aTV*TV(u) + aL1*||Phi*u||_1 + 0.5*||F_p*u - f_p||_2^2
%
aTV = 1.e-6;
aL1 = 0.e-6;

h = figure(1);
set(h,'units','normalized','outerposition',[0 .4 .9 .6]);

for nimg = 1:length(Ims)

    % load an image
    I = single(Ims{nimg});
    [m n] = size(I);
    N = m*n;

    % generate mask: two methods
    mth = 1;
    if mth == 1
        acc = 3.6;
        picks = selectPF(n/acc,n);
        K = length(picks);
    else
        Ls = 45;
        picks = fftshift(MRImask(n,Ls));
        picks = union(find(picks~=0),1);
        K = length(picks);
    end

    % image information display
    fprintf('Image %i: size %3i by %3i, N/M = %6.2f\n',nimg,m,n,N/K);
    snr(2*I,I);
    subplot(2,6,nimg); imshow(I,[]);
    title(['Original ' int2str(m) ' x ' int2str(n)]);
    drawnow;

    % add noise
    sigma = 0.01;
    noise = sigma*(randn(K,1) + sqrt(-1)*randn(K,1));

    % DWT and IDWT
    if exist('midwt','file')
        wav = daubcqf(2);
        Psi = @(x) midwt(x,wav);
        PsiT = @(x) mdwt(x,wav);
    elseif exist('wavedec2','file')
        Psi = @(x) Wavedb1Phi(x,1);
        PsiT = @(x) Wavedb1Phi(x,0);
    else
        Psi = @(x) x; PsiT = Psi;
    end

    % observation B
    FI = fft2(I);
    B = FI(picks) + noise;

    % run RecPF
    opts = [];
    t = cputime;
    U = RecPF(m,n,aTV,aL1,picks,B,PsiT,Psi,opts);
    t = cputime - t;

    % plot reconstructed images
    subplot(2,6,6+nimg);
    imshow(U,[]);
    title(sprintf('%4.2fdB, %4.2fs',snr(U,I),t));
    drawnow;

end