function [Out] = icaDim(Origdata,DEMDT,VN,Iterate,NDist)
% Matthew F. Glasser, Chad Donahue, Steve Smith, Christian Beckmann

%%%%%%%%%%%%%%%%%%%%
%Variables
Out.VNDIM=VN; %Variance Normalization Dimensionality initially set to 1
lnb = 0.5; %Lower noise bound for Golden Section Search
stabThresh = Iterate; %How many iterations meeting dimThresh criterion (1 for VNDIM=1 results, 2 for converged results)

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

%Remove Constant Timeseries and Detrend Data
if DEMDT==1
    data = Origdata(std(Origdata,[],2)>0,:);
    data_detrend = detrend(data')';
    data_trend = data - data_detrend;
elseif DEMDT==0
    data = Origdata(std(Origdata,[],2)>0,:);
    data_detrend = demean(data')';
    data_trend = single(zeros(size(data,1),size(data,2)));
elseif DEMDT==-1
    data = Origdata(std(Origdata,[],2)>0,:);
    data_detrend = data;
    data_trend = single(zeros(size(data,1),size(data,2)));
end

%Precompute PCA reconstruction
%octave doesn't have "rng", but it does accept "rand('twister', 0);" for matlab compatibility
rand('twister', 0);
[u,EigS,v]=nets_svds(data_detrend',0); %Eigenvalues of detrended data
Out.DOF=sum(diag(EigS)>(std(diag(EigS))*0.1)); %Compute degrees of freedom

c=1;
stabCount = 0;
while stabCount < stabThresh %Loop until dim output is stable

    c
    rand('twister', 0);
    %Variance normalization via PCA reconstruction: Isolate unstructured noise
    if VN~=0
      noise_unst = (u(:,Out.VNDIM(c):Out.DOF)*EigS(Out.VNDIM(c):Out.DOF,Out.VNDIM(c):Out.DOF)*v(:,Out.VNDIM(c):Out.DOF)')';
      Out.noise_unst_std = max(std(noise_unst,[],2),0.001);
      data_detrend_vn = data_detrend ./ repmat(Out.noise_unst_std,1,size(data_detrend,2));
    elseif VN==0
      data_detrend_vn = data_detrend;
      Out.noise_unst_std = single(ones(size(data_detrend,1),1));
    end
    if size(data_detrend_vn,1)<size(data_detrend_vn,2)
        if DEMDT~=-1
            d_pcaNorm=eig(cov(data_detrend_vn'));
        else
            d_pcaNorm=eig(data_detrend_vn*data_detrend_vn');
        end
    else
        if DEMDT~=-1
            d_pcaNorm=eig(cov(data_detrend_vn));
        else
            d_pcaNorm=eig(data_detrend_vn'*data_detrend_vn);
        end
    end
    Out.lambda_pcaNorm=flipud(d_pcaNorm);

    %Enable fitting multiple noise distributions
    DOF=Out.DOF;
    %DOF=sum(Out.lambda_pcaNorm>std(Out.lambda_pcaNorm)*0.1); %Recompute DOF because of demeaning in cov
    MaxX=length(data_detrend_vn);
    if DEMDT==-1
        MaxX=1000000;
    end
    lambda=Out.lambda_pcaNorm;
    S=0;
    clear EN
    for i=1:NDist
        [x(i),en] = FitWishart(lnb,S,DOF,MaxX,lambda(1:DOF));
        lambda=lambda(1:DOF)-en(1:DOF);
        en=[en;single(zeros(Out.DOF-length(en)+1,1))];
        EN(:,i)=en(1:Out.DOF);
        MaxX=x(i);
        DOF=min(find(lambda <= 0));
        lambda=[lambda(1:DOF);single(zeros(Out.DOF-DOF+1,1))];
        lambda=lambda(1:Out.DOF);
    end

    Out.EN=sum(EN,2);
    %Out.x(c)=x(1);
    Out.x(c)=round(x(end));
    %DOF=round(Out.DOF);

    %MaxS=5;
    %DOF=Out.DOF;
    %MaxX=round(Out.x(c));
    %MaxX=length(data_detrend_vn);
    %lambda=Out.lambda_pcaNorm;
    %[Out.x(c),Out.EN,Out.s(c)] = SmoothEst(lnb,MaxS,DOF,MaxX,lambda);

    %[Out.x(c),Out.EN] = FitWishart(lnb,Out.s(c),DOF,MaxX,lambda(1:DOF));

    %Divide eigenvalues by null for adjusted output values
    Out.lambdaAdj = abs(Out.lambda_pcaNorm(1:DOF)./(Out.EN(1:DOF)));
    %Out.lambdaAdj(DOF:end)=1;

    %Normalize adjusted eigenvalues to 1 for input to laplacian
    %Out.lambdaAdj_norm = Out.lambdaAdj ./ max(Out.lambdaAdj);
    Out.pcaDim_lambdaAdj = pca_dim(Out.lambdaAdj',Out.x(c));

    %Maximum of laplacian is estimate of optimal dimensionality
    [~, Out.calcDim]=max(Out.pcaDim_lambdaAdj.lap(1:DOF));

    if VN~=1
      stabCount = 2;
    end

    %Next loop will use calculated dimensionality
    c=c+1;

    Out.VNDIM(c) = Out.calcDim;
    %manual display of Out fields, to make octave less verbose
    disp('Out =');
    disp(['   VNDIM: ' mat2str(Out.VNDIM)]);
    disp(['   DOF: ' mat2str(Out.DOF)]);
    disp(['   noise_unst_std: array of size ' mat2str(size(Out.noise_unst_std))]);
    disp(['   lambda_pcaNorm: array of size ' mat2str(size(Out.lambda_pcaNorm))]);
    disp(['   EN: array of size ' mat2str(size(Out.EN))]);
    disp(['   x: ' mat2str(Out.x)]);
    disp(['   lambdaAdj: array of size ' mat2str(size(Out.lambdaAdj))]);
    disp(['   pcaDim_lambdaAdj: struct']);
    disp(['   calcDim: ' mat2str(Out.calcDim)]);

    %Store dims in array, check number of occurances to prevent dim loops
    stabCount = sum(Out.VNDIM==Out.calcDim);

end %End while loop for dim calcs
rand('twister', 0);
if DEMDT~=-1
    [u,EigS,v]=nets_svds(demean(data_detrend_vn)',0);
else
    [u,EigS,v]=nets_svds(data_detrend_vn',0);
end
u(isnan(u))=0; v(isnan(v))=0;
Out.EigSAdj=single(zeros(length(EigS),1));
Out.grot_one=diag(EigS(1:length(Out.EN),1:length(Out.EN)));
Out.grot_two=Out.grot_one.^2;
Out.grot_three=(Out.grot_two./max(Out.grot_two)).*max(Out.lambda_pcaNorm);
Out.grot_four=Out.grot_three-(Out.EN/median((Out.lambda_pcaNorm(1:Out.DOF)./max(Out.lambda_pcaNorm(1:Out.DOF)))./(diag(EigS(1:Out.DOF,1:Out.DOF).^2)./max(diag(EigS(1:Out.DOF,1:Out.DOF).^2)))));
Out.grot_five=(Out.grot_four./max(Out.lambda_pcaNorm)).*max(diag(EigS.^2));
firstOne = min(find(Out.grot_five <= 0));
Out.grot_six=Out.grot_five;
if ~length(firstOne)==0
    Out.grot_six(firstOne:end) = 0;
    Out.NewDOF=firstOne-1;
else
    Out.NewDOF=length(Out.grot_six);
end
%manual display of Out fields, to make octave less verbose
disp('Out =');
disp(['   VNDIM: ' mat2str(Out.VNDIM)]);
disp(['   DOF: ' mat2str(Out.DOF)]);
disp(['   noise_unst_std: array of size ' mat2str(size(Out.noise_unst_std))]);
disp(['   lambda_pcaNorm: array of size ' mat2str(size(Out.lambda_pcaNorm))]);
disp(['   EN: array of size ' mat2str(size(Out.EN))]);
disp(['   x: ' mat2str(Out.x)]);
disp(['   lambdaAdj: array of size ' mat2str(size(Out.lambdaAdj))]);
disp(['   pcaDim_lambdaAdj: struct']);
disp(['   calcDim: ' mat2str(Out.calcDim)]);
disp(['   EigSAdj: array of size ' mat2str(size(Out.EigSAdj))]);
disp(['   grot_one: array of size ' mat2str(size(Out.grot_one))]);
disp(['   grot_two: array of size ' mat2str(size(Out.grot_two))]);
disp(['   grot_three: array of size ' mat2str(size(Out.grot_three))]);
disp(['   grot_four: array of size ' mat2str(size(Out.grot_four))]);
disp(['   grot_five: array of size ' mat2str(size(Out.grot_five))]);
disp(['   grot_six: array of size ' mat2str(size(Out.grot_six))]);
disp(['   NewDOF: ' mat2str(Out.NewDOF)]);

Out.EigSAdj(1:length(Out.EN))=sqrt(Out.grot_six);

Out.data=single(zeros(size(Origdata,1),size(Origdata,2)));
if DEMDT~=-1
    Out.data(std(Origdata,[],2)>0,:)=data_trend + (((u*diag(Out.EigSAdj)*v')'+repmat(mean(data_detrend_vn),size(data_detrend_vn,1),1)) .* repmat(Out.noise_unst_std,1,size(data_detrend_vn,2)));
else
    Out.data(std(Origdata,[],2)>0,:)=data_trend + (((u*diag(Out.EigSAdj)*v')') .* repmat(Out.noise_unst_std,1,size(data_detrend_vn,2)));
end
temp=single(zeros(size(Origdata,1),1)); temp(std(Origdata,[],2)>0,:)=Out.noise_unst_std; Out.noise_unst_std=max(temp,0.001); clear temp;

end %End function

function [out] = lpdist(in)
    out=pdist(log(in));
end

function [x,EN] = FitWishart(lnb,S,DOF,MaxX,lambda) %FitWishart(lnb,step,DOF,MaxX,lambda)
    rand('twister', 0);
    EigDn1=round(DOF*lnb); %Isolate search to noise
    EigDn2=round(DOF-1); %Reqd for post MR+FIX deconcatinated tcs
    %EigDn2=round(DOF*0.75); %Reqd for post MR+FIX deconcatinated tcs

    a = DOF; %Lower bound for search range
    b = MaxX; %Upper bound for search range
    epsilon = 1; %Accuracy/stopping criterion
    iter = 500; %# iterations/secondary stopping criterion
    tau = double((sqrt(5)-1)/2); %Golden ratio (constant), 0.618...
    k = 1; %Iteration count

    %Initial section ranges to instantiate optimization
    x1 = a+(1-tau)*(b-a);
    x2 = a+tau*(b-a);
    %x1=a;
    %x2=b;

    %Calculate initial null spectra
    EN_x1 = iFeta([0:0.001:5],DOF,x1)'; %Call feta to calc null spectrum
    %EN_x1=flipud(eig(cov(Smooth(randn(round(x1),DOF),S))));
    EN_x1=EN_x1*median(lambda(EigDn1:EigDn2)./EN_x1(EigDn1:EigDn2)); %Remove offset between null & data
    f_x1 = lpdist([EN_x1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']); %Compute pairwise distance b/w null & data

    EN_x2 = iFeta([0:0.001:5],DOF,x2)';
    %EN_x2=flipud(eig(cov(Smooth(randn(round(x2),DOF),S))));
    EN_x2=EN_x2*median(lambda(EigDn1:EigDn2)./EN_x2(EigDn1:EigDn2));
    f_x2 = lpdist([EN_x2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

    while (abs(b-a)>epsilon) && (k<iter) %Loop until low error OR max iter met
        %k=k+1;
        rand('twister', 0);
        disp([num2str(a) ' ' num2str(x1) ' ' num2str(x2) ' ' num2str(b)]);
        %Check both terms for minimal pairwise distance and continue toward minimum
        if (f_x1<f_x2)
            b=x2;
            x2=x1;
            f_x2=f_x1;%don't recompute, this is the entire point of golden search
            EN_x2=EN_x1;
            x1=a+(1-tau)*(b-a);

            EN_x1 = iFeta([0:0.001:5],DOF,x1)';
            %EN_x1=flipud(eig(cov(Smooth(randn(round(x1),DOF),S))));
            EN_x1=EN_x1*median(lambda(EigDn1:EigDn2)./EN_x1(EigDn1:EigDn2));
            f_x1 = lpdist([EN_x1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

        else
            a=x1;
            x1=x2;
            f_x1=f_x2;
            EN_x1=EN_x2;
            x2=a+tau*(b-a);

            EN_x2 = iFeta([0:0.001:5],DOF,x2)';
            %EN_x2=flipud(eig(cov(Smooth(randn(round(x2),DOF),S))));
            EN_x2=EN_x2*median(lambda(EigDn1:EigDn2)./EN_x2(EigDn1:EigDn2));
            f_x2 = lpdist([EN_x2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);
        end
        k = k+1;
    end

    %Compute null spectrum w/ optimal pairwise distance for output
    if (f_x1<f_x2)
        x = x1;
    else
        x = x2;
    end
    rand('twister', 0);
    %disp(randn(1,1));
    EN=flipud(eig(cov(Smooth(randn(round(x),DOF),S))));

    x=x/1+S;

    %Remove offset from null spectrum
    EN = EN*median(lambda(EigDn1:EigDn2) ./ EN(EigDn1:EigDn2));
end

function [x,EN,s] = SmoothEst(lnb,MaxS,DOF,MaxX,lambda)

    EigDn1=round(DOF*lnb); %Isolate search to noise
    EigDn2=round(DOF-1);

    M=randn(MaxX,DOF);

    a = 0; %Lower bound for search range
    b = MaxS; %Upper bound for search range
    epsilon = 0.01; %Accuracy/stopping criterion
    iter = 500; %# iterations/secondary stopping criterion
    tau = double((sqrt(5)-1)/2); %Golden ratio (constant), 0.618...
    k = 1; %Iteration count

    %Initial section ranges to instantiate optimization
    s1 = a+(1-tau)*(b-a);
    s2 = a+tau*(b-a);
    %x1=a;
    %x2=b;

    %Calculate initial null spectra
    %EN_x1 = iFeta([0:step:5],DOF,x1)'; %Call feta to calc null spectrum
    EN_s1=flipud(eig(cov(Smooth(M,s1/(2*sqrt(2*log(2)))))));
    EN_s1=EN_s1*median(lambda(EigDn1:EigDn2)./EN_s1(EigDn1:EigDn2)); %Remove offset between null & data
    f_s1 = lpdist([EN_s1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']); %Compute pairwise distance b/w null & data

    %EN_x2 = iFeta([0:step:5],DOF,x2)';
    EN_s2=flipud(eig(cov(Smooth(M,s2/(2*sqrt(2*log(2)))))));
    EN_s2=EN_s2*median(lambda(EigDn1:EigDn2)./EN_s2(EigDn1:EigDn2));
    f_s2 = lpdist([EN_s2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

    while (abs(b-a)>epsilon) && (k<iter) %Loop until low error OR max iter met
        %k=k+1;
        disp([num2str(a) ' ' num2str(s1) ' ' num2str(s2) ' ' num2str(b)]);
        %Check both terms for minimal pairwise distance and continue toward minimum
        if (f_s1<f_s2)
            b=s2;
            s2=s1;
            f_s2=f_s1;%don't recompute, this is the entire point of golden search
            EN_s2=EN_s1;
            s1=a+(1-tau)*(b-a);

            %EN_x1 = iFeta([0:step:5],DOF,x1)';
            EN_s1=flipud(eig(cov(Smooth(M,s1/(2*sqrt(2*log(2)))))));
            EN_s1=EN_s1*median(lambda(EigDn1:EigDn2)./EN_s1(EigDn1:EigDn2));
            f_s1 = lpdist([EN_s1(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);

        else
            a=s1;
            s1=s2;
            f_s1=f_s2;
            EN_s1=EN_s2;
            s2=a+tau*(b-a);

            %EN_x2 = iFeta([0:step:5],DOF,x2)';
            EN_s2=flipud(eig(cov(Smooth(M,s2))));
            EN_s2=EN_s2*median(lambda(EigDn1:EigDn2)./EN_s2(EigDn1:EigDn2));
            f_s2 = lpdist([EN_s2(EigDn1:EigDn2)'; lambda(EigDn1:EigDn2)']);
        end
        k = k+1;
    end

    %Compute null spectrum w/ optimal pairwise distance for output
    if (f_s1<f_s2)
        s = s1;
    else
        s = s2;
    end

    %EN = iFeta([0:step:5],DOF,x)';
    %EN=flipud(eig(cov(randn(round(x),DOF))));
    EN=flipud(eig(cov(Smooth(M,s/(2*sqrt(2*log(2)))))));
    x=MaxX/(1+s); %Number of resels
    %Remove offset from null spectrum
    EN = EN*median(lambda(EigDn1:EigDn2) ./ EN(EigDn1:EigDn2));
end

function [O] = Smooth(M,S)
if S==0
    O=M;
else
sigma = S/(2*sqrt(2*log(2)));
%sz = S*6;    % length of gaussFilter vector
%x = linspace(-sz / 2, sz / 2, sz);
%gaussFilter = exp(-x .^ 2 / (2 * sigma ^ 2));
%gaussFilter = gaussFilter / sum (gaussFilter); % normalize

width = round((6*sigma - 1)/2);
support = (-width:width);
gaussFilter = exp( -(support).^2 ./ (2*sigma^2) );
gaussFilter = gaussFilter/ sum(gaussFilter);

O=single(zeros(size(M,1),size(M,2)));

for i=1:size(M,2)
    op
    O(:,i) = conv (M(:,i), gaussFilter, 'same');
    %O(:,i) = filter (gaussFilter,1,M(:,i));
end
end
end


%CA driver
%HPP-gas

clear all
clf

nx=52; %must be divisible by 4
ny=100;

z=zeros(nx,ny);
o=ones(nx,ny);
sand = z ;
sandNew = z;
gnd = z ;
diag1 = z;
diag2 = z;
and12 = z;
or12 = z;
sums = z;
orsum = z;

gnd(1:nx,ny-3)=1 ; % right ground line
gnd(1:nx,3)=1 ; % left ground line
gnd(nx/4:nx/2-2,ny/2)=1; %the hole line
gnd(nx/2+2:nx,ny/2)=1; %the hole line
gnd(nx/4, 1:ny) = 1; %top line
gnd(3*nx/4, 1:ny) = 1 ;%bottom line

%fill the left side
r = rand(nx,ny);
sand(nx/4+1:3*nx/4-1, 4:ny/2-1) = r(nx/4+1:3*nx/4-1, 4:ny/2-1)<0.3;
%sand(nx/4+1:3*nx/4-1, ny*.75:ny-4) = r(nx/4+1:3*nx/4-1, ny*.75:ny-4)<0.75;
%sand(nx/2,ny/2) = 1;
%sand(nx/2+1,ny/2+1) = 1;

imh = image(cat(3,z,sand,gnd));
set(imh, 'erasemode', 'none')
axis equal
axis tight

for i=1:1000
    p=mod(i,2); %margolis neighborhood

    %upper left cell update
    xind = [1+p:2:nx-2+p];
    yind = [1+p:2:ny-2+p];

    %See if exactly one diagonal is ones
    %only (at most) one of the following can be true!
    diag1(xind,yind) = (sand(xind,yind)==1) & (sand(xind+1,yind+1)==1) & ...
        (sand(xind+1,yind)==0) & (sand(xind,yind+1)==0);

    diag2(xind,yind) = (sand(xind+1,yind)==1) & (sand(xind,yind+1)==1) & ...
        (sand(xind,yind)==0) & (sand(xind+1,yind+1)==0);

    %The diagonals both not occupied by two particles
    and12(xind,yind) = (diag1(xind,yind)==0) & (diag2(xind,yind)==0);

    %One diagonal is occupied by two particles
    or12(xind,yind)  = diag1(xind,yind) | diag2(xind,yind);

    %for every gas particle see if it near the boundary
    sums(xind,yind) = gnd(xind,yind) | gnd(xind+1,yind) | ...
                        gnd(xind,yind+1) | gnd(xind+1,yind+1) ;

    % cell layout:
    % x,y    x+1,y
    % x,y+1  x+1,y+1
    %If (no walls) and (diagonals are both not occupied)
    %then there is no collision, so move opposite cell to current cell
    %If (no walls) and (only one diagonal is occupied)
    %then there is a collision so move ccw cell to the current cell
    %If (a wall)
    %then don't change the cell (causes a reflection)
    sandNew(xind,yind) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind+1,yind+1)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind,yind+1)) + ...
        (sums(xind,yind) & sand(xind,yind));

    sandNew(xind+1,yind) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind,yind+1)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind,yind))+ ...
        (sums(xind,yind) & sand(xind+1,yind));

    sandNew(xind,yind+1) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind+1,yind)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind+1,yind+1))+ ...
        (sums(xind,yind) & sand(xind,yind+1));

     sandNew(xind+1,yind+1) = ...
        (and12(xind,yind)  & ~sums(xind,yind) & sand(xind,yind)) + ...
        (or12(xind,yind) & ~sums(xind,yind) & sand(xind+1,yind))+ ...
        (sums(xind,yind) & sand(xind+1,yind+1));

    sand = sandNew;

    set(imh, 'cdata', cat(3,z,sand,gnd) )
    drawnow
end


function [traj, infStates] = tapas_hgf_binary_pu_tbt(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf_binary_pu_tbt(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf_binary_pu_tbt(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016-2017 Rebecca Lawson, 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 paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_binary_pu_tbt_transp(r, p);
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = (length(p)+1)/5;

    if l ~= floor(l)
        error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
    end
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
rho  = p(2*l+1:3*l);
ka   = p(3*l+1:4*l-1);
om   = p(4*l:5*l-2);
th   = exp(p(5*l-1));
eta0 = p(5*l);
eta1 = p(5*l+1);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];
al= [0; r.u(:,2)];

% Number of trials (including prior)
n = length(u);

% Assume that if u has more than one column, the last contains t
try
    if r.c_prc.irregular_intervals
        if size(u,2) > 1
            t = [0; r.u(:,end)];
        else
            error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
        end
    else
        t = ones(n,1);
    end
catch
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        t = ones(n,1);
    end
end

% Initialize updated quantities

% Representations
mu = NaN(n,l);
pi = NaN(n,l);

% Other quantities
muhat = NaN(n,l);
pihat = NaN(n,l);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,1) = tapas_sgm(mu_0(1), 1);
pi(1,1) = Inf;
mu(1,2:end) = mu_0(2:end);
pi(1,2:end) = 1./sa_0(2:end);

% Pass through representation update loop
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 2nd level prediction
        muhat(k,2) = mu(k-1,2) +t(k) *rho(2);

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1) = tapas_sgm(ka(1) *muhat(k,2), 1);

        % Precision of prediction
        pihat(k,1) = 1/(muhat(k,1)*(1 -muhat(k,1)));

        % Mean update
        mu(k,1) = u(k);
        und1 = exp(-(u(k) -eta1)^2/(2*al(k)));
        und0 = exp(-(u(k) -eta0)^2/(2*al(k)));
        mu(k,1) = muhat(k,1) *und1 /(muhat(k,1) *und1 +(1 -muhat(k,1)) *und0);

        % Prediction error
        da(k,1) = mu(k,1) -muhat(k,1);

        % 2nd level
        % ~~~~~~~~~
        % Prediction: see above

        % Precision of prediction
        pihat(k,2) = 1/(1/pi(k-1,2) +exp(ka(2) *mu(k-1,3) +om(2)));

        % Updates
        pi(k,2) = pihat(k,2) +ka(1)^2/pihat(k,1);
        mu(k,2) = muhat(k,2) +ka(1)/pi(k,2) *da(k,1);

        % Implied posterior precision at first level
        sgmmu2 = tapas_sgm(ka(1) *mu(k,2), 1);
        pi(k,1) = pi(k,2)/(sgmmu2*(1-sgmmu2));

        % Volatility prediction error
        da(k,2) = (1/pi(k,2) +(mu(k,2) -muhat(k,2))^2) *pihat(k,2) -1;

        if l > 3
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 3:l-1
                % Prediction
                muhat(k,j) = mu(k-1,j) +t(k) *rho(j);

                % Precision of prediction
                pihat(k,j) = 1/(1/pi(k-1,j) +t(k) *exp(ka(j) *mu(k-1,j+1) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1);

                % Updates
                pi(k,j) = pihat(k,j) +1/2 *ka(j-1)^2 *w(k,j-1) *(w(k,j-1) +(2 *w(k,j-1) -1) *da(k,j-1));

                if pi(k,j) <= 0
                    error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
                end

                mu(k,j) = muhat(k,j) +1/2 *1/pi(k,j) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Volatility prediction error
                da(k,j) = (1/pi(k,j) +(mu(k,j) -muhat(k,j))^2) *pihat(k,j) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l) = mu(k-1,l) +t(k) *rho(l);

        % Precision of prediction
        pihat(k,l) = 1/(1/pi(k-1,l) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1);

        % Updates
        pi(k,l) = pihat(k,l) +1/2 *ka(l-1)^2 *w(k,l-1) *(w(k,l-1) +(2 *w(k,l-1) -1) *da(k,l-1));

        if pi(k,l) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu(k,l) = muhat(k,l) +1/2 *1/pi(k,l) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Volatility prediction error
        da(k,l) = (1/pi(k,l) +(mu(k,l) -muhat(k,l))^2) *pihat(k,l) -1;
    else

        mu(k,:) = mu(k-1,:);
        pi(k,:) = pi(k-1,:);

        muhat(k,:) = muhat(k-1,:);
        pihat(k,:) = pihat(k-1,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Implied learning rate at the first level
sgmmu2 = tapas_sgm(ka(1) *mu(:,2), 1);
lr1    = diff(sgmmu2)./da(2:n,1);
lr1(da(2:n,1)==0) = 0;

% Remove representation priors
mu(1,:)  = [];
pi(1,:)  = [];

% Check validity of trajectories
if any(isnan(mu(:))) || any(isnan(pi(:)))
    error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
else
    % Check for implausible jumps in trajectories
    dmu = diff(mu(:,2:end));
    dpi = diff(pi(:,2:end));
    rmdmu = repmat(sqrt(mean(dmu.^2)),length(dmu),1);
    rmdpi = repmat(sqrt(mean(dpi.^2)),length(dpi),1);

    jumpTol = 16;
    if any(abs(dmu(:)) > jumpTol*rmdmu(:)) || any(abs(dpi(:)) > jumpTol*rmdpi(:))
        error('tapas:hgf:VarApproxInvalid', 'Variational approximation invalid. Parameters are in a region where model assumptions are violated.');
    end
end

% Remove other dummy initial values
muhat(1,:) = [];
pihat(1,:) = [];
v(1,:)     = [];
w(1,:)     = [];
da(1,:)    = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,l);
psi(:,2)   = 1./pi(:,2);
psi(:,3:l) = pihat(:,2:l-1)./pi(:,3:l);
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = lr1;
wt(:,2)   = psi(:,2);
wt(:,3:l) = 1/2 *(v(:,2:l-1) *diag(ka(2:l-1))) .*psi(:,3:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,4);
infStates(:,:,1) = traj.muhat;
infStates(:,:,2) = traj.sahat;
infStates(:,:,3) = traj.mu;
infStates(:,:,4) = traj.sa;

return;


function printH(H)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n=size(H,1);
m=size(H,2);
fprintf('H     =');
fprintf('\n');

for i=1:n
    for j=1:m
        if H(i,j)==0,continue;end
        fprintf('%10.5f',H(i,j));
    end
    fprintf('\n');
end

return;


function p = basicHKL(cs,varargin)
% plot symmetry
%
% Input
%  cs - symmetry
%
% Output
%
% Options
%  antipodal      - include <VectorsAxes.html antipodal symmetry>

[h,k,l] = meshgrid(-1:1);

p = Miller(h(:),k(:),l(:),cs);

p(isinf(p.dspacing)) = [];

p = unique(p.symmetrise,'noSymmetry');


function [d,g,rr,ss]=v_sigalign(s,r,maxd,m,fs)
%V_SIGALIGN align a clean reference with a noisy signal [d,g,rr,ss]=(s,r,maxd,m,fs)
% Inputs:
%            m  mode
%                 u = unity gain
%                 g = find optimal gain [default]
%                 a = A-weight the signals
%                 b = weight signals by BS-468
%                 s = find delay to maximize the correlation coefficient between r and s [default]
%                 S = find delay to maximize the energy of the component of r in s
%                 p = plot result
%            s  test signal
%            r  reference signal
%         maxd  [+-max] or [min max] delay allowed in samples or fractions of length(r)
%               default is maximum that ensures at least 50% of r or s in the overlap
%           fs  sample frequency (only used for filtering and plotting)
%
% Outputs:
%            d = optimum delay to apply to r
%            g = optimal gain to apply to r
%           rr = g*r(* -d)  [zero padded to match s if ss output is not given]
%           ss = s truncated if necessary to match the length of rr

%      Copyright (C) Mike Brookes 2011
%      Version: $Id: v_sigalign.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Bugs/Suggestions
% 1. add option to calculate a DC offset
% 2. optionally find optimal fractional time shift
% 3. split long signals into chunks to reduce memory requirements

ns=length(s);
nr=length(r);
if numel(s)~=ns || numel(r)~=nr
    error('Inputs cannot be matrices');
end
s=s(:);
r=r(:);
if nargin<3
    maxd=[];
end
switch numel(maxd)
    case 0
        if nr<ns
            lmm=[-0.25*nr ns-0.75*nr];
        else
            lmm=[-0.25*ns nr-0.75*ns];
        end
    case 1
        lmm=[-maxd maxd];
    otherwise
        lmm=maxd(1:2);
end
lmm=round(lmm.*(1+(nr-1)*(abs(lmm)<1)));  % convert fractions of nr to samples
lmin=lmm(1);
lmax=lmm(2);
lags=lmax-lmin+1;
if lags<=0
    error('Invalid lag limits');
end
if nargin<4 || ~numel(m)
    m='gs';
end
if nargin<5 || ~numel(fs)
    fs=[];
else
    if any(m=='a')
        [b,a]=v_stdspectrum(2,'z',fs);
        s=filter(b,a,s);
        r=filter(b,a,r);
    elseif any(m=='b')
        [b,a]=v_stdspectrum(8,'z',fs);
        s=filter(b,a,s);
        r=filter(b,a,r);
    end
end

% now do cross correlation

rxi=max(1,1-lmin);   % first reference sample needed
rxj=min(nr,ns-lmax); % last reference sample needed
nrx=rxj-rxi+1;          % length of reference segment
if nrx<1
    error('Reference signal too short');
end
fl=2^nextpow2(lmax-lmin+nrx);
sxi=max(1,rxi+lmin); % first signal sample needed
sxj=min(ns,rxj+lmax); % last signal sample needed
rs=v_irfft(v_rfft([s(sxi:sxj); zeros(fl-sxj+sxi-1,1)]).*conj(v_rfft([r(rxi:rxj); zeros(fl-rxj+rxi-1,1)])));
rsu=rs(1:lags);
ssq=cumsum(s(sxi:sxj).^2);
ssqd=[ssq(nrx); ssq(nrx+1:lmax-lmin+nrx)-ssq(1:lmax-lmin)];
if any (m=='S') % maximize energy of common component
    [cmx,icx]=max(abs(rsu)); % maximize cross correlation
else
    [cmx,icx]=max(rsu.^2./ssqd); % maximize correlation coefficient
end
d=icx-1+lmin;
ia=max(1,d+1); % first sample of s in common region
ja=min(ns,d+nr); % last sample of s in common region
ija=ia:ja;
ijad=ija-d;
rr=r(ijad);
ss=s(ija);
if any (m=='u')
    g=1;
else
g=sum(rr.*ss)/sum(rr.^2);   % gain to apply to r
end
rr=rr*g;
if ~nargout || any(m=='p')
    xco=sum(rr.*ss)/sqrt(sum(rr.^2)*sum(ss.^2));
    snr=sum(rr.^2)/sum((rr-ss).^2);
    if numel(fs)==1
        tun='s';
    else
        tun='samples';
        fs=1;
    end
    subplot(311);
    plot(ija/fs,rr);
    pm='+-';
    title(sprintf('Ref delay = %.2g %s, %cGain = %.2g dB, Xcorr = %.2g, SNR = %.2g dB',d/fs,tun,pm(1+(g<0)),20*log10(g),xco,10*log10(snr)));
    ylabel('Reference');
    set(gca,'XLim',ija([1 end])/fs);
    axh(2)=gca;
    subplot(312);
    plot(ija/fs,ss);
    ylabel('Signal');
    set(gca,'XLim',ija([1 end])/fs);
    axh(1)=gca;
    subplot(313);
    plot(ija/fs,ss-rr);
    ylabel('Residual');
    xlabel(sprintf('Time (%s)',tun));
    set(gca,'XLim',ija([1 end])/fs);
    axh(3)=gca;
    linkaxes(axh(1:3),'x');
end
if nargout==3
    rr=[zeros(ia-1,1); rr; zeros(ns-ja,1)]; % force to be the size of s
end


function f = comp_ifilterbank(c,g,a,L)
%COMP_IFILTERBANK Compute inverse filterbank

%   called by comp_ifilterbank, performs frequency domain
%   filtering for filters with finite impulse responses

M = numel(g);
classname = assert_classname(c{1});

% Divide filters into time domain and frequency domain groups
mFreq = 1:M;
mTime = mFreq(cellfun(@(gEl) isfield(gEl,'h') ,g)>0);
mFreq(mTime) = [];

f = [];

if ~isempty(mTime)
   % Pick imp. resp.
   gtime = cellfun(@(gEl) gEl.h, g(mTime),'UniformOutput',0);

   % Call the routine
   gskip = cellfun(@(gEl) gEl.offset ,g(mTime));
   f = comp_ifilterbank_td(c(mTime),gtime,a(mTime),L,gskip,'per');
end

if ~isempty(mFreq)
   % Pick frequency domain filters
   gfreq = g(mFreq);
   % Divide filters into the full-length and band-limited groups
   mFreqFullL = 1:numel(gfreq);
   amFreqCell = mat2cell(a(mFreq,:).',size(a,2),ones(1,numel(mFreq)));
   mFreqBL = mFreqFullL(cellfun(@(gEl,aEl) numel(gEl.H)~=L || (numel(aEl)>1 && aEl(2) ~=1), gfreq(:),amFreqCell(:))>0);
   mFreqFullL(mFreqBL) = [];

   mFreqFullL = mFreq(mFreqFullL);
   mFreqBL = mFreq(mFreqBL);

   F = [];
   if ~isempty(mFreqBL)
      conjG = cellfun(@(gEl) cast(gEl.H,classname), g(mFreqBL),'UniformOutput',0);
      foff = cellfun(@(gEl) gEl.foff, g(mFreqBL));
      % Cast from logical to double.
      realonly = cellfun(@(gEl) cast(isfield(gEl,'realonly') && gEl.realonly,'double'), g(mFreqBL));
      F = comp_ifilterbank_fftbl(c(mFreqBL),conjG,foff,a(mFreqBL,:),realonly);
   end

   if ~isempty(mFreqFullL)
      conjG = cellfun(@(gEl) cast(gEl.H,classname), g(mFreqFullL),'UniformOutput',0);

      % In case some of the filters were BL
      if isempty(F)
         F = comp_ifilterbank_fft(c(mFreqFullL),conjG,a(mFreqFullL));
      else
         F = F + comp_ifilterbank_fft(c(mFreqFullL),conjG,a(mFreqFullL));
      end
   end

   % In case some of the filters were TD
   if isempty(f)
      f = ifft(F);
   else
      f = f + ifft(F);
   end
end

% W = size(c{1},2);
% M = numel(g);
% classname = assert_classname(c{1});
%
% f=zeros(L,W,classname);
%
% % This routine must handle the following cases
% %
% %   * Time-side or frequency-side filters (test for  isfield(g,'H'))
% %
% %   * Cell array or matrix input (test for iscell(c))
% %
% %   * Regular or fractional subsampling (test for info.isfractional)
%
%
% for m=1:M
%     conjG=conj(comp_transferfunction(g{m},L));
%
%     % For Octave 3.6 compatibility
%     conjG=cast(conjG,classname);
%
%     % Handle fractional subsampling (this implies frequency side filters)
%     if isfield(g{m},'H') && numel(g{m}.H)~=L
%         N=size(c{m},1);
%         Llarge=ceil(L/N)*N;
%         amod=Llarge/N;
%
%         for w=1:W
%             % This repmat cannot be replaced by bsxfun
%             innerstuff=middlepad(circshift(repmat(fft(c{m}(:,w)),amod,1),-g{m}.foff),L);
%             innerstuff(numel(g{m}.H)+1:end) = 0;
%             f(:,w)=f(:,w)+(circshift(innerstuff.*circshift(conjG,-g{m}.foff),g{m}.foff));
%         end;
%     else
%         if iscell(c)
%             for w=1:W
%                 % This repmat cannot be replaced by bsxfun
%                 f(:,w)=f(:,w)+(repmat(fft(c{m}(:,w)),a(m),1).*conjG);
%             end;
%         else
%             for w=1:W
%                 % This repmat cannot be replaced by bsxfun
%                 f(:,w)=f(:,w)+(repmat(fft(c(:,m,w)),a(m),1).*conjG);
%             end;
%         end;
%     end;
% end;
%
% f = ifft(f);


function [rpav]=realized_preaveraged_variance(price,time,timeType,samplingType,samplingInterval,options)
% Estimated quadratic variation using Preaveraged Realized Variance
%
% USAGE:
%   [RPAV] = realized_preaveraged_variance(PRICE)
%   [RPAV] = realized_preaveraged_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL)
%   [RPAV] = realized_preaveraged_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,OPTIONS)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - [OPTIONAL] m by 1 vector of times where TIME(i) corresponds to PRICE(i).
%   TIMETYPE         - [OPTIONAL] String describing the way times are measured
%                       'wall'    24-hour clock of the form HHMMSS.mmm, e.g. 101543 or 153217
%                       'seconds' Time measured in seconds past midnight
%                       'unit'  Unit normalized date format, e.g. .1, .234, .9
%                         Unit normalized times are more general than the other types and can be
%                         applied to data from more than one calendar day
%   SAMPLINGTYPE     - [OPTIONAL] String describing the type of sampling to use when
%                        filtering PRICE
%                        'CalendarTime' - Sample in calendar time using observations separated by
%                          SAMPLINGINTERVAL seconds
%                        'CalendarUniform' - Sample in calendar time using SAMPLINGINTERVAL
%                          observations spread uniformly between TIME(1) and TIME(m)
%                        'BusinessTime' - Sample in business (tick) time using observation separated
%                          by SAMPLINGINTERVAL ticks
%                        'BusinessUniform' - Sample in business (tick) time using observations
%                          uniformly spaced in business time.
%                        'Fixed' - Sample at specific points in time. When using fixed,
%                          SAMPLINGINTERVAL must be a n by 1 vector of times with the same TIMETYPE
%                          as TIME (i.e. seconds if TIME is in seconds)
%   SAMPLINGINTERVAL  - [OPTIONAL] Scalar integer or n by 1 vector whose meaning depends on the
%                         selected SAMPLINGTYPE
%   OPTIONS           - [OPTIONAL] Preaveraged Realized Variance option structure initialized by calling
%                         realized_options('Preaveraging'). See help realized_options for a description of
%                         available options.
%
% OUTPUTS:
%   RK          - Preaveraged realized variance estimate
%
% COMMENTS:
%  Follows Christensen, Oomen and Podolski (2014) mostl closely, with the
%  noise variance estimator usef in Hautsch and Podolski (2013)
%
% EXAMPLES:
%
%  See also REALIZED_OPTIONS, REALIZED_KERNEL, REALIZED_NOISE_ESTIMATE, REALIZED_VARIANCE,
%  REALIZED_VARIANCE_OPTIMAL_SAMPLING, REALIZED_RANGE, REALIZED_QUANTILE_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 2/27/2014

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 1
        m = length(price);
        time = linspace(9.5*3600,16*3600,m)';
        timeType = 'seconds';
        samplingType = 'businesstime';
        samplingInterval = 1;
        options = realized_options('preaveraging');
    case 5
        options = realized_options('preaveraging');
    case 6
        % Nothing
    otherwise
        error('One, five or six inputs required.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Need method to estimate omega, and options.

% Inserted to protect against inputing integer times
time = double(time);

% 1. Filter the rpice
filteredPrice = realized_price_filter(price,time,timeType,samplingType,samplingInterval);
returns = diff(log(filteredPrice));
% 2. Compute rpreaveraged returns
theta = options.theta;
m = length(returns);
K = ceil(theta * sqrt(m));
g = @(x) min(x,1-x);
w = g((1:(K-1))/K);

preav_returns = nan(m-K+2,1);
for i=1:m-K+2
    preav_returns(i) = w*returns(i:i+K-2);
end

% 3. Compute constants
psi_1 = K*sum((g((1:K)/K) - g((0:(K-1))/K)).^2);
psi_2 = 1/K*sum((g((1:(K-1))/K)).^2);
% 4. Estimate noise
[noiseVariance, ~, ~, noiseEstimateOomen] = realized_noise_estimate(price, time, timeType, options);

omega = noiseEstimateOomen;
if omega<0
    omega = noiseVariance;
end
% 5. Compute Preaveraed Variance
const1 = m/(m-K+2);
const2 = 1/(K*psi_2);
bias = psi_1/(theta.^2 * psi_2) * omega^2;

rpav = const1 * const2 * sum(preav_returns.^2) - bias;


function lo=lpcrf2lo(rf)
%LPCRF2LO Convert reflection coefficients to log area ratios LO=(RF)
%the output values are limited to about +-14.5

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcrf2lo.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

r=max(min(rf,1-1E-6),1E-6-1);
lo=log((1-r)./(1+r));


function [V, converged, i] = fdpf(Ybus, Sbus, V0, Bp, Bpp, ref, pv, pq, mpopt)
%FDPF  Solves the power flow using a fast decoupled method.
%   [V, CONVERGED, I] = FDPF(YBUS, SBUS, V0, BP, BPP, REF, PV, PQ, MPOPT)
%   solves for bus voltages given the full system admittance matrix (for
%   all buses), the complex bus power injection vector (for all buses),
%   the initial vector of complex bus voltages, the FDPF matrices B prime
%   and B double prime, and column vectors with the lists of bus indices
%   for the swing bus, PV buses, and PQ buses, respectively. The bus voltage
%   vector contains the set point for generator (including ref bus)
%   buses, and the reference angle of the swing bus, as well as an initial
%   guess for remaining magnitudes and angles. MPOPT is a MATPOWER options
%   vector which can be used to set the termination tolerance, maximum
%   number of iterations, and output options (see MPOPTION for details).
%   Uses default options if this parameter is not given. Returns the
%   final complex voltages, a flag which indicates whether it converged
%   or not, and the number of iterations performed.
%
%   See also RUNPF.

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   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.

%% default arguments
if nargin < 7
    mpopt = mpoption;
end

%% options
tol     = mpopt.pf.tol;
max_it  = mpopt.pf.fd.max_it;
lu_vec  = have_feature('lu_vec');

%% initialize
converged = 0;
i = 0;
V = V0;
Va = angle(V);
Vm = abs(V);

%% set up indexing for updating V
npv = length(pv);
npq = length(pq);

%% evaluate initial mismatch
mis = (V .* conj(Ybus * V) - Sbus(Vm)) ./ Vm;
P = real(mis([pv; pq]));
Q = imag(mis(pq));

%% check tolerance
normP = norm(P, inf);
normQ = norm(Q, inf);
if mpopt.verbose > 1
    fprintf('\niteration     max mismatch (p.u.)  ');
    fprintf('\ntype   #        P            Q     ');
    fprintf('\n---- ----  -----------  -----------');
    fprintf('\n  -  %3d   %10.3e   %10.3e', i, normP, normQ);
end
if normP < tol && normQ < tol
    converged = 1;
    if mpopt.verbose > 1
        fprintf('\nConverged!\n');
    end
end

%% reduce B matrices
Bp = Bp([pv; pq], [pv; pq]);
Bpp = Bpp(pq, pq);

%% factor B matrices
if lu_vec
    [Lp,  Up,  pp,  qp ] = lu(Bp,  'vector');
    [Lpp, Upp, ppp, qpp] = lu(Bpp, 'vector');
    [junk, iqp ] = sort(qp);
    [junk, iqpp] = sort(qpp);
    % [~, iqp ] = sort(qp);
    % [~, iqpp] = sort(qpp);
else
    [Lp, Up, Pp] = lu(Bp);
    [Lpp, Upp, Ppp] = lu(Bpp);
end

%% do P and Q iterations
while (~converged && i < max_it)
    %% update iteration counter
    i = i + 1;

    %%-----  do P iteration, update Va  -----
    if lu_vec
        dVa = -( Up \  (Lp \ P(pp)) );
        dVa = dVa(iqp);
    else
        dVa = -( Up \  (Lp \ (Pp * P)));
    end

    %% update voltage
    Va([pv; pq]) = Va([pv; pq]) + dVa;
    V = Vm .* exp(1j * Va);

    %% evalute mismatch
    mis = (V .* conj(Ybus * V) - Sbus(Vm)) ./ Vm;
    P = real(mis([pv; pq]));
    Q = imag(mis(pq));

    %% check tolerance
    normP = norm(P, inf);
    normQ = norm(Q, inf);
    if mpopt.verbose > 1
        fprintf('\n  P  %3d   %10.3e   %10.3e', i, normP, normQ);
    end
    if normP < tol && normQ < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nFast-decoupled power flow converged in %d P-iterations and %d Q-iterations.\n', i, i-1);
        end
        break;
    end

    %%-----  do Q iteration, update Vm  -----
    if lu_vec
        dVm = -( Upp \ (Lpp \ Q(ppp)) );
        dVm = dVm(iqpp);
    else
        dVm = -( Upp \ (Lpp \ (Ppp * Q)) );
    end

    %% update voltage
    Vm(pq) = Vm(pq) + dVm;
    V = Vm .* exp(1j * Va);

    %% evalute mismatch
    mis = (V .* conj(Ybus * V) - Sbus(Vm)) ./ Vm;
    P = real(mis([pv; pq]));
    Q = imag(mis(pq));

    %% check tolerance
    normP = norm(P, inf);
    normQ = norm(Q, inf);
    if mpopt.verbose > 1
        fprintf('\n  Q  %3d   %10.3e   %10.3e', i, normP, normQ);
    end
    if normP < tol && normQ < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nFast-decoupled power flow converged in %d P-iterations and %d Q-iterations.\n', i, i);
        end
        break;
    end
end

if mpopt.verbose
    if ~converged
        fprintf('\nFast-decoupled power flow did not converge in %d iterations.\n', i);
    end
end


%compare max, min a mean manipulability for vanilla
%load('/Users/arturogilaparicio/Desktop/arte/robots/RETHINK/sgo_v0.5/experiment1/mat/experiment1A.mat')

[correct_manips]=eval_experiment(Gout, random_manips);

[y,i]=max(sum(correct_manips'));
max_manip = correct_manips(i,:);

[y,i]=min(sum(correct_manips'));
min_manip = correct_manips(i,:);

mean_manip = mean(correct_manips);

figure, plot(max_manip), hold
plot(min_manip)
plot(mean_manip)


% GP_COV_PP - Piecewise polynomial covariance function with compact support
% for Gaussian processes.
%
% [K, DK_LOGTHETA, DK_X2] = GP_COV_CS(X1, X2, LOGTHETA)

% Last modified 2010-10-06
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function covfunc = gp_cov_pp_init(x1,x2)

% Dimensions
d = rows(x2); % dimensionality of inputs
m = cols(x1); % number of other inputs
n = cols(x2); % number of inputs

q = 2; % PP parameter, fix to 2 for now..

% Distance matrix
D = sqrt(sq_dist(X1,X2));

covfunc = @set_hyperparameters;

  function [K,L] = set_hyperparameters(logtheta)

  % Threshold
  thres = exp(logtheta(1)); % length scale, threshold
  R = D./thres;
  I = sparse(R<1);
  NNZ = nnz(I); % number of nonzero elements

  j = floor(rows(x1)) + q + 1;
  switch q
    %%%%%%%%%%
   case 0
    error('not yet implemented for q=0')
    K(I) = (1-R(I)) .^ j;
    %%%%%%%%%%
   case 1
    error('not yet implemented for q=1')
    %%%%%%%%%%

   case 2
    a = j^2 + 4*j + 3;
    b = 3*j + 6;
    c = 3;
    z = 1/3;

    % Helper
    R2 = spalloc(m,n,NNZ);
    R2(I) = D2(I) ./ (thres^2);

    % Covariance matrix
    K = spalloc(m,n,NNZ);
    K(I) = z * (1-R(I)).^(j+2) .* (a*R2(I) + b*R(I) + c);

    dK_dR = spalloc(m,n,NNZ); % copy sparseness
    dK_dR(I) = -z * (j+2) * (1-R(I)).^(j+1) .* (a*R2(I) + b*R(I) + c) ...
        + z * (1-R(I)).^(j+2) .* (2*a*R(I) + b);

    % Gradient for hyperparameters
    if nargout >= 2
      % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
      dK_dlogtheta = spalloc(m,n,NNZ);
      dK_dlogtheta(I) = dK_dR(I) .* (-D(I) ./ thres);
      dK_dlogtheta = full(dK_dlogtheta); % blaaah.. :(
    end

    % Gradients for inputs x2
    if nargout >= 3
      if isempty(x2)
        error('Can''t calculate gradient: x2 not given');
      end
      % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
      % TODO: you have dK_dR and dD_dx but NOT dR_dD!!
      dK_dx2 = bsxfun(@times, reshape(full(dK_dR./thres),[1,m,n]), dD_dx2);
      % blaah the need for fullness.. :(
    end

    %%%%%%%%%%

   case 3
    error('not yet implemented for q=3')
    %%%%%%%%%%
   otherwise
    error('q not valid')
  end

  if ~issparse(K)
    error('K not sparse, it should, wtf?!');
  end

  if nargout >= 2
    LD = ldlchol(K);
    L = ldlsplit(LD);
  end
  end

end

x = [];  % data inputs
D = [];  % distance matrix for data
K = [];  % data covariance
LD = []; % LDL for data covariance
hyperparameters = [];

covstruct.set_data_inputs = @set_data_inputs;

  function set_data_inputs(inputs)
  % Evaluate Euclidean distances
  if ~isequal(x,inputs)
    x = inputs;
    D = sqrt(sq_dist(x));
    K = [];
    LD = [];
  end
  end

  function set_hyperparameters(params)
  if ~isequal(hyperparameters,params)
    hyperparameters = params;
    K = [];
    LD = [];
  end
  end

if nargin < 3
  x2 = [];
end
if nargin < 4
  distfun = @sqdistEuclidean;
end
if nargin < 5
  q = 2;
end

% 2-norm distances
if ~isempty(x2)
  if nargout <= 1
    D = feval(distfun, x1, x2, false);
  else
    [D, dD_dx2] = feval(distfun, x1, x2, false);
  end
  D2 = D.^2; % get this from the distfun?
else
  % Distances for variances
  D = spalloc(cols(x1),1,0); %zeros(cols(x1),1);
  D2 = D; %zeros(cols(x1),1);
end

% Dimensions
d = rows(x2); % dimensionality of inputs
m = cols(x1); % number of other inputs
n = cols(x2); % number of inputs

% Threshold
thres = exp(logtheta(1)); % length scale, threshold
R = D./thres;
I = sparse(R<1);
NNZ = nnz(I); % number of nonzero elements
%K = spones(I); % copy sparseness
% $$$ Dsp = spalloc(m,n,NNZ);
% $$$ Dsp(I) = D(I);
% $$$ D2sp = spalloc(m,n,NNZ);
% $$$ D2sp(I) = D2(I);

j = floor(rows(x1)) + q + 1;
switch q
  %%%%%%%%%%
 case 0
  error('not yet implemented for q=0')
  K(I) = (1-R(I)) .^ j;
  %%%%%%%%%%
 case 1
  error('not yet implemented for q=1')
  %%%%%%%%%%

 case 2
  a = j^2 + 4*j + 3;
  b = 3*j + 6;
  c = 3;
  z = 1/3;

  % Helper
  R2 = spalloc(m,n,NNZ);
  R2(I) = D2(I) ./ (thres^2);

  % Covariance matrix
  K = spalloc(m,n,NNZ);
  K(I) = z * (1-R(I)).^(j+2) .* (a*R2(I) + b*R(I) + c);

  dK_dR = spalloc(m,n,NNZ); % copy sparseness
  dK_dR(I) = -z * (j+2) * (1-R(I)).^(j+1) .* (a*R2(I) + b*R(I) + c) ...
      + z * (1-R(I)).^(j+2) .* (2*a*R(I) + b);

  % Gradient for hyperparameters
  if nargout >= 2
    % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
    dK_dlogtheta = spalloc(m,n,NNZ);
    dK_dlogtheta(I) = dK_dR(I) .* (-D(I) ./ thres);
    dK_dlogtheta = full(dK_dlogtheta); % blaaah.. :(
  end

  % Gradients for inputs x2
  if nargout >= 3
    if isempty(x2)
      error('Can''t calculate gradient: x2 not given');
    end
    % TODO 3D SPARSE MATRICES DO NOT WORK. USE CELL ARRAYS?
    % TODO: you have dK_dR and dD_dx but NOT dR_dD!!
    dK_dx2 = bsxfun(@times, reshape(full(dK_dR./thres),[1,m,n]), dD_dx2);
    % blaah the need for fullness.. :(
  end

  %%%%%%%%%%

 case 3
  error('not yet implemented for q=3')
  %%%%%%%%%%
 otherwise
  error('q not valid')
end

if ~issparse(K)
  error('K not sparse, it should, wtf?!');
end


%CAVITYCONTROL demonstrates the control of lid-driven cavity flow using the
%Koopman-MPC framework described in
% "A data-driven Koopman model predictive control framework for nonlinear
% flows" H. Arbabi, M. Korda and I. Mezic

clc,clear
addpath('./thehood')
% quadratic programming solver
addpath('./thehood/qpOASES-3.1.0/interfaces/matlab')

if (exist('qpOASES_sequence','file') ~= 3)

    error(['You have to activate the MATLAB interface for qpOASES first:' ...
        ' unzip the qpOASES-3.1.0 in "thehood" folder, then '...
        'go to ".\thehood\qpOASES-3.1.0\interfaces\matlab" and run make.m,' ...
        'then run again'])

end

% simulation and flow parameters
SimPar.Reynolds = 13000;
SimPar.dt =.01;            % time step of the ODE solver
SimPar.N = 49;             % size of the computaional grid is (N+1)^2
SimPar.T = 0.2;            % length of each run - continuous-time cavity flow is approximated
                           % using T-time discrete map

load('CavityStateLibrary.mat','LimitCycle_Re13k','FixedPoint_Re10k','UnstableFixedPoint_Re13k')

% initial condition
 x0 = LimitCycle_Re13k(:,25);   % some point on the limit cyle

%% ************************PART I: Identification ********************** %%
%% STEP2: Identification via EDMD
% there are two options:
% 1- generate the data via runing the following program
% (for default values takes ~10hrs on a powerful desktop)
% DataFileName=GenerateCavityData;

% 2- download the data file cited in README.md and set
DataFileName = 'Cavity_data_4EDMD_0.mat';

% Generating Koopman-linear predictors with various number of measurements
CavitySystemID(DataFileName);

%  load any of the predictors
load('KoopmanLinSys_Re13_k50_random');    % predictor with k=50 random measurements

%% ************************PART II: control ********************** %%
% set up MPC controller
disp('setting up MPC controller ...')

n = size(A,1);  % state dimension
r = size(C,1);  % output dimension

% cost matrices
Qy = speye(r);  % output weight matrix
R = 0; % zero weight on input (there are input constraints)

% Prediction horizon
Tpred = 10;
Np = round(Tpred / SimPar.dt);

% State constraints
x_min = nan(n,1);
x_max = nan(n,1);

% Input constraints
umin = 11/13;
umax = 15/13;

% Precompute and possibly save MPC big matrices to speed construction of the controller.
% (This "lifting" has nothing to do with the koopman lifting)
[Ab, Bb] = createMPCmatrices(A,{B},Np);

% compute controller handle
tic
[~,~,mpcCont_lift]  = qpOases_MPC_controller(A,B,C,0,Qy,R,Qy,Np,umin, umax, x_min, x_max,'qpoases',[],[],[],[],[],Ab,Bb);
toc

% reference state
xref = FixedPoint_Re10k;    % unstable fixed point at Re=13k
xref_mpc = xref - x_mean; % mean subtracted!

% closed-loop simulation setup
% nonlinear solver
f = @(x,u)(NonlinearFlowSolver(x,CreateLidVelocity(u,SimPar.N),SimPar));

%% initialize
X = x0;
U = [];
cost = (CollectOutput(x0 - xref))'*Qy*(CollectOutput(x0 - xref));
Q_KE = getCostMatrix(SimPar.N); % the weight matrix to compute kinetic energy
KE_discrepancy = (x0 - xref)'*Q_KE*(x0 - xref);

% run to build the initial delay embeded state
for i = 1:nd

%     Nonlinear simulation with base input
    X = [ X, f(X(:,end),1) ];
    U = [U 1];
end

% now build the initial embedded state
disp('initialization complete')

%% closed-loop simulation

Tsim = 100;
Nsim = Tsim / SimPar.T;

for i = 1:Nsim
    if mod(i,50)==0
        fprintf('Closed-loop simulation, %f %% completed \n', 100 * i / Nsim)
    end
    % create reference output
    yref = CollectOutput(xref_mpc(:,end));

    % build the state of the Koopman predictor
    y = CollectOutput(  bsxfun(@minus,X(:,end-nd+1:end),x_mean) ); % collect the measurements
    y = DelayEmbed(y,nd);   % delay embed the measurements
    ue = DelayEmbed(U(:,end-nd+1:end-1),nd-1);  % delay embed the input
    znow = [y;ue;KE_embed(y);1];  % form the state (g(zeta) in the paper)

    % compute control
    u = mpcCont_lift(znow,yref);

    % Nonlinear simulation
    X = [ X, f(X(:,end),u(:,1)) ];
    U = [U u(:,1)];

    % compute the tracking error and kinetic energy of state discrepancy
    ynow = CollectOutput(  bsxfun(@minus,X(:,end),x_mean) );
    cost = [ cost, (ynow-yref)'*Qy*(ynow-yref) ];
    KE_discrepancy = [KE_discrepancy, (X(:,end) - xref)'*Q_KE*(X(:,end) - xref)];

end

%% plots
set(0,'defaultTextInterpreter','latex', ...
    'defaultLegendInterpreter','latex', ...
    'defaultAxesTickLabelInterpreter','latex');
t = (0:Nsim)*SimPar.T;
figure(20),clf
subplot(2,2,1)
plot(t,U(1,nd:end),'linewidth',2);
xlabel('$t$','fontsize',12);
title('input')
ylim([umin umax])
set(gca,'YTick',linspace(umin,umax,5))

subplot(2,2,2)
plot(t,KE_discrepancy); hold on
title('kinetic energy of discrepency')

subplot(2,2,3)
plot(t,cost); hold on
title('control tracking error')

subplot(2,2,4)
PlotVorticity(X(:,end)-xref);
hold on
[ Grid ] = CavityGridOperators( SimPar.N );
plot(CollectOutput(Grid.xx),CollectOutput(Grid.yy),'x')
axis square
axis([-1 1 -1 1])
title('vorticity discrepancy at final time and sensor locations')


function RCM = DEM_demo_Bayesian_Model_Reduction
% This demonstration code illustrates the application of post hoc model
% optimisation or Bayesian model reduction (BMR) in identifying gene and
% gene-gene interaction effects in behavioural or physiological variables.
% The basic idea is to replace conventional heuristics based on the
% assumption that the contribution of any gene is sampled from a sparse
% distribution (such that a small number contribute and a large number do
% not) with an explicit search over a model space that includes all sparse
% and non-sparse models.  This exhaustive search rests upon recent
% advances in model optimisation based upon variational Bayesian model
% inversion.  In short, it is possible to estimate the posterior
% distribution of model parameters under a reduced model, given the
% posterior and prior distributions under a full model.
% In this context, a reduced model corresponds to a model in which some
% parameters are removed (by shrinking their prior variance to zero).
% This means that it is only necessary to invert the full model and then
% perform automatic BMR (over all possible combinations of parameters)
% using a greedy search based upon the free energy approximation to log
% model evidence.  With sufficient signal to noise, this scheme can
% recover the small number of effects, even in under determined or
% ill-posed problems (where the number of potential effects can vastly
% exceed the number of samples).
%
% The illustration below uses 128 subjects who have been measured three
% times (say in three brain regions) and we want to model these
% measurements in terms of first and second order genetic contributions
% given 8 (binary) genetic variables and all (unique) pair wise
% interactions.  This means that there are 36 unknown parameters
% (excluding a constant and, say, age confounds over subjects).  In the
% scheme below, each measurement is inverted separately under a simple
% (polynomial) model with uninformative priors on the parameters and
% (precision) hyper-parameters describing beliefs about signal to noise.
% A fixed effects Bayesian model averaging (BMA) scheme is used in
% combination with BMR to identify the best model out of all possible
% combinations of first and second order effects.  With the signal to
% noise and number of samples used in this simulation, the recovery is
% generally perfect.  This scheme also illustrates inference over a
% partition of model space (or families of models).

%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_demo_Bayesian_Model_Reduction.m 6306 2015-01-18 20:50:38Z karl $

rng('default')

% Genomic data (G)
%--------------------------------------------------------------------------
Ns = 256;                                   % unmber of subjects
Ng = 8;                                    % unmber of genes
Nr = 3;                                    % unmber of (fMRI) regions

% Genotype
%--------------------------------------------------------------------------
G     = rand(Ns,Ng) > 1/2;

% design matix (U) with second-order (gene-gene interaction) effects
%--------------------------------------------------------------------------
GG    = [];
for i = 1:Ns
    u     = [];
    for j = 1:Ng
        for k = (j + 1):Ng
          u(end + 1) = G(i,j)*G(i,k);
        end
    end
    GG = [GG; u];
end
U  = [G GG];
Nb = size(U,2);
R  = ones(1,Nr);
A  = zeros(Nb,1);

% first-order effects (g1 abd g4) and one interaction (g1 x g2)
%--------------------------------------------------------------------------
A([1 4 (Ng + 3)],:) = [1/2 1 1];             % effect sizes

% Simulate data with a SNR of about 4:1
%--------------------------------------------------------------------------
y  = U*A*R + randn(Ns,Nr);                 % SD(noise) = 1

% Bayesian model inversion
%==========================================================================

% Model specification
%--------------------------------------------------------------------------
M.IS   = @(P,M,U) U*P.A;
M.pE.A = zeros(Nb,1);
M.pC.A = zeros(Nb,1) + 128;
M.hE   = 0;
M.hC   = 1/128;

% confounds (at the between subject level)
%--------------------------------------------------------------------------
age   = rand(Ns,1);
Y.X0  = [age ones(Ns,1)];

for i = 1:Nr

    % model inversion for this region
    %----------------------------------------------------------------------
    Y.y       = y(:,i);
    [Ep,Cp]   = spm_nlsi_GN(M,U,Y);

    % save model for subsequent BMS
    %----------------------------------------------------------------------
    DCM{i}.M  = M;
    DCM{i}.Ep = Ep;
    DCM{i}.Cp = Cp;

end

% Bayesian model reduction: see below for family fun
%==========================================================================
RCM  = spm_dcm_post_hoc(DCM,@fun);

% show results
%--------------------------------------------------------------------------
spm_figure('Getwin','Model posterior (over families)'); clf

Qf         = zeros(1,8);
Qf(fun(A)) = 1;                           % true family
Pf         = zeros(1,8);
Pf(1:length(RCM.Pf)) = RCM.Pf;            % psoteror family probailities

subplot(2,1,1)
bar([Pf; Qf]')
xlabel('familiy')
title('Model posterior (over families)','FontSize',16)
axis square
legend({'esimated','true'})

subplot(2,1,2)
bar([RCM.Ep.A A])
title('Recovered and true effects','FontSize',16)
xlabel('parameter')
axis square

% Partition of mdoel space into famllies
%==========================================================================
function k = fun(A)

f1  = any(A(1:4));
f2  = any(A(5:8));
f3  = any(A(9:12));

if ~f1 && ~f2 && ~f3, k = 1; end
if ~f1 && ~f2 &&  f3, k = 2; end
if ~f1 &&  f2 && ~f3, k = 3; end
if ~f1 &&  f2 &&  f3, k = 4; end
if  f1 && ~f2 && ~f3, k = 5; end
if  f1 && ~f2 &&  f3, k = 6; end
if  f1 &&  f2 && ~f3, k = 7; end
if  f1 &&  f2 &&  f3, k = 8; end


function F = sign(F, pref)
%SIGN   Sign function of a CHEBFUN.
%   G = SIGN(F) returns a piecewise constant CHEBFUN G such that G(x) = 1 in the
%   interval where F(x) > 0, G(x) = -1 in the interval where F(x) < 0 and G(x) =
%   0 in the interval where F(x) = 0. Breakpoints in G are introduced at zeros
%   of F.
%
%   For the nonzero values of complex F, SIGN(F) = F./ABS(F)
%
% See also ABS, HEAVISIDE, ROOTS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Deal with the empty case:
if ( isempty(F) )
    return
end

if ( nargin < 2 )
    pref = chebfunpref();
end

for k = 1:numel(F)
    F(k) = signColumn(F(k), pref);
end

end

function g = signColumn(f, pref)

% Add breaks at the appropriate roots of f:
g = addBreaksAtRoots(f, pref);

% Call SIGN on each of the FUNs: (result will be smooth)
for k = 1:numel(g.funs)
    g.funs{k} = sign(g.funs{k}, pref);
end

% Take the sign of the pointValues in the first row:
g.pointValues = sign(g.pointValues);

% Remove unnecessary breakpoints:
g = merge(g, pref);

end


function [x, info] = umfpack_btf (A, b, Control)
%UMFPACK_BTF factorize A using a block triangular form
%
% Example:
%   x = umfpack_btf (A, b, Control)
%
% solve Ax=b by first permuting the matrix A to block triangular form via dmperm
% and then using UMFPACK to factorize each diagonal block.  Adjacent 1-by-1
% blocks are merged into a single upper triangular block, and solved via
% MATLAB's \ operator.  The Control parameter is optional (Type umfpack_details
% and umfpack_report for details on its use).  A must be square.
%
% See also umfpack, umfpack2, umfpack_details, dmperm

% Copyright 1995-2007 by Timothy A. Davis.

if (nargin < 2)
    help umfpack_btf
    error ('Usage: x = umfpack_btf (A, b, Control)') ;
end

[m n] = size (A) ;
if (m ~= n)
    help umfpack_btf
    error ('umfpack_btf:  A must be square') ;
end
m1 = size (b,1) ;
if (m1 ~= n)
    help umfpack_btf
    error ('umfpack_btf:  b has the wrong dimensions') ;
end

if (nargin < 3)
    Control = umfpack2 ;
end

%-------------------------------------------------------------------------------
% find the block triangular form
%-------------------------------------------------------------------------------

% dmperm built-in may segfault in MATLAB 7.4 or earlier; fixed in MATLAB 7.5
% since dmperm now uses CSparse
[p,q,r] = dmperm (A) ;
nblocks = length (r) - 1 ;

info = [0 0 0] ;    % [nnz(L), nnz(U), nnz(F)], optional 2nd output

%-------------------------------------------------------------------------------
% solve the system
%-------------------------------------------------------------------------------

if (nblocks == 1 | sprank (A) < n)					    %#ok

    %---------------------------------------------------------------------------
    % matrix is irreducible or structurally singular
    %---------------------------------------------------------------------------

    [x info2] = umfpack2 (A, '\', b, Control) ;
    info = [info2(78) info2(79) 0] ;

else

    %---------------------------------------------------------------------------
    % A (p,q) is in block triangular form
    %---------------------------------------------------------------------------

    b = b (p,:) ;
    A = A (p,q) ;
    x = zeros (size (b)) ;

    %---------------------------------------------------------------------------
    % merge adjacent singletons into a single upper triangular block
    %---------------------------------------------------------------------------

    [r, nblocks, is_triangular] = merge_singletons (r) ;

    %---------------------------------------------------------------------------
    % solve the system: x (q) = A\b
    %---------------------------------------------------------------------------

    for k = nblocks:-1:1

	% get the kth block
        k1 = r (k) ;
        k2 = r (k+1) - 1 ;

	% solve the system
        [x2 info2] = solver (A (k1:k2, k1:k2), b (k1:k2,:), ...
	    is_triangular (k), Control) ;
	x (k1:k2,:) = x2 ;

        % off-diagonal block back substitution
        F2 = A (1:k1-1, k1:k2) ;
        b (1:k1-1,:) = b (1:k1-1,:) - F2 * x (k1:k2,:) ;

        info (1:2) = info (1:2) + info2 (1:2) ;
        info (3) = info (3) + nnz (F2) ;

    end

    x (q,:) = x ;

end

%-------------------------------------------------------------------------------
% merge_singletons
%-------------------------------------------------------------------------------

function [r, nblocks, is_triangular] = merge_singletons (r)
%
% Given r from [p,q,r] = dmperm (A), where A is square, return a modified r that
% reflects the merger of adjacent singletons into a single upper triangular
% block.  is_triangular (k) is 1 if the kth block is upper triangular.  nblocks
% is the number of new blocks.

nblocks = length (r) - 1 ;
bsize = r (2:nblocks+1) - r (1:nblocks) ;
t = [0 (bsize == 1)] ;
z = (t (1:nblocks) == 0 & t (2:nblocks+1) == 1) | t (2:nblocks+1) == 0 ;
y = [(find (z)) nblocks+1] ;
r = r (y) ;
nblocks = length (y) - 1 ;
is_triangular = y (2:nblocks+1) - y (1:nblocks) > 1 ;

%-------------------------------------------------------------------------------
% solve Ax=b, but check for small and/or triangular systems
%-------------------------------------------------------------------------------

function [x, info] = solver (A, b, is_triangular, Control)
if (is_triangular)
    % back substitution only
    x = A \ b ;
    info = [nnz(A) 0 0] ;
elseif (size (A,1) < 4)
    % a very small matrix, solve it as a dense linear system
    x = full (A) \ b ;
    n = size (A,1) ;
    info = [(n^2+n)/2 (n^2+n)/2 0] ;
else
    % solve it as a sparse linear system
    [x info] = umfpack_solve (A, '\', b, Control) ;
end


function J = vl_imup(I)
% VL_IMUP Upsample an image by two
%   J=VL_IMUP(I) doubles the resolution of the image I by using
%   bilinear interpolation.
%
%   See also: VL_IMDOWN(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

[M,N,K] = size(I) ;

J = zeros(2*M,2*N,K) ;

J(1:2:end,1:2:end,:) = I ;

J(2:2:end,1:2:end,:) = 0.5*(I+[I(2:end,:,:);I(end,:,:)]) ;
J(1:2:end,2:2:end,:) = 0.5*(I+[I(:,2:end,:),I(:,end,:)]) ;
J(2:2:end,2:2:end,:) = ...
  0.25*(...
  J(2:2:end,1:2:end-1,:)+...
  J(1:2:end-1,2:2:end,:)+...
  [J(2:2:end,3:2:end,:),J(2:2:end,end-1,:)]+...
  [J(3:2:end,2:2:end,:);J(end-1,2:2:end,:)]) ;


%% update_vel_pos(p,n);
function update_vel_pos()
global vel El El_lim_up El_lim_low
global n w k_pb_i R iteration k_pb_f  k_gb_i k_gb_f k_pb k_gb c_gb c_pb delta_t;
global NumVer p pbest_loc gbest_loc
R = n /iteration;
k_pb = k_pb_i + R*(k_pb_f - k_pb_i);
k_gb = k_gb_i + R*(k_gb_f - k_gb_i);
c_pb = rand*k_pb;
c_gb = rand*k_gb;

      vel(1,1:NumVer,p) = w*vel(1,1:NumVer,p) + c_pb*(pbest_loc(1,1:NumVer,p) - El(1,1:NumVer,p)) + c_gb*(gbest_loc(1,1:NumVer) - El(1,1:NumVer,p));

%Velocity Upper Bound Velocity Lower Bound
     for chgdir=1:NumVer
      if((vel(1,chgdir,p) > (El_lim_up(1,chgdir) - El(1,chgdir,p))))
          vel(1,chgdir,p) = -0.5*vel(1,chgdir,p);%(El_lim_up(1,1:NumVer) - El(1,1:NumVer,p));%             x(i)=x(i)+2*v(i);
      end
      if((vel(1,chgdir,p) < (El_lim_low(1,chgdir) - El(1,chgdir,p))))
          vel(1,chgdir,p) = -0.5*vel(1,chgdir,p);%(El_lim_up(1,1:NumVer) - El(1,1:NumVer,p));
      end
     end

 El(1,1:NumVer,p)= El(1,1:NumVer,p)+delta_t*vel(1,1:NumVer,p);
end


function [aa,z] = fitnesstra3f(pop,para)
[pops,numvar]=size(pop);
for i=1:pops
    [phi,conff]=invkin3(para(4),para(5),pop(i,4)) ;%final conficuration
    z(i)=phi;
    cond=[para(1:3),conff];
    chrom=[pop(i,1:3),pop(i,5:end)];
    kk=trajt3(cond,chrom);
    tt=torque3(kk);
    ft=ftorque3(tt);
    poo=kk(1:3,:);
    fq=sum(sum(abs(diff(poo'))));
    pos=forkin3(kk(1,:),kk(2,:),kk(3,:));xx=pos(1,:);yy=pos(2,:);
    x=diff(xx);
    y=diff(yy);
    dis=sqrt(x.^2+y.^2);
    fdis=sum(dis);
    time=pop(i,8)+pop(i,9);
    a1=1.7;
    a2=2;
    a3=2;
    a4=1;
    fit=a1*ft+a2*fq+a3*fdis+a4*time;
    aa(i)=1/fit;
end


function  [energyVal] =  energyVADVal(frame_x, Fs)
energyVal = norm(frame_x);


function [haus, meanAbsSurfDist] = calc_HausdorffMetric(structNum1,structNum2,planC)
%function [haus, meanAbsSurfDist] = calc_HausdorffMetric(structNum1,structNum2,planC)
%
%This function computes Hausdorff distance between structNum1 and structNum2
%
%APA, 11/20/2014

if ~exist('planC','var')
    global planC
end

[~, x1V, y1V, z1V, planC] = getStructSurface(structNum1,planC);
[~, x2V, y2V, z2V, planC] = getStructSurface(structNum2,planC);

haus = NaN;
meanAbsSurfDist = NaN;
if ~isempty(x1V) && ~isempty(x1V)
    [haus,meanAbsSurfDist] = hausdorff([x1V(:) y1V(:) z1V(:)],[x2V(:) y2V(:) z2V(:)]);
end


function [J, S] = GALA_invert(BF,tS)

YTY   = sparse(0);

Nl = tS.Nl;

for p=1:Nl

    BFp = load(BF{p});

    % load data
    D{p} = spm_eeg_load(BFp.data.D);

    % load gain matrix
    modality='MEG'; % temporary - should be replaced by tS.modality
    L1 = BFp.sources.L.(modality);
    Nd = length(L1);
    Lp{p} = [];
    for i = 1:Nd
        lf = L1{i};
        Lp{p} = cat(2, Lp{p}, lf);
    end
    % eliminate low SNR spatial modes
    U = spm_svd((Lp{p}*Lp{p}'),exp(-16));
    ULp{p} = U'*Lp{p};
    Nm(p) = size(ULp{p},1);
    Up{p} = U;

    trial = D{p}.condlist;      % get conditions list
    Nt = 2;                     % temporary invert only first one
    Nb = size(D{p},2);          % Number of time bins

    % get (spatially aligned) data
    YY = 0; N=0;
    for j = 1:Nt                                % loop over conditions
        Y{p,j}  = Up{p}'*D{p}(:,:,j);
        YY    = YY + Y{p,j}'*Y{p,j};
        N     = N + Nb;
    end

    YTY        = YTY + YY;

end

% Initialize matrices comtrolling covariance reparametrization
Is = 1:Nd;
for p=1:Nl
    Isp{p} = 1:Nd;
end
lIs = 1:Nd*Nl;
Vs = speye(Nd*Nl);

mNm = mean(Nm);
scale = 1/sqrt(trace(YTY)/(N*mNm));
YTY = YTY*(scale^2);

% temporal projector
[V E]  = spm_svd(YTY,exp(-8));              % get temporal modes
E      = diag(E)/trace(YTY);                % normalise variance
Nr  = length(E);                            % number of temporal modes
S   = V(:,1:Nr);                            % temporal modes
VE  = sum(E(1:Nr));                         % variance explained

fprintf('Using %i temporal modes, ',Nr)
fprintf('accounting for %0.2f percent average variance\n',full(100*VE))

% scale and align data (spatial and temporary modes)
for p=1:Nl
    MY=[];
    for j = 1:Nt                % loop over Nt conditions
        MY{j}   = Y{p,j}*S*scale;
    end
    UYp{p} = spm_cat(MY);
end

% prepare long data and leadfields and channel noise components
UL=[]; Qe=[];
for p=1:Nl
    UL = blkdiag(UL,ULp{p});
    Qep{p} = Up{p}'*Up{p};
    Qe = full(blkdiag(Qe,Qep{p}));
end
UY = spm_cat(UYp');

Scale = sqrt(trace(UL*UL')/mNm);
UL = UL/Scale;

% prepare smoothing kernel - it could be done as in spm but this scheme
% applicable for FreeSurfer meshes
% -----------------------------------------------------------------------
radius = 4;

vert  = D{1}.inv{D{1}.val}.mesh.tess_mni.vert;
face  = D{1}.inv{D{1}.val}.mesh.tess_mni.face;

AA=[];
AAA     = spm_mesh_distmtx(struct('vertices',vert,'faces',face),0);
% find subsets of certain distance
AA{1}=speye(length(vert));
AA{2}=AAA;
B = AA{1}+AA{2};
A = B;
for i=2:radius-1
    in = find(AAA^i);
    Bn=sparse(length(vert),length(vert));
    Bn(in)=1;
    AA{i+1}=full(Bn-B);
    B=Bn;
end

y=[1 0.8 0.37 0.2];

QG = sparse(length(vert),length(vert));
for i=1:radius
    QG=QG + AA{i}*y(i);
end
QG =sparse(QG(:,:));
K    = sparse(QG.*(QG > exp(-8)));
clear AA AAA B Bn QG
% -----------------------------------------------------------------------

% covariance of the data
YY = UY*UY';

% noise covariance component
Qn{1} = Qe; % it's the first one for channel noise

% template for source-noise covariance
sQt = kron(speye(Nl),speye(Nd));

thresh = 0.5;       % threshold

for it=1:tS.iter

    % starting from 2nd iteration add source-noise covariance matrices
    if it>1
        sQn = Vn*sQt*Vn; %Vn - noise-vertices matrix, defined below
        Qn{it} = UL*sQn*UL';
    end

    % first ROI covariance matrix - strong correlation between subjects
    psQ1 = kron(ones(Nl),K);
%     psQ1 = kron(ones(Nl),speye(Nd));
    sQ1 = Vs*psQ1*Vs;
    Q1 = full(UL*sQ1*UL');

    % second ROI covariance matrix - subjects specific activity
    psQ2 = kron(speye(Nl),K);
%     psQ2 = kron(speye(Nl),speye(Nd));
    sQ2 = Vs*psQ2*Vs;
    Q2 = full(UL*sQ2*UL');

    Qs = {Q1 Q2};
%     Qs = {Q2};

    % ML estimation of hyperparameters
    [Cy,h,Ph,F] = spm_reml_sc(YY,[],[Qn Qs],1,-4,16);
    % take ROI components and discard noise components
    DD = h(end-1)*sQ1 + h(end)*sQ2;
%     DD = h(end)*sQ2;
    Cy = full(Cy);

    M = DD*UL'/Cy;

    % long (all subjects) source activity
    J = M*UY;

    % goodness of fit
    SSR  = sum(var((UY - UL*J),0,2));
    SST  = sum(var( UY,0,2));
    R2   = 100*(SST - SSR)/SST;
    fprintf('\nPercent variance explained GALA %.2f\n',full(R2));
    fprintf('F = %.10f\n',full(F));
%     fprintf('L = %.10f\n',full(-spm_logdet(Cy)- trace(UY'*inv(Cy)*UY)));
%     fprintf('ML fit = %.10f\n',-trace(UY'*inv(Cy)*UY));
%     fprintf('ML det = %.10f\n\n',full(-spm_logdet(Cy)));

    % idea - get vertices with max covariance both within and between
    % subjects. As the result they should not be the same (indices) for
    % different subjects, so correlated between subjects part of covarince
    % prior is overlapping but not identical patches

    % pay attention - it's to exclude disconected vertices from Jcov
    % it seems ssQ1 = spones(sQ1) works better than simple sQ1
    % may be because in Jcov.*sQ1 there is double attenuation of tails

    if it<tS.iter

        ssQ1 = spones(sQ1);

        % real calculation for big matrices
        J = full(J);

        lJcov = zeros(1,Nl*Nd);
        for i=1:Nd*Nl
            if any(J(i,:))
                Jcovi = J*J(i,:)';
                sJcovi = Jcovi.*ssQ1(:,i);
                lJcov(i) = squeeze(sum(sJcovi));
            end
        end

        % reparametrization of covariance matrices
        lIso = lIs;
        lIs = find(lJcov>quantile(lJcov,thresh));   % ROI vertices update
        lnIs = setdiff(lIso,lIs);                   % noise vertices update

        Vs = zeros(Nd*Nl,1);
        Vn = zeros(Nd*Nl,1);
        Vs(lIs) = 1;
        Vn(lnIs) = 1;
        Vs = spdiags(Vs,0,Nd*Nl,Nd*Nl);     % ROI vertices matrix update
        Vn = spdiags(Vn,0,Nd*Nl,Nd*Nl);     % noise vertices matrix update

        thresh = thresh+(1-thresh)/2;               % threshold update

    end

    % display

%     MM.vertices = vert;
%     MM.faces = face;
%
%     ahp=[];
%     figure;
%     for p=1:Nl
%         lJcovp{p} = lJcov(1+(p-1)*Nd:Nd+(p-1)*Nd);
%
%         ahp(p)=subplot(2,2,p);
%         srcs_disp1 = lJcovp{p}/max(abs(lJcovp{p}));
%
%         cla; axis off
%
%         fig1 = patch('vertices',MM.vertices,'faces',MM.faces,'FaceVertexCData',srcs_disp1');
%
%         set(fig1,'FaceColor',[.5 .5 .5],'EdgeColor','none');
%         shading interp
%         lighting gouraud
%         %camlight
%         zoom off
%         lightangle(0,270);lightangle(270,0),lightangle(90,0),lightangle(0,45),lightangle(0,135);
%         material([.1 .1 .4 .5 .4]);
%         %view(140,15);
%         caxis([0 1])
%         colormap(jet);
%
%     end
%
%     hlink = linkprop(ahp, {'CameraPosition','CameraUpVector'});
%     key = 'graphics_linkprop';
%     setappdata(ahp(1),key,hlink);

end

end


function y=ill(t,x)
a=1;b=0.3;
y=[a*x(1)*x(2)-b*x(1),-a*x(1)*x(2)]';


% Test file for ADCHEBFUN VOLT

function pass = test_volt

K = @(s, t)  exp(-(s-t).^2);

% List of trigonometric functions to test.
func = @(u) volt(K, u);

% Tolerance for Taylor testing
tolOrder = 1e-2;
tolDiff = 1e-12;
% Initialise vector with pass information
pass = zeros(1, numel(func));

% Call the valueTesting method, which also returns linearity information
[err, lin] = adchebfun.valueTesting(func);

% First, check that the computed function values match what we expect
pass(1) = ( err == 0 );

% Call the taylorTesting method
[order1, order2, nDiff2] = adchebfun.taylorTesting(func,2);

% We expect all elements of ORDER1 to be close to 1. Since the methods being
% tested in this case are all linear, ORDER2 will be noise. However, since
% the methods are indeed linear, we should expect nDiff2 to have values all
% close to machine epsilon, which we can use to check for the correctness of
% the derivative computed.
pass(2) = ( (max(abs(order1 - 1)) < tolOrder) && ...
    (max(abs(nDiff2)) < tolDiff) );

% Check that we received the correct linearity information
pass(3) = ( lin == 1 );

end


function [pv, ph] = splitpg(pg)
% [pv, ph] = splitpg(pg)

if iscell(pg)
    for f = 1:numel(pg)
        pv{f} = [pg{f}(:, 1) sum(pg{f}(:, 2:6), 2) pg{f}(:, 7)];
        ph{f} = pg{f}(:, 2:6) ./ repmat(max(pv{f}(:, 2), 1E-6), [1 5]);
    end
else
    pv = [pg(:, 1) sum(pg(:, 2:6), 2) pg(:, 7)];
    ph = pg(:, 2:6) ./ repmat(max(pv(:, 2), 1E-6), [1 5]);
end


classdef medianFilter < EBSDFilter

  properties
    numNeighbours % number of neigbours to consider (default 1)
  end

  methods

    function F = medianFilter(varargin)
      %

      F.numNeighbours = get_option(varargin,'neighbours',1);

      addlistener(F,'isHex','PostSet',@check);

      function check(varargin)
        if F.isHex
          warning(['Hexagonal grids are not yet fully supportet for the medianFilter. ' ...
            'It might give reasonable results anyway']);
        end
      end

    end

    function ori = smooth(F,ori,quality)

      ori(quality==0) = nan;

      % this projects to the fundamental region around the mean
      [~,ori] = mean(ori);

      % make verything quaternion
      q = quaternion(ori);

      % some shortcuts
      nn = F.numNeighbours;
      dn = 1+2*nn;

      % the mean distance from every candiate to all others
      meanDist = zeros([size(q),2*F.numNeighbours+1,2*F.numNeighbours+1]);

      % make q a bit larger
      q = [quaternion.nan(F.numNeighbours,size(q,2)+2*F.numNeighbours);...
        [quaternion.nan(size(q,1),F.numNeighbours),...
        q,quaternion.nan(size(q,1),F.numNeighbours)];...
        quaternion.nan(F.numNeighbours,size(q,2)+2*F.numNeighbours)];

      % compute for any candiate the mean distance to all other points
      % the first two loops are for the candidate
      for i1 = 1:dn
        for j1 = 1:dn

          % the candidate
          qq = q(i1+(0:end-dn),j1+(0:end-dn));
          count = zeros(size(qq));

          % compute the distance from the candidate to all other candidates
          for i2 = 1:dn
            for j2 = 1:dn

              omega = angle(qq,q(i2+(0:end-dn),j2+(0:end-dn)));
              [meanDist(:,:,i1,j1),count] = nanplus(meanDist(:,:,i1,j1),omega,count);

            end
          end

          meanDist(:,:,i1,j1) = meanDist(:,:,i1,j1) ./ count;

        end
      end

      % find median
      meanDist = reshape(meanDist,[size(qq),(2*F.numNeighbours+1)^2,]);
      [mm,id] = min(meanDist,[],3);

      [i,j] = ind2sub(size(qq),1:length(qq));
      [ii,jj] = ind2sub([2*F.numNeighbours+1 2*F.numNeighbours+1],id);

      % in regions where everything is nan take simply the center point
      % we may later weaken this to allow inpainting
      ii(isnan(mm)) = nn+1;
      jj(isnan(mm)) = nn+1;

      % compute the final indece to the median
      ind = sub2ind(size(q),i(:)+ii(:)-1,j(:)+jj(:)-1);

      % switch to median
      ori(1:length(ori)) = q(ind);

    end
  end
end


%TRAINCC Train combining classifier if needed
%
%   W = TRAINCC(A,W,CCLASSF)
%
% INPUT
%   A        Training dataset
%   W        A set of classifiers to be combined
%   CCLASSF  Combining classifier
%
% OUTPUT
%   B        Combined classifier mapping
%
% DESCRIPTION
% The combining classifier CCLASSF is trained by the dataset A*W, if
% training is needed. W is typically a set of stacked (operating in the same
% feature space) or parallel (operating in different feature spaces;
% performed one after another) classifiers to be combined. E.g. if V1, V2
% and V3 are base classifiers, then V = [V1,V2,V3,...] is a stacked
% classifier and V = [V1;V2;V3;...] is a parallel one. If CCLASSF is one of
% the fixed combining rules like MAXC, then training is skipped.
%
% This routine is typically called by combining classifier schemes like
% BAGGINGC and BOOSTINGC.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, STACKED, PARALLEL, BAGGINGC

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: traincc.m,v 1.4 2010/06/25 07:55:34 duin Exp $

function w = traincc(a,w,cclassf)

		if (~ismapping(cclassf))
		error('Combining classifier is an unknown mapping.')
	end

	% If CCLASSF is already a combining classifier, just apply it. Otherwise,
	% train it using A*W.
	if isuntrained(w)
		w = a*w;  % train base classifiers
	end

	w = w*cclassf;
	if isuntrained(w)
		w = a*w;  % train combiner when needed
	end

	w = setcost(w,a);

return


function bezier_surface_node_print ( node_num, node_xyz )

%*****************************************************************************80
%
%% BEZIER_SURFACE_NODE_PRINT prints nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XYZ(3,NODE_NUM), the coordinates of the
%    nodes.
%
  dim_num = 3;

  r8mat_transpose_print ( dim_num, node_num, node_xyz, ...
    '  Bezier Surface Nodes:' );

  return
end


%  INTERNAL FUNCTION: initialize the storage of filtering
%
%  ::
%
%    [Filters,K_store,iF_store,v_store]=initialize_storage(a,P,PAI,Q,p0,...
%    exo_nbr,horizon,m,nsteps,smpl,h,store_filters)
%
%  Args:
%
%     - **a** [cell]: initial conditions of the filter for each regime
%     - **P** [cell]: initial covariance of the filter for each regime
%     - **PAI** [vector]: initial probability distribution of regimes
%     - **p0** [scalar]: number of observables
%     - **exo_nbr** [scalar]: number of exogenous
%     - **horizon** [{1}|scalar]: number of anticipated steps + 1
%     - **nsteps** [scalar]: number of forecast steps
%     - **smpl** [scalar]: number of observations
%     - **store_filters** [0|1|2|3]: 0 (no storage), 1(predicted only),
%       2(predicted and updated), 3(predicted, updated and smoothed)
%
%  Returns:
%     :
%
%     - **Filters** [struct]: structure with different fields
%     - **K_store** [cell]: Place holder for Kalman gains
%     - **iF_store** [cell]: Place holder for inverses of covariance matrices
%       of forecast errors
%     - **v_store** [cell]: Place holder for forecast errors
%
%


classdef prtPreProcEnergyNormalizeRows < prtPreProc
    % prtPreProcEnergyNormalizeRows Normalize the rows of the data to have unit
    % energy
    %

    properties (SetAccess=private)

        name = 'Energy Normalize Rows'  %  MinMax Rows
        nameAbbreviation = 'ENR'  % MMR
    end

    properties
        %no properties
        energyOffset = 0;
    end

    methods

        function self = prtPreProcEnergyNormalizeRows(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end
    end

    methods (Access = protected, Hidden = true)

        function self = trainAction(self,DataSet) %#ok<INUSD>
            %do nothing
        end

        function DataSet = runAction(self,DataSet) %#ok<MANU>

            theData = DataSet.getObservations;
            theData = bsxfun(@rdivide,theData,self.energyOffset + sqrt(sum(theData.^2,2)));
            DataSet = DataSet.setObservations(theData);
        end

        function xOut = runActionFast(self,xIn,ds) %#ok<INUSD>
            xOut = bsxfun(@rdivide,xIn,self.energyOffset + sqrt(sum(xIn.^2,2)));
        end
    end

    methods (Hidden)
        function str = exportSimpleText(self)
            titleText = sprintf('%% prtPreProcEnergyNormalizeRows\n');
            energyOffsetText = prtUtilMatrixToText(full(self.energyOffset),'varName','energyOffset');
            str = sprintf('%s%s',titleText,energyOffsetText); % No parameters
        end
    end

end


function [I,oct] = note2interval(N,key)
% MUSIC.NOTE2INTERVAL Converts a note string to a semitone interval and octave.
%   [I,O] = MUSIC.NOTE2INTERVAL(N) returns the interval I and octave O at which
%   note N is found in the key of 'C'. N is a note in scientific pitch notation
%   (e.g. 'A#3') or a simple note string without an octave specification (e.g.
%   'A#'). If the octave is not defined O will be NaN. N may be a cell array of
%   strings.
%
%   [I,O] = MUSIC.NOTE2INTERVAL(N,KEY) returns the interval at which note N is
%   found in the key of KEY. KEY may be a character note (e.g., 'A', 'F#'), or
%   an interval offset from 'C'.
%
%   Example
%      [I,O] = music.note2interval('F#')       % returns 6, NaN
%      [I,O] = music.note2interval('F#5','G')  % returns 11, 5
%
%   See also music.note2tone, music.note2freq, music.interval2note.

%    Author: E. Johnson
%    Copyright 2010 The MathWorks, Inc.

% Resolve character key to offset from C
if nargin < 2
    key = 0;
end
if ischar(key)
    key = music.note2interval(key);
end

% The interval offset from the beginning of the notes octave, can be used to
% index into this table. Which column is used depends on whether or not the key
% is expressed using sharps or flats.
noteTable = { 'C'   'C' ;
              'C#'  'Db';
              'D'   'D' ;
              'D#'  'Eb';
              'E'   'E' ;
              'F'   'F' ;
              'F#'  'Gb';
              'G'   'G' ;
              'G#'  'Ab';
              'A'   'A' ;
              'A#'  'Bb';
              'B'   'B' ; };

N  = cellstr(N);  % ensure N is a cell array
sz = size(N);     % record original size of N
N  = N(:);        % ensure N is a column vector

% Use regular expression to separate octave from note.
C = regexp(N,'([AaBbCcDdEeFfGg][Bb#]?)(\d*$)','tokens');

note = cellfun( @(c)c{1}{1}, C, 'UniformOutput', false );  % simple note
oct  = cellfun( @(c)str2double(c{1}{2}), C );              % octave

ind  = cellfun(@(n)find(strcmpi(noteTable,n),1),note);

[row,col] = ind2sub(size(noteTable),ind); %#ok

I = row - 1 - key;
I = mod(I,12);

I   = reshape(I,  sz);
oct = reshape(oct,sz);


function [L,Lc,Lli] = toFramePlucker(C,Li)

% TOFRAMEPLUCKER  Transform plucker line to a given frame.
%   L = TOFRAMEPLUCKER(C,Li) expresses in frame C=[t;q] the line Li
%   originally expressed in global frame.
%
%   The formula for transformation is
%
%       L = [ Rt       [it]_x*Rt ]
%           [ zeros(3)        Rt ] * Li
%
%   where
%       Rt    = q2R(iq)
%      [it]_x = hat(it)
%       iq    = q2qc(q)
%       it    = t2it(t,q)
%       t     = C(1:3)
%       q     = C(4:7)
%
%   [L,Lc,Lli] = TOFRAMEPLUCKER(...) returns the Jacobians wrt C
%   and Li.
%
%   See also FROMFRAMEPLUCKER, TXP, HAT, CROSS, Q2R, Q2QC, T2IT.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% extract input vectors ai and bi
ai = Li(1:3);
bi = Li(4:6);

t = C.t;
q = C.q;

if nargout == 1

    % get it and Rt of inverse transform
    it = t2it(t,q);
    Rt = C.Rt;

    % compute a and b
    b  = Rt*bi;
    a  = Rt*ai + hat(it)*b;

    % Build output
    L = [a;b];

else

    % get it and iq of inverse transform
    [it,ITt,ITq]     = t2it(t,q);

    % compute a and b
    [b,Bq,Bbi]       = Rtp(q,bi);
    [txb,TXBit,TXBb] = crossJ(it,b);
    [Ra,RAq,RAai]    = Rtp(q,ai);

    a = Ra + txb;

    % Jacobians from the chain rule
    At  = TXBit*ITt;
    Aq  = RAq + TXBit*ITq + TXBb*Bq;
    Aai = RAai;
    Abi = TXBb*Bbi;

    % Build outputs
    L   = [a;b];

    Lc  = [At Aq;zeros(3) Bq];
    Lli = [Aai Abi;zeros(3) Bbi];

end

return

%%

syms a b c d x y z real
syms L1 L2 L3 L4 L5 L6 real

q = [a;b;c;d];
t = [x;y;z];
C = [t;q];
Li = [L1;L2;L3;L4;L5;L6];

[L,Lc,Lli] = toFramePlucker(C,Li);

simplify(Lc  - jacobian(L,C))
simplify(Lli - jacobian(L,Li))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function y = ft_preproc_slidingrange(dat, width, normalize, varargin)

% FT_PREPROC_SLIDINGRANGE computes the range of the data in a sliding time
% window of the width specified.
%
% Use as
%   [dat] = ft_preproc_slidingrange(dat, width, normalize)
% where
%   dat        data matrix (Nchans x Ntime)
%   width      width of the smoothing kernel, this should be an odd number since the window needs to be centered on an individual sample
%   normalize  boolean, whether to normalize the range of the data with the square root of the window size (default = false)
%
% If the data contains NaNs, these are ignored for the computation, but retained in
% the output.
%
% See also PREPROC

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

if nargin>3
  % for backward compatibility, this function was first implemented as ft_preproc_slidingrange(dat, width, ...)
  % with 'normalize' as an optional key-value pair, but all other PREPROC functions take fixed input arguments
  normalize = varargin{1};
end

if nargin<3 || isempty(normalize)
  normalize = false;
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

if mod(width+1, 2)
  ft_error('width should be an odd number');
end

% compute half width
h = (width-1)/2;

n = size(dat,2);
minval = zeros(size(dat));
maxval = zeros(size(dat));

for i=1:n
  begsample = i-h;
  endsample = i+h;
  if begsample<1
    begsample = 1;
  end
  if endsample>n
    endsample=n;
  end
  minval(:,i) = min(dat(:,begsample:endsample),[],2);
  maxval(:,i) = max(dat(:,begsample:endsample),[],2);
end

y = maxval - minval;

if istrue(normalize)
  y = y ./ sqrt(width);
end


function [meanImage] = motionCompComputeMean(view, scans, frames)

%    meanImage = motionCompComputeMean(view, [scans], [frames])
%
% gb 01/22/05
%
% Computes mean image across frames for all scans
% Default : scans = current scan
%           frames = all frames

% Initilizes arguments and variables
global dataTYPES
curType = viewGet(view,'curdatatype');

if ieNotDefined('scans')
    scans = viewGet(view,'curScan');
end
if ieNotDefined('frames')
    frames = 1:(dataTYPES(curType).scanParams(scans(1)).nFrames);
end

nVoxels = sliceDims(view,scans(1));
nSlices = numberSlices(view,scans(1));

% In order to save memory, it is necessary to compute the mean scan after scan.
%       - If the input 'scans' is a single value, it returns the mean image
%         across this scan.
%       - If the input 'scans' is an array, it calls it recursively scan
%         after scan and computes the mean of the different mean images

if length(scans) == 1

    % If 'scans' is a single value
    % Loads the whole volume
    scan = scans;
    tSeriesAllSlices = motionCompLoadImages(view, scan);

    % Computes the mean image across frames for this scan
    meanImage = mean(tSeriesAllSlices(frames,:,:,:),1);

    return

else

    % If 'scans' is an array
    % Initializes the variable
    meanImages = zeros([length(scans) nVoxels nSlices]);

    % Calls itself recursively scan after scan
    for scanIndex = 1:length(scans)
        scanNum = scans(scanIndex);

        fprintf('Computing mean for scan %d... ',scanNum);
        [meanImages(scanIndex,:,:,:)] = motionCompComputeMean(view, scanNum, frames);
        fprintf('Done\n');
    end

    % Computes the mean of the mean images
    meanImage = mean(meanImages,1);

    return
end


% Test file for trigtech/cell2mat.m

function pass = test_cell2mat(pref)

if ( nargin < 2 )
    pref = trigtech.techPref();
end

testclass = trigtech();

f = testclass.make(@(x) [sin(pi*x) cos(pi*x) exp(2*1i*pi*x)], [], pref);
g = testclass.make(@(x) sin(pi*x), [], pref);
h = testclass.make(@(x) [cos(pi*x) exp(2*1i*pi*x)], [], pref);

F = cell2mat([g h]);
pass(1) = all( sum(F - f) < max(vscale(f)*eps) );

end


function [IX0,SigmaX0,deltaMuX0,suffStat] = VBA_IX0(X0,y,posterior,suffStat,dim,u,options)
% Gauss-Newton update of initial conditions

if options.DisplayWin % Display progress
    set(options.display.hm(1),'string','VB Gauss-Newton on initial conditions... ');
    set(options.display.hm(2),'string',' ');
    drawnow
end

% check infinite precision transition pdf
iQx0 = VBA_inv(options.priors.iQx{1},options.params2update.x{1},'replace');
IN = diag(~~diag(iQx0));

% Get precision parameters
alphaHat = posterior.a_alpha./posterior.b_alpha;

% Preallocate intermediate variables
indIn = options.params2update.x0;
muX0 = options.priors.muX0;
x0 = muX0;
x0(indIn) = X0;

% Evaluate evolution function at current mode
[fx0,dF_dX0] = VBA_evalFun('f',x0,posterior.muTheta,u(:,1),options,dim,1);

% error terms
dx = IN*(posterior.muX(:,1)- fx0);
dx2 = dx'*iQx0*dx;
dx0 = muX0-x0;

% posterior covariance matrix terms
Q = options.priors.SigmaX0(indIn,indIn);
iQ = VBA_inv(Q);
iSigmaX0 = iQ + alphaHat.*dF_dX0(indIn,:)*iQx0(indIn,indIn)*dF_dX0(indIn,:)';

% posterior covariance matrix
SigmaX0 = VBA_inv(iSigmaX0);

% mode
tmp = iQ*dx0(indIn) + alphaHat.*dF_dX0(indIn,:)*iQx0(indIn,indIn)*dx;
deltaMuX0 = SigmaX0*tmp;

% variational energy
IX0 = -0.5.*dx0'*iQ*dx0 - 0.5*alphaHat.*dx2;
if VBA_isWeird (IX0)
    div = 1;
    IX0 = -Inf;
else
    div = 0;
end

% update sufficient statistics
dx20 = suffStat.dx(:,1)'*iQx0*suffStat.dx(:,1);
suffStat.dx2 = suffStat.dx2 - dx20 + dx2; % correct states squared error
suffStat.dx(:,1) = dx;
suffStat.dx0 = dx0;
suffStat.div = div;
suffStat.IX0 = IX0;


x=1:20; y=1:10; z=-10:10;
[x,y,z]=meshgrid(x,y,z);
v=x.^2.*y.*(z+1);
isosurface(x,y,z,v)


function simplex_model = find_simplex_models(p);

for i = 1:length(p)
    simplex_model(i)= 0;
    if p{i}.K.f == 0
        continue
    elseif any(p{i}.K.q > 0) | any(p{i}.K.s > 0)
        continue
    elseif p{i}.K.f ~= 1
        continue
    else
        aux = p{i};
        b = aux.F_struc(1,1);
        a = aux.F_struc(1,2:end);
        if all(abs(a) == 1)
            b = b/sign(-a(1));
            aux.F_struc(1:p{i}.K.f,:) = [];
            aux.K.f = 0;
            [aux,lower,upper] = find_simple_variable_bounds(aux);
            if all(lower == 0) & aux.K.l == 0 & all((upper == b) | isinf(upper))
                simplex_model(i)=1;
            end
        else
            continue
        end
    end
end


function	[A,W,SW,S,Q,ix_act] = ...
				compo_update_act(XX,A,W,SW,S,Q,SY,i_up,jx,ix_act)
% Update component
% XX = X(ix_act,:)*X'  [M_act x M_all]

% New alpha
a0 = A(i_up);
s0 = S(i_up);
b0 = 1 - s0./a0;	% scale factor for Q & S
qq0  = SY*sum(Q(:,i_up).^2,1);
anew = s0.^2 ./(qq0 - s0.*b0);

%if (anew - a0) < eps, return; end;

A(i_up) = anew;

% Weight  & variance update
ss = 1./(SW(jx,jx) + 1./(anew - a0));
SX = SW(jx,:);
SW = SW - ss * (SX' * SX);
SW = 0.5 * (SW + SW');	% guarantee symmetric matrix
Wj = W(:,jx) * ss;
W  = W - Wj * SX;

% Residual correlation
% XX = X(ix_act,:)*X'  [M_act x M_all]
P  = SX * XX;
S  = abs(S + ss * P.^2);
Q  = Q + Wj * P;
return