% Copyright (C) 2012 Quan Wang <wangq10@rpi.edu>,
% Signal Analysis and Machine Perception Laboratory,
% Department of Electrical, Computer, and Systems Engineering,
% Rensselaer Polytechnic Institute, Troy, NY 12180, USA
%
% You are free to use this software for academic purposes if you cite our paper:
% Quan Wang, Kim L. Boyer,
% The active geometric shape model: A new robust deformable shape model and its applications,
% Computer Vision and Image Understanding, Volume 116, Issue 12, December 2012, Pages 1178-1194,
% ISSN 1077-3142, 10.1016/j.cviu.2012.08.004.
%
% For commercial use, please contact the authors.

function [xc yc a b phi]=fit_arb_ellipse_force(init,increment,threshold,bound,field_x,field_y,iter,show_fitness)

x0=init(1);
y0=init(2);
a0=init(3);
b0=init(4);
phi0=init(5);
d_xc=increment(1);
d_yc=increment(2);
d_a=increment(3);
d_b=increment(4);
d_phi=increment(5);
t_xc=threshold(1);
t_yc=threshold(2);
t_a=threshold(3);
t_b=threshold(4);
t_phi=threshold(5);
a_low=bound(1);
a_up=bound(2);
b_low=bound(3);
b_up=bound(4);

%%  Fit an arbitrary ellipse in the force filed. xc, yc, a, b and phi will be returned.
%   The parametric equations of the arbitrary ellipse:
%       x=xc+a*cos(theta)*cos(phi)-b*sin(theta)*sin(phi)
%       y=yc+a*cos(theta)*sin(phi)+b*sin(theta)*cos(phi)
%   x0, y0: initial position of center
%   a0, b0: initial semi-major and semi-minor axes
%   phi0: initial orientation
%   d_xc, d_yc: increment of xc and yc in each loop
%   d_a, d_b: increment of a and b in each loop
%   d_phi: increment of phi in each loop
%   t_xc, t_yc: threshold needed to update xc and yc
%   t_a, t_b: threshold needed to update a and b
%   t_phi: threshold needed to update phi
%   a_low, a_up: lower bound and upper bound of a
%   b_low, b_up: lower bound and upper bound of b
%   field_x: the x component of force field
%   field_y: the y component of force field
%   iter: number of iterations
%   show_fitness: a flag of whether to show the fitness function

% note: m rows and n columns, but x is column and y is row here
[m n]=size(field_x);

xc=x0;
yc=y0;
a=a0;
b=b0;
phi=phi0;

for it=1:iter
    [x,y,theta]=arb_ellipse_in_image(m,n,xc,yc,a,b,phi);
    N=max(size(x));

    %% torque along the ellpise about center
    torque_v=[0 0 0]; % the torque as a vector
    for i=1:max(size(theta))
        torque_v=torque_v+cross([x(i)-xc,y(i)-yc,0],...
            [field_x(y(i),x(i)),field_y(y(i),x(i)),0]);
    end
    torque=torque_v(3);
    torque=torque/N^2;
    if torque>t_phi
        phi=phi+d_phi;
    elseif torque<-t_phi
        phi=phi-d_phi;
    end

    %% F_around
    F_round=[0,0];
    for i=1:max(size(theta))
        F_round=F_round+[field_x(y(i),x(i)),field_y(y(i),x(i))];
    end
    F_round=F_round/max(size(theta));

    %% F_left, which is on the left quarter of the ellipse, and inward
    index = find( theta>pi*3/4 & theta<pi*5/4 );
    F_left=0;
    for i=index
        F_left=F_left+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[cos(phi),sin(phi)]);
    end
    F_left=F_left/max(size(index));

    %% F_right, which is on the right quarter of the ellipse, and inward
    index = find( theta<pi/4 | theta>pi*7/4 );
    F_right=0;
    for i=index
        F_right=F_right+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[-cos(phi),-sin(phi)]);
    end
    F_right=F_right/max(size(index));

    %% F_up, which is on the up quarter of the ellipse, and inward
    index = find( theta>pi/4 & theta<pi*3/4 );
    F_up=0;
    for i=index
        F_up=F_up+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[sin(phi),-cos(phi)]);
    end
    F_up=F_up/max(size(index));

    %% F_down, which is on the low quarter of the ellipse, and inward
    index = find( theta>pi*5/4 & theta<pi*7/4 );
    F_down=0;
    for i=index
        F_down=F_down+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[-sin(phi),cos(phi)]);
    end
    F_down=F_down/max(size(index));

    %% update xc and yc
    F_left_right=dot(F_round,[1,0]);
    if F_left_right>t_xc
        xc=xc+d_xc;
    elseif F_left_right<-t_xc
        xc=xc-d_xc;
    end

    F_down_up=dot(F_round,[0,1]);
    if F_down_up>t_yc
        yc=yc+d_yc;
    elseif F_down_up<-t_yc
        yc=yc-d_yc;
    end

    %% update xc and yc again according to diagonal force
    F_diag1=dot(F_round,[0.7071,0.7071]);
    if F_diag1>t_xc+t_yc
        xc=xc+d_xc;
        yc=yc+d_yc;
    elseif F_diag1<-t_xc-t_yc
        xc=xc-d_xc;
        yc=yc-d_yc;
    end

    F_diag2=dot(F_round,[-0.7071,0.7071]);
    if F_diag2>t_xc+t_yc
        xc=xc-d_xc;
        yc=yc+d_yc;
    elseif F_diag2<-t_xc-t_yc
        xc=xc+d_xc;
        yc=yc-d_yc;
    end

    %% update a and b

    if F_left+F_right>t_a
        a=a-d_a;
    elseif F_left+F_right<-t_a
        a=a+d_a;
    end

    if F_up+F_down>t_b
        b=b-d_b;
    elseif F_up+F_down<-t_b
        b=b+d_b;
    end

    if b>a
        temp=a;a=b;b=temp;
        phi=mod(phi+pi/2,pi);
    end

    %% restrict a and b using lower and upper bounds
    if a>a_up
        a=a_up;
    end
    if a<a_low
        a=a_low;
    end
    if b>b_up
        b=b_up;
    end
    if b<b_low
        b=b_low;
    end

    %% fitness function
    if show_fitness==1
        beta=0.9;
        fit0=0;
        fit1=0;
        fit2=0;
        [x,y,theta]=arb_ellipse_in_image(m,n,xc,yc,a,b,phi);
        [x1,y1,theta1]=arb_ellipse_in_image(m,n,xc,yc,a*beta,b*beta,phi);
        [x2,y2,theta2]=arb_ellipse_in_image(m,n,xc,yc,a/beta,b/beta,phi);
        for i=1:max(size(theta))
            fit0=fit0+norm([field_x(y(i),x(i)),field_y(y(i),x(i))]);
        end
        for i=1:max(size(theta1))
            fit1=fit1+norm([field_x(y1(i),x1(i)),field_y(y1(i),x1(i))]);
        end
        for i=1:max(size(theta2))
            fit2=fit2+norm([field_x(y2(i),x2(i)),field_y(y2(i),x2(i))]);
        end
        fit0=fit0/max(size(theta));
        fit1=fit1/max(size(theta1));
        fit2=fit2/max(size(theta2));
        fit_save(it)=fit0-fit1/2-fit2/2;
    end

end

if show_fitness==1
    figure;hold on;
    plot(1:iter,fit_save,'b','LineWidth',2);
    legend('fitness function');
    grid on;
    xlabel('iteration');
    ylabel('fitness function');
    title('fitness function in each iteration');
end


function [ t, rank ] = subset_lex_successor ( n, t, rank )

%*****************************************************************************80
%
%% SUBSET_LEX_SUCCESSOR computes the subset lexicographic successor.
%
%  Discussion:
%
%    In the original code, there is a last element with no successor.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the master set.
%    N must be positive.
%
%    Input/output, integer T(N), describes a subset.  T(I) is 0 if
%    the I-th element of the master set is not in the subset, and is
%    1 if the I-th element is part of the subset.
%    On input, T describes a subset.
%    On output, T describes the next subset in the ordering.
%    If the input T was the last in the ordering, then the output T
%    will be the first.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    t(1:n) = 0;
    rank = 0;
    return
  end
%
%  Check.
%
  subset_check ( n, t );

  for i = n : -1 : 1

    if ( t(i) == 0 )
      t(i) = 1;
      rank = rank + 1;
      return
    else
      t(i) = 0;
    end

  end

  rank = 0;

  return
end


%% Analyzing Neural Time Series Data
% Matlab code for Chapter 29
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% Figure 29.2

% create two signals
time    = 0:.0001:1;
signal1 = sin(2*pi*10*time);
signal2 = rand(size(signal1))*2-1; % uniform random numbers in the same scale as the sine wave

% plot signals
figure
subplot(221)
plot(time,signal1)
set(gca,'xlim',[time(1) time(end)])
subplot(222)
plot(time,signal2,'r')
set(gca,'xlim',[time(1) time(end)])

% bin data (via Matlab function hist)
nbins = 50;
[hdat1,x1] = hist(signal1,nbins);
[hdat2,x2] = hist(signal2,nbins);

% convert histograms to probability values
hdat1 = hdat1./sum(hdat1);
hdat2 = hdat2./sum(hdat2);

% plot histograms
subplot(223)
plot(x1,hdat1)
hold on
plot(x2,hdat2,'r')
legend({'Sine wave';'Random data'})
xlabel('Value bins')
ylabel('Probability')

subplot(224)
plot(sort(signal1))
hold on
plot(sort(signal2),'r')
set(gca,'xlim',[0 length(signal1)])

% The following code using the 'eval' command may seem a bit needlessly
% complex, but it introduces you to this useful Matlab function. The
% main advantage here is to call different variables inside a loop,
% which would otherwise not be possible because it is the variable
% names that are different, not indices into variables. For example,
% notice how the variables hdat1 and hdat2 are called.
for i=1:2
    eval([ 'entro(' num2str(i) ') = -sum(hdat' num2str(i) '.*log2(hdat' num2str(i) '+eps));' ]);
end

% the following code will do this same thing:
% entro(1) = -sum(hdat1.*log2(hdat1+eps));
% entro(2) = -sum(hdat2.*log2(hdat2+eps));

disp([ 'Entropies of sine wave and random noise are ' num2str(entro(1)) ' and ' num2str(entro(2)) '.' ]);

%% Figure 29.3

% range of bin numbers
nbins = 10:2000;

entropyByBinSize=zeros(size(nbins));

for nbini=1:length(nbins)

    % bin data, transform to probability, and eliminate zeros
    hdat = hist(signal1,nbins(nbini));
    hdat = hdat./sum(hdat);

    % compute entropy
    entropyByBinSize(nbini) = -sum(hdat.*log2(hdat+eps));
end

figure
plot(nbins,entropyByBinSize)
xlabel('Number of bins'), ylabel('Entropy')

%% Figure 29.4

% optimal number of bins for histogram based on a few different guidelines
n = length(signal1);
maxmin_range = max(signal1)-min(signal1);

% note: the function iqr (inter-quartile range) is in the stats toolbox.
% If you don't have this toolbox, you can write your own similar function
% by sorting the values, finding the values that are 25% and 75% of the
% sorted distribution, and then subtracting the 25% number from the 75% number.
fd_bins      = ceil(maxmin_range/(2.0*iqr(signal1)*n^(-1/3))); % Freedman-Diaconis
scott_bins   = ceil(maxmin_range/(3.5*std(signal1)*n^(-1/3))); % Scott
sturges_bins = ceil(1+log2(n)); % Sturges

figure
subplot(211)
% plot up to 50 bins
[junk,maxNbins] = min(abs(nbins-50)); % index of nbins that most closely matches 50
plot(nbins(1:maxNbins),entropyByBinSize(1:maxNbins))

hold on
plot([fd_bins fd_bins],get(gca,'ylim'),'m','linew',2)
plot([scott_bins scott_bins],get(gca,'ylim'),'k','linew',2)
plot([sturges_bins sturges_bins],get(gca,'ylim'),'r','linew',2)

legend({'entropy';'Freedman-Diaconis';'Scott';'Sturges'})
xlabel('Number of bins'), ylabel('Entropy')

subplot(223)
[y,x]=hist(signal1,fd_bins);
h=bar(x,y,'histc');
set(h,'linestyle','none');
set(gca,'xlim',[min(signal1) max(signal1)*1.1])
xlabel('Value'), ylabel('Count')
title('Optimal number of bins (FD rule)')

subplot(224)
[y,x]=hist(signal1,2000);
h=bar(x,y,'histc');
set(gca,'xlim',[min(signal1) max(signal1)]*1.05)
xlabel('Value'), ylabel('Count')
title('Too many bins (2000)')

%% Figure 29.5

load sampleEEGdata

% entropy over all sensors
time4entropy = [  100  400 ]; % in ms
base4entropy = [ -400 -100 ]; % in ms
topo_entropy = zeros(size(EEG.chanlocs));

timeidx=zeros(size(time4entropy)); baseidx=zeros(size(base4entropy));
for i=1:2
    [junk,timeidx(i)] = min(abs(EEG.times-time4entropy(i)));
    [junk,baseidx(i)] = min(abs(EEG.times-base4entropy(i)));
end

for chani=1:EEG.nbchan

    % entropy during task
    tempdat = EEG.data(chani,timeidx(1):timeidx(2),:);
    hdat = hist(tempdat(:),25);
    hdat = hdat./sum(hdat);
    task_entropy =  -sum(hdat.*log2(hdat+eps));

    % entropy during pre-stim baseline
    tempdat = EEG.data(chani,baseidx(1):baseidx(2),:);
    hdat = hist(tempdat(:),25);
    hdat = hdat./sum(hdat);
    base_entropy =  -sum(hdat.*log2(hdat+eps));

    % compute entropy
    topo_entropy(chani) = task_entropy - base_entropy;
end

figure
% Note: topoplot is a function in the eeglab toolbox
topoplot(topo_entropy,EEG.chanlocs,'maplimits',[-.5 .5],'plotrad',.53,'electrodes','off','numcontour',0);

% entropy over time in one electrode (fcz or po8)
sensor4entropy = 'fcz';
times2save     = -300:50:1200;
timewindow     = 400; % ms

timewindowidx = round(timewindow/(1000/EEG.srate)/2);
times2saveidx = zeros(size(times2save));
for i=1:length(times2save)
    [junk,times2saveidx(i)]=min(abs(EEG.times-times2save(i)));
end

electrodeidx = find(strcmpi(sensor4entropy,{EEG.chanlocs.labels}));

timeEntropy = zeros(1,length(times2save));

for timei = 1:length(times2save)

    tempdata = EEG.data(electrodeidx,times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,1:30);
    hdat = hist(tempdata(:),25);
    hdat = hdat./sum(hdat);

    % compute entropy
    timeEntropy(1,timei) = -sum(hdat.*log2(hdat+eps));

    tempdata = EEG.data(electrodeidx,times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,end-30:end);
    hdat = hist(tempdata(:),25);
    hdat = hdat./sum(hdat);

    % compute entropy
    timeEntropy(2,timei) = -sum(hdat.*log2(hdat+eps));
end

figure
plot(times2save,timeEntropy)
xlabel('Time (ms)'), ylabel('Entropy (bits)')
title([ 'Entropy over time from electrode ' sensor4entropy ])
legend({'First 30 trials';'last 30 trials'})

%% Figure 29.6

% Note about this figure: The panels use the same code but with different
% input signals. Comment out some of the lines below to recreate each panel.

% right panel: random noise
signal1 = rand(size(signal1))*2-1;
signal2 = rand(size(signal1))*2-1;

% center panel: one pure sine wave and one sine wave plus random noise
signal1 = sin(2*pi*10*time);
signal2 = signal1 + randn(size(signal1))/2;

% left panel: one pure sine wave and its inverse
signal1 = sin(2*pi*10*time);
signal2 = -signal1;

% determine the optimal number of bins for each variable
n            = length(signal1);
maxmin_range = max(signal1)-min(signal1);
fd_bins1     = ceil(maxmin_range/(2.0*iqr(signal1)*n^(-1/3))); % Freedman-Diaconis

n            = length(signal2);
maxmin_range = max(signal2)-min(signal2);
fd_bins2     = ceil(maxmin_range/(2.0*iqr(signal2)*n^(-1/3))); % Freedman-Diaconis

% and use the average...
fd_bins = ceil((fd_bins1+fd_bins2)/2);

% bin data (using histc this time)
edges = linspace(min(signal1),max(signal1),fd_bins+1);
[nPerBin1,bins1] = histc(signal1,edges);

edges = linspace(min(signal2),max(signal2),fd_bins+1);
[nPerBin2,bins2] = histc(signal2,edges);

% compute joint frequency table
jointprobs = zeros(fd_bins);
for i1=1:fd_bins
    for i2=1:fd_bins
        jointprobs(i1,i2) = sum(bins1==i1 & bins2==i2);
    end
end
jointprobs=jointprobs./sum(jointprobs(:));

figure
subplot(211)
plot(time,signal1)
subplot(212)
plot(time,signal2)

figure
imagesc(jointprobs)
colormap gray
set(gca,'clim',[0 .01],'ydir','normal')
colorbar
xlabel('Signal 2 bin'), ylabel('Signal 1 bin')

%% Figure 29.7

figure
subplot(221)
x = 0:.001:1;
y = x;
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

subplot(222)
x = 0:.001:1;
y = -x.^3;
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

subplot(223)
x=cos(0:.01:2*pi);
y=sin(0:.01:2*pi);
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

subplot(224)
x=[cos(0:.01:2*pi) cos(0:.01:2*pi)+1];
y=[sin(0:.01:2*pi) sin(0:.01:2*pi)-1];
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

%% Figure 29.8

% Theoretical size of errors as a function of histogram bins and N
% the Matlab 'inline' function is a useful tool to substitute small
% functions (1-2 lines of code) that only need to be run locally.
% However, as mentioned in chapter 26, this function will be removed in
% future versions of Matlab.
entropy_error = inline('(b-1)./(2.*n.*log(2))');
mutinfo_error = inline('(b-1).^2./(2.*n.*log(2))');

n = 20:300;
nfixed = 15;

figure
subplot(211)
plot(n,entropy_error(nfixed,n))
hold on
plot(n,entropy_error(ceil(1+log2(n)),n),'r')
legend({[ num2str(nfixed) ' bins' ];'Sturges'' rule'})
xlabel('Number of data points'), ylabel('Entropy error (bits)')
set(gca,'xlim',[n(1) n(end)])

subplot(212)
plot(n,mutinfo_error(nfixed,n))
hold on
plot(n,mutinfo_error(ceil(1+log2(n)),n),'r')
legend({[ num2str(nfixed) ' bins' ];'Sturges'' rule'})
xlabel('Number of data points'), ylabel('Mutual information error')
set(gca,'xlim',[n(1) n(end)])

%% Figure 29.9

x = [cos(0:.01:2*pi) cos(0:.01:2*pi)+1];
y = [sin(0:.01:2*pi) sin(0:.01:2*pi)-1];

figure
subplot(221)
plot(x,y,'.')
axis([-3 3 -3 3])

subplot(212)

noiselevels = 0:.01:1;
mi = zeros(size(noiselevels));
for ni=1:length(noiselevels)
    mi(ni) = mutualinformationx(x,y+randn(size(y))*noiselevels(ni),20);
end

plot(noiselevels,mi)
xlabel('Noise level'), ylabel('Mutual information')

subplot(222)
plot(x,y+randn(size(y))*noiselevels(round(ni/2)),'.');
axis([-3 3 -3 3])

%% Figure 29.9d (takes a while to run)

nrange = 300:205:3000;
noiselevels = 0:.01:1;

mi = zeros(length(nrange),length(noiselevels));
b  = zeros(1,length(nrange)); % number of histogram bins

for ni=1:length(nrange)

    % define time
    t = linspace(0,2*pi,nrange(ni));
    % define signals
    x = [cos(t) cos(t)+1];
    y = [sin(t) sin(t)-1];

    for noi=1:length(noiselevels)
        if noi==1 % keep number of bins constant across noise levels within each number of points
            [mi(ni,noi),~,b(ni)] = mutualinformationx(x,y+randn(size(y))*noiselevels(noi));
        else
            mi(ni,noi) = mutualinformationx(x,y+randn(size(y))*noiselevels(noi),b(ni));
        end
    end
end

% convert to % change from best-case scenario (no noise, large N)
mip=100.*(mi-mi(end,1))./mi(end,1);

figure
contourf(noiselevels,nrange,mip,40,'linecolor','none')
set(gca,'clim',[-100 0])
colorbar
xlabel('Noise level'), ylabel('N (data length)')
title('Percent decrease in MI due to noise')

%% Figure 29.10

electrodes4mi = {'fz';'o1'};
timewindow = 400; % in ms
times2save = -400:100:1200;

% convert ms to indices
timewindowidx = round(timewindow/(1000/EEG.srate)/2);
times2saveidx = zeros(size(times2save));
for i=1:length(times2save)
    [junk,times2saveidx(i)]=min(abs(EEG.times-times2save(i)));
end

electrodesidx(1) = find(strcmpi(electrodes4mi{1},{EEG.chanlocs.labels}));
electrodesidx(2) = find(strcmpi(electrodes4mi{2},{EEG.chanlocs.labels}));

% initialize outputs
entropy = zeros(3,length(times2save));
mi      = zeros(2,length(times2save));
nbins   = zeros(1,length(times2save));

for timei = 1:length(times2save)
    datax = EEG.data(electrodesidx(1),times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);
    datay = EEG.data(electrodesidx(2),times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);

    [mi(1,timei),entropy(:,timei),nbins(timei)] = mutualinformationx(datax,datay);
    [mi(2,timei),entropy(:,timei)             ] = mutualinformationx(datax,datay,70);
end

figure
set(gcf,'name',[ 'Mutual information between ' electrodes4mi{1} ' and ' electrodes4mi{2} ])

subplot(221)
plot(times2save,mi(1,:))
xlabel('Time (ms)'), ylabel('MI (bits)')
title('Variable bin length')
set(gca,'xlim',[times2save(1)-50 times2save(end)+50],'ylim',[min(mi(:))-.01 max(mi(:))+.01])

subplot(222)
plot(nbins,mi(1,:),'.')
xlabel('bin length'), ylabel('MI (bits)')
title('Bin length vs. MI')
set(gca,'ylim',[min(mi(:))-.01 max(mi(:))+.01])

subplot(223)
plot(times2save,mi(2,:))
xlabel('Time (ms)'), ylabel('MI (bits)')
title('Constant bin length')
set(gca,'xlim',[times2save(1)-50 times2save(end)+50],'ylim',[min(mi(:))-.01 max(mi(:))+.01])

%% Figure 29.11

% (Figure 10 must be generated before running this cell.)
frex = logspace(log10(4),log10(40),20);
baselinetime = [-500 -200];

% baseline from ms to idx
[junk,baseidx(1)] = min(abs(times2save-baselinetime(1)));
[junk,baseidx(2)] = min(abs(times2save-baselinetime(2)));

% specify convolution and wavelet info
time = -1:1/EEG.srate:1;
half_of_wavelet_size = (length(time)-1)/2;
n_wavelet     = length(time);
n_data        = EEG.pnts*EEG.trials;
n_convolution = n_wavelet+n_data-1;
% FFT of data
fft_EEG1 = fft(reshape(EEG.data(electrodesidx(1),:,:),1,EEG.pnts*EEG.trials),n_convolution);
fft_EEG2 = fft(reshape(EEG.data(electrodesidx(2),:,:),1,EEG.pnts*EEG.trials),n_convolution);

% initialize outputs
mi   = zeros(2,length(frex),length(times2save));
ispc = zeros(length(frex),length(times2save));
powc = zeros(length(frex),length(times2save));

for fi=1:length(frex)
    % create wavelet and get its FFT
    fft_wavelet = fft(exp(2*1i*pi*frex(fi).*time) .* exp(-time.^2./(2*(4/(2*pi*frex(fi)))^2)),n_convolution);

    % convolution of each electrode with wavelet
    convres     = ifft(fft_wavelet.*fft_EEG1,n_convolution);
    analytic1   = reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials);
    convres     = ifft(fft_wavelet.*fft_EEG2,n_convolution);
    analytic2   = reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials);

    for timei = 1:length(times2save)
        datax = analytic1(times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);
        datay = analytic2(times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);

        % compute MI
        mi(1,fi,timei) = mutualinformationx(log10(abs(datax).^2),log10(abs(datay).^2),50);
        mi(2,fi,timei) = mutualinformationx(angle(datax),angle(datay),20);

        % also compute ISPC-time for comparison
        ispc(fi,timei) = mean(abs(mean(exp(1i*(angle(datay)-angle(datax))),2)),1);

        % also compute power correlations-time
        dataxr = tiedrank(abs(datax));
        datayr = tiedrank(abs(datay));
        n = timewindowidx*2+1;
        powc(fi,timei) = mean( 1-6*sum((dataxr-datayr).^2)/(n*(n^2-1)) );
    end

    disp([ 'Finished frequency ' num2str(fi) ' out of ' num2str(length(frex)) ]);
end

figure, set(gcf,'name',[ 'Mutual information between ' electrodes4mi{1} ' and ' electrodes4mi{2} ])

for i=1:2
    % plot baseline-subtracted corrected MI
    subplot(2,2,i)
    contourf(times2save,frex,squeeze(mi(i,:,:))-repmat(mean(mi(i,:,baseidx(1):baseidx(2)),3)',1,length(times2save)),40,'linecolor','none')
    set(gca,'clim',[-.075 .075],'yscale','log','ytick',round(logspace(log10(frex(1)),log10(frex(end)),6)))
end

% plot power correlations
subplot(223)
contourf(times2save,frex,powc-repmat(mean(powc(:,baseidx(1):baseidx(2)),2),1,length(times2save)),40,'linecolor','none')
set(gca,'clim',[-.2 .2],'yscale','log','ytick',round(logspace(log10(frex(1)),log10(frex(end)),6)))

% plot phase synchronization
subplot(224)
contourf(times2save,frex,ispc-repmat(mean(ispc(:,baseidx(1):baseidx(2)),2),1,length(times2save)),40,'linecolor','none')
set(gca,'clim',[-.1 .1],'yscale','log','ytick',round(logspace(log10(frex(1)),log10(frex(end)),6)))

%% Figure 29.12

time    = 0:.0001:1;
signal1 = sin(2*pi*10*time);
signal2 = -signal1;

lagz=1:10:1500;
milags=zeros(size(lagz));

for li=1:length(lagz)
    milags(li) = mutualinformationx(signal1,[signal2(lagz(li):end) signal2(1:lagz(li)-1)],15);
end

figure
plot(lagz/(1/mean(diff(time))),milags)
xlabel('Lag (seconds)'), ylabel('mutual information (bits)')

% now on real data (6 Hz power MI from figure 11)

time = -1:1/EEG.srate:1;
fft_wavelet = fft(exp(2*1i*pi*frex(5).*time) .* exp(-time.^2./(2*(4/(2*pi*frex(5)))^2)),n_convolution);

% convolution of each electrode with wavelet
convres = ifft(fft_wavelet.*fft_EEG1,n_convolution);
pow1    = log10(abs(reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials)).^2);
convres = ifft(fft_wavelet.*fft_EEG2,n_convolution);
pow2    = log10(abs(reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials)).^2);

onecycle = round(1000/frex(5));
onecycleidx = round(onecycle/(1000/EEG.srate));

lagz=-onecycleidx:onecycleidx;
milags=zeros(size(lagz));

for li=1:length(lagz)

    if lagz(li)<0
        milags(li) = mutualinformationx(pow1(1:end+lagz(li),:),pow2(-lagz(li)+1:end,:),30); % reverse sign for negative lags
    elseif lagz(li)==0
        milags(li) = mutualinformationx(pow1,pow2,30); % special case for no lag
    elseif lagz(li)>0
        milags(li) = mutualinformationx(pow1(lagz(li)+1:end,:),pow2(1:end-lagz(li),:),30);
    end
end

figure
plot(1000*lagz/(1/mean(diff(time))),milags)
xlabel([ electrodes4mi{1} ' leads ' electrodes4mi{2} ' ... Lag (ms) ... ' electrodes4mi{2} ' leads ' electrodes4mi{1} ]), ylabel('mutual information (bits)')
set(gca,'xlim',[-onecycle onecycle])

%% end.


%% Dynamic Matrix Control Tutorial
% Dynamic Matrix Control (DMC) was the first Model Predictive Control (MPC)
% algorithm introduced in early 1980s. Nowadays, DMC is available in almost
% all commercial industrial distributed control systems and process
% simulation software packages. This tutorial intends to explain the
% features of DMC using the dmc function developed by the author.

%% Example: A Water Heater
% Consider a water heater as shown in the following figure, where the cold
% water is heated by means of a gas burner. The aim of DMC is by
% manipulating valve to control the gas flow so that the outlet temperature
% is at desired level.
%%
%
% <<WaterHeater.PNG>>
%

%% The Step Response Model
% The DMC algorithm works with a step response model. The first step to
% design a DMC controller is to perform a step test on the plant to
% generate a step response model. The step response of the water heater is
% obtained through such a test and given as follows:
p.sr = [0;0;0.271;0.498;0.687;0.845;0.977;1.087;1.179;1.256;...
        1.320;1.374;1.419;1.456;1.487;1.513;1.535;1.553;1.565;1.581;...
        1.592;1.600;1.608;1.614;1.619;1.632;1.627;1.630;1.633;1.635];

% The response is converged after 30 steps.

%% DMC without Setpoint Prediction
% Setup the DMC
N=120;  % Total simulation length (samples)
p.p=10; % Prediction horizon
p.m=5;  % Moving horizon
p.la=1; % Control weight
% Reference (setpoint)
R=[ones(30,1);zeros(30,1);ones(30,1);zeros(30,1)];
p.y=0;  % Initial output
p.v=[]; % empty past input to indicate initialization
% buffer of input to cope with time delay
u=zeros(3,1);
% Initialization of variables for results
Y=zeros(N,1);
U=zeros(N,1);
% DMC Simulation
for k=1:120
    p.a=0;
    p.r=R(k);   % DMC only knows current setpoint
    if k>60     % change smoothing factor for second half simulation
        p.a=0.7;
    end
    p=dmc(p);
    Y(k)=p.y;
    U(k)=p.u;
    u=[u(2:3);p.u];
    p.y=0.8351*p.y+0.2713*u(1); % actual plant output
end
% DMC results
subplot(211)
plot(1:N,Y,'b-',1:N,R,'r--',[60 60],[-0.5 1.5],':','linewidth',2)
title('solid: output, dashed: reference')
text(35,1,'\alpha=0')
text(95,1,'\alpha=0.7')
axis([0 120 -0.5 1.5])
subplot(212)
[xx,yy]=stairs(1:N,U);
plot(xx,yy,'-',[60 60],[-0.5 1.5],':','linewidth',2)
axis([0 120 -0.5 1.5])
title('input, \lambda=1')
xlabel('time, min')

%% DMC with Setpoint Prediction
% Setup the DMC
N=120;  % Total simulation length (samples)
p.p=10; % Prediction horizon
p.m=5;  % Moving horizon
p.la=1; % Control weight
% Reference (setpoint)
R=[ones(30,1);zeros(30,1);ones(30,1);zeros(30,1)];
p.y=0;  % Initial output
p.v=[]; % empty past input to indicate initialization
% buffer of input to cope with time delay
u=zeros(3,1);
% Initialization of variables for results
Y=zeros(N,1);
U=zeros(N,1);
% DMC Simulation
for k=1:120
    p.a=0;
    p.r=R(k:min(N,k+p.p)); % DMC knows future setpoint
    if k>60     % change smoothing factor for second half simulation
        p.a=0.7;
    end
    p=dmc(p);
    Y(k)=p.y;
    U(k)=p.u;
    u=[u(2:3);p.u];
    p.y=0.8351*p.y+0.2713*u(1); % actual plant output
end
% DMC results
subplot(211)
plot(1:N,Y,'b-',1:N,R,'r--',[60 60],[-0.5 1.5],':','linewidth',2)
title('solid: output, dashed: reference')
text(35,1,'\alpha=0')
text(95,1,'\alpha=0.7')
axis([0 120 -0.5 1.5])
subplot(212)
[xx,yy]=stairs(1:N,U);
plot(xx,yy,'-',[60 60],[-0.5 1.5],':','linewidth',2)
axis([0 120 -0.5 1.5])
title('input, \lambda=1')
xlabel('time, min')

%% DMC with Different Control Weight
% Setup the DMC
N=120;  % Total simulation length (samples)
p.p=10; % Prediction horizon
p.m=5;  % Moving horizon
p.la=0.1; % Control weight
% Reference (setpoint)
R=[ones(30,1);zeros(30,1);ones(30,1);zeros(30,1)];
p.y=0;  % Initial output
p.v=[]; % empty past input to indicate initialization
% buffer of input to cope with time delay
u=zeros(3,1);
% Initialization of variables for results
Y=zeros(N,1);
U=zeros(N,1);
% DMC Simulation
for k=1:120
    p.a=0;
    p.r=R(k:min(N,k+p.p)); % DMC knows future setpoint
    if k>60     % change smoothing factor for second half simulation
        p.a=0.7;
    end
    p=dmc(p);
    Y(k)=p.y;
    U(k)=p.u;
    u=[u(2:3);p.u];
    p.y=0.8351*p.y+0.2713*u(1); % actual plant output
end
% DMC results
subplot(211)
plot(1:N,Y,'b-',1:N,R,'r--',[60 60],[-0.5 1.5],':','linewidth',2)
title('solid: output, dashed: reference')
text(35,1,'\alpha=0')
text(95,1,'\alpha=0.7')
axis([0 120 -0.5 1.5])
subplot(212)
[xx,yy]=stairs(1:N,U);
plot(xx,yy,'-',[60 60],[-0.5 1.5],':','linewidth',2)
axis([0 120 -1 2])
title('input, \lambda=0.1')
xlabel('time, min')

%% Reference
% Camacho, E.F. and Bordons, C., Model Predictive Control, Springer-Verlag,
% 1999.


function [R,L_lb,L_cpd] = rankest(T,options)
%RANKEST Estimate rank.
%   rankest(T) plots an L-curve of number of rank-one terms in a canonical
%   polyadic decomposition. The x-axis corresponds to the number of
%   rank-one terms, and the y-axis corresponds to the relative error of the
%   CPD in that many rank-one terms. Additionally, the corner R of the
%   resulting L-curve is estimated.
%
%   R = rankest(T) does not plot anything and instead returns the number of
%   rank-one terms R corresponding to the corner of the L-curve.
%
%   [R,L_lb,L_cpd] = rankest(T) also returns the L-curve ranks L_lb(:,1)
%   and L_cpd(:,1) and the corresponding lower bound on the truncation
%   error L_lb(:,2) and relative error of the CPD approximation L_cpd(:,2),
%   respectively.
%
%   rankest(T,options) may be used to set the following options:
%
%      options.MaxR =           - Maximum number of rank-one terms to try.
%      numel(T)/max(size_tens)
%      options.MinR = 1         - Minimum number of rank-one terms to try.
%      options.MinRelErr = 1e-2 - Determines an upper threshold for the
%                                 number of rank-one terms R to try.
%      options.Solver = @cpd    - The solver used to compute the CPD for
%                                 each number of rank-one terms R. Called
%                                 as options.Solver(T,R, ...
%                                 options.SolverOptions), where
%                                 options.SolverOptions is an options
%                                 structure passed to the solver.
%      options.XMultiplier = 1  - The importance of the number of rank-one
%                                 terms in determining the L-curve corner,
%                                 relative to the importance of the
%                                 relative error of the approximation.
%
%   See also mlrankest.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] J.L. Castellanos, S. Gomez, V. Guerra, "The triangle method for
%       finding the corner of the L-curve," Applied Numerical Mathematics,
%       Vol. 43, No. 4, 2002, pp. 359-373.

% Incomplete/sparse tensors not supported yet.
T = fmt(T,true);
if isstruct(T), size_tens = T.size;
else size_tens = size(T); end

% Check the options structure.
if nargin < 2, options = struct; end
if ~isfield(options,'MaxR')
    options.MaxR = prod(size_tens)/max(size_tens);
end
if ~isfield(options,'MinR'), options.MinR = 1; end
if ~isfield(options,'MinRelErr'), options.MinRelErr = 1e-2; end
if ~isfield(options,'Solver'), options.Solver = @cpd; end
if ~isfield(options,'SolverOptions'), options.SolverOptions = struct; end
if ~isfield(options.SolverOptions,'Compression')
    options.SolverOptions.Compression = false;
end
if ~isfield(options.SolverOptions,'Display')
    options.SolverOptions.Display = false;
end
if ~isfield(options,'XMultiplier'), options.XMultiplier = 1; end

% Compute lower bound on truncation error.
frobT = frob(T);
if ~isstruct(T)
    [~,~,sv] = mlsvd(T);
    Rmax = max(cellfun(@length,sv))-1;
    lb = max(cell2mat(cellfun( ...
             @(s)[sqrt(flipud(cumsum(flipud(s(:).^2)))).'/frobT ...
             zeros(1,Rmax+1-length(s))],sv(:),'UniformOutput',false)));
    lb = lb(2:end);
else
    Rmax = 0;
    lb = [];
end

% Determine the range of rank-one terms to test.
if ~isempty(lb)
    R = find(lb(options.MinR:end)<=options.MinRelErr,1,'first')+ ...
        options.MinR-1;
    if isempty(R), R = max(options.MinR,Rmax+1); end
    R = R:options.MaxR;
else
    R = 1:options.MaxR;
end

% Compute the relative error for each number of rank-one terms R(r).
relerr = zeros(1,length(R));
for r = 1:length(R)

    % Compute the CPD in R rank-one terms.
    [U,output] = options.Solver(T,R(r),options.SolverOptions);
    if isfield(output,'Refinement') && ...
       isfield(output.Refinement,'fval')
        relerr(r) = sqrt(2*output.Refinement.fval(end))/frobT;
    elseif isfield(output,'Algorithm') && ...
           isfield(output.Algorithm,'fval')
        relerr(r) = sqrt(2*output.Algorithm.fval(end))/frobT;
    elseif isfield(output,'fval')
        relerr(r) = sqrt(2*output.fval(end))/frobT;
    else
        relerr(r) = frob(cpdres(T,U))/frobT;
    end

    % Stop is options.MinRelErr has been reached.
    if relerr(r) <= options.MinRelErr
        R = R(1:r);
        relerr = relerr(1:r);
        break;
    end

    % Update the plot.
    if r > 1 && r < length(R) && nargout == 0, Lcurve(true); end

end

% Compute L-curve corner using the triangle method [1].
logx = options.XMultiplier*R(:);
logy = log10(relerr(:));
if R(1) ~= 1 && ~isempty(lb)
    logx = [options.XMultiplier;logx];
    logy = [log10(lb(1));logy];
end
ab = cat(3,bsxfun(@minus,logx,logx.'),bsxfun(@minus,logy,logy.'));
ac = cat(3,logx(end)-logx.',logy(end)-logy.');
area = bsxfun(@times,ab(:,:,1),ac(:,:,2)) - ...
       bsxfun(@times,ab(:,:,2),ac(:,:,1));
cosa = bsxfun(@times,ab(:,:,1),ac(:,:,1)) + ...
       bsxfun(@times,ab(:,:,2),ac(:,:,2));
cosa = bsxfun(@rdivide,cosa./sqrt(ab(:,:,1).^2+ab(:,:,2).^2), ...
                       sqrt(ac(:,:,1).^2+ac(:,:,2).^2));
cosa(area >= 0 | tril(true(size(cosa))) | cosa <= cos(7*pi/8)) = -1;
[a,opt] = max(max(cosa(:,2:end)));
if isempty(a), opt = 1; end
if a == -1, opt = size(cosa,2)-1; end

% Display output.
L_lb = [(1:Rmax).' lb(:)];
L_cpd = [R(:) relerr(:)];
if nargout == 0, Lcurve(false); end
R = R(opt);

function Lcurve(update)
    if R(r) <= 1, return; end
    style = {'Marker','+','MarkerSize',2.5};
    if ~update
        semilogy(R(opt),relerr(opt),'rs','LineStyle','none'); hold on;
    end
    semilogy(R(1:length(relerr)),relerr,'r',style{:}); hold on;
    semilogy(1:length(lb),lb,style{:});
    semilogy([1 R(r)],options.MinRelErr*[1 1],'k:');
    text(1,options.MinRelErr,'MinRelErr','VerticalAlignment','Top');
    hold off;
    ylim([min(options.MinRelErr/(10^0.5),min(relerr(1:r))) lb(1)]);
    xlim([1 R(r)]);
    ylabel('frob(cpdres(T,U))/frob(T)'); xlabel('R');
    xt = get(gca,'XTick'); set(gca,'XTick',xt(mod(xt,1) == 0));
    if update
        if ~isempty(lb)
            legend(['CPD error (trying R = ' int2str(R(r+1)) '...)'], ...
                'Lower bound on error','Location','NE');
        else
            legend(['CPD error (trying R = ' int2str(R(r+1)) '...)'], ...
                'Location','NE');
        end
    else
        if ~isempty(lb)
            legend(['L-curve corner at R = ' int2str(R(opt))], ...
                'CPD error','Lower bound on error','Location','NE');
        else
            legend(['L-curve corner at R = ' int2str(R(opt))], ...
                'CPD error','Location','NE');
        end
    end
    set(datacursormode(gcf),'UpdateFcn',@datacursor);
    drawnow;
end

function txt = datacursor(~,event_obj)
    pos = get(event_obj,'Position');
    txt = {['R: ' int2str(pos(1))], ...
           ['relative error: ' num2str(pos(2))]};
end

end


function Serr=specerr(S,J,err,trialave,numsp)
% Function to compute lower and upper confidence intervals on the spectrum
% Usage: Serr=specerr(S,J,err,trialave,numsp)
% Outputs: Serr (Serr(1,...) - lower confidence level, Serr(2,...) upper confidence level)
%
% Inputs:
% S - spectrum
% J - tapered fourier transforms
% err - [errtype p] (errtype=1 - asymptotic estimates; errchk=2 - Jackknife estimates;
%                   p - p value for error estimates)
% trialave - 0: no averaging over trials/channels
%            1 : perform trial averaging
% numsp    - number of spikes in each channel. specify only when finite
%            size correction required (and of course, only for point
%            process data)
%
% Outputs:
% Serr - error estimates. Only for err(1)>=1. If err=[1 p] or [2 p] Serr(...,1) and Serr(...,2)
% contain the lower and upper error bars with the specified method.
if nargin < 4; error('Need at least 4 input arguments'); end;
if err(1)==0; error('Need err=[1 p] or [2 p] for error bar calculation. Make sure you are not asking for the output of Serr'); end;
[nf,K,C]=size(J);
errchk=err(1);
p=err(2);
pp=1-p/2;
qq=1-pp;

if trialave
   dim=K*C;
   C=1;
   dof=2*dim;
   if nargin==5; dof = fix(1/(1/dof + 1/(2*sum(numsp)))); end
   J=reshape(J,nf,dim);
else
   dim=K;
   dof=2*dim*ones(1,C);
   for ch=1:C;
     if nargin==5; dof(ch) = fix(1/(1/dof + 1/(2*numsp(ch)))); end
   end;
end;
Serr=zeros(2,nf,C);
if errchk==1;
   Qp=chi2inv(pp,dof);
   Qq=chi2inv(qq,dof);
   Serr(1,:,:)=dof(ones(nf,1),:).*S./Qp(ones(nf,1),:);
   Serr(2,:,:)=dof(ones(nf,1),:).*S./Qq(ones(nf,1),:);
elseif errchk==2;
   tcrit=tinv(pp,dim-1);
   for k=1:dim;
       indices=setdiff(1:dim,k);
       Jjk=J(:,indices,:); % 1-drop projection
       eJjk=squeeze(sum(Jjk.*conj(Jjk),2));
       Sjk(k,:,:)=eJjk/(dim-1); % 1-drop spectrum
   end;
   sigma=sqrt(dim-1)*squeeze(std(log(Sjk),1,1)); if C==1; sigma=sigma'; end;
   conf=repmat(tcrit,nf,C).*sigma;
   conf=squeeze(conf);
   Serr(1,:,:)=S.*exp(-conf); Serr(2,:,:)=S.*exp(conf);
end;
Serr=squeeze(Serr);


function ccl_sparse_test ( )

%*****************************************************************************80
%
%% CCL_SPARSE_TEST uses CCL to build a sparse grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Local parameters:
%
%    Local, integer D, the spatial dimension.
%
%    Local, integer MAXK, the maximum level to check.
%
  d = 10;
  maxk = 7;
  func = 'prod( exp(-(x/2).^2/2)/2/sqrt(2*pi), 2)';
  trueval = fu_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CCL_SPARSE_TEST:\n' );
  fprintf ( 1, '  CCL sparse grid:\n' );
  fprintf ( 1, '  Clenshaw-Curtis Linear sparse grid.\n' );
  fprintf ( 1, '  Exact integral is %g\n', trueval );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   D  Level   Nodes    SG error    MC error\n' );
  fprintf ( 1, '\n' );

  for k = 1 : maxk
%
%  Compute the sparse grid estimate.
%
    [ x, w ] = nwspgr ( 'ccl', d, k );
    fx = eval ( func );
    SGappr = w' * fx;
    SGerror = sqrt ( ( SGappr - trueval ).^2 ) / trueval;
%
%  Average 1000 Monte Carlo estimates.
%
    numnodes = length ( w );
    sim = zeros ( 1000, 1 );
    for r = 1 : 1000
      x = rand ( numnodes, d );
      fx = eval ( func );
      sim(r) = mean ( fx );
    end
    simerror = sqrt ( mean ( ( sim - trueval ).^2 ) ) / trueval;

    fprintf( '  %2d     %2d  %6d  %10.5g  %10.5g\n', ...
      d, k, numnodes, SGerror, simerror )

  end

  return
end


function a = gk323_inverse ( n )

%*****************************************************************************80
%
%% GK323_INVERSE returns the inverse of the GK323 matrix.
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i == j )
        if ( i == 1 | i == n )
          a(i,j) = - 0.5 * ( n - 2 ) / ( n - 1 );
        else
          a(i,j) = - 1.0;
        end
      elseif ( i == j+1 | i == j-1 )
        a(i,j) = 0.5;
      elseif ( i == 1 & j == n )
        a(i,j) = 0.5 / ( n - 1 );
      elseif ( i == n & j == 1 )
        a(i,j) = 0.5 / ( n - 1 );
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [zPred,PzPred,otherInfo]=sqrtKalmanMeasPred(xPred,SPred,H)
%%SQRTKALMANMEASPRED Perform the measurement prediction part of the
%           measurement update step of the square-root Kalman filter. The
%           function sqrtKalmanUpdateWithPred can be used to complete the
%           measurement update. Separating the measurement prediction step
%           from the rest of the update step can make the creation of
%           multiple measurement association hypotheses from a single
%           target prediction more efficient. The full measurement update
%           function is sqrtKalmanUpdate.
%
%INPUTS: xPred The xDimXnumComp set of numComp predicted target states.
%        SPred The xDimXxDimXnumComp lower-triangular predicted square-root
%              state covariance matrices.
%            H The zDimXxDim measurement matrix. The measurement is modeled
%              as z=H*x+noise and the model is the same for all components
%              given in xPred and SPred.
%
%OUTPUTS: zPred The zDimXnumComp measurement predictions from the filter.
%        PzPred The zDimXzDimXnumComp covariance matrix associated with
%               zPred.
%     otherInfo A structure containing members of intermediate results of
%               this function that can be passed to sqrtKalmanUpdate
%               when updating with a measurement.
%
%The mathematics behind the specific square root implementation used here
%are described in Appendix G of [1].
%
%EXAMPLE:
%With this example, we demonstrate that one gets the same result using
%KalmanUpdate versus calling sqrtKalmanMeasPred and then
%sqrtKalmanUpdateWithPred.
% xPred=[1e3;-2e3;100;200];
% SPred=chol([28,   3.5,    6,  8.5;
%            3.5,    23,  8.5,   11;
%              6,   8.5,   18, 13.5;
%            8.5,    11, 13.5,   13],'lower');
% z=1e3*[-5.498856156296510;
%        1.199241491470584];
% SR=eye(2);
% H=[0, 4, 9, 8;
%    6, 3, 0, 6];
%
% %The update in one step.
% [xUpdate,SUpdate,innov,Szz,W]=sqrtKalmanUpdate(xPred,SPred,z,SR,H);
% %The update in two steps.
% [zPred,PzPred,otherInfo]=sqrtKalmanMeasPred(xPred,SPred,H);
% [xUpdate1,SUpdate1,innov1,Szz1,W1]=sqrtKalmanUpdateWithPred(z,SR,zPred,otherInfo);
% %One will see that the one and two step updates agree.
% max(abs([xUpdate1-xUpdate;SUpdate1(:)-SUpdate(:);innov1(:)-innov;Szz1(:)-Szz(:);W1(:)-W(:)]))
%
%REFERENCES
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

zDim=size(H,1);
xDim=size(H,2);
numComp=size(xPred,2);

zPred=H*xPred;

PzPred=zeros(zDim,zDim,numComp);
Pxz=zeros(xDim,zDim,numComp);
for k=1:numComp
    temp=H*SPred(:,:,k);
    PzPred(:,:,k)=temp*temp';

    Pxz(:,:,k)=SPred(:,:,k)*SPred(:,:,k)'*H';
    %Pxz is not needed for the measurement prediction, but we compute it
    %here, so that it need not be recomputed again and again if
    %sqrtKalmanUpdateWithPred is called for multiple measurements.
end
otherInfo.xPred=xPred;
otherInfo.SPred=SPred;
otherInfo.Pxz=Pxz;
otherInfo.H=H;

end

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


function ns3de_test02 ( )

%*****************************************************************************80
%
%% NS3DE_TEST02 samples the residual at the initial time.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/navier_stokes_3d_exact/ns3de_test02.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 January 2015
%
%  Author:
%
%    John Burkardt
%
  a = pi / 4.0;
  d = pi / 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'NS3DE_TEST02\n' );
  fprintf ( 1, '  Sample the Navier-Stokes residuals' );
  fprintf ( 1, '  at the initial time T = 0, using a region that is \n' );
  fprintf ( 1, '  the cube centered at (0,0,0) with "radius" 1.0,\n' );
  fprintf ( 1, '  Parameter A = %g\n', a );
  fprintf ( 1, '  Parameter D = %g\n', d );

  n = 1000;
  x = 2.0 * rand ( n, 1 ) - 1.0;
  y = 2.0 * rand ( n, 1 ) - 1.0;
  z = 2.0 * rand ( n, 1 ) - 1.0;
  t = 0.0;

  [ ur, vr, wr, pr ] = resid_ethier ( a, d, n, x, y, z, t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '           Minimum       Maximum\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ur:  %14.6g  %14.6g\n', min ( abs ( ur ) ), max ( abs ( ur ) ) );
  fprintf ( 1, '  Vr:  %14.6g  %14.6g\n', min ( abs ( vr ) ), max ( abs ( vr ) ) );
  fprintf ( 1, '  Wr:  %14.6g  %14.6g\n', min ( abs ( wr ) ), max ( abs ( wr ) ) );
  fprintf ( 1, '  Pr:  %14.6g  %14.6g\n', min ( abs ( pr ) ), max ( abs ( pr ) ) );

  return
end


function [h0, h1] = dfilters(fname, type)
% DFILTERS	Generate directional 2D filters
%
%	[h0, h1] = dfilters(fname, type)
%
% Input:
%	fname:	Filter name.  Available 'fname' are:
%		'haar':		the "Haar" filters
%		'vk':		McClellan transformed of the filter from
%				    the VK book
%		'ko':		orthogonal filter in the Kovacevic's paper
%		'kos':		smooth 'ko' filter
%		'lax':		17 x 17 by Lu, Antoniou and Xu
%		'sk':		9 x 9 by Shah and Kalker
%		'cd':		7 and 9 McClellan transformed by
%				                       Cohen and Daubechies
%		'pkva':		ladder filters by Phong et al.
%		'oqf_362':	regular 3 x 6 filter
%       'dvmlp'  :  regular linear phase biorthogonal filter with 3 dvm
%		'sinc':		ideal filter (*NO perfect recontruction*)
%       'dmaxflat': diamond maxflat filters obtained from a three stage ladder
%
%	     type:	'd' or 'r' for decomposition or reconstruction filters
%
% Output:
%	h0, h1:	diamond filter pair (lowpass and highpass)

% To test those filters (for the PR condition for the FIR case), verify that:
% conv2(h0, modulate2(h1, 'b')) + conv2(modulate2(h0, 'b'), h1) = 2
% (replace + with - for even size filters)
%
% To test for orthogonal filter
% conv2(h, reverse2(h)) + modulate2(conv2(h, reverse2(h)), 'b') = 2

% The diamond-shaped filter pair
switch fname
    case {'haar'}
	if lower(type(1)) == 'd'
	    h0 = [1, 1] / sqrt(2);
	    h1 = [-1, 1] / sqrt(2);
	else
	    h0 = [1, 1] / sqrt(2);
	    h1 = [1, -1] / sqrt(2);
	end

    case 'vk'	% in Vetterli and Kovacevic book
	if lower(type(1)) == 'd'
	    h0 = [1, 2, 1] / 4;
	    h1 = [-1, -2, 6, -2, -1] / 4;
	else
	    h0 = [-1, 2, 6, 2, -1] / 4;
	    h1 = [-1, 2, -1] / 4;
	end

	% McClellan transfrom to obtain 2D diamond filters
	t = [0, 1, 0; 1, 0, 1; 0, 1, 0] / 4;	% diamond kernel
	h0 = ftrans2(h0, t);
	h1 = ftrans2(h1, t);

    case 'ko'	% orthogonal filters in Kovacevic's thesis
	a0 = 2; a1 = 0.5; a2 = 1;

	h0 = [0,      -a1, -a0*a1, 0;
	      -a2, -a0*a2, -a0,    1;
	      0, a0*a1*a2, -a1*a2, 0];

	% h1 = qmf2(h0);
	h1 = [0, -a1*a2, -a0*a1*a2, 0;
	      1,     a0, -a0*a2, a2;
	      0, -a0*a1,  a1, 0];

	% Normalize filter sum and norm;
	norm = sqrt(2) / sum(h0(:));

	h0 = h0 * norm;
	h1 = h1 * norm;

	if type == 'r'
	    % Reverse filters for reconstruction
	    h0 = h0(end:-1:1, end:-1:1);
	    h1 = h1(end:-1:1, end:-1:1);
	end

    case 'kos'	% Smooth orthogonal filters in Kovacevic's thesis
	a0 = -sqrt(3); a1 = -sqrt(3); a2 = 2+sqrt(3);

	h0 = [0,      -a1, -a0*a1, 0;
	      -a2, -a0*a2, -a0,    1;
	      0, a0*a1*a2, -a1*a2, 0];

	% h1 = qmf2(h0);
	h1 = [0, -a1*a2, -a0*a1*a2, 0;
	      1,     a0, -a0*a2, a2;
	      0, -a0*a1,  a1, 0];

	% Normalize filter sum and norm;
	norm = sqrt(2) / sum(h0(:));

	h0 = h0 * norm;
	h1 = h1 * norm;

	if type == 'r'
	    % Reverse filters for reconstruction
	    h0 = h0(end:-1:1, end:-1:1);
	    h1 = h1(end:-1:1, end:-1:1);
	end

    case 'lax'	% by Lu, Antoniou and Xu
	h = [-1.2972901e-5,  1.2316237e-4, -7.5212207e-5,  6.3686104e-5, ...
	      9.4800610e-5, -7.5862919e-5,  2.9586164e-4, -1.8430337e-4; ...

	      1.2355540e-4, -1.2780882e-4, -1.9663685e-5, -4.5956538e-5, ...
	     -6.5195193e-4, -2.4722942e-4, -2.1538331e-5, -7.0882131e-4; ...

	     -7.5319075e-5, -1.9350810e-5, -7.1947086e-4,  1.2295412e-3, ...
	      5.7411214e-4,  4.4705422e-4,  1.9623554e-3,  3.3596717e-4; ...

	      6.3400249e-5, -2.4947178e-4,  4.4905711e-4, -4.1053629e-3, ...
	     -2.8588307e-3,  4.3782726e-3, -3.1690509e-3, -3.4371484e-3; ...

	      9.6404973e-5, -4.6116254e-5,  1.2371871e-3, -1.1675575e-2, ...
	      1.6173911e-2, -4.1197559e-3,  4.4911165e-3,  1.1635130e-2; ...

	     -7.6955555e-5, -6.5618379e-4,  5.7752252e-4,  1.6211426e-2, ...
	      2.1310378e-2, -2.8712621e-3, -4.8422645e-2, -5.9246338e-3; ...

	      2.9802986e-4, -2.1365364e-5,  1.9701350e-3,  4.5047673e-3, ...
	     -4.8489158e-2, -3.1809526e-3, -2.9406153e-2,  1.8993868e-1; ...

	     -1.8556637e-4, -7.1279432e-4,  3.3839195e-4,  1.1662001e-2, ...
	     -5.9398223e-3, -3.4467920e-3,  1.9006499e-1,  5.7235228e-1];

	h0 = sqrt(2) * [h, h(:, end-1:-1:1); ...
			h(end-1:-1:1, :), h(end-1:-1:1, end-1:-1:1)];

	h1 = modulate2(h0, 'b');

    case 'sk'	% by Shah and Kalker
	h = [ 0.621729,    0.161889,  -0.0126949, -0.00542504, 0.00124838; ...
	      0.161889,   -0.0353769, -0.0162751, -0.00499353, 0; ...
	     -0.0126949,  -0.0162751,  0.00749029, 0, 0; ...
	     -0.00542504,  0.00499353, 0, 0, 0; ...
	      0.00124838, 0, 0, 0, 0];

	h0 = sqrt(2) * [h(end:-1:2, end:-1:2), h(end:-1:2, :); ...
			h(:, end:-1:2), h];

	h1 = modulate2(h0, 'b');

    case 'dvmlp'
    q= sqrt(2); b=.02; b1 = b*b;
    h  = [b/q 0 -2*q*b 0 3*q*b 0 -2*q*b 0 b/q;
          0 -1/(16*q) 0 9/(16*q) 1/q  9/(16*q) 0 -1/(16*q) 0;
          b/q 0 -2*q*b 0 3*q*b 0 -2*q*b 0 b/q];
    g0 = [-b1/q   0   4*b1*q   0    -14*q*b1    0   28*q*b1    0       -35*q*b1  0    28*q*b1    0     -14*q*b1   0    4*b1*q  0     -b1/q;
           0     b/(8*q)   0  -13*b/(8*q) b/q  33*b/(8*q) -2*q*b -21*b/(8*q) 3*q*b -21*b/(8*q)   -2*q*b 33*b/(8*q)  b/q -13*b/(8*q) 0  b/(8*q) 0;
          -q*b1  0  -1/(256*q) + 8*q*b1  0   9/(128*q) - 28*q*b1   -1/(q*16)   -63/(256*q) + 56*q*b1   9/(16*q)   87/(64*q)-70*q*b1      9/(16*q)   -63/(256*q) + 56*q*b1    -1/(q*16)   9/(128*q) - 28*q*b1   0   -1/(256*q) + 8*q*b1   0   -q*b1;
          0     b/(8*q)   0  -13*b/(8*q) b/q  33*b/(8*q) -2*q*b -21*b/(8*q) 3*q*b -21*b/(8*q)   -2*q*b 33*b/(8*q)  b/q -13*b/(8*q) 0  b/(8*q) 0;
          -b1/q   0   4*b1*q   0    -14*q*b1    0   28*q*b1    0       -35*q*b1  0    28*q*b1    0     -14*q*b1   0    4*b1*q  0     -b1/q];
    h1 = modulate2(g0,'b' );
    h0 = h;
    if lower(type(1)) == 'r'
        h1 = modulate2(h ,'b');
        h0 = g0;
    end

    case {'cd', '7-9'}	% by Cohen and Daubechies
	% 1D prototype filters: the '7-9' pair
	h0 = [0.026748757411, -0.016864118443, -0.078223266529, ...
	      0.266864118443, 0.602949018236, 0.266864118443, ...
	      -0.078223266529, -0.016864118443, 0.026748757411];
	g0 = [-0.045635881557, -0.028771763114, 0.295635881557, ...
	      0.557543526229, 0.295635881557, -0.028771763114, ...
	      -0.045635881557];

	if lower(type(1)) == 'd'
	    h1 = modulate2(g0, 'c');
	else
	    h1 = modulate2(h0, 'c');
	    h0 = g0;
	end

	% Use McClellan to obtain 2D filters
	t = [0, 1, 0; 1, 0, 1; 0, 1, 0] / 4;	% diamond kernel
	h0 = sqrt(2) * ftrans2(h0, t);
	h1 = sqrt(2) * ftrans2(h1, t);

    case {'pkva', 'ldtest'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilter(fname);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

 case {'pkva-half4'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilterhalf( 4);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

    case {'pkva-half6'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilterhalf( 6);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

    case {'pkva-half8'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilterhalf( 8);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

    case 'sinc'	% The "sinc" case, NO Perfect Reconstruction
	% Ideal low and high pass filters
	flength = 30;

	h0 = fir1(flength, 0.5);
	h1 = modulate2(h0, 'c');

	% Use McClellan to obtain 2D filters
	t = [0, 1, 0; 1, 0, 1; 0, 1, 0] / 4;	% diamond kernel
	h0 = sqrt(2) * ftrans2(h0, t);
	h1 = sqrt(2) * ftrans2(h1, t);

    case {'oqf_362'}	% Some "home-made" filters!
        h0 = sqrt(2) / 64 * ...
             [	sqrt(15),	-3,	0; ...
                 0,		5,	-sqrt(15); ...
                 -2*sqrt(15),	30,	0; ...
                 0,		30,	2*sqrt(15); ...
                 sqrt(15),	5, 	0; ...
                 0, 		-3,	-sqrt(15)]';

         h1 = -reverse2(modulate2(h0, 'b'));

	if type == 'r'
	    % Reverse filters for reconstruction
	    h0 = h0(end:-1:1, end:-1:1);
	    h1 = h1(end:-1:1, end:-1:1);
	end

    case {'test'}	% Only for the shape, not for PR
	h0 = [0, 1, 0; 1, 4, 1; 0, 1, 0];
	h1 = [0, -1, 0; -1, 4, -1; 0, -1, 0];

    case {'testDVM'}	% Only for directional vanishing moment
	h0 = [1, 1; 1, 1] / sqrt(2);
	h1 = [-1, 1; 1, -1] / sqrt(2);

    case 'qmf'	% by Lu, Antoniou and Xu
	% ideal response
    % window
    m=2;
    n=2;
    w=[];
    w1d=kaiser(4*m+1,2.6);
    for n1=-m:m
        for n2 = -n:n
            w(n1+m+1,n2+n+1) = w1d(2*m+n1+n2+1)*w1d(2*m+n1-n2+1) ;
        end
    end
    h=[];
    for n1=-m:m
        for n2 = -n:n
            h(n1+m+1,n2+n+1) =  .5*sinc((n1+n2)/2)*.5*sinc((n1-n2)/2);
        end
    end
    c=sum(sum(h));
    h = sqrt(2)*h/c;
    h0=h.*w;
    h1 = modulate2(h0,'b');

    %h0 = modulate2(h,'r');
    %h1 = modulate2(h,'b');

     case 'qmf2'	% by Lu, Antoniou and Xu
	% ideal response
    % window

    h=[-.001104 .002494 -0.001744 0.004895 -0.000048 -.000311;
    0.008918 -0.002844 -0.025197 -0.017135 0.003905 -0.000081;
    -0.007587 -0.065904 00.100431 -0.055878 0.007023 0.001504;
    0.001725 0.184162 0.632115 0.099414 -0.027006 -0.001110;
    -0.017935 -0.000491 0.191397 -0.001787 -0.010587 0.002060;
    .001353 0.005635 -0.001231 -0.009052 -0.002668 0.000596];
    h0 = h./sum(sum(h));
    h1 = modulate2(h0,'b');

    %h0 = modulate2(h,'r');
    %h1 = modulate2(h,'b');

   case 'dmaxflat4'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(4,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end
   case 'dmaxflat5'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(5,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end

   case 'dmaxflat6'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(6,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end

   case 'dmaxflat7'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(7,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end

    otherwise
	% Assume the "degenerated" case: 1D wavelet filters
	[h0, h1] = wfilters(fname, type);
end


function PlotAdaptiveContour2D(u, levels, tol)

% function PlotAdaptiveContour2D(u, levels, tol)
% Purpose: adaptively refine the mesh to approximately locate isocontours

Globals2D;

% build interpolation matrix (coarse->fine)
ri(:,1) = 0.5*(-(r+s)*( 0) + (1+r)*( 0) + (1+s)*(-1) );
si(:,1) = 0.5*(-(r+s)*(-1) + (1+r)*( 0) + (1+s)*( 0) );

ri(:,2) = 0.5*(-(r+s)*(-1) + (1+r)*( 0) + (1+s)*(-1) );
si(:,2) = 0.5*(-(r+s)*(-1) + (1+r)*(-1) + (1+s)*( 0) );

ri(:,3) = 0.5*(-(r+s)*( 1) + (1+r)*( 0) + (1+s)*( 0) );
si(:,3) = 0.5*(-(r+s)*(-1) + (1+r)*( 0) + (1+s)*(-1) );

ri(:,4) = 0.5*(-(r+s)*(-1) + (1+r)*(-1) + (1+s)*( 0) );
si(:,4) = 0.5*(-(r+s)*( 1) + (1+r)*( 0) + (1+s)*( 0) );

%interp = Vandermonde2D(N, ri(:), si(:))*invV;
interp = InterpMatrix2D(ri(:),si(:));

Nlevels = length(levels);

xref = x; yref = y; uref = u; Kref = K;

sk = 1;
F = spalloc(Np,Np,1);
for i=0:N % old ordering
  for j=0:N - i
    if(i+j<=1), F(sk,sk) = 1.; end;
    sk = sk+1;
  end
end

hold on
for nlev=1:Nlevels
  lev = levels(nlev);

  xref = x; yref = y; uref = u; Kref = K; Jref = J;

  err = 1;
  while(err > tol)

    umin = min(uref, [], 1);
    umax = max(uref, [], 1);

    refineflag = (umin <= lev & umax >= lev);

    toref = find( refineflag);
    Nref = length(toref);

    uref = reshape(interp*uref(:,toref), Np, 4*Nref);
    xref = reshape(interp*xref(:,toref), Np, 4*Nref);
    yref = reshape(interp*yref(:,toref), Np, 4*Nref);

    Kref = 4*Nref;

    ufilt = V*(F*(invV*uref));
    err = max(max(abs(ufilt-uref)));

  end

  ri = [-1;1;-1]; si = [-1;-1;1]; refNp = length(ri);
  interp1 = InterpMatrix2D(ri,si);
  xref = interp1*xref; yref = interp1*yref; uref = interp1*uref;

  ltri = delaunay(ri,si);
  Nltri = size(ltri,1);
  tri = zeros(Kref*Nltri, Nfaces);
  ks = (1:Nltri)';
  for k=1:Kref
    tri(ks,:) = ltri + (k-1)*refNp;
    ks = ks + Nltri;
  end

  PlotContour2D(tri, xref(:), yref(:), uref(:), levels)

end
hold off
return