% main box sizes:
a=2;
b=1;

Nb=5; % number of boxes

% random boxes sizes:
% mab=mean([a b]);
% aa=0.05*mab+0.3*mab*rand(1,Nb);
% bb=0.05*mab+0.3*mab*rand(1,Nb);

mrg=0.95; % for margine
aa=[1    1     1     0.5  0.5];
bb=[1    0.25  0.25  0.5  0.5];
aa=aa*mrg;
bb=bb*mrg;

m2=min([aa bb]/2); % smallest half-size
AA=aa.*bb; % boxes areas

penalty=0.2*a*b;
nac=0.8; % negative area coefficient

N=5000; % population size
ng=20000; % number of generations
pmpe=0.1; % places exchange mutation probability
pmbj=0.02; % big gauss jump
pmsj=0.04; % small gauss jump
pmrr=0.1; % random rotation
pmvi=0.1; % random visible/invisible
pmne=0.2; % move to nearest adge

figure;
%ha1=axes;
ha1=subplot(2,1,1);
plot([0 a a 0 0], [0 0 b b 0],'b-');
xlim([-0.1*a 1.1*a]);
ylim([-0.1*b 1.1*b]);
set(ha1,'NextPlot','add');
ht=title(ha1,'start');
ha2=subplot(2,1,2);
drawnow;

set_cl; % set color table cl to plot boxes with different colors

% random initial population:
G=zeros(N,4*Nb);
Gch=zeros(N,4*Nb); % children
for Nc=1:N % for each individual
    G1=zeros(4,Nb); % one individual
    % G1(1,i)=1 if i-box is visible
    % G1(2,i)=1 if i-box is rotated at 90 degrees
    % G1(3,i) - x-coordinate of i-box center
    % G1(4,i) - y-coordinate of i-box center

    G1(1,:)=double(rand(1,Nb)<0.2);
    G1(2,:)=double(rand(1,Nb)<0.5);

    G1(3,:)=m2+(a-m2)*rand(1,Nb);
    G1(4,:)=m2+(b-m2)*rand(1,Nb);

    G(Nc,:)=(G1(:))'; % (G1(:))' converts matrix to row-vector
end

hi=imagesc(G,'parent',ha2);
drawnow;

Gpr1=zeros(4,Nb);
Gpr2=zeros(4,Nb); % two parents
Gch1=zeros(4,Nb);
Gch2=zeros(4,Nb); % two children
for ngc=1:ng % generations counting
    % find fitnesses:
    fitnesses=zeros(N,1);
    for Nc=1:N % for each individual
        G1(:)=(G(Nc,:))';
        vis=G1(1,:);
        ind=find(vis);
        L=length(ind);
        if L>0
            % only visible:
            rot=G1(2,ind);
            x=G1(3,ind);
            y=G1(4,ind);
            if L==1
                aaa=aa(ind);
                bbb=bb(ind);
                if rot
                    tmp=aaa;
                    aaa=bbb;
                    bbb=tmp;
                end
                A0=AA(ind); % box area
                x1=max([x-aaa/2  0]);
                y1=max([y-bbb/2  0]);
                x2=min([x+aaa/2  a]);
                y2=min([y+bbb/2  b]);
                % x1 - x2,  y1 - y2 is box (part of current box) that inside main box
                if (x1>=x2)||(y1>=y2)
                    A=0; % box that inside main box area
                else
                    A=(x2-x1)*(y2-y1); % box that inside main box area
                end
                %if A<A0 % if not fully inside main box
                if (aaa/2<=x)&&(x<=a-aaa/2)&&(bbb/2<=y)&&(y<=b-bbb/2) % if filly inside
                    fitness=A;
                else
                    fitness=A-nac*(A0-A)-penalty;
                end

            else
                fitness=0;
                ispen=false; % true if penality

                % check cross with main box:
                % add boxes arreas and strong subtract out areas:
                for n=1:L % for each box
                    ind1=ind(n);
                    aaa=aa(ind1);
                    bbb=bb(ind1);
                    if rot(n)
                        tmp=aaa;
                        aaa=bbb;
                        bbb=tmp;
                    end
                    A0=AA(ind1); % box area
                    x1=max([x(n)-aaa/2  0]);
                    y1=max([y(n)-bbb/2  0]);
                    x2=min([x(n)+aaa/2  a]);
                    y2=min([y(n)+bbb/2  b]);
                    % x1 - x2,  y1 - y2 is box (part of current box) that inside main box
                    if (x1>=x2)||(y1>=y2)
                        A=0; % box that inside main box area
                    else
                        A=(x2-x1)*(y2-y1); % box that inside main box area
                    end
                    %if A<A0 % if not fully inside main box
                        %fitness=fitness + A-nac*(A0-A);
                        %ispen=true; % penality
                    %else
                        %fitness=fitness + A;
                    %end

                    if (aaa/2<=x(n))&&(x(n)<=a-aaa/2)&&(bbb/2<=y(n))&&(y(n)<=b-bbb/2) % if filly inside
                        fitness=fitness + A;
                    else
                        fitness=fitness + A-nac*(A0-A);
                        ispen=true; % penality
                    end

                end

                % for each pair of boxes:
                for n1=1:L-1
                    ind1=ind(n1);
                    aaa1=aa(ind1);
                    bbb1=bb(ind1);
                    if rot(n1)
                        tmp=aaa1;
                        aaa1=bbb1;
                        bbb1=tmp;
                    end
                    A1=AA(ind1);
                    x1=x(n1);
                    y1=y(n1); % position of 1st box of pair
                    for n2=n1+1:L
                        ind2=ind(n2);
                        aaa2=aa(ind2);
                        bbb2=bb(ind2);
                        if rot(n2)
                            tmp=aaa2;
                            aaa2=bbb2;
                            bbb2=tmp;
                        end
                        A2=AA(ind2);
                        x2=x(n2);
                        y2=y(n2); % position of 2nd box of pair
                        dx=abs(x1-x2);
                        dy=abs(y1-y2); % distancies
                        a12=(aaa1/2+aaa2/2);
                        b12=(bbb1/2+bbb2/2);
                        if (dx<a12)&&(dy<b12) % if cross
                            ispen=true;
                            Ac=(a12-dx)*(b12-dy); % area of cross
                            fitness=fitness-Ac-Ac; % becuse area of n1 and n2 was added fully
                            fitness=fitness-2*nac*Ac;
                        end

                    end
                end

                if ispen
                    fitness=fitness-penalty;
                end

            end
        else
            fitness=0;
        end
        fitnesses(Nc)=fitness;
    end

    [fb bi]=max(fitnesses); % best

    % plot best:
    G1(:)=(G(bi,:))';
    Gb=G(bi,:); % best
    if mod(ngc,10)==0
        cla(ha1);
        Atmp=0;
        for Nbc=1:Nb
            vis1=G1(1,Nbc);
            if vis1
                rot1=G1(2,Nbc);
                aaa=aa(Nbc);
                bbb=bb(Nbc);
                if rot1
                    tmp=aaa;
                    aaa=bbb;
                    bbb=tmp;
                end
                x=G1(3,Nbc);
                y=G1(4,Nbc);
                plot([x-aaa/2  x+aaa/2  x+aaa/2  x-aaa/2  x-aaa/2],...
                     [y-bbb/2  y-bbb/2  y+bbb/2  y+bbb/2  y-bbb/2],...
                     '-','color',cl(Nbc,:),...
                     'parent',ha1);
                hold on;
                Atmp=Atmp+aaa*bbb;
            end
        end
        plot([0 a a 0 0], [0 0 b b 0],'b-','parent',ha1);
        xlim(ha1,[-0.1*a 1.1*a]);
        ylim(ha1,[-0.1*b 1.1*b]);

        set(hi,'Cdata',G);

        nvb=length(find(G1(1,:))); % number of visible boxes

        set(ht,'string',[' generation: ' num2str(ngc)  ', boxes: ' num2str(nvb) ', area: ' num2str(fb)]);

        drawnow;
    end

    % prepare for crossover, selection:
    fmn=min(fitnesses);
    fst=std(fitnesses);
    if fst<1e-7
        fst=1e-7;
    end
    fmn1=fmn-0.01*fst; % little low then minimum
    P=fitnesses-fmn1; % positive values
    p=P/sum(P); % probabilities
    ii=roulette_wheel_indexes(N,p);
    Gp=G(ii,:); % parents

    % crossover:
    for n=1:2:N
        pr1=Gp(n,:);
        pr2=Gp(n+1,:); % two parents
        % in matrix form:
        Gpr1(:)=pr1';
        Gpr2(:)=pr2';

        for Nbc=1:Nb

            % visibility:
            if rand<0.5
                Gch1(1,Nbc)=Gpr1(1,Nbc);
            else
                Gch1(1,Nbc)=Gpr2(1,Nbc);
            end
            if rand<0.5
                Gch2(1,Nbc)=Gpr1(1,Nbc);
            else
                Gch2(1,Nbc)=Gpr2(1,Nbc);
            end

            % rotation:
            if rand<0.5
                Gch1(2,Nbc)=Gpr1(2,Nbc);
            else
                Gch1(2,Nbc)=Gpr2(2,Nbc);
            end
            if rand<0.5
                Gch2(2,Nbc)=Gpr1(2,Nbc);
            else
                Gch2(2,Nbc)=Gpr2(2,Nbc);
            end

            % position:
            % child 1:
            %i3=ceil(3*rand);
            %i3=roulette_wheel_indexes(1,[0.2 0.4 0.4]);
            i3=1+ceil(2*rand);
            switch i3
                case 1 % get mean position
                    Gch1(3,Nbc)=(Gpr1(3,Nbc)+Gpr2(3,Nbc))/2;
                    Gch1(4,Nbc)=(Gpr1(4,Nbc)+Gpr2(4,Nbc))/2;
                case 2 %get position of parent 1
                    Gch1(3,Nbc)=Gpr1(3,Nbc);
                    Gch1(4,Nbc)=Gpr1(4,Nbc);
                case 3 %get position of parent 2
                    Gch1(3,Nbc)=Gpr2(3,Nbc);
                    Gch1(4,Nbc)=Gpr2(4,Nbc);
            end
            % child 2:
            %i3=ceil(3*rand);
            %i3=roulette_wheel_indexes(1,[0.2 0.4 0.4]);
            i3=1+ceil(2*rand);
            switch i3
                case 1 % get mean position
                    Gch2(3,Nbc)=(Gpr1(3,Nbc)+Gpr2(3,Nbc))/2;
                    Gch2(4,Nbc)=(Gpr1(4,Nbc)+Gpr2(4,Nbc))/2;
                case 2 %get position of parent 1
                    Gch2(3,Nbc)=Gpr1(3,Nbc);
                    Gch2(4,Nbc)=Gpr1(4,Nbc);
                case 3 %get position of parent 2
                    Gch2(3,Nbc)=Gpr2(3,Nbc);
                    Gch2(4,Nbc)=Gpr2(4,Nbc);
            end

        end
        ch1=(Gch1(:))';
        ch2=(Gch2(:))';
        Gch(n,:)=ch1;
        Gch(n+1,:)=ch2;

    end
    G=Gch; % now children

    % mutations:
    % places exchange
    for Nc=1:N % for each individual
        if rand<pmpe
            G1(:)=(G(Nc,:))';
            ir1=ceil(Nb*rand);
            ir2=ceil(Nb*rand);
            tmp1=G1(3:4,ir1);
            G1(3:4,ir1)=G1(3:4,ir2);
            G1(3:4,ir2)=tmp1;
            G(Nc,:)=(G1(:))';
        end
    end

    % big gauss jump:
    for Nc=1:N % for each individual
        if rand<pmbj
            G1(:)=(G(Nc,:))';
            ir=ceil(Nb*rand);
            G1(3:4,ir)=G1(3:4,ir)+[0.05*a*randn;
                                   0.05*b*randn];
            G(Nc,:)=(G1(:))';
        end
    end

    % small gauss jump:
    for Nc=1:N % for each individual
        if rand<pmsj
            G1(:)=(G(Nc,:))';
            ir=ceil(Nb*rand);
            G1(3:4,ir)=G1(3:4,ir)+[0.005*a*randn;
                                   0.005*b*randn];
            G(Nc,:)=(G1(:))';
        end
    end

    % random rotation:
    for Nc=1:N % for each individual
        if rand<pmrr
            G1(:)=(G(Nc,:))';
            ir=ceil(Nb*rand);
            G1(2,ir)=double(rand<0.5);
            G(Nc,:)=(G1(:))';
        end
    end

    % random visible/invisible:
    for Nc=1:N % for each individual
        if rand<pmvi
            G1(:)=(G(Nc,:))';
            ir=ceil(Nb*rand);
            G1(1,ir)=double(rand<0.5);
            G(Nc,:)=(G1(:))';
        end
    end

    % move to nearest edge:
    for Nc=1:N % for each individual
        if rand<pmne
            G1(:)=(G(Nc,:))';
            ir=ceil(Nb*rand); % random small box
            rv=find((G1(1,:))&((1:Nb)~=Nc)); % find rest visible
            if rand<0.5
                % to veritcile edge
                eax=[G1(3,rv)-aa(rv)/2  G1(3,rv)+aa(rv)/2  0  a]; % edge xs
                deax=[(G1(3,ir)-aa(ir)/2) - eax  (G1(3,ir)+aa(ir)/2) - eax]; % distancies
                [dmn indm]=min(abs(deax));
                G1(3,ir)=G1(3,ir)-deax(indm);
            else
                % to horizontal edge
                eay=[G1(4,rv)-bb(rv)/2  G1(4,rv)+bb(rv)/2  0  b]; % edge ys
                deay=[(G1(4,ir)-bb(ir)/2) - eay  (G1(4,ir)+bb(ir)/2) - eay]; % distancies
                [dmn indm]=min(abs(deay));
                G1(4,ir)=G1(4,ir)-deay(indm);
            end
        end
    end

    % ellitism:
    G(1,:)=Gb;

end


% Test of subdivision of a base polyhedron.

mesh_types = {'tetra','oct','ico'};
nmesh = length(mesh_types);
nsub = 3;

clf;
for i=1:nmesh
    mesh_type = mesh_types{i};
    [vertex,face] = compute_base_mesh(mesh_type);
    for s=0:nsub
        subplot(nmesh,nsub+1,s+1+(nsub+1)*(i-1));
        plot_mesh(vertex,face);
        lighting flat;
        if s~=nsub
			options.spherical = 1;
            options.relaxation = 3;
            [vertex,face] = perform_mesh_subdivision(vertex,face,1,options);
        end
    end
end


function plotprmm(catalogObject)
    %CATALOG.PLOTPRMM Plot the peak rate and maximum magnitude of a
    %set of events
    figure
    symsize = get_symsize(catalogObject);
    t=catalogObject.gettimerange();
    days = t(2) - t(1);
    if all(isnan(catalogObject.mag))
        warning('No magnitude data to plot');
    else

        % plot magnitudes
        subplot(2,1,1), scatter(catalogObject.otime, catalogObject.mag, symsize);
        %stem(catalogObject.otime, catalogObject.mag);
        set(gca, 'XLim', [floor(t(1)) ceil(t(2))]);
        datetick('x');
        xlabel('Date');
        ylabel('Magnitude');
        grid on;

        % put 'MM' label by max mag event
        [mm, mmi] = max(catalogObject.mag);
        text(catalogObject.otime(mmi), catalogObject.mag(mmi), 'MM','color','r');
        disp(sprintf('MM=%.1f occurs at %.1f%% of time series',mm,100*(catalogObject.otime(mmi) - t(1))/days));

        % plot event rate in 100 equal bins

        binsize = days/100;
        erobj = catalogObject.eventrate('binsize',binsize);
        %erobj = catalogObject.eventrate();
        subplot(2,1,2),plot(erobj.time, erobj.counts);
        set(gca, 'XLim', [floor(t(1)) ceil(t(2))]);
        datetick('x');
        xlabel('Date');
        ylabel('Event Rate');
        grid on;

        % put 'PR' label by peak-rate
        [pr, pri] = max(erobj.counts);
        text(erobj.time(pri), erobj.counts(pri), 'PR','color','r');
        disp(sprintf('PR=%d occurs at %.1f%% of time series',pr,100*(erobj.time(pri) - erobj.snum)/(erobj.enum-erobj.snum)));
    end
end


%% spmax
% Below is a demonstration of the features of the |spmax| function

%%
clear; close all; clc;

%% Syntax
% |[maxVal,maxInd]=spmax(A,B,dim,nanflag,logicRelevant,nanOut);|

%% Description
% This function is like the max function but is designed for sparse arrays.
% In particular it allows one to "ignore zeros" in the determination of the
% maxima.

%% Examples
%

%%
% Create example matrix
i=[2 1 1 2  2 3  3 4 4 5  5 5 6 6 7 8];
j=[1 1 2 3  4 5  6 7 8 9 10 11 12 13 13 13];
s=[-1 3 1 2 -1 1 -2 5 5 -1 0 2 3 10 11 NaN];
siz=max([i(:);j(:)]+1)*ones(1,2);
A=sparse(i,j,s,siz(1),siz(2),numel(s));
A=A+A';

full(A) % View matrix

L=sparse(i,j,1,siz(1),siz(2),numel(s));
logicRelevant=(L+L')>0;

%%
% Compute maxima allong a certain direction (while omit nan is default)

amaxRows=spmax(A,[],1);
full(amaxRows)

amaxColumns=spmax(A,[],2);
full(amaxColumns)

%%
% Including nans

amaxRows=spmax(A,[],1,'includenan');
full(amaxRows)

amaxColumns=spmax(A,[],2,'includenan');
full(amaxColumns)

%%
% Computing maxima across all desired relevant entries (including
% "relevant/real zeros")

amaxRows=spmax(A,[],1,'omitnan',logicRelevant);
full(amaxRows)

amaxColumns=spmax(A,[],2,'omitnan',logicRelevant);
full(amaxColumns)

%%
% Computin maxima across all desired relevant entries and output NaN where
% the sparse array only contains "non-relevant or non-real" zeros.

nanOut=1;

amaxRows=spmax(A,[],1,'omitnan',logicRelevant,nanOut);
full(amaxRows)

amaxColumns=spmax(A,[],2,'omitnan',logicRelevant,nanOut);
full(amaxColumns)

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [u,v]=pow2cep(m,c,mode)
%CEP2POW convert cepstral means and variances to the power domain
% Inputs:
%    m: vector giving means in the power domain
%    c: covariance matrix in the power domain
% mode: 'c'  pow=exp(irdct(cep))   [default]
%       'f'  pow=exp(rsfft(cep)/n)  [fft length even]
%       'fo' pow=exp(rsfft(cep)/n)  [fft length odd]
%       'i'  pow=exp(cep)           [ no transformation ]
%
% Outputs:
%    u: row vector giving the cepstral means with u(1) the 0'th cepstral coefficient
%    v: cepstral covariance matrix

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: pow2cep.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3 mode='c'; end
if min(size(c))==1
   v=diag(c);
end
m=m(:)';        % force to be a row vector
q=log(1+c./(m'*m));
p=log(m)-0.5*diag(q)';
if any(mode=='f')
   n=2*length(m)-2;
   if any(mode=='o')
      n=n+1;
   end
   u=rsfft(p,n);
   v=rsfft(rsfft(q,n)',n);
elseif any(mode=='i')
    u=p;
    v=q;
else
   u=rdct(p);
   v=rdct(rdct(q)');
end


function plotCPs(CPs)
% DESCRIPTION
% Plot Contribution Plots
%
%    plotCPs(CPs)
%
% INPUT
%   CPs         Contribution Plots
%
% Created on 18th April 2019, by Kepeng Qiu.
%-------------------------------------------------------------%

%
figure
temp = mean(abs(CPs),1);
bar(temp/sum(temp,2));

% Axis settings
tgca = 16;  % font size
tfont = 'Helvetica'; % font type
% tfont = 'Arial'; % font type
% set(gca,'yscale','log')
set(gca,'FontSize',tgca,'FontName',tfont)

% legend settings
tlegend = tgca*0.9;
legend({'Contribution Plots'},'FontSize',tlegend , ...
    'FontWeight','normal','FontName',tfont)

% label settings
tlabel = tgca*1.1;
xlabel('Variable','FontSize',tlabel,'FontWeight','normal', ...
    'FontName',tfont,'Color','k')
ylabel('CPs','FontSize',tlabel,'FontWeight','normal', ...
    'FontName',tfont,'Color','k')

end


function [ a, b ] = p06_ab ( m )

%*****************************************************************************80
%
%% P06_AB returns bounds for problem 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Output, real A(M,1), B(M,1), lower and upper bounds.
%
  a(1:m,1) = 0.0;
  b(1:m,1) = 1.0;

  return
end


  function ss = outer_sum(xx,yy)
%|function ss = outer_sum(xx,yy)
%|
%| compute an "outer sum" x + y'
%| that is analogous to the "outer product" x * y'
%|
%| in
%|	xx	[nx 1]
%|	yy	[1 ny]
%|		more generally: xx [(dim)] + yy [L,1] -> xx [(dim) LL]
%| out
%|	ss [nx ny]	ss(i,j) = xx(i) + yy(j)
%|
%| Copyright 2001, Jeff Fessler, University of Michigan

if ~nargin, ir_usage, end
if streq(xx, 'test'), outer_sum_test, return, end

% for 1D vectors, allow rows or cols for backward compatibility
if ndims(xx) == 2 && min(size(xx)) == 1 && ndims(yy) == 2 && min(size(yy)) == 1
	nx = length(xx);
	ny = length(yy);
	xx = repmat(xx(:), [1 ny]);
	yy = repmat(yy(:)', [nx 1]);
	ss = xx + yy;
return
end

%if size(xx,1) == 1
%	warn 'xx is a row vector? are you sure?'
%end

% otherwise, xx is not a vector, but yy must be

xdim = size(xx);
ydim = size(yy);
if ndims(yy) ~= 2 || min(size(yy)) ~= 1
	error 'yy must be a vector'
end
sdim = [xdim length(yy)];
xo = repmat(xx, [ones(1, length(xdim)) length(yy)]); % [xdim] -> [xdim ny]
yo = repmat(yy(:), [1 xdim]); yo = permute(yo, [2 3 1]);
% yo = repmat(yy(:)', [xdim 1 1]); % not sure how to make this work.
ss = xo + yo;

function outer_sum_test
%xx = [1:4];
%yy = [0:10:50];
%pr outer_sum(xx,yy)
pr 'outer_sum([1:4], [0:10:50])'
xx = outer_sum([1:4], [0:10:50]);
pr 'outer_sum(xx, [100 200])'


function superima = QLsuperimage(C, S, iscale)
%------------------------------------------------------------------------------
% Creates an image that goes with a multiresolutiondecomposition that has been
% made in the way of LISQ. The superimage, that is an image that comprises
% all levels, is constructed as a rectangular gridfunction.
%
% C        is a one-dimensional array that contains the coefficients of the
%          Quincunx Lifting Scheme (LISQ) decompositions.
% S        is the bookkeeping vector
% iscale   optional inputparameter
%          1 show superimage as is (default)
%          2 show superimage after removal of outliers
%          3 show superimage after enhancement of contrast using
%            histogram equalisation (if available)
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: November 24, 2003.
%  2003 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
 if nargin == 3
   iopt = iscale;
 elseif nargin == 2
   iopt = 1;
 else
   error(' QLsuperimage - number of arguments should be either 2 or 3 ');
 end
%
 [nS, mS] = size(S);
 if mS ~= 6
   error(' QLsuperimage - unexpected dimensions of bookkeeping vector ');
 end
levels = S(nS,1);
if mod(levels, 2) == 1
  error('  QLsuperimage - only an even number of levels is accepted ');
elseif levels < 1
  superima = [];
else
%
% Firstly, we determine the dimensions of the superimage to be and create the
% necessary space (see LISQ/storeR.m and LISQ/QLiftRec2Nevill.m)
%
  [Detail10, Detail01] = retrieveQ1001(1, 'd', C, S);
  sizeR = size(Detail10) + size(Detail01);
  n  = sizeR(1); m = sizeR(2);
  nH = round((n+m+1.999)/2)+round((m+0.999)/2);
  nm = round((n+m+1.999)/2)+1;
  nV = 1;
  levels = S(nS,1);
  for k = 1:2:levels
    nV = nV + nm;
    nm = round((nm+0.999)/2);
  end
  Approx = retrieveR(levels, 'a', C, S);
  [nl, ml] = size(Approx); nV = nV + nl;
  superima = ones(nV, nH);
%
% Secondly, we fill the superimage with all subsequent detail coefficients
% Put the details at the rotated quincunx grid (level 1)
  background=max(max(max(Detail10)), max(max(Detail01)));
  RotaDet=rota1001fill(Detail10, Detail01, background);
  clear Detail10 Detail01;
%-----intermezzo---begin--
  hgram=ones(1,64);
  hgram(64)=round(31.5*(n/m+m/n))+1;
% hgram is a histogram that is desired to be matched by the image after
% its values have been transformed by histeq, see the documentation of
% histeq() in the Matlab Image Processing Toolbox.
% Due to the rotation of the quincunx grid to make it visible, there
% is a substantial area for padding with one colour (white or nearly
% white). Here follows an overview of the quantities of pixels involved
% at the rotated quincunx grid (asymptotically):
%  Let n and m be the dimensions of the original image.
%
%                               2
%                      ( n + m )
%  Grand total:         -------
%                          4
%
%                         n m
%  Used pixels:           ---
%                          2
%
%                        2    2
%                       n  + m
%  Pixels for padding:  --------
%                          4
%
%                            2    2
%                  n m      n  + m                 n   m
%  Used:padding =  ---   :  -------  = 63 : 31.5*( - + - )
%                   2          4                   m   n
%
%-----intermezzo---end----
  [nl, ml] = size(RotaDet);
  OrigV=1; OrigH=1;
  superima(OrigV:(OrigV+nl-1), OrigH:(OrigH+ml-1)) = imequi(RotaDet,iopt,hgram);
  Detail11 = retrieveR(2, 'd', C, S);
  OrigV=1; OrigH=ml+1;
  [nr, mr] = size(Detail11);
  superima(OrigV:(OrigV+nr-1), OrigH:(OrigH+mr-1)) = imequi(Detail11,iopt);
% Now the other levels, if any
  OrigV=OrigV+nl; OrigH=1;
  for k = 3:2:levels
%   Put the details at the rotated quincunx grid
    [Detail10, Detail01] = retrieveQ1001(k, 'd', C, S);
    background=max([max(max(Detail10))  max(max(Detail01))]);
    RotaDet = rota1001fill(Detail10, Detail01, background);
    clear Detail10 Detail01;
    [nl, ml] = size(RotaDet);
    superima(OrigV:(OrigV+nl-1), OrigH:(OrigH+ml-1)) = imequi(RotaDet,iopt,hgram);
    Detail11 = retrieveR(k+1, 'd', C, S);
    OrigH=ml+1;
    [nr, mr] = size(Detail11);
    superima(OrigV:(OrigV+nr-1), OrigH:(OrigH+mr-1)) = imequi(Detail11,iopt);
    clear Detail11;
    OrigV=OrigV+nl; OrigH=1;
  end
% Thirdly, we fill the superimage with approximation coefficients.
  [nl, ml] = size(Approx);
  superima(OrigV:(OrigV+nl-1), OrigH:(OrigH+ml-1)) = imequi(Approx,iopt);
end
%------------------------------------------------------------------------------


function [C, S] = QLiftDec2MaxMin(X, N)
%-----------------------------------------------------------------------------
% QLiftDec2MaxMin
% Multilevel 2-D decomposition by the lifting scheme and using quincunx grids
%
% The MaxMin scheme has been proposed by Heijmans and Goutsias, see e.g.
%    H.J.A.M. Heijmans, J. Goutsias,
%    Multiresolution signal decomposition schemes.
%    Part 2: morphological wavelets.
%    CWI Report PNA-R9905, Amsterdam, 1999.
%    http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04625D.pdf
%
% Calls for: QLmaxlev,
%            storeQ1001, storeR,
%            getcolor01, getcolor10, getcolor00, getcolor11,
%            putcolor01, putcolor10, putcolor00, putcolor11.
% See also: QLiftRec2MaxMin
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: May 16, 2002.
% (c) 1999-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
%Firstly, check input data
%
if  isempty(X)
  error(' QLiftDec2MaxMin - empty matrix ');
else
  if mod(N, 2) == 1
    error(' QLiftDec2MaxMin - only an even number of levels is accepted ');
  end
  if QLmaxlev(size(X), 'maxmin') < N
    error(' QLiftDec2MaxMin - too many levels requested ');
  end
  if N < 2
    disp([' QLiftDec2MaxMin - WARNING too few levels requested ' ...
          '-> empty decomposition ']);
  end
end
%
%Secondly, start decomposition
%
O = X; % For the sake of efficient use of memory this could be improved upon.
% We descend to coarser grids, integer lev indicates number of scale.
C = []; S = [];

for lev=1:2:N
%
   [nO, mO] = size(O);
   if ( nO < 3 ) || ( mO < 3)
     error(' QLiftDec2MaxMin - too many levels ');
   end
   minO = min(min(O));
   maxO = max(max(O));
   cmin = minO-(maxO-minO);
   cmax = maxO+(maxO-minO);
%
%  The Lifting Scheme proceeds from a rectangular grid
%  towards a quincunx grid.
%
%  Stage: predict
   A00 =getcolor00(O);
   A11 =getcolor11(O);
%  Quincunx grid Q0011 is the union of the values at .00 and .11: "even slots"
%  Quincunx grid Q1001 is the union of the values at .10 and .01: "odd slots"
   Q1001D01 = getcolor01(O) - synA01max(A11, A00, cmin);                % Y1
   Q1001D10 = getcolor10(O) - synA10max(A11, A00, cmin);                % Y1
%  At this point the union (quincunx) of Q1001D01 & Q1001D10
%  contains the DETAILS of O.
%
%  For the inverse transform Q1001D01 and Q1001D10 have to be stored:
   [C, S] = storeQ1001( Q1001D10, Q1001D01, lev, 'd', C, S);
%
%  Stage: update
   Q0011A00 = A00 + ...
        max(zeros(size(A00)), synA00max(Q1001D10, Q1001D01, cmin));     % X1
   clear A00;
   Q0011A11 = A11 + ...
        max(zeros(size(A11)), synA11max(Q1001D10, Q1001D01, cmin));     % X1
   clear A11 Q1001D10 Q1001D01;
%  At this point the union (quincunx) of Q0011A00 & Q0011A11
%  contains the updated APPROXIMATION of O, the DETAILS of O
%  were in the union (quincunx) of Q1001D01 & Q1001D10 (see above).
%
%  The Lifting Scheme proceeds by a subsequent step from quincunx
%  to rectangular grid.
%
%  Q0011 is split into the 11 colour with the "odd slots" and
%  the 00 colour with the "even slots".
%
%  Stage: predict
   DETAIL11 = Q0011A11 - synA11Qmin(Q0011A00, size(Q0011A11), cmax);        % Y2
   clear Q0011A11;
%  Stage: update
   APPROX00 = Q0011A00 + ...
     min(zeros(size(Q0011A00)), synA00Qmin(DETAIL11, size(Q0011A00), cmax));% X2
%
%  DETAIL11 presents the detail gridfunction w.r.t. Q0011
%  APPROX00 now represents the updated version of the approximation of Q0011
%
%  For the inverse transform DETAIL11 has to be stored:
   [C, S] = storeR( DETAIL11, lev+1, 'd', C, S);
   clear Q0011A00 DETAIL11;
%
%  At this point gridfunction DETAIL11 containing the DETAILS has been stored,
%  gridfunction APPROX00 contains the updated APPROXIMATION, on the (down-
%  sampled) rectangular grid and has to be stored as well if at the highest
%  scale.
%  Note that APPROX00 is downsampled onto a rectangular grid with dimensions of
%  half size of the original O.
   if lev+1 >= N
     [C, S] = storeR(APPROX00, lev+1, 'a', C, S);
%    It is obligatory that at least at one scale the Approximation has to be
%    stored or else the scheme cannot be inverted.
     clear APPROX00;
%    In the Lifting Scheme all scales have now been processed!
   else
%    We proceed to the next scale.
     O = APPROX00; clear APPROX00;
   end
end
%-----------------------------------------------------------------------------


function [slice] = spm_vb_taylor_R(Y,slice)
% Get Taylor series approximation to posterior correlation matrices
% FORMAT [slice] = spm_vb_taylor_R(Y,slice)
%
% Y        - data
% slice    - VB-GLMAR data structure
%
% See paper VB3.
%__________________________________________________________________________
% Copyright (C) 2005-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_vb_taylor_R.m 6079 2014-06-30 18:25:37Z spm $

% Get mean hyperparameter values
h0 = [];
if slice.p > 0
    if size(slice.ap_mean,2)==1
        % Single voxel in slice
        a      = slice.ap_mean';
        a_cov  = slice.a_cov{1};
    else
        a      = mean(slice.ap_mean');
        a_covs = cat(3,slice.a_cov{:});
        a_cov  = mean(a_covs,3);
    end
    slice.mean.a     = a;
    slice.mean.a_cov = a_cov;
    h0 = a';
end
slice.mean.b = mean(slice.b');

lambda = mean(slice.mean_lambda);
slice.mean.lambda = lambda;
h0 = [h0;lambda];

R = spm_vb_get_R(slice,h0);
slice.mean.R = R;

delta = 0.0001;
% Get first order Taylor terms about slice
% mean values of a and lambda
for i=1:length(h0)
    h    = h0;
    h(i) = h(i)-delta;

    [R1] = spm_vb_get_R(slice,h);

    h    = h0;
    h(i) = h(i) + delta;
    % Loop over hyperparameters
    R2   = spm_vb_get_R(slice,h);

    dR(:,:,i) = (R2-R1) / (2*delta);
end
slice.mean.h0 = h0;
slice.mean.dR = dR;


function [fYr, nMn, nDay, nHr, nMin, nSec]=decyear2mat(fDy)
    % Calculate decimal year format to matrix with columns year, month, day, hour, minute, and second.
    % [fYr, nMn, nDay, nHr, nMin, nSec] = decyear2mat(fDy)
    % ------------------------------------------------------------------------------------------------
    % Calculate decimal year format (from zmap-function decyear, i.e. 1998.734)
    % to matrix with columns year, month, day, hour, minute, and second.
    % This function was programmed because datevec.m does not work with decimal
    % year input format
    %
    % Input parameters:
    %   fDy     Decimal year (like 1998.2515) as vector or float
    %
    % Output parameters:
    %   fYr     decimal year
    %   nMn     month
    %   nDay    day
    %   nHr     hour
    %   nMin    minute
    %   nSec    second
    %
    % Example [fYr, nMn, nDay, nHr, nMin, nSec]=decyear2mat(decyear([1989 12 31 10 35 44.3]))
    %
    % Thomas van Stiphout
    % Mai 9, 2007

    disp('~/zmap/src/decyear2mat.m')

    % save year in decimal format
    fYr=fDy;
    % define leap years
    bLeapYr = rem(fix(fDy),4) == 0 & rem(fix(fDy),100) ~= 0 | rem(fix(fDy),400) == 0 ;

    % loop over each date
    for i=1:size(bLeapYr,1)
        if bLeapYr(i) % for leap years
            mDay=[0,31,60,91,121,152,182,213,244,274,305,335]'; %leapyear
            nMn(i)=sum(mDay<rem(fDy(i),1).*366); % calculate year
            nDay(i)=ceil(rem(fDy(i),1).*366)-mDay(nMn(i)); % calculate days
            nHr(i)=rem(fDy(i),1).*366.*24-(mDay(nMn(i))+nDay(i)-1).*24;
            nMin(i)=rem(nHr(i),1).*60;
            nHr(i)=fix(nHr(i)); % hours
            nSec(i)=rem(nMin(i),1).*60; % seconds
            nMin(i)=fix(nMin(i)); % minutes

        else
            mDay= [0,31,59,90,120,151,181,212,243,273,304,334]';%cumulative days in one year
            nMn(i)=sum(mDay<rem(fDy(i),1)*365);
            nDay(i)=ceil(rem(fDy(i),1)*365)-mDay(nMn(i));
            nHr(i)=rem(fDy(i),1)*365*24-(mDay(nMn(i))+nDay(i)-1)*24;
            nMin(i)=rem(nHr(i),1)*60;
            nHr(i)=fix(nHr(i));
            nSec(i)=rem(nMin(i),1)*60;
            nMin(i)=fix(nMin(i));
        end
    end
end


% I think this script was written before understanding that rudder derivatives
% are readily available from Tornado (?).
% Compute the first order derivative of all coefficients with respect to
% elevator (horizontal stabilizer rudder) deflection at different alphas. Yields
% derivative of coefficients per radian.
% Tornado\T135_export has to be in the path
% See also solverloop5.m line 312 on how to to a sweep over rudder deflections.

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

currDir = pwd;
cd(fileparts(which(mfilename)));
scriptDir = pwd;

% Load the geometry and the initial state
load('../aircraft/ExperimentalCarrier.mat')
load('../aircraft/State_1.5alpha_12.5ms.mat')

% Must run in tornado root directory for loading airfoil profile later
cd('F:\svn\dev\matlab\tornado\T135_export')

% Alpha range
alphaStart = -10/180*pi;
alphaEnd = 20/180*pi;
numAlphas = 31; % must be > 1

resultsPos = [];
resultsNeg = [];
settings=config('startup');

%lattictype=1; %Standard VLM
lattictype=0;%Tornado freestream following wake VLM

alphaStep = (alphaEnd - alphaStart)/(numAlphas-1);
alpha = zeros(numAlphas,1);
CXdr = zeros(numAlphas, 1);
CYdr = zeros(numAlphas, 1);
CZdr = zeros(numAlphas, 1);
Cldr = zeros(numAlphas, 1);
Cmdr = zeros(numAlphas, 1);
Cndr = zeros(numAlphas, 1);

[n,m]=find(geo.flapped');
resetdelta=geo.flap_vector;
positiveFlap = 1*pi/180; % in radian
negativeFlap = -1*pi/180; % in radian
flapDiff = positiveFlap - negativeFlap;
rudder = 1; % the first rudder is on the horizontal stabilizer in ExperimentalCarrier geometry.

%% loop over alphas
for alphaIndex=1:numAlphas

    % Modify state
    alphaCurrent = alphaStart + (alphaIndex-1)*alphaStep;
    state.alpha = alphaCurrent;
    alpha(alphaIndex) = alphaCurrent;
    state.betha = 0.0;

    %% Positive Flap
    % Modify geometry
    geo.flap_vector(m(rudder),n(rudder))=positiveFlap;
    % Regenerate lattice.
    [lattice,ref]=fLattice_setup2(geo,state,lattictype);
    % Compute the solution
    [resultsPos]=solver9(resultsPos,state,geo,lattice,ref);
    [resultsPos]=coeff_create3(resultsPos,lattice,state,ref,geo);

    %% Negative Flap
    % Modify geometry
    geo.flap_vector(m(rudder),n(rudder))=negativeFlap;
    % Regenerate lattice.
    [lattice,ref]=fLattice_setup2(geo,state,lattictype);
    % Compute the solution
    [resultsNeg]=solver9(resultsNeg,state,geo,lattice,ref);
    [resultsNeg]=coeff_create3(resultsNeg,lattice,state,ref,geo);

    %% Derivative

    CXdr(alphaIndex, 1) = (resultsPos.CX - resultsNeg.CX)/flapDiff;
    CYdr(alphaIndex, 1) = (resultsPos.CY - resultsNeg.CY)/flapDiff;
    CZdr(alphaIndex, 1) = (resultsPos.CZ - resultsNeg.CZ)/flapDiff;
    Cldr(alphaIndex, 1) = (resultsPos.Cl - resultsNeg.Cl)/flapDiff;
    Cmdr(alphaIndex, 1) = (resultsPos.Cm - resultsNeg.Cm)/flapDiff;
    Cndr(alphaIndex, 1) = (resultsPos.Cn - resultsNeg.Cn)/flapDiff;

    %% Output
    disp(['alpha ' num2str(alphaCurrent*180/pi) ' deg']);
end

geo.flap_vector=resetdelta;

cd(currDir);

save

alphaDegrees = alpha*180/pi;

figure(23);
subplot(2,3,1);
plot(alphaDegrees,CXdr);
title('CXdr (longitudinal)');
xlabel('alpha');
zlabel('longitudinal coefficient derivative');

subplot(2,3,2);
plot(alphaDegrees,CYdr);
title('CYdr (lateral)');
xlabel('alpha');
zlabel('lateral coefficient derivative');

subplot(2,3,3);
plot(alphaDegrees, CZdr);
title('CZdr (lift)');
xlabel('alpha');
zlabel('lift force coefficient derivative');

subplot(2,3,4);
plot(alphaDegrees, Cldr);
title('Cldr (roll)');
xlabel('alpha');
zlabel('roll moment coefficient derivative');

subplot(2,3,5);
plot(alphaDegrees, Cmdr);
title('Cmdr (pitch)');
xlabel('alpha');
zlabel('pitch moment coefficient derivative');

subplot(2,3,6);
plot(alphaDegrees, Cndr);
title('Cndr (yaw)');
xlabel('alpha');
zlabel('yaw moment coefficient derivative');


function rhs = globNedFitRHS3D(fm,fp, mesh, fem, dind, vind)

%% USAGE: generate global load vector on a tetrahedral mesh
%
% INPUTS:
% fun --- the load function from PDE (pde.f)
% mesh --- a struct data contains mesh information.
% fem --- global DoF for test function space
% dind --- derivative info for test function
%            d = [0,0,0]: function value
%            d = [1,0,0]: Dx value
%            d = [0,1,0]: Dy value
%            d = [0,0,1]: Dz value
%
% OUTPUTS:
% rhs --- global rhs vector

% Last Modified: 08/07/2020 by Xu Zhang

%% 1. RHS on non-interface elements
dof = fem.ldof;  nloc = dof; nt = length(mesh.t);
A = fem.area; gw = fem.gw; gx = fem.gx; gy = fem.gy; gz = fem.gz;
X = zeros(nloc*nt, 1);

feEvalBas = @EvalNed1Bas3D;

f = feval(fm,gx,gy,gz);
tId2 = (mesh.tLoc == 2);
f(tId2,:) = feval(fp,gx(tId2,:),gy(tId2,:),gz(tId2,:));
ind = 0;
I = reshape(fem.g2ldof,nloc*nt,1);
for i = 1:dof
    ibas = feEvalBas(fem.bas, ':', gx, gy, gz, i, dind, vind);
    X(ind+1:ind+nt) = A.*sum((ibas.*f).*gw',2).*fem.t_e_orit(:,i);
    ind = ind + nt;
end
rhs = sparse(I,1,X,length(fem.gdof),1);


function result = convolutionalPropagation(edges, edgeWeights, fixedVertices, ...
                    fixedDists,areaWeights,kernel,kernelTranspose,entropyLimit)

ne = size(edges,1);
nv = max(edges(:));
n = size(fixedDists,1);

% Initialize marginals
vv = ones(n,ne);
ww = ones(n,ne);
d = zeros(n,ne);

result = ones(n,nv);
result(:,fixedVertices) = fixedDists;

% For convenience, a binary array
isBoundary = zeros(nv,1);
isBoundary(fixedVertices) = 1;

% For each vertex compute his incoming and outgoing edges
incomingEdges = cell(nv,1);
outgoingEdges = cell(nv,1);

for i=1:ne
    e1 = edges(i,1);
    e2 = edges(i,2);
    outgoingEdges{e1} = [outgoingEdges{e1} i];
    incomingEdges{e2} = [incomingEdges{e2} i];
end

% No reason to project a distribution whose neighbors haven't been projected
queue = zeros(nv,1);
queue(fixedVertices) = 1;

projectEntropy = 0;

for j=1:1000
    toVisit = find(queue);

%     if mod(j,2)==0
%         toVisit = toVisit(end:-1:1); % reverse order each time for fun
%     end
%     toVisit = toVisit(randperm(length(toVisit))); % to avoid bias

    % Reset queue
    queue = zeros(nv,1);
    queue(fixedVertices) = 1;

    oldResult = result;

    for k=1:length(toVisit)
        v = toVisit(k);

        if isBoundary(v)
            p = result(:,v);

            for i=1:length(outgoingEdges{v}) %(v,w)
                e = outgoingEdges{v}(i);
                ww(:,e) = p ./ kernelTranspose(vv(:,e).*areaWeights);
            end

            for i=1:length(incomingEdges{v}) %(w,v)
                e = incomingEdges{v}(i);
                vv(:,e) = p ./ kernel(ww(:,e).*areaWeights);
            end
        else % not boundary
            omega = sum(edgeWeights(outgoingEdges{v}))+sum(edgeWeights(incomingEdges{v}));
            p = ones(n,1);

            for i=1:length(outgoingEdges{v}) %(v,w)
                e = outgoingEdges{v}(i);
                d(:,e) = ww(:,e).*kernelTranspose(vv(:,e).*areaWeights);
                d(:,e) = max(d(:,e),1e-10);
                p = p .* d(:,e).^(edgeWeights(e)/omega);
            end

            for i=1:length(incomingEdges{v}) %(w,v)
                e = incomingEdges{v}(i);
                d(:,e) = vv(:,e).*kernel(ww(:,e).*areaWeights);
                d(:,e) = max(d(:,e),0);
                p = p .* d(:,e).^(edgeWeights(e)/omega);
            end

            entropy = -sum(p.*log(p).*areaWeights);
            if nargin == 8 && entropy > entropyLimit && projectEntropy
                try % just ignore projection if it fails
                    fn = @(x) full(-sum(x*areaWeights.*((p.^x).*log(p))) - entropyLimit);
                    options = optimset('Display','none','tolfun',1e-6,'tolx',1e-6);
                    a = fzero(fn,[0 10],options);
                    p = p.^a;
                end
            end

            result(:,v) = p;

            for i=1:length(outgoingEdges{v}) %(v,w)
                e = outgoingEdges{v}(i);
                ww(:,e) = ww(:,e) .* p ./ d(:,e);
            end

            for i=1:length(incomingEdges{v}) %(w,v)
                e = incomingEdges{v}(i);
                vv(:,e) = vv(:,e) .* p ./ d(:,e);
            end
        end

        queue(edges(incomingEdges{v},:)) = 1;
        queue(edges(outgoingEdges{v},:)) = 1;
        queue(v) = 1;
    end

%     subplot(1,2,1);
%     imagesc(log(max(result,1e-20)));
%     colorbar;
%     title(sprintf('Iteration %d, log scale',j));
%     axis off;
%
%     subplot(1,2,2)
%     imagesc(result);
%     colorbar;
%     title(sprintf('Iteration %d, result',j));
%     axis off;
%
%     drawnow;

    change = sum(sum(bsxfun(@times,areaWeights,abs(result-oldResult))))/nv;
    change = full(change);

    fprintf('Iteration %d:  %g\n', j, change);

    if ~projectEntropy && change < 1e-3 && length(toVisit) == nv % was -3
        fprintf('Starting to project entropy.\n');
        projectEntropy = 1;
    elseif change < 1e-5 && j > 5 && projectEntropy
        return
    end
end


function feat = poly2featuresOnly(u, v, imsize, v0, yc, f)

imh = imsize(1);
imw = imsize(2);

if isempty(f)
    f = 1.38; %S*max(size(im)) / imh;
end

u = u(:)';
v = v(:)';

[tmp, ind] = min(u);
u = [u(ind:end)  u(1:ind-1)];
v = [v(ind:end)  v(1:ind-1)];

u = (u - imw/2) ./ imh;
v = 1 - (v ./ imh);

[v1, ind1] = min(v); % lowest point on object in image

[footx, footz] = computeGroundPosition([min(u) max(u)], [v1 v1], v0, yc, f);
footz(2) = footz(1) + footx(2)-footx(1);

[cx, cz] = computeGroundPosition(u, v, v0, yc, f);

data.u = (u*imh+imw/2)';
data.v = ((1-v)*imh)';
data.x3d = cx;
data.z3d = cz;
data.foot = [footx footz];

feat = contactdata2features(data);

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

function [x, z] = computeGroundPosition(u, v, v0, yc, f)

z = yc*f./max((v0-v), 0.001);
x = u.*z./f;


function [W,CovW,X,CovX,R] = rotate_to_pca(W,CovW,X,CovX,weightsWW)
% [W,CovW,X,CovX,R] = rotate_to_pca(W,CovW,X,CovX,weightsW)

% Dimensionalities
[M,D] = size(W);
[D,N] = size(X);

% Convert cell covariance matrices to arrays
cellW = false;
if iscell(CovW)
  CovW = covcell_to_covarray(CovW);
  cellW = true;
end

cellX = false;
if iscell(CovX)
  CovX = covcell_to_covarray(CovX);
  cellX = true;
end

if nargin < 5 || isempty(weightsWW)
  weightsWW = ones(M,1);
else
  weightsWW = weightsWW(:) .* ones(M,1);
end

%% Find mixing rotation Xpca = R * Xgp

R = 1;

% 1) Whiten <XX'>

XX = X * X' + sum(CovX,3);
[V,A] = svd(XX/N);
R = diag(sqrt(1./diag(A))) * V';
% Rotate W
W = W / R;
for i=1:rows(W)
  CovW(:,:,i) = R' \ CovW(:,:,i) / R;
end

% 2) Orthogonalise weighted <W'W>

% Evaluate weighted second moment
WW = W' * diag(weightsWW) * W;
for m=1:M
  WW = WW + weightsWW(m) * CovW(:,:,m);
end

% Diagonalise it
[V,D] = svd(WW);
R = V' * R;

% Rotate W
W = W * V;
for m=1:M
  CovW(:,:,m) = V' * CovW(:,:,m) * V;
end

% $$$ % Debug the rotation!
% $$$ weights2 = cosd(data.coordinates(2,:));
% $$$ WW = W' * diag(weights2) * W;
% $$$ for m=1:M
% $$$   WW = WW + weights2(m) * CovW(:,:,m);
% $$$ end
% $$$ XX = R * (X*X'+sum(CovX,3)) * R';
% $$$ WW(1:10,1:10)
% $$$ XX(1:10,1:10) / size(X,2)

% Rotate X to PCA
X = R * X;
for n=1:N
  CovX(:,:,n) = R * CovX(:,:,n) * R';
end

% Convert back to cells
if cellW
  CovW = covarray_to_covcell(CovW);
end
if cellX
  CovX = covarray_to_covcell(CovX);
end


function [obj_out, varargout] = image_math(obj1, varargin)
% Perform simple mathematical and boolean operations on image objects
%
% :Usage:
% ::
%
%    obj_out = image_math(obj1, [optional inputs, e.g., a 2nd object, keywords])
%
% For objects: Type methods(object_name) for a list of special commands
%              Type help object_name.method_name for help on specific
%              methods.
%
% :Inputs:
%
%   **obj1:**
%        An image_vector object
%
% :Optional Inputs:
%
%   **obj2:**
%        An additional image_vector object
%   **{'add', 'plus'}:**
%        Keyword to perform image-wise addition of images in obj1
%                              and obj2.  Assumes these are paired/matched objects.
%   **{'subtract', 'minus'}:**
%        Keyword to perform image-wise subtraction of images
%                              in obj1 and obj2
%   **{'cat', 'concatenate'}:**
%        Concatenate obj1 and obj2 image-wise.  Requires same
%                              number of voxels in both image sets.  Returns effects
%                              codes of 1, -1 in obj_out.Y.
%   **{'power'}:**
%        Keyword to raise data to power element-wise; obj.dat = obj.dat.^b;
%                              Followed by exponent to apply (b)
%
%   **{'rmssd'}:**
%        Keyword to perform root-mean-square successive differences.
%        Meaningful if image series is a timeseries.
%
%        [obj, rmssd, wh_outliers] = image_math(obj, 'rmssd');
%        Does not affect object.
%
% :Outputs:
%
%   **obj_out:**
%        The result - an image_vector object
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2015  Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..

% ..
% DEFAULTS AND INPUTS
% ..

keyword = '';  % initalize optional variables to default values here.
% keyword. should enter one...
obj2 = [];
my_exponent = [];

% optional inputs with default values
% -----------------------------------

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}

            case {'add', 'plus'}, keyword = 'plus'; varargin{i} = [];
            case {'subtract', 'minus'}, keyword = 'minus'; varargin{i} = [];
            case {'cat', 'concatenate'}, keyword = 'cat'; varargin{i} = [];

            case 'power', keyword = 'power'; varargin{i+1} = my_exponent;
                %case 'basistype', basistype = varargin{i+1}; varargin{i+1} = [];

            case 'rmssd', keyword = 'rmssd'; varargin{i} = [];

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end

    elseif isa(varargin{i}, 'image_vector')

        obj2 = varargin{i}; varargin{i} = [];

    end
end

% -------------------------------------------------------------------------
% DATA CHECKS
% -------------------------------------------------------------------------

switch keyword

    case ''
        disp('Nothing to do.')
        return

    case 'power'
        %Check if image_vector object
        if ~isa(obj1,'image_vector')
            error('Input Data is not an image_vector object')
        end

    case {'plus', 'minus', 'cat'}
        n1 = size(obj1.dat, 2);
        n2 = size(obj2.dat, 2);

        y1 = ones(n1, 1);
        y2 = ones(n2, 1);

        %Check if image_vector object
        if ~isa(obj1,'image_vector') || ~isa(obj2,'image_vector')
            error('Input Data is not an image_vector object')
        end

        %Check number of rows
        if size(obj1.dat,1) ~= size(obj2.dat,1)
            % Voxel list mismatch
            % Try to fix

            obj1 = replace_empty(obj1);
            obj2 = replace_empty(obj2);

            if size(obj1.dat,1) ~= size(obj2.dat,1)
                % If still different, give a warning and resample.

                warning('Image_math: Objects being operated on do not appear to be in the same space. Resampling 2nd object, but proceed with caution!!');

                obj2 = resample_space(obj2, obj1);
            end

            %error('number of voxels is different between objects.')
        end

        if strcmp(keyword, 'cat')
            % we are done

        else

            % The rest is for plus/minus, which assume paired images

            %Check number of columns
            if n1 ~= n2
                error('Sizes of objects do not match.');
            end

        end

    case 'rmssd'

    otherwise
        warning(['Unknown keyword:' keyword]);
        return

end % keyword ; data checks

% -------------------------------------------------------------------------
% RUN OPERATION
% -------------------------------------------------------------------------

switch keyword

    case ''
        disp('Nothing to do.')
        return

    case 'plus'

        obj_out = obj1;
        obj_out.dat = obj_out.dat + obj2.dat;
        obj_out.history{end+1} = 'Image-wise addition operation by image_math';
        obj_out.dat_descrip = cell(1, 3);
        obj_out.dat_descrip{1} = 'Names of images added in next cells, 1st set plus 2nd';
        obj_out.dat_descrip{2} = obj1.fullpath;
        obj_out.dat_descrip{3} = obj2.fullpath;
        obj_out.image_names = [];
        obj_out.fullpath = [];
        obj_out.files_exist = false;

    case 'minus'

        obj_out = obj1;
        obj_out.dat = obj_out.dat - obj2.dat;
        obj_out.history{end+1} = 'Image-wise subtraction operation by image_math';
        obj_out.dat_descrip = cell(1, 3);
        obj_out.dat_descrip{1} = 'Names of images subtracted in next cells, 1st set minus 2nd';
        obj_out.dat_descrip{2} = obj1.fullpath;
        obj_out.dat_descrip{3} = obj2.fullpath;
        obj_out.image_names = [];
        obj_out.fullpath = [];
        obj_out.files_exist = false;

    case 'cat'

        obj_out = obj1;
        obj_out.dat = [obj_out.dat obj2.dat];
        obj_out.history{end+1} = 'Image-wise concatenation operation by image_math';
        obj_out.image_names = char(obj1.image_names, obj2.image_names);
        obj_out.fullpath = char(obj1.fullpath, obj2.fullpath);
        if isfield(obj_out, 'Y')
            obj_out.Y = [y1; -y2];
            obj_out.Y_descrip = 'Effects codes for image set A (1) and B (-1) added by image_math';
        end
    case 'power'
        obj_out = obj1;
        obj_out.dat = obj_out.dat .^ my_exponent;
        obj_out.history{end+1} = sprintf('Raised to %3.0f element-wise by image_math', my_exponent);

    case 'rmssd'

        obj_out = obj1;

        sdiffs = diff(obj1.dat')';
        sdiffs = [mean(sdiffs, 2) sdiffs]; % keep in image order
        rmssd = ( mean(sdiffs .^ 2) ) .^ .5; % rmssd - root mean square successive diffs

        % avoid first time point being very different and influencing distribution and plots.
        rmssd(1) = median(rmssd);

        % z-scores of rmssd
        wh_outliers = scale(rmssd) > 3;

        varargout{1} = rmssd;
        varargout{2} = wh_outliers;

    otherwise
        warning(['Unknown keyword:' keyword]);
        return

end % switch keyword; operation

end % main function


%Charge Neutrality Equation
%Assuming that the material contains only one type of dopant
%NA (Acceptor)
function F = neutralp(Nv,Nc,NA,Ev,Ec,Ea,Efp,k,T,gA)
F = Nv*((exp(-((Ev-Efp)/(k*T)))+3*sqrt(pi/2)*((((Ev-Efp)/(k*T))+2.13)+((abs(((Ev-Efp)/(k*T))-2.13)).^2.4+9.6).^(5/12)).^(-3/2)).^-1)-Nc*((exp(-((Efp-Ec)/(k*T)))+3*sqrt(pi/2)*((((Efp-Ec)/(k*T))+2.13)+((abs(((Efp-Ec)/(k*T))-2.13)).^2.4+9.6).^(5/12)).^(-3/2)).^-1)-(NA/(1+gA*exp((Ea-Efp)/(k*T))));


%% This file is the PDE simulation file of SINDy-PI discovered Belousov-Zhabotinsky reaction.
% Coded By: K
% Last Updated: 2019/09/19
%%
function [rhs,x_t,z_t,s_t,u_t,x,z,s,u,x_x,z_x,s_x,u_x,x_y,z_y,s_y,u_y,x_xx,z_xx,s_xx,u_xx,x_yy,z_yy,s_yy,u_yy,x_lap,z_lap,s_lap,u_lap]=...
    BZ_Reaction_SINDy_PI_PDE(t,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,NeedDev)

% Calculate u and v terms
xt=reshape((xzsut(1:N)),n,n);
zt=reshape((xzsut((N+1):(2*N))),n,n);
st=reshape((xzsut(2*N+1:3*N)),n,n);
ut=reshape((xzsut((3*N+1):(4*N))),n,n);

x=real(ifft2(xt));
z=real(ifft2(zt));
s=real(ifft2(st));
u=real(ifft2(ut));

% Reaction Terms (You need to manully code up the equation discovered by DL-SINDy)
xtrhs=reshape((fft2(  (0.24667*x + 0.33333*s + 0.5*z + 3.3333*x.*s - 5.0*x.*z + 2.1333*x.^2 - 3.3333*x.^3)./(x + 0.1)  )),N,1);
ztrhs=reshape((fft2(  x + 0.4*u - 1.3*z  )),N,1);
strhs=reshape((fft2(  0.17333*x-0.66667*s  )),N,1);
utrhs=reshape((fft2(  100*z-133.33*u  )),N,1);

rhs=[-K22.*xzsut(1:N)+xtrhs
     -0.1*K22.*xzsut(N+1:2*N)+ztrhs
     -K22.*xzsut(2*N+1:3*N)+strhs
     -K22.*xzsut(3*N+1:4*N)+utrhs
     ];

 % If you don't need to extract the value, don't run the following code to
 % speed up
if NeedDev==1
    % Get the derivative you want
    x_x=real(ifft2(reshape((1j*Kx).*xzsut(1:N),n,n)));
    z_x=real(ifft2(reshape((1j*Kx).*xzsut(N+1:2*N),n,n)));
    s_x=real(ifft2(reshape((1j*Kx).*xzsut(2*N+1:3*N),n,n)));
    u_x=real(ifft2(reshape((1j*Kx).*xzsut(3*N+1:4*N),n,n)));
    %
    x_y=real(ifft2(reshape((1j*Ky).*xzsut(1:N),n,n)));
    z_y=real(ifft2(reshape((1j*Ky).*xzsut(N+1:2*N),n,n)));
    s_y=real(ifft2(reshape((1j*Ky).*xzsut(2*N+1:3*N),n,n)));
    u_y=real(ifft2(reshape((1j*Ky).*xzsut(3*N+1:4*N),n,n)));
    %
    x_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(1:N),n,n)));
    z_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(N+1:2*N),n,n)));
    s_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(2*N+1:3*N),n,n)));
    u_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(1:N),n,n)));
    z_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(N+1:2*N),n,n)));
    s_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(2*N+1:3*N),n,n)));
    u_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_t=real(ifft2(reshape(rhs(1:N),n,n)));
    z_t=real(ifft2(reshape(rhs(N+1:2*N),n,n)));
    s_t=real(ifft2(reshape(rhs(2*N+1:3*N),n,n)));
    u_t=real(ifft2(reshape(rhs(3*N+1:4*N),n,n)));
    %
    x_lap=real(ifft2(reshape(-K22.*xzsut(1:N),n,n)));
    z_lap=real(ifft2(reshape(-K22.*xzsut(N+1:2*N),n,n)));
    s_lap=real(ifft2(reshape(-K22.*xzsut(2*N+1:3*N),n,n)));
    u_lap=real(ifft2(reshape(-K22.*xzsut(3*N+1:4*N),n,n)));

end


function r82row_order_type_test ( )

%*****************************************************************************80
%
%% R82ROW_ORDER_TYPE_TEST tests R82ROW_ORDER_TYPE.
%
%  Discussion:
%
%    An R82ROW is a (2,N) array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 4;
  test_num = 10;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R82ROW_ORDER_TYPE_TEST\n' );
  fprintf ( 1, '  R82ROW_ORDER_TYPE classifies an R8VEC as\n' );
  fprintf ( 1, '  -1: no order\n' );
  fprintf ( 1, '   0: all equal;\n' );
  fprintf ( 1, '   1: ascending;\n' );
  fprintf ( 1, '   2: strictly ascending;\n' );
  fprintf ( 1, '   3: descending;\n' );
  fprintf ( 1, '   4: strictly descending.\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    [ x, seed ] = r8mat_uniform_01 ( 2, n, seed );

    x(1:2,1:n) = round ( 3.0 * x(1:2,1:n) );

    order = r82row_order_type ( n, x );

    string = sprintf ( '  Order type = %d\n', order );

    r82row_print ( n, x, string );

  end

  return
end


function options = reg_fine_rigid(rgb1, I1, wl, options)
%REG_FINE_RIGID Summary of this function goes here
%   Detailed explanation goes here

t_start = tic;
% optimize the objective function by block coordinate descent
options = reg_fine_rigid_internal(rgb1, I1, wl, options);

disp(['Elapsed time for fine-scale rigid registration is ',num2str(toc(t_start))]);

function options = reg_fine_rigid_internal(rgb1, I1, wl, options)
I = double(rgb1) / 255;

last_pos = NaN(1,6);

for iter = 1:20
    try
        t_start_iter = tic;

        % optimize w.r.t. degree, t, s
        [degree, t, s] = optimize_wrt_T_s(I, I1, wl, options);
        options.degree = degree;
        options.t = t;
        options.s = s;

        % optimize w.r.t. lambda
        sigma = optimize_wrt_sigma(I, I1, options);
        options.sigma = sigma;

        % check convergence
        if all([degree,t,s,sigma] == last_pos)
            disp(['Iteration ends at ',num2str(iter)]);
            break;
        else
            last_pos = [degree,t,s,sigma];
        end
        disp(['Iteration ',num2str(iter),' lasts ',num2str(toc(t_start_iter))]);
    catch err
        if strcmp(err.identifier, 'Registration:DerivativeBoundary')
            disp(['Stop optimization since boundary touch when ', ...
                'calculating derivatives']);
            break;
        else
            throw(err);
        end
    end
end


function a = dirichlet_fit_s_simple(data,a)
% DIRICHLET_FIT_S_SIMPLE   Initial guess for Dirichlet precision.
%
% DIRICHLET_FIT_S_SIMPLE(data,a) returns an initial guess for the Dirichlet
% parameter vector A, by scaling the input A.

bar_p = mean(log(data));
m = a/sum(a);
bar_p = sum(m.*bar_p);
s = 1/(sum(m.*log(m)) - bar_p);
s = s*(length(m)-1)/2;
a = s*m;


function r8vec_sorted_split_test ( )

%*****************************************************************************80
%
%% R8VEC_SORTED_SPLIT_TEST tests R8VEC_SORTED_SPLIT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 25;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_SORTED_SPLIT_TEST\n' );
  fprintf ( 1, '  R8VEC_SORTED_SPLIT splits a sorted vector into\n' );
  fprintf ( 1, '  entries less than and greater than a\n' );
  fprintf ( 1, '  splitting value.\n' );

  b = 0.0;
  c = 10.0;
  seed = 123456789;

  [ a, seed ] = r8vec_uniform_ab ( n, b, c, seed );

  a(1:n) = round ( a(1:n) ) / 2.0;

  a = r8vec_sort_heap_a ( n, a );

  split = 0.5 * ( a(1) + a(n) );

  r8vec_print ( n, a, '  The sorted array:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Splitting value is %f\n', split );

  [ i_lt, i_gt ] = r8vec_sorted_split ( n, a, split );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Lower index I_LT = %d\n', i_lt );
  fprintf ( 1, '  Upper index I_GT = %d\n', i_gt );

  return
end


function sparse_count_test08 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% SPARSE_COUNT_TEST08 tests OWN_E_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_COUNT_TEST08\n' );
  fprintf ( 1, '  OWN_E_SIZE returns the number of\n' );
  fprintf ( 1, '  distinct points in an OWN_E sparse grid made from \n' );
  fprintf ( 1, '  product grids formed from open weakly nested \n' );
  fprintf ( 1, '  quadrature rules with exponential growth, including:\n' );
  fprintf ( 1, '  * GGH_E, the Generalized Gauss-Hermite Exponential Growth Family;\n' );
  fprintf ( 1, '  * GH_E, the Gauss-Hermite Exponential Growth Family;\n' );
  fprintf ( 1, '  * LG_E, the Gauss-Legendre Exponential Growth Family;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %10d', dim_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   LEVEL_MAX\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = own_e_size ( dim_num, level_max );
      fprintf ( 1, '  %10d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [dcenter,i0,i1,j0,j1] = dcenter_pair(trx,fly1,fly2)
% modified AR 3/8/2018 add i0,i1,j0,j1 to output
% initialize
dcenter = nan(1,trx(fly1).nframes);

% get start and end frames of overlap
t0 = max(trx(fly1).firstframe,trx(fly2).firstframe);
t1 = min(trx(fly1).endframe,trx(fly2).endframe);
i0 = nan;
i1 = nan;
j0 = nan;
j1 = nan;
% no overlap
if t1 < t0,
  return;
end

% indices for these frames
i0 = t0 + trx(fly1).off;
i1 = t1 + trx(fly1).off;
j0 = t0 + trx(fly2).off;
j1 = t1 + trx(fly2).off;

% centroid distance
dx = trx(fly2).x_mm(j0:j1)-trx(fly1).x_mm(i0:i1);
dy = trx(fly2).y_mm(j0:j1)-trx(fly1).y_mm(i0:i1);
z = sqrt(dx.^2 + dy.^2);
dcenter(i0:i1) = z;


function [OfConstraint, OfAll] = determineFluxValuesOnBoundary(model, solution)
% This function determines the number of reactions in the flux distributions that are on
% the boundaries
%
% [OfConstraint, OfAll] = determineFluxValuesOnBoundary(model, solution)
%
% INPUT
% model         Model structure
% solution      Solution structure
%
% OUTPUT
% OfConstraint  Fraction of flux values that are on the lower and upper bounds
%               of all constrained reactions (assuming a minimum infinity
%               of -1,000,000 and a  maximum infinity of 1,000,000
% OfAll         Fraction of flux values that are on the lower and upper bounds
%               of all reactions in the model
%
% Ines Thiele, December 2018
%
%
%
minInf = -1000000;
maxInf = 1000000;
% find reactions that have flux values on upper bound
I = find(abs(solution.full)>1e-6);
% find all non-zero and non-inf bounds
J = find(abs(model.ub)~=0);
Ji = find(abs(model.ub)<=abs(minInf));
IJ = intersect(I,J);
IJ = intersect(IJ,Ji);
Usedub = model.ub(IJ);
UsedF = solution.full(IJ);
UsedR = model.rxns(IJ);
OnUB= length(find(abs(Usedub-UsedF)<1e-5));

% find reactions that have flux values on lower bound
I = find(abs(solution.full)>1e-6);
% find all non-zero and non-inf bounds
J = find(abs(model.lb)~=0);
Ji = find(abs(model.lb)<=abs(maxInf));
IJ = intersect(I,J);
IJ = intersect(IJ,Ji);
Usedlb = model.lb(IJ);
UsedF = solution.full(IJ);
UsedR = model.rxns(IJ);
OnLB= length(find(abs(Usedlb-UsedF)<1e-5));

% constraint reactions
minConstraints = length(intersect(find(model.lb>minInf),find(model.lb)));
maxConstraints =length(intersect(find(model.ub<maxInf),find(model.ub)));
PercentageConstraintRxns_model = (minConstraints + maxConstraints)*100/length(model.ub);
%percentage of all reactions
OfAll = (OnLB+OnUB)/length(model.rxns);
OfConstraint = (OnLB+OnUB)/(minConstraints + maxConstraints);


%% This file is the main file of using the SINDy-PI method to
% infer the Single Pendulum on Cart Model.
%
% Date: 2019/07/26
% Coded By: K

%% Close all, clear all, clc
close all;clear all; clc;
dir
set(0,'defaulttextInterpreter','latex')
[status,message,messageid] = mkdir('Results');
addpath('Function');
%% Simulate the budworm population growth and gather the simulation data

%Define the model parameters
M=1;m=1;L=1;g=9.81;

% Define noise level and add gaussian noise to the data
noise=0.02;

% Define whehter you have control, if you have it, please define it
Control=1;

%Define whether you want to shuffel the final data
Shuffle=0;

% Peform simulation
state0=[0.3;0;1;0];state0_test=[0.1;0;0.3;0];
dt=0.001;T=16;T_test=2;
tspan=0:dt:T;tspan_test=0:dt:T_test;
u=-0.2+0.5*sin(6*(tspan'));
u_test=-1+1*sin((tspan_test'))+3*sin(2*(tspan_test'));
[dData,Data]=Get_Sim_Data(@(t,y,u)SinglePendulum_ODE(t,y,u,M,m,L,g),state0,u,tspan,noise,Control,Shuffle);
dData(:,1)=Data(:,3);dData(:,2)=Data(:,4);
[dData_test,Data_test]=Get_Sim_Data(@(t,y,u)SinglePendulum_ODE(t,y,u,M,m,L,g),state0_test,u_test,tspan_test,noise,Control,Shuffle);
dData_test(:,1)=Data_test(:,3);dData_test(:,2)=Data_test(:,4);
%% Plot the simulation data
% close all
% HorizontalRange=5*L;
% VerticalRange=2.5*L;
% displayrate=1;
% cartoonOffLineAcce(Data(:,2),Data(:,1),L,HorizontalRange,VerticalRange,displayrate)
%%
% figure(1)
% plot(tspan,Data(:,1))
% title('Theta')
% grid on
% %
% figure(2)
% plot(tspan,Data(:,2))
% title('Position')
% grid on
% %
% figure(3)
% plot(tspan,Data(:,3))
% title('dTheta')
% grid on
% %
% figure(4)
% plot(tspan,Data(:,4))
% title('Velocity')
% grid on
% %
% figure(5)
% plot(tspan,u,'linewidth',3,'color','black')
% box('off')
% axis('on')

%% Now perform sparse regression of non-linear dynamics

% Get the number of states we have
[dtat_length,n_state]=size(Data);

% Define the control input(Should be zero in our example)
n_control=1;

% Choose whether you want to display actual ODE or not
disp_actual_ode=1;

% If the ODEs you want to display is the actual underlyting dynamics of the
% system, please set actual as 1
actual=1;

% Print the actual ODE we try to discover
digits(4)
z_vars=sym('z',[1,n_state]);
u_vars=sym('u',[1,n_control]);
d_vars=sym('dz',[1,n_state]);
ODEsP=SinglePendulum_ODE(0,z_vars,u_vars,M,m,L,g);
fprintf('The actual ODE of the system is/are :\n')
for i=1:n_state
    fprintf(strcat(char(d_vars(1,i)),'=',char(ODEsP(i,1)),'\n'));
end

% The implicit ODE has the following form:
% dz1=z3
% dz2=z4
% dz3=((981*sin(z1))/50 + u1*cos(z1) + z3^2*cos(z1)*sin(z1))/(cos(z1)^2 - 2)
% dz4=-(u1 + z3^2*sin(z1) + (981*cos(z1)*sin(z1))/100)/(cos(z1)^2 - 2)

% Create symbolic states
dz=sym('dz',[n_state,1]);
z=sym('z',[n_state,1]);
uc=sym('u',[n_control,1]);

% Now we first create the parameters of the function right hand side
Highest_Poly_Order_Guess=1;
Highest_Trig_Order_Guess=2;
Highest_U_Order_Guess=0;

% Then create the right hand side library parameters
Highest_Poly_Order=2;
Highest_Trig_Order=2;
Highest_U_Order=0;
Highest_dPoly_Order=1;

%% Define parameters for the sparese regression
lam=[1e-4;5e-4;1e-3;2e-3;3e-3;4e-3;5e-3;6e-3;7e-3;8e-3;9e-3;1e-2;2e-2;3e-2;4e-2;5e-2;...
    6e-2;7e-2;8e-2;9e-2;1e-1;2e-1;3e-1;4e-1;5e-1;6e-1;7e-1;8e-1;9e-1;1;1.5;2;2.5;3;3.5;4;4.5;5;...
    6;7;8;9;10;20;30;40;50;100;200];

N_iter=20;
disp=0;
LibPoly=[1;1;0;0];
LibTrig=[0;0;3;4];
%
GuessPoly=[1;1;0;0];
GuessTrig=[0;0;1;1];

% Normalize the library?
NormalizeLib=0;

% Define a cell matrix to store the variable
Xi_Final=cell(n_state,1);
Score_Final=cell(n_state,1);

for iter=1:n_state
    % Change the library base on what equation you want to work on
    Highest_Poly_Order=LibPoly(iter);
    Highest_Poly_Order_Guess=GuessPoly(iter);
    Highest_Trig_Order=LibTrig(iter);
    Highest_Trig_Order_Guess=GuessTrig(iter);

    fprintf('Calculating the %i expression \n',iter)

    % According to the previous parameter generate the left hand side guess
    [LHS_Data,LHS_Sym]=GuessLib(Data,dData(:,iter),iter,u,Highest_Poly_Order_Guess,Highest_Trig_Order_Guess,Highest_U_Order_Guess);

    %Generate the corresponding data
    [SINDy_Data,SINDy_Struct]=SINDyLib(Data,dData(:,iter),iter,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order);

    % Run the for loop and try all the left hand guess
    for i=1:length(LHS_Sym)
        fprintf('\n\t\t Testing the left hand side as %s... \n',cell2sym(LHS_Sym(1,i)))

        % Exclude the guess from SINDy-PI library
        [RHS_Data,RHS_Struct]=ExcludeGuess(SINDy_Data,SINDy_Struct,LHS_Sym{i});

        if iter==1
            Xi=cell(length(LHS_Sym),length(lam));
            Score=zeros(length(LHS_Sym),length(lam));
            ODE=cell(length(LHS_Sym),length(lam));
        end

        % Define dummy variables for parfor
        LHS_Sym_Dum=LHS_Sym{i};
        LHS_Data_Dum=LHS_Data(:,i);
        dz_Dum=dz(iter);
        dData_test_dum=dData_test(:,iter);
        for j=1:length(lam)
            fprintf('\n\t\t Testing the lambda as %d... \n',lam(j,1))
            % Perform the sparse regression problem
            [Xi{i,j},ODE{i,j}]=sparsifyDynamics(RHS_Data,LHS_Data_Dum,LHS_Sym_Dum,lam(j,1),N_iter,RHS_Struct,disp,NormalizeLib);

            % Perform sybolic calculation and solve for dX
            Eqn=LHS_Sym_Dum==ODE{i,j};
            digits(4)
            ODE_Guess=simplify(vpa(solve(Eqn,dz_Dum)));
            % Store the result of each valuation
            try
                ODEs(i,j)=ODE_Guess;
                func=matlabFunction(ODEs(i,j),'Vars',{z,uc});
                funcVal=func(Data_test',u_test')';
                Score(i,j)=norm(dData_test_dum-funcVal)/norm(dData_test(:,iter));
            catch
                ODEs(i,j)=0;
                Score(i,j)=NaN;
            end
        end
    end

    % Calculate the minimum score
    [minVal,minIndex]=min(Score,[],2);
    [minVal2,minIndex2]=min(minVal);

    % Store the best ODE approximation
    ODE_Best(iter,1)=ODEs(minIndex2,minIndex(minIndex2));

    % Print the result
    fprintf('\n \tThe SINDy-PI discovered Best ODE for %i equation is:\n',iter)
    digits(4)
    fprintf(strcat(char(dz(iter)),'=',char(simplify(ODE_Best(iter,1))),'\n'));

    % Save this result
    Xi_Final{iter,1}=Xi;
    Score_Final{iter,1}=Score;

    % Plot the score
    figure(iter)
    hold on
    for k=1:length(LHS_Sym)
        plot(lam,Score(k,:),'color',[1 iter*0.2 iter*0.25],'linewidth',2.5)
        scatter(lam,Score(k,:),100,'MarkerFaceColor',[1 iter*0.2 iter*0.25],'MarkerEdgeColor',[0 0 0],'linewidth',2.5)
        set(gca,'XScale','log')
    end

end

%% Now generate the ODE function file and test the accuracy of the
% identified system
Noise=0;

% Now generate this best guess ODE
Generate_ODE_RHS(ODE_Best(:,1),n_state,n_control);

% Simulate the system with the SINDy-PI identified ODE
%state0_test=[0.5;0.1;0.1;-0.1];
state0_test=[pi;0;0;0];
u_test=-0.5+0.2*sin((tspan_test'))+0.3*sin(2*(tspan_test'));
LHS_Sym=0;
[dData_test,Data_test]=Get_Sim_Data(@(t,y,u)SinglePendulum_ODE(t,y,u,M,m,L,g),state0_test,u_test,tspan_test,Noise,Control,Shuffle);
[d_Data_Es,Data_Es]=Get_Sim_Data(@(t,y,u)Sindy_ODE_RHS(t,y,u),state0_test,u_test,tspan_test,Noise,Control,Shuffle);

%% Print the Result
disp_best=1;
if disp_best==1
    fprintf('The SINDy-PI discovered Best ODE is:\n')
    digits(4)
    for i=1:n_state
        fprintf(strcat(char(dz(i)),'=',char(simplify(ODE_Best(i,1))),'\n'));
    end
end
%% Save the result
File_Name=strcat('Results/Noise_Level_',num2str(noise),'.mat');
save(File_Name,'Xi_Final','Score_Final','ODE_Best','state0_test',...
    'u_test','dData_test','Data_test','d_Data_Es','Data_Es','tspan_test')

%% Plot the simulation data
close all
% Create the new directory to save the plot
[fld_status, fld_msg, fld_msgID]=mkdir('Figures');
%
figure(1)
plot(tspan_test,Data_test(:,1),'linewidth',4.5,'Color','black')
hold on
plot(tspan_test,Data_Es(:,1),'linewidth',4.5,'linestyle','--','color','blue')
% legend('Actual Dynamics','Best Model')
% title('Validation','FontSize',24)
% xlabel('Time $(t)$','FontSize', 24)
% ylabel('$\theta(t)$','FontSize', 24)
set(gca,'FontSize',34);
grid on
h = gca;
set(gca,'xticklabel',[])
set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
box('on')
%print('-depsc2', '-loose', 'Figures/SinglePendulum_Thetat1.eps');

%
figure(2)
plot(tspan_test,Data_test(:,2),'linewidth',4.5,'Color','black')
hold on
plot(tspan_test,Data_Es(:,2),'linewidth',4.5,'linestyle','--','color','blue')
% legend('Actual Dynamics','Best Model')
% title('Validation','FontSize',18)
% xlabel('Time $(t)$','FontSize', 18)
% ylabel('$x(t)$','FontSize', 18)
set(gca,'FontSize',34);
grid on
%h = gca; h.XAxis.Visible = 'off';
set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
box('on')
print('-depsc2', '-loose', 'Figures/SinglePendulum_x.eps');

%%
figure(3)
plot(tspan_test,Data_test(:,3),'linewidth',2,'Color','green')
hold on
plot(tspan_test,Data_Es(:,3),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('Time $(t)$','FontSize', 18)
ylabel('$\dot{\theta}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_dThetat.eps');

%%
figure(4)
plot(tspan_test,Data_test(:,4),'linewidth',2,'Color','green')
hold on
plot(tspan_test,Data_Es(:,4),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('Time $(t)$','FontSize', 18)
ylabel('$\dot{x}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_dx.eps');

%
figure(5)
plot(Data_test(:,1),Data_test(:,3),'linewidth',2,'Color','green')
hold on
plot(Data_Es(:,1),Data_Es(:,3),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('$\theta(t)$','FontSize', 18)
ylabel('$\dot{\theta}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_theta_vs_dtheta.eps');

%
figure(6)
plot(Data_test(:,2),Data_test(:,4),'linewidth',2,'Color','green')
hold on
plot(Data_Es(:,2),Data_Es(:,4),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('$x(t)$','FontSize', 18)
ylabel('$\dot{x}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_x_vs_dx.eps');


function rswVector = rotateVectorFromEciToRsw(eciVector, rVect, vVect)
    R = normVector(rVect);
    W = normVector(crossARH(rVect, vVect));
    S = normVector(crossARH(W,R));

    ECI2RSWRotMat = [R,S,W];

    rswVector = ECI2RSWRotMat \ eciVector;
end


clc
clear all
bb=8; % block size
K=256; % number of atoms in the dictionary
img =imread('Test_Fig2_Missing.png');

[N,M,dim]=size(img);
img = double(img);
%Compute mask and extracting its patches
Mask = double(~(img(:,:,1)==0));
blkMask=im2col(Mask,[bb,bb],'sliding');  % distinct  sliding
img_yuv = rgb2ycbcr(uint8(img));
img_inpaint_yuv = zeros(size(img_yuv));
% Interpolation CbCr Componet
img_inpaint_yuv(:,:,2) = Interpolation(double(img_yuv(:,:,2)),~Mask);
img_inpaint_yuv(:,:,3) = Interpolation(double(img_yuv(:,:,3)),~Mask);
IMin0 = double(img_yuv(:,:,1));

load Dict
load Coeff

% Creating the output image
imag_Y=ImageRecover(IMin0,Dict,Coeff);
imag_Y=max(min(imag_Y,255),0);

img_inpaint_yuv(:,:,1) = imag_Y;
img_inpaint_rgb = ycbcr2rgb(uint8(img_inpaint_yuv));
imshow(uint8(img_inpaint_rgb))
imwrite(img_inpaint_rgb,strcat('KSVD_Result_','iter_25','.png'),'png')


function outputImage = ifft(this, applicationDimension)
% IFFT (including IFFT-shift and normalization) using image2k
%
%   Y = MrImage()
%   Y.ifft(applicationDimension)
%
% This is a method of class MrImage.
%
% IN
%
% OUT
%
% EXAMPLE
%   Y = MrImage();
%   Y.ifft(4); % convert each voxel's time series to frequency space by
%               applying iFFT to 4th dimension
%   Y.ifft('2D') % slice-wise iFFT for transversal slices
%   Y.ifft([1 2]) % slice-wise iFFT, same as previous
%   Y.ifft([2 3]) % slice-wise iFFT, but for sagittal slices (dim 2 and 3 = 1
%                %  slice)
%   Y.ifft('3D') % volume-wise iFFT
%   Y.ifft([1 2 3]) % volume-wise iFFT, same as previous
%   Y.ifft([1 2 4]) % volume-wise iFFT, but for time series of a k-space
%                  % voxel, acquired in a 2D k-space
%
%
%   See also MrImage MrImage.image2k

% Author:   Saskia Bollmann & Lars Kasper
% Created:  2015-12-12
% Copyright (C) 2015 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (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/>.

outputImage = image2k(this, applicationDimension);


function res = vg_method_train(X, Y, args)

% Variational Garrote: performs linear regression with L0-norm penalty (Spike and Slab model)
%   See file test_vg.m for an example of use
%
% required parameters (n input dimension, p samples)
%   X     : n x p (training set, input)
%   Y     : 1 x s (training set, output)
%
% args: optional parameters (default)
%   method      : method for optimization 'dual' or 'regression' for fixed gamma ('dual')
%   maxiter     : maximum number of iterations for optimization for fixed gamma (1e4)
%   max_sum_m   : increases gamma values until sum(m)=max_sum_m  (n/2)
%   beta_max    : increases gamma values until beta=beta_max (1e3)
%   n_gamma     : number of gamma values to scan (50)
%   dmmin       : convergence threshold for mean field error (1e-12)
%   valset      : part of the training set used for validation (0.1*p)

%----------------
% REQUIRED PARAMS

n = size(X,1);

%----------------
% OPTIONAL PARAMS

% method for optimization {dual or regression} for fixed gamma
if ~isfield(args, 'method') method='dual'; else method=args.method; end

% maximum number of iterations for optimization for fixed gamma
if ~isfield(args, 'maxiter') maxiter=1e4; else maxiter=args.maxiter; end

% increases gamma values until sum(m)=max_sum_m
if ~isfield(args, 'max_sum_m') max_sum_m=n/2; else max_sum_m=args.max_sum_m; end

% increases gamma values until beta=beta_max
if ~isfield(args, 'beta_max') beta_max=1e3; else beta_max=args.beta_max; end

% number of gamma values to scan
if ~isfield(args, 'n_gamma') n_gamma=50; else n_gamma=args.n_gamma; end

% convergence threshold for mean field error
if ~isfield(args, 'dmmin') dmmin=1e-12; else dmmin=args.dmmin; end

% part of the training set used for validation (default 0.1*p)
if ~isfield(args, 'valset') valset=ceil(.25*n); else valset=args.valset; end

% randomly split training and validation datasets
dataok = false;
nits = 1;
p = size(X,2)-valset;
pv = valset;
while ~dataok && nits < 10
    it = randperm(size(X,2));
    xv = X(:,it(1:valset));
    yv = Y(:,it(1:valset));
    x = X(:,it(valset+1:end));
    y = Y(:,it(valset+1:end));

    % normalize training
    x=x-mean(x,2)*ones(1,p);
    dx=sqrt(1/p*sum(x.^2,2));
    x=x./(dx*ones(1,p));
    y=y-mean(y);

    % normalize validation
    xv=xv-mean(xv,2)*ones(1,pv);
    dxv=sqrt(1/pv*sum(xv.^2,2));
    xv=xv./(dxv*ones(1,pv));
    yv=yv-mean(yv);

    dataok = ~any(isnan(x(:))) && ~any(isnan(xv(:)));
    nits=nits+1;
end
if nits==10
    error('VG Error: Increase training set size');
end

%----------------
% compute garrote solution for range of gammas
% first from gamma_min to gamma_max and then in
% a second pass from gamma_max to gamma_min.

% C is input data covariance matrix.
if strcmp(method, 'regression')
    if n<=1500,
        C=x*x'/p;
    end;
end

% b is input output covariance
b=x*y'/p;

% sigma is output variance
sigmay=y*y'/p;

% set gamma range (min, max and step size)
delta=1e-8;
[b2sort,isort]=sort(b.^2,'descend');
bsort=b(isort);
gamma_min=log(delta*sigmay/p/max(abs(b)));
eps_gamma=0.001;
gamma_max=eps_gamma*gamma_min;
gamma_all =linspace(gamma_min,gamma_max,n_gamma);

% initial step size of mean field update
eta0=1; %e-2;
% initial step size for change in w in dual.m
eta_w0=0.02;

% input data variance
chi_ii=1/p*sum(x.^2,2);
if sum(abs(chi_ii-1)>1e-10),
    fprintf('input design matrix is not normalized\n');
    pause
end;

lg=n_gamma;
kl_all=inf(lg,2);
v_all=inf(lg,2,n);
m_all=inf(lg,2,n);
beta_all=inf(lg,2);
v_mf_all=inf(lg,n);
m_mf_all=inf(lg,n);
iter_all=inf(lg,2);
beta_mf_all=inf(1,lg);
error_mf_all=inf(1,lg);
errorv_mf_all=inf(1,lg);
m=zeros(1,n);

% the estimated inverse noise variance beta is initialized as the
% output variance
beta=1/sigmay;
i=0;

% for gamma is gamma_min to gamma_max, or when some criteria are
% satisfied
while (beta<beta_max)&&(i<n_gamma)&&(sum(m)<max_sum_m),
	i=i+1;
	gamma=gamma_all(i);
    eval(method);
	v_all(i,1,:)=v;
	m_all(i,1,:)=m;
	beta_all(i,1)=beta;
	iter_all(i,1)=iter;
	kl_all(i,1)=kl1;
	fprintf('gamma = %f beta = %f sum(m) = %f iter = %d kl = %f\n',gamma,beta,sum(m),iter,kl1);
end;

if beta>=beta_max
 fprintf('-----------------------------------------------------------\n');
 fprintf('beta > beta_max (%.3f > %.3f)\n', beta, beta_max);
 if i>1
    m = squeeze(m_all(i-1,1,:))';
 end
end
if sum(m)>=max_sum_m
 fprintf('-----------------------------------------------------------\n');
 fprintf('sum(m) > max_sum_m (%.3f > %.3f)\n', sum(m), max_sum_m);
end

% for gamma is current gamma decreasing to gamma_min
imax=i-1;
for i=imax:-1:1,
	gamma=gamma_all(i);
    eval(method);
	v_all(i,2,:)=v;
	m_all(i,2,:)=m;
	beta_all(i,2)=beta;
	iter_all(i,2)=iter;
	kl_all(i,2)=kl1;
	fprintf('gamma = %f beta = %f sum(m) = %f iter = %d kl = %f\n',gamma,beta,sum(m),iter,kl1);
end;

% select for each gamma from these two solutions the one with lowest KL
[klmin, imin]=min(kl_all,[],2);
for i=1:imax,
	v_mf_all(i,:)=squeeze(v_all(i,imin(i),:))';
	m_mf_all(i,:)=squeeze(m_all(i,imin(i),:))';
	beta_mf_all(i)=beta_all(i,imin(i));
	error_mf_all(i)=1/p*sum((y-v_mf_all(i,:)*x).^2,2);
	errorv_mf_all(i)=1/pv*sum((yv-v_mf_all(i,:)*xv).^2,2);
end;

% select the gamma that optimizes the validation error (errorv_mf_all)
[minerrorv i]=min(errorv_mf_all(1:imax));

res.gamma_mf=gamma_all(i);
res.v_mf=v_mf_all(i,:);
res.m_mf=m_mf_all(i,:);
res.n_mf1=sum(res.m_mf>0.5);

res.error_mf=error_mf_all(i);
res.errorv_mf=errorv_mf_all(i);
res.beta_mf=beta_mf_all(i);

if (res.beta_mf==beta_max)
	fprintf('beta_max too small: beta_mf %6.4f, beta_max %6.4f\n', beta_mf(iruns),beta_max);
	pause
end;
if (res.gamma_mf==gamma_min)
	fprintf('gamma at minimum range boundary\n');
end;
if (res.gamma_mf==gamma_max)
	fprintf('gamma at maxium range boundary\n');
end;


classdef CEC2017_F13 < PROBLEM
% <single> <real> <constrained>
% CEC'2017 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% G. Wu, R. Mallipeddi, and P. N. Suganthan, Problem definitions and
% evaluation criteria for the CEC 2017 competition on constrained real-
% parameter optimization, National University of Defense Technology, China,
% 2016.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2017.mat'),'Data');
            obj.O = Data{12}.o;
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Y = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = sum(100*(Y(:,1:end-1).^2-Y(:,2:end)).^2+(Y(:,1:end-1)-1).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Y = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = sum(Y.^2-10*cos(2*pi*Y)+10,2) - 100;
            PopCon(:,2) = sum(Y,2) - 2*size(Y,2);
            PopCon(:,3) = 5 - sum(Y,2);
        end
    end
end


function test_pull1602

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_specest_irasa ft_freqanalysis

% simulate data
time = (1:2000)/1000;
for rpt = 1:20
  if false
    % We do not have that many concurrent licenses of the DSP Systems toolbox (https://nl.mathworks.com/products/dsp-system.html)
    % which was used in the initial version of this script. Therefore this test script does not use the DSP toolbox by default.
    dspobj = dsp.ColoredNoise('Color', 'pink', 'SamplesPerFrame', length(time));
    signal = dspobj()';
  else
    % integrate over time to create pink noise with 1/f power spectrum
    signal = cumsum(randn(1,length(time)));
  end

  % add line noise
  data.trial{1,rpt}     = signal + cos(2*pi*50*time) + cos(2*pi*100*time);
  data.time{1,rpt}      = time;
  data.label{1}         = 'chan';
  data.trialinfo(rpt,1) = rpt;
end

% using unfiltered data
cfg = [];
cfg.method  = 'irasa';
cfg.output  = 'original';
cfg.pad     = 'nextpow2';
freq = ft_freqanalysis(cfg, data);

cfg.output = 'fractal';
freqI = ft_freqanalysis(cfg, data);

% what happens if we use bandpassfiltered data?
cfg2            = [];
cfg2.bpfilter   = 'yes';
cfg2.bpfilttype = 'firws';
cfg2.bpfreq     = [60 150];
datafilt1 = ft_preprocessing(cfg2, data);

cfg2.bpfilter   = 'no';
cfg2.hpfilter   = 'yes';
cfg2.hpfreq     = 60;
cfg2.hpfilttype = 'firws';
datafilt2 = ft_preprocessing(cfg2, data);

cfg2.bpfilter   = 'no';
cfg2.hpfilter   = 'no';
cfg2.dftfilter  = 'yes'; % notch filter to filter out line noise
cfg2.dftfreq    = [50 100];
datafilt3 = ft_preprocessing(cfg2, data);

cfg.output = 'original';
freqfilt1  = ft_freqanalysis(cfg, datafilt1);
freqfilt2 = ft_freqanalysis(cfg, datafilt2);
freqfilt3 = ft_freqanalysis(cfg, datafilt3);

cfg.output = 'fractal';
freqfiltI = ft_freqanalysis(cfg, datafilt1);
freqfiltI2 = ft_freqanalysis(cfg, datafilt2);
freqfiltI3 = ft_freqanalysis(cfg, datafilt3);

allpow = [
  freq.powspctrm
  freqfilt1.powspctrm
  freqfilt2.powspctrm
  freqfilt3.powspctrm
  freqI.powspctrm
  freqfiltI.powspctrm
  freqfiltI2.powspctrm
  freqfiltI3.powspctrm
  ];

figure;
semilogy(freq.freq, allpow);
legend({
  'orig-unfilterd'
  'orig-bpfiltered'
  'orig-hpfiltered'
  'orig-dftfiltered'
  'frac-unfiltered'
  'frac-bpfiltered'
  'frac-hpfiltered'
  'frac-dftfiltered'
  });


function varargout = symmetrise(m,varargin)
% symmetrcially equivalent directions and its multiple
%
% Syntax
%   mSym = symmetrise(m)
%
%   % include antipodal symmetry
%   mSym = symmetrise(m,'antipodal')
%
%   % exclude antipodal symmetry
%   mSym = symmetrise(m,'noAntipodal')
%
%   % every symmetrically equivalent direction only once
%   [mSym,l,sym] = symmetrise(m,'unique')
%
%   % every symmetrically equivalent axis only once
%   [mSym,l,sym] = symmetrise(v,S,'unique','noAntipodal')
%
% Input
%  v - @Miller
%
% Output
%  mSym - sym * m  @Miller
%  l    - multiplicity of the crystal directions
%  sym  - @rotation
%
% Flags
%  antipodal   - include <VectorsAxes.html antipodal symmetry>
%  noAntipodal - do not include antipodal symmetry (without option unique)
%  noAntipodal - do not remove antipodal vectors (with option unique)
%  unique      - only return distinct axes or directions (noAntipodal)
%

[varargout{1:nargout}] = symmetrise@vector3d(m,m.CS,varargin{:});


% HIV system
% System identification: (Delay)DMDc

clear all, close all, clc
figpath = '../FIGURES/HIV/'; mkdir(figpath)
datapath = '../DATA/HIV/'; mkdir(datapath)
addpath('../utils');

SystemModel = 'HIV';

% Model type selected based on DATA_ENSEMBLE and  Ndelay
%% Generate Data
InputSignalType = 'prbs';
Nvar = 5;
Ndelay = 1;  % Choose 1 for DMDc, and 10 for DelayDMDc in paper
DATA_ENSEMBLE = 0; % Choose 0 to reproduce results in paper

%%
if DATA_ENSEMBLE == 1 % ONLY FOR Ndelay = 1 implemented
    ModelName = 'DMDc';
    getTrainingData_Ensemble

    %% Reshape
    X = x(1:end-1,:,:);         % Array of snapshots
    Xp = x(2:end,:,:);          % time-shifted Array
    U = u(1:end-1,:,:);         % Array of inputs
    M = size(X,1);
    n = size(X,2);

    % Reorganize data from array to time-state matrix
    X_tmp = zeros(Nvar,Nic*M);
    Xp_tmp = zeros(Nvar,Nic*M);
    U_tmp = zeros(1,Nic*M);
    for iIC = 1:Nic
        for i = 1:Nvar
            X_tmp(i,(iIC-1)*M+1:iIC*M) = X(:,i,iIC);
            Xp_tmp(i,(iIC-1)*M+1:iIC*M) = Xp(:,i,iIC);
        end
        U_tmp(1,(iIC-1)*M+1:iIC*M) = U(:,1,iIC);
    end
    X = X_tmp; Xp = Xp_tmp; U = U_tmp;
    clear X_tmp Xp_tmp U_tmp
    M = size(X,2);

    %% Construct data matrices // Mean-correction
    xmean = zeros(1,5); %mean(X,2)';%xref;
    X   = X - repmat(xmean',[1 M]);
    Xp  = Xp - repmat(xmean',[1 M]);

    r1 = size(X,1); r2 = size(Xp,1);
    [sysmodel_DMDc,Psi,Psi_p] = DelayDMDc_MV(zeros(5,1),U,r1,r2,dt,size(X,1),size(U,1),2,X,Xp);

    Nt = length(t)-1;

    %% Validation over training phase
    xDMDc_Valid = zeros(size(X));
    for iIC = 1:Nic
        [xDMDc,~] = lsim(sysmodel_DMDc,squeeze(u(1:Nt,1,iIC)),t(1:Nt),x0_ensemble(iIC,:));
        xDMDc = xDMDc + repmat(xmean,[Nt 1]);
        xDMDc_Valid(1:5,(iIC-1)*Nt+1:iIC*Nt) = xDMDc';
        disp(['PROGRESS: ',num2str(100*iIC/Nic),'%'])
    end
    %%
    figure, hold on, box on
    plot(X','-k','LineWidth',2)
    plot(xDMDc_Valid','--','LineWidth',2)
else

    getTrainingData
    %% DMDc: B = unknown  and with time delay coordinates
    Ndelay = 1; % 1 for DMDc, 10 for DelayDMDc in paper
    if Ndelay == 1
        ModelName = 'DMDc';
    elseif Ndelay>1
        ModelName = 'DelayDMDc';
    end

    % Construct data matrices
    Hu = getHankelMatrix_MV(u,Ndelay);
    xmean = xref; %zeros(1,Nvar);%xref;%mean(x);%xref; %mean(x);
    X   = x - repmat(xmean,[T 1]);
    Hx  = getHankelMatrix_MV(X,Ndelay);
    numOutputs = size(Hx,1); numInputs = size(Hu,1); numVar = 5;
    r1 = size(Hx,1); r2 = size(Hx,1);
    [sysmodel_DMDc,U,Up] = DelayDMDc_MV(Hx,Hu,size(Hx,1),size(Hx,1),dt,size(Hx,1),size(Hu,1),2);

    Nt = length(t)-Ndelay+1;
    %% Validation over training phase
    [xDMDc,~] = lsim(sysmodel_DMDc,Hu',tspan(1:Nt),Hx(:,1));
    xDMDc = xDMDc(:,end-Nvar+1:end);
    xDMDc = xDMDc + repmat(xmean,[Nt 1]);

    %% Show prediction over training stage
    clear ph
    figure,box on,
    ccolors = get(gca,'colororder');
    ccolors_valid = [ccolors(1,:)-[0 0.2 0.2];
        ccolors(2,:)-[0.1 0.2 0.09];
        ccolors(3,:)-[0.1 0.2 0.09];
        ccolors(4,:)-[0.1 0.1 0.2];
        ccolors(5,:)-[0.1 0.2 0.09]];
    for i = 1:Nvar
        ph(i) = semilogy(tspan,x(:,i),'-','Color',ccolors(i,:),'LineWidth',1); hold on
    end
    for i = 1:Nvar
        ph(Nvar+i) = semilogy(tspan(Ndelay:Nt+Ndelay-1),xDMDc(:,i),'--','Color',ccolors_valid(i,:),'LineWidth',2);
    end
    xlabel('Time')
    ylabel('xi')
    legend(ph([1,6]),'True',ModelName)
    set(gca,'LineWidth',1, 'FontSize',14)
    set(gcf,'Position',[100 100 300 200])
    set(gcf,'PaperPositionMode','auto')
    print('-depsc2', '-loose', '-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'.eps']);

end
%% Save Data & Model
Model.name = ModelName;
Model.sys = sysmodel_DMDc;
Model.Ndelay = Ndelay;
Model.dt = dt;
Model.xmean = xmean;
save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'.mat']),'Model')


function [Dx, Vx, mean_suv, max_suv, min_suv, Slope]=analyze_ivh(structNum, scanSet, plot_flag)
% IVH analysis
global planC
indexS=planC{end};
optS.IVHBinWidth=0.05;
[scansV, volsV] = getIVH(structNum, scanSet, planC);
[scanBinsV, volsHistV] = doseHist(scansV, volsV, optS.IVHBinWidth);
cumVolsV = cumsum(volsHistV);
cumVols2V  = cumVolsV(end) - cumVolsV;  %cumVolsV is the cumulative volume lt that corresponding scan
% calculate stats
mean_suv= calc_meanDose(scanBinsV, volsHistV);
max_suv=  calc_maxDose(scanBinsV, volsHistV);
min_suv=  calc_minDose(scanBinsV, volsHistV);
D50 = calc_Dx(scanBinsV, volsHistV,50)

Slope=calc_Slope(scanBinsV, volsHistV, D50, 0);

range = max_suv-min_suv;
param = [10:10:90];
for i=1:length(param)
    per=param(i)*range/100+min_suv; % ith percentile
    Dx(i)= calc_Dx(scanBinsV, volsHistV,param(i));
    Vx(i)= calc_Vx(scanBinsV, volsHistV,per,1);
end

%including that scan bin.
% if plot_flag
% %     if ~isempty(planC{indexS.scan}(scanSet).scanOffset)
% %         h = plot([0, scanBinsV - planC{indexS.scan}(scanSet).scanOffset], [1, cumVols2V/cumVolsV(end)]);
% %     else
% %        h = plot([0, scanBinsV], [1, cumVols2V/cumVolsV(end)]);
% %     end
% end

return


function [in] = um2in(um)
% Convert length from micrometers (or microns) to inches.
% Chad A. Greene 2012
in = um/25400;


function [xL,xR] = click_stereo(NUMBER_OF_POINTS,IL,IR,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left);

figure(1);
image(IL);

figure(2);
image(IR);

[ny,nx] = size(IL);

xL = [];
xR = [];

for kk = 1:NUMBER_OF_POINTS,

    figure(1);
    hold on;
    x = ginput(1);
    plot(x(1),x(2),'g.');
    hold off;
    x = x'-1;

    xL = [xL x];

    [epipole] = compute_epipole(x,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left);

    figure(2);
    hold on;
    h = plot(epipole(1,:)+1,epipole(2,:)+1,'r.','markersize',1);
    hold off;

    x2 = ginput(1);
    x2 = x2' - 1;

    NN = size(epipole,2);
    d = sum((epipole - repmat(x2,1,NN)).^2);
    [junk,indmin] = min(d);

    x2 = epipole(:,indmin);

    xR = [xR x2];

    delete(h);

    figure(2);
    hold on;
    plot(x2(1)+1,x2(2)+1,'g.');
    drawnow;
    hold off;

end;


function bnet = fgraph_to_bnet(fg)
% FGRAPH_TO_BNET Convert a factor graph to a Bayes net
% bnet = fgraph_to_bnet(fg)
%
% We assume all factors are tabular_CPD.
% We create 1 dummy observed node for every factor.

N = fg.nvars + fg.nfactors;
vnodes = 1:fg.nvars;
fnodes = fg.nvars+1:N;
dag = zeros(N);
for x=1:fg.nvars
  dag(x, fnodes(fg.dep{x})) = 1;
end
ns = [fg.node_sizes ones(1, fg.nfactors)];
discrete = [fg.dnodes fnodes];
bnet = mk_bnet(dag, ns, 'discrete', discrete);
for x=1:fg.nvars
  bnet.CPD{x} = tabular_CPD(bnet, x, 'CPT', 'unif');
end
ev = cell(1, fg.nvars); % no evidence
for i=1:fg.nfactors
  f = fnodes(i);
  e = fg.equiv_class(i);
  pot = convert_to_pot(fg.factors{e}, 'd', fg.dom{i}, ev);
  m = pot_to_marginal(pot);
  bnet.CPD{f} = tabular_CPD(bnet, f, 'CPT', m.T);
end


classdef CEC2010_F13 < PROBLEM
% <single> <real> <constrained>
% CEC'2010 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% R. Mallipeddi and P. N. Suganthan, Problem definitions and evaluation
% criteria for the CEC 2010 competition on constrained real-parameter
% optimization, Nanyang Technological University, Singapore, 2010.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2010.mat'),'Data');
            obj.O = Data{13};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 500;
            obj.upper    = zeros(1,obj.D) + 500;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = mean(-Z.*sin(sqrt(abs(Z))),2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = -50 + mean(Z.^2,2)/100;
            PopCon(:,2) = 50*mean(sin(pi*Z/50),2);
            PopCon(:,3) = 75 - 50*(sum((Z.^2/4000),2)-prod((cos(Z./repmat(sqrt(1:size(Z,2)),size(Z,1),1))),2)+1);
        end
    end
end