function soln = particleSwarmOptimization(problem)
% soln = particleSwarmOptimization(problem)
%
% This function computes the solution to the optimization problem using
% particle swarm optimization
%
% INPUTS:
%   problem = struct that describes the optimization problem
%       .options.maxIter = number of iterations for the search
%       .options.populationCount = number of particles to use in search
%       .options.w = search parameter - damping
%       .options.pl = search paramter - local search
%       .options.pg = search parameter - global search
%       .xLow = [nDim, 1] = column vector for lower bound on initial search space
%       .xUpp = [nDim, 1] = column vector for upper bound on initial search space
%       .objFun = function handle:
%           f = objFun(x)
%               x = [nDim, nPop] = vectorized objective function
%               f = [1,nPop] = cost for each column in x
%
% OUTPUTS:
%   soln = output struct with solution and history log
%       .x = best point found
%       .f = objective function at best point found
%       .log = history of the search, used for plotting and analysis
%       .problem = copy of the problem struct
%
% NOTES:
%   This is just a quick test function that I wrote to study how the
%   optimization parameters effect the behavior of the optimization, as
%   well as to figure out how it works with a noisy objective function. For
%   more rigerous optimization the code should be improved, most notably by
%   adding a convergence criteria and exit flag.
%
%

maxIter = problem.options.maxIter;
nPop = problem.options.populationCount;
nDim = length(problem.xLow);

% Memory allocation
Gx = zeros(nDim,1,maxIter);  %Global best solution point
Gf = zeros(1,1,maxIter);  %Global best solution value
Lx = zeros(nDim,nPop,maxIter);  %Local best solution
Lf = zeros(1,nPop,maxIter);  %Local best value
X = zeros(nDim,nPop,maxIter);  %Particle location
F = zeros(1,nPop,maxIter);  %objFun(X);
V = zeros(nDim,nPop,maxIter);  %Particle velocity

% Linear transform for vectorized random numbers
a = problem.xLow*ones(1,nPop);
b = (problem.xUpp-problem.xLow)*ones(1,nPop);

% Initialization:
X(:,:,1) = a + b.*rand(nDim,nPop);
V(:,:,1) = -abs(b) + 2*abs(b).*rand(nDim,nPop);
F(1,:,1) = problem.objFun(X(:,:,1));
Lx(:,:,1) = X(:,:,1);
Lf(1,:,1) = F(1,:,1);
[Gf(1,1,1),idx] = min(F(1,:,1));
Gx(:,1,1) = Lx(:,idx,1);

% Run the optimization here:
pl = problem.options.pl;
pg = problem.options.pg;
w = problem.options.w;
for i=2:maxIter
    rl = rand(nDim,nPop);  %Local random search dist
    rg = rand(nDim,nPop);  %global random search dist

    %%% Update all points
    V(:,:,i) = w*V(:,:,i-1) + ...    % Inertial terms
        pl.*rl.*(Lx(:,:,i-1)-X(:,:,i-1)) + ...  % Local search
        pg.*rg.*(Gx(:,1,i-1)*ones(1,nPop)-X(:,:,i-1));   %Global search
    X(:,:,i) = X(:,:,i-1) + V(:,:,i);  %Position update
    F(1,:,i) = problem.objFun(X(:,:,i));  %Objective function evaluation

    %%% Update local best:
    update = F(1,:,i) < Lf(1,:,i-1);  %Points that have new best values
    Lf(1,update,i) = F(1,update,i);
    Lf(1,~update,i) = Lf(1,~update,i-1);
    Lx(:,update,i) = X(:,update,i);
    Lx(:,~update,i) = Lx(:,~update,i);

    %%% Update global best:
    [Gf(1,1,i),idx] = min(Lf(1,:,i));
    Gx(:,1,i) = Lx(:,idx,i);

end

% Store the final solution
soln.x = reshape(Gx(:,1,end),nDim,1);
soln.f = Gf(end);

% Store all of the search history for analysis
soln.log.Gx = Gx;
soln.log.Gf = Gf;
soln.log.Lx = Lx;
soln.log.Lf = Lf;
soln.log.X = X;
soln.log.F = F;
soln.log.V = V;
soln.problem = problem;

end


function r8mat_normal_ab_test ( )

%*****************************************************************************80
%
%% R8MAT_NORMAL_AB_TEST tests R8MAT_NORMAL_AB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8MAT_NORMAL_AB_TEST\n' );
  fprintf ( 1, '  R8MAT_NORMAL_AB computes a scaled pseudonormal matrix.\n' );

  m = 5;
  n = 4;
  mu = 100.0;
  sigma = 5.0;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  MU = %g\n', mu );
  fprintf ( 1, '  SIGMA = %g\n', sigma );
  fprintf ( 1, '  SEED = %d\n', seed );

  [ r, seed ] = r8mat_normal_ab ( m, n, mu, sigma, seed );

  r8mat_print ( m, n, r, '  The matrix:' );

  return
end


%% OPTI Toolbox Mixed-Integer Linear Programming Examples
%
% This file contains a number of MILP problems and demonstrates how to
% solve them using the OPTI Toolbox. You should read and complete
% BasicUsage.m & LinearProgramming.m BEFORE running the below examples.
%
%   Copyright (C) 2014 Jonathan Currie (IPL)

% There is also a page on the Wiki which supplements this example:
web('https://www.inverseproblem.co.nz/OPTI/index.php/Probs/MILP');

%% Determing which Solver to Use
% OPTI Toolbox comes with a number of MILP solvers, thus to determine which
% ones are available on your system you can type:
clc
optiSolver('MILP')

%% Example 1
% This is a simple two decision variable MILP which will use for the next
% few examples
clc
f = -[6 5]';                %Objective Function (min f'x)
A = [1,4; 6,4; 2, -5];      %Linear Inequality Constraints (Ax <= b)
b = [16;28;6];
lb = [0;0];                 %Bounds on x (lb <= x <= ub)
ub = [10;10];
xtype = 'II';               %Integer Variables (I = integer, C = continuous, B = binary)

% Building an MILP problem is very similar to an LP, except just add the
% 'xtype' argument for integer variables
Opt = opti('f',f,'ineq',A,b,'bounds',lb,ub,'xtype',xtype)

%Call solve to solve the problem
[x,fval,exitflag,info] = solve(Opt)

%% Example 2 - Alternative Integer Setup
% You can also supply an vector of integer indices indicating the position of
% continuous and integer variables respectively (note no binary variables
% can be entered this way)

xtype = [1 2];

Opt = opti('f',f,'ineq',A,b,'bounds',lb,ub,'xtype',xtype);
solve(Opt);

%% Example 3 - Plotting the Solution
% Several problem types have a default plot command available. Note for
% MILP plots it will also plot the integer constraints.

plot(Opt)

%% Example 4 - Solving a slightly bigger MILP
%Build the opti problem:
clc

%Problem
f = -[1 2 3 1]';
A = [-1 1 1 10; 1 -3 1 0];
b = [20;30];
Aeq = [0 1 0 -3.5];
beq = 0;
lb = [0;0;0;2];
ub = [40;inf;inf;3];
xtype = 'CCCI';

Opt = opti('f',f,'ineq',A,b,'eq',Aeq,beq,'bounds',lb,ub,'xtype',xtype)

[x,fval,exitflag,info] = solve(Opt)

%% Example 5 - Calling a solver directly
% All OPTI Linear and Quadratic solvers use standard MATLAB function
% prototypes (such as linprog or quadprog), so you can skip using the OPTI
% class all together. Note be sure to check the integer argument form for
% the particular solver by typing e.g. help opti_glpk
clc
[x,fval,exitflag,info] = opti_glpk(f,A,b,Aeq,beq,lb,ub,xtype)

%% Example 6 - Sparse MILPs
% As with LPs, all solvers are setup to directly solve sparse systems, which
% is the preferred format for most solvers:
clc
% A larger sparse MILP
load sparseMILP1;

opts = optiset('solver','glpk');    %Solve with GLPK
Opt = opti('f',f,'ineq',A,b,'eq',Aeq,beq,'xtype',find(xint),'options',opts)
[x,fval,exitflag,info] = solve(Opt);
fval
info

%% Problem 4
% MILP with Special Ordered Sets (SOS)
clc
%Problem
f = [-1 -1 -3 -2 -2]';
A = [-1 -1 1 1 0;
      1 0 1 -3 0];
b = [30;30];
lb = zeros(5,1);
ub = [40;1;inf;inf;1];

%SOS type 1
sos_type = '1';
sos_index = [1 2 3 4 5]';
sos_weight = [1 2 3 4 5]';

% Build the problem, specifying the three SOS fields. Note only some MILP
% solvers are setup to solve problems with SOS:
opts = optiset('solver','cbc');
Opt = opti('f',f,'ineq',A,b,'bounds',lb,ub,'sos',sos_type,sos_index,sos_weight,'options',opts)

[x,fval,exitflag,info] = solve(Opt)


function [class, Simil] = classifier(data, ideals, y)
%   INPUT:
%
%   data    =    datamatrix
%   ideals  =    idealvectors
%   y(1)    =    p-value in similarity measure
%   y(2)    =    alpha value for OWA weights
%   y(3)    =    used owa aggregator
%
%
%   OUTPUT:
%
%   class   =    column vector of the classes in which the samples are classified
%   Simil   =    similarity values for each class

[nc, v_dim] = size(ideals);
d_dim = size(data,1);
Simil = zeros(d_dim, nc);

if nargin==2
    y = [1, 1, 1];
end

for j = 1 : nc

    Ideal = repmat(ideals(j,:),d_dim,1);

    if y(3) == 1 %Using OWA with Linguistic quantifier 1
        w=owaw1(v_dim,y(2));
        tmpmatrix=(1-abs(data.^y(1)-Ideal.^y(1))).^(1/y(1));
        Simil(:,j)=owamatrix(tmpmatrix,w);
    elseif    y(3) == 2 %Using OWA with Linguistic quantifier 2
        w=owaw2(v_dim,y(2));
        tmpmatrix=(1-abs(data.^y(1)-Ideal.^y(1))).^(1/y(1));
        Simil(:,j)=owamatrix(tmpmatrix,w);
    elseif    y(3) == 3 %Using OWA with Linguistic quantifier 3
        w=owaw3(v_dim,y(2));
        tmpmatrix=(1-abs(data.^y(1)-Ideal.^y(1))).^(1/y(1));
        Simil(:,j)=owamatrix(tmpmatrix,w);
    elseif    y(3) == 4 %Using OWA with Linguistic quantifier 4
        w=owaw4(v_dim,y(2));
        tmpmatrix=(1-abs(data.^y(1)-Ideal.^y(1))).^(1/y(1));
        Simil(:,j)=owamatrix(tmpmatrix,w);
    elseif  y(3) == 5   %O'Hagan's method
        tmpmatrix=(1-abs(data.^y(1)-Ideal.^y(1))).^(1/y(1));
        weights=Ohaganw(v_dim,y(2));
        index= 1;
        w=weights(index,:);
        Simil(:,j)=owamatrix(tmpmatrix,w);
    end

end

[simil_val, class] = max(Simil');
class=class';


function [f] = spm_mountaincar_fun(P,G)
% [Cross-entropy] objective function for mountain car problem
% FORMAT [f] = spm_mountaincar_fun(P,G)
%
% P = spm_vec(P)
% P.a - 0th order coefficients of force
% P.b - 1st order coefficients of force
% P.c - 2nd order coefficients of force
% P.d - action efficacy
%
% G   - world model; including
%    G.fq : function fq(x) returning desired equilibrium density at x
%    G.X  : matrix of locations in x
%
% f   - KL divergence between actual and desired equilibrium densities
% x   - cell of grid point support
%
% see:
% Gaussian Processes in Reinforcement Learning
% Carl Edward Rasmussen and Malte Kuss
% Max Planck Institute for Biological Cybernetics
% Spemannstraße 38, 72076 T¨ubingen, Germany
% {carl,malte.kuss}@tuebingen.mpg.de
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mountaincar_fun.m 7679 2019-10-24 15:54:07Z spm $

% place paramters in model
%--------------------------------------------------------------------------
G(1).pE = P;

% get equilibrium density
%--------------------------------------------------------------------------
[M0,q0] = spm_fp(G);

% desired equilibrium density
%--------------------------------------------------------------------------
q   = feval(G(1).fq,G(1).X);

% KL divergence or cross entropy
%--------------------------------------------------------------------------
q   = q/sum(q(:));
D   = q(:)./q0(:);
i   = find(D > exp(-16));
f   = q(i)'*log(D(i));


%sim_spoil.m
%Jamie Near, 2014.
%
% USAGE:
% d_out = sim_spoil(d_in,H,angle)
%
% DESCRIPTION:
% This function simulates the effect of a rotation about the z-axis.
%
% INPUTS:
% d_in      = input density matrix structure.
% H         = Hamiltonian operator structure.
% angle     = Spoil angle (degrees).
%
% OUTPUTS:
% d_out     = output density matrix following z-rotation.

function d_out = sim_spoil(d_in,H,angle)

for m=1:length(H)
    %make vector of spoil angles;
    angle=angle*ones(length(H(m).shifts),1);

    %Make spoiler hamiltonian;
    spoil=zeros(2^H(m).nspins,2^H(m).nspins);
    for n=1:H(m).nspins
        spoil=spoil+((angle(n)*pi/180)*H(m).Iz(:,:,n));
    end

    %Do matrix multiplication;
    p=expm(1i*spoil);
    d_out{m} = p' * d_in{m} * p;
end


lambda1= 1;    % [cells/(mm^3 day)] Production source rate of CD4+ T-cells
d      = 0.1;  % Death rate of CD4+ T-cells
alpha1  = 1;   % Infection rate
a      = 0.2;
p1     = 1;
p2     = 1;
c1     = 0.03;
c2     = 0.06;
b1     = 0.1;
b2     = 0.01;
q      = 0.5;
eta    = 0.9799;
h      = .1;


function d = AngDiff(x, y)
%ANGDIFF  Compute angle difference accurately
%
%   D = ANGDIFF(X, Y) computes Y - X, reduces the result to (-180,180] and
%   rounds the result.  X and Y must be in [-180,180].  X and Y can be any
%   compatible shapes.

  [d, t] = sumx(-x, y);
  c = (d - 180) + t > 0;
  d(c) = (d(c) - 360) + t(c);
  c = (d + 180) + t <= 0;
  d(c) = (d(c) + 360) + t(c);
end


function [S_sorted,s]=sort_ordinal(S)
% SORT_ORDINAL reorders nodes to emphasize persistent structure in an ordered multilayer partition
%
% Version: 2.2.0
% Date: Thu 11 Jul 2019 12:25:42 CEST
%
% Nodes are reordered using the optimal leave order for the
% average linkage hierarchical clustering tree based on Hamming distance
% between community assignments
%
% Call as:
%
%     [S_sorted, s] = sort_ordinal(S)
%
% Input:
%
%     S: multilayer partition (matrix of size NxT, where N is the number
%        of nodes and T is the number of layers)
%
% Output:
%
%     S_sorted: reordered multilayer partition
%
%     s: mapping of nodes to reordered nodes
%
% Note that S_sorted=S(s, :)

d1=pdist(S,'hamming');
Z1=linkage(d1,'average');
s=optimalleaforder(Z1,d1);
S_sorted=S(s,:);


function [exo_irf_record,exo_irf_estimates]=irfexo(beta_gibbs,It,Bu,IRFperiods,IRFband,n,m,p,k,prior)

% create the cell aray that will store the values from the simulations
exo_irf_record=cell(n,m);

% deal with shocks in turn
for ii=1:m

   % step 1: repeat the simulation process a number of times equal to the number of Gibbs iterations
   for kk=1:It-Bu

   % step 3: draw beta from its posterior distribution
   beta=beta_gibbs(:,kk);
   % reshape to obtain B
   B=reshape(beta,k,n);
   % recover C, the matrix of coefficients on exogenous
   C=B(end-m+1:end,:);
   % create a matrix of zeros of dimension p*n
   Y=zeros(p,n);
   %  step 2: set the value of the last row, column i, equal to 1
   Y(p,:)=C(ii,:);

      % step 4: for each iteration kk, repeat the algorithm for periods T+1 to T+h
      for jj=1:IRFperiods-1
% % %           if prior==61
% % %          %use the function lagx to obtain the matrix X; retain only the last row
% % %    X=bear.lagx(Y,p-1);
% % %    X=X(end,:);
% % %
% % %
% % %     else

      % use the function lagx to obtain a matrix temp, containing the endogenous regressors
      temp=bear.lagx(Y,p-1);

      % define the vector X
      X=[temp(end,:) zeros(1,m)];
% % %           end
      % obtain the predicted value for T+jj
      yp=X*B;

      % concatenate yp at the top of Y
      Y=[Y;yp];

      % repeat until values are obtained for T+h
      end

   % step 5: record the results from current iteration in cell irf_record
      % loop over variables
      for jj=1:n
      % consider column jj of matrix 'Y' and trim the (p-1) initial periods: what remains is the series of IRFs for period T to period T+h-1, for variable jj
      temp=Y(p:end,jj);
      % record these values in the corresponding matrix of irf_record
      exo_irf_record{jj,ii}(kk,:)=temp';
      end

   % then go for next iteration
   end

% conduct the same process with shocks in other variables
end

% create first the cell that will contain the IRF estimates
exo_irf_estimates=cell(n,m);

% for the response of each variable to each shock, and each IRF period, compute the median, lower and upper bound from the Gibbs sampler records
% consider variables in turn
for ii=1:n
   % consider shocks in turn
   for jj=1:m
      % consider IRF periods in turn
      for kk=1:IRFperiods
      % compute first the lower bound
      exo_irf_estimates{ii,jj}(1,kk)=quantile(exo_irf_record{ii,jj}(:,kk),(1-IRFband)/2);
      % then compute the median
      exo_irf_estimates{ii,jj}(2,kk)=quantile(exo_irf_record{ii,jj}(:,kk),0.5);
      % finally compute the upper bound
      exo_irf_estimates{ii,jj}(3,kk)=quantile(exo_irf_record{ii,jj}(:,kk),1-(1-IRFband)/2);
      end
   end
end


function y = objfun(x)

y = (1/3)*x(1)^2+(1/2)*x(2)^2;


function pdepetrans
% 1D transport - modelling with extensions for decay and fast sorption
%    using MATLAB pdepe
%
%   $Ekkehard Holzbecher  $Date: 2006/03/16 $
%--------------------------------------------------------------------------
T = 1;                     % maximum time [s]
L = 1;                     % length [m]
D = 1;                     % diffusivity [m*m/s]
v = 1;                     % velocity [m/s]
lambda = 0.0;              % decay constant [1/s]
sorption = 2;              % sorption-model: no sorption (0), linear (1),
                           %                 Freundlich (2), Langmuir (3)
k1 = 0.0004;               % sorption parameter 1 (R=0 for linear isotherm with Kd, else k1=R)
k2 = 0.5;                  % sorption parameter 2 (Kd for linear isotherm with Kd)
rhob = 1300;               % porous medium bulk density [kg/m*m*m]
theta = 0.2;               % porosity [-]
c0 = 0.0;                  % initial concentration [kg/m*m*m]
cin = 1;                   % boundary concentration [kg/m*m*m]

M = 40;                    % number of timesteps
N = 40;                    % number of nodes
%-------------------------- output parameters
gplot = 0;                 % =1: breakthrough curves; =2: profiles
gsurf = 0;                 % surface
gcont = 0;                 % =1: contours; =2: filled contours
ganim = 2;                 % animation of profiles; =1: single line; =2: all lines

t = linspace (T/M,T,M);    % time discretization
x = linspace (0,L,N);      % space discretization

%----------------------execution-------------------------------------------
if sorption == 1 & k1 <=0
    k1 = 1+k2*rhob/theta;
else
    if sorption > 1 k1 = rhob*k1/theta; end
end
options = odeset; if (c0 == 0) c0 = 1.e-20; end
c = pdepe(0,@transfun,@ictransfun,@bctransfun,x,[0 t],options,D,v,lambda,sorption,k1,k2,c0,cin);

%---------------------- graphical output ----------------------------------
switch gplot
    case 1
        plot ([0 t],c)        % breakthrough curves
        xlabel ('time'); ylabel ('concentration');
    case 2
        plot (x,c','--')      % profiles
        xlabel ('space'); ylabel ('concentration');
end
if gsurf                      % surface plot
    figure; surf (x,[0 t],c);
    xlabel ('space'); ylabel ('time'); zlabel('concentration');
end
if gcont figure; end
switch gcont
    case 1
        contour (x,[0 t],c)   % contours
        grid on; xlabel ('space'); ylabel ('time');
    case 2
        contourf(x,[0 t],c)   % filled contours
        colorbar; xlabel ('space'); ylabel ('time');
end
if (ganim)
    [FileName,PathName] = uiputfile('*.mpg');
    figure; if (ganim > 1) hold on; end
    for j = 1:size(c,1)
        axis manual;  plot (x,c(j,:),'r','LineWidth',2);
        YLim = [min(c0,cin) max(c0,cin)];
        legend (['t=' num2str(T*(j-1)/M)]);
        Anim(j) = getframe;
        plot (x,c(j,:),'b','LineWidth',2);
    end
    mpgwrite (Anim,colormap,[PathName '/' FileName]);     % mgwrite not standard MATLAB
    movie (Anim,0);   % play animation
end

%----------------------functions------------------------------
function [c,f,s] = transfun(x,t,u,DuDx,D,v,lambda,sorption,k1,k2,c0,cin)
switch sorption
    case 0
        R = 1;
    case 1
        R = k1;
    case 2
        R = 1+k1*k2*u^(k2-1);
    case 3
        R = 1+k1*k2*u/(k2+u)/(k2+u);
end
c = R;
f = D*DuDx;
s = -v*DuDx -lambda*R*u;
% --------------------------------------------------------------
function u0 = ictransfun(x,D,v,lambda,sorption,k1,k2,c0,cin)
u0 = c0;
% --------------------------------------------------------------
function [pl,ql,pr,qr] = bctransfun(xl,ul,xr,ur,t,D,v,lambda,sorption,k1,k2,c0,cin)
pl = ul-cin;
ql = 0;
pr = 0;
qr = 1;


% Test file for Gabor_region_covariance() and region_covariance()

close all;
clear;
clc;

%% load image
img = imread('../images/peppers.png');
% resize
img_resize = imresize(img, [32 32]);
% show resized image
%imshow(img_resize)

%% test Gabor_region_covariance()
clear options;
orientation = 8;                % orientation
scale = 5;                      % scale
options.gw_display = true;      % display Gabor wavelet
options.spd_projection = true;  % project onto SPD
[GRCM1, GRCM2, GRCM3] = Gabor_region_covariance(img_resize, orientation, scale, options);

%% test region_covariance()
clear options;
options.spd_projection = true;  % project onto SPD
[RCM1, RCM2, RCM3, RCM4, RCM5] = region_covariance(img_resize, options);


function main
%
% This is a demo program of the paper J. Tian, L. Ma, and W. Yu, "Ant
% colony optimization for wavelet-based image interpolation using a
% three-component exponential mixture model," Expert Systems with
% Applications, Vol. 38, No. 10, Sept. 2011, pp. 12514-12520.
%
% Contact: eejtian@gmail.com
%
% Note that the PSNR values could be slightly different with that reported
% in paper due to the random number generator.
%
% The input image should have a square size.
%
% Acknowledgement: This program needs Wavelet transform toolbox, which is
% downloaded from http://www-stat.stanford.edu/~wavelab/.

clear all; close all; clc;

img_truth = im2double(imread('lena_truth.bmp'));

% Generate low-resolution image
psf_filter =  fspecial('average', 3);
img_in = imfilter(img_truth, psf_filter, 'symmetric');
wfilter_type = 'bior2.2';
[Lo_D,Hi_D,Lo_R,Hi_R] = wfilters(wfilter_type);
Lo_D = Lo_D ./ sqrt(2);
Hi_D = Hi_D ./ sqrt(2);
Lo_R = Lo_R .* sqrt(2);
Hi_R = Hi_R .* sqrt(2);
[ca1,ch1,cv1,cd1] = dwt2(img_in, Lo_D, Hi_D, 'mode', 'per');
img_low = ca1;

%--------------------------------------------------------------------------
%-------------- PASS 1: Generate mask using ACO for interpolation ---------
%--------------------------------------------------------------------------
[ca1,ch1,cv1,cd1] = dwt2(img_low, Lo_D, Hi_D, 'mode', 'per');
[ca2,ch2,cv2,cd2] = dwt2(ca1, Lo_D, Hi_D, 'mode', 'per');

ch1_mask = func_aco_thresholding_estimate_mask(ch1);
cv1_mask = func_aco_thresholding_estimate_mask(cv1);
cd1_mask = func_aco_thresholding_estimate_mask(cd1);
ch2_mask = func_aco_thresholding_estimate_mask(ch2);
cv2_mask = func_aco_thresholding_estimate_mask(cv2);
cd2_mask = func_aco_thresholding_estimate_mask(cd2);

%--------------------------------------------------------------------------
%-------------- PASS 2: Image interpolation in wavelet domain -------------
%--------------------------------------------------------------------------
% Parameter setting
img_in = img_low;
nEnlargeFactor = 2;
win_size = 7;
nTotalIteration = 10;

[ca1,ch1,cv1,cd1] = dwt2(img_in, Lo_D, Hi_D, 'mode', 'per');
[ca2,ch2,cv2,cd2] = dwt2(ca1, Lo_D, Hi_D, 'mode', 'per');

[ch1_par1, ch1_par2, ch1_par3] = func_proposed_three_state_estimation_aco_seg(ch1, ch1_mask, win_size);
[ch2_par1, ch2_par2, ch2_par3] = func_proposed_three_state_estimation_aco_seg(ch2, ch2_mask, win_size);
[cd1_par1, cd1_par2, cd1_par3] = func_proposed_three_state_estimation_aco_seg(cd1, cd1_mask, win_size);
[cd2_par1, cd2_par2, cd2_par3] = func_proposed_three_state_estimation_aco_seg(cd2, cd2_mask, win_size);
[cv1_par1, cv1_par2, cv1_par3] = func_proposed_three_state_estimation_aco_seg(cv1, cv1_mask, win_size);
[cv2_par1, cv2_par2, cv2_par3] = func_proposed_three_state_estimation_aco_seg(cv2, cv2_mask, win_size);

% Variance estimation
ch0_par1 = func_proposed_par_propogation(ch1_par1,ch2_par1,nEnlargeFactor);
cv0_par1 = func_proposed_par_propogation(cv1_par1,cv2_par1,nEnlargeFactor);
cd0_par1 = func_proposed_par_propogation(cd1_par1,cd2_par1,nEnlargeFactor);

ch0_par2 = func_proposed_par_propogation(ch1_par2,ch2_par2,nEnlargeFactor);
cv0_par2 = func_proposed_par_propogation(cv1_par2,cv2_par2,nEnlargeFactor);
cd0_par2 = func_proposed_par_propogation(cd1_par2,cd2_par2,nEnlargeFactor);

ch0_par3 = func_proposed_par_propogation(ch1_par3,ch2_par3,nEnlargeFactor);
cv0_par3 = func_proposed_par_propogation(cv1_par3,cv2_par3,nEnlargeFactor);
cd0_par3 = func_proposed_par_propogation(cd1_par3,cd2_par3,nEnlargeFactor);

% Coefficient index extrapolation
ch0_mask = imresize(ch1_mask, size(ch1_mask)*nEnlargeFactor, 'nearest');
cv0_mask = imresize(cv1_mask, size(cv1_mask)*nEnlargeFactor, 'nearest');
cd0_mask = imresize(cd1_mask, size(cd1_mask)*nEnlargeFactor, 'nearest');

rand('state',sum(100*clock));
ch0_mask_change = rand(size(ch0_mask));
ch0_mask_change = (ch0_mask_change>=0.5);
ch0_mask = (ch0_mask==1).*(ch0_mask_change==1).*0 + (ch0_mask==1).*(ch0_mask_change==0).*1 ...
            + (ch0_mask==-1).*(ch0_mask_change==1).*0 + (ch0_mask==-1).*(ch0_mask_change==0).*-1 ...
            + (ch0_mask==0).*ch0_mask;

cv0_mask_change = rand(size(cv0_mask));
cv0_mask_change = (cv0_mask_change>=0.5);
cv0_mask = (cv0_mask==1).*(cv0_mask_change==1).*0 + (cv0_mask==1).*(cv0_mask_change==0).*1 ...
            + (cv0_mask==-1).*(cv0_mask_change==1).*0 + (cv0_mask==-1).*(cv0_mask_change==0).*-1 ...
            + (cv0_mask==0).*cv0_mask;

cd0_mask_change = rand(size(ch0_mask));
cd0_mask_change = (ch0_mask_change>=0.5);
cd0_mask = (cd0_mask==1).*(cd0_mask_change==1).*0 + (cd0_mask==1).*(cd0_mask_change==0).*1 ...
            + (cd0_mask==-1).*(cd0_mask_change==1).*0 + (cd0_mask==-1).*(cd0_mask_change==0).*-1 ...
            + (cd0_mask==0).*cd0_mask;

% Generate the highest subbands
result = zeros(size(img_in,1).*nEnlargeFactor, size(img_in,2).*nEnlargeFactor);

for ii=1:nTotalIteration
    ch0 = func_proposed_gen_subband(ch0_par1, ch0_par2, ch0_par3, ch0_mask);
    cv0 = func_proposed_gen_subband(cv0_par1, cv0_par2, cv0_par3, cv0_mask);
    cd0 = func_proposed_gen_subband(cd0_par1, cd0_par2, cd0_par3, cd0_mask);
    temp = idwt2(img_in,ch0,cv0,cd0, Lo_R, Hi_R, 'mode', 'per');
    temp(temp<0) = 0;
    temp(temp>1) = 1;
    result = result + temp./nTotalIteration;
end

% Write the output image and perform PSNR evaluation
fprintf('The psnr is %.2f dB \n', func_psnr_gray(img_truth.*255, result.*255));
imwrite(uint8(result.*255), ['lena_rec.bmp'], 'bmp');

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function result = func_aco_thresholding_estimate_mask(wxin)

ant_search_range_row_min = ones(size(wxin));
ant_search_range_row_max = ones(size(wxin)).*size(wxin,1);
ant_search_range_col_min = ones(size(wxin));
ant_search_range_col_max = ones(size(wxin)).*size(wxin,2);

% System setup
ant_move_step_within_iteration = 15;
total_iteration_num = 10;
search_clique_mode = 8;

wx = abs(wxin);
[nrow, ncol] = size(wx);
wx_1D = func_2D_LexicoOrder(wx);

% initialization
h = zeros(size(wx));
h_1D = func_2D_LexicoOrder(h);
p = 1./(wx.*(wx~=0)+1.*(wx==0)).*(wx~=0)+(wx==0);
p_1D = func_2D_LexicoOrder(p);

alpha = 1;
beta = 2;
rho = 0.1;
w = 0.6;
A = 5000;
B = 10;

%use one ant for each pixel position
ant_total_num = nrow*ncol;
ant_current_row = zeros(ant_total_num, 1);
ant_current_col = zeros(ant_total_num, 1);
ant_current_val = zeros(ant_total_num, 1);
rr = (meshgrid(1:nrow))'; cc = meshgrid(1:ncol);
ant_current_row = rr(:);
ant_current_col = cc(:);
ant_current_val = wx_1D((ant_current_row-1).*ncol+ant_current_col);

ant_search_range_row_min = ant_search_range_row_min(:);
ant_search_range_row_min = padarray(ant_search_range_row_min, [0 search_clique_mode-1],'replicate','post');
ant_search_range_row_max = ant_search_range_row_max(:);
ant_search_range_row_max = padarray(ant_search_range_row_max, [0 search_clique_mode-1],'replicate','post');
ant_search_range_col_min = ant_search_range_col_min(:);
ant_search_range_col_min = padarray(ant_search_range_col_min, [0 search_clique_mode-1],'replicate','post');
ant_search_range_col_max = ant_search_range_col_max(:);
ant_search_range_col_max = padarray(ant_search_range_col_max, [0 search_clique_mode-1],'replicate','post');

for nIteration = 1: total_iteration_num

    ant_current_path_val_mean = zeros(ant_total_num,1);
    ant_current_path_val_mean = ant_current_val;

    for nMoveStep = 1: ant_move_step_within_iteration-1

        if search_clique_mode == 4
            ant_search_range_row = [ant_current_row-1, ant_current_row, ant_current_row+1, ant_current_row];
            ant_search_range_col = [ant_current_col, ant_current_col+1, ant_current_col, ant_current_col-1];
        elseif search_clique_mode == 8
            ant_search_range_row = [ant_current_row-1, ant_current_row-1, ant_current_row-1, ant_current_row, ant_current_row,ant_current_row+1, ant_current_row+1, ant_current_row+1];
            ant_search_range_col = [ant_current_col-1, ant_current_col, ant_current_col+1, ant_current_col-1, ant_current_col+1, ant_current_col-1, ant_current_col, ant_current_col+1];
        end

        ant_current_row_extend = padarray(ant_current_row, [0 search_clique_mode-1],'replicate','post');
        ant_current_col_extend = padarray(ant_current_col, [0 search_clique_mode-1],'replicate','post');
        ant_search_range_val = zeros(ant_total_num,search_clique_mode);

        %replace the positions our of the image's range

        temp = (ant_search_range_row>=ant_search_range_row_min) & (ant_search_range_row<=ant_search_range_row_max) & (ant_search_range_col>=ant_search_range_col_min) & (ant_search_range_col<=ant_search_range_col_max);
        ant_search_range_row = temp.*ant_search_range_row + (~temp).*ant_current_row_extend;
        ant_search_range_col = temp.*ant_search_range_col + (~temp).*ant_current_col_extend;

        ant_search_range_transit_prob_h = zeros(size(ant_search_range_val));
        ant_search_range_transit_prob_p = zeros(size(ant_search_range_val));

        for ii=1:search_clique_mode
            ant_search_range_val(:,ii) = wx_1D((ant_search_range_row(:,ii)-1).*ncol+ant_search_range_col(:,ii));

            temp = ant_search_range_val(:,ii);
            temp = abs(temp - ant_current_path_val_mean);
            temp = 1./(temp.*(temp~=0)+1.*(temp==0)) .* (temp~=0)+(temp==0);

            ant_search_range_transit_prob_h(:,ii) = temp;
            ant_search_range_transit_prob_p(:,ii) = p_1D((ant_search_range_row(:,ii)-1).*ncol+ant_search_range_col(:,ii));

        end
        temp = (ant_search_range_transit_prob_h.^alpha) .* (ant_search_range_transit_prob_p.^beta);

        temp_sum = sum(temp,2);
        temp_sum = padarray(temp_sum, [0 search_clique_mode-1],'replicate','post');

        ant_search_range_transit_prob = temp ./ temp_sum;

        % generate a random number to determine the next position.
        rand('state', sum(100*clock));
        temp = rand(ant_total_num,1);
        temp = padarray(temp, [0 search_clique_mode-1],'replicate','post');
        temp = cumsum(ant_search_range_transit_prob,2)>=temp;
        temp = padarray(temp, [0 1],'pre');
        temp = (diff(temp,1,2)==1);

        temp_row = (ant_search_range_row .* temp)';
        [ii, jj, vv] = find(temp_row);
        ant_next_row = vv;
        temp_col = (ant_search_range_col .* temp)';
        [ii, jj, vv] = find(temp_col);
        ant_next_col = vv;

        ant_current_row = ant_next_row;
        ant_current_col = ant_next_col;
        ant_current_val = wx_1D((ant_current_row-1).*ncol+ant_current_col);
        ant_current_path_val_mean = (ant_current_path_val_mean.*nMoveStep + ant_current_val)/(nMoveStep+1);

        rr = ant_current_row;
        cc = ant_current_col;
        p_1D((rr-1).*ncol+cc,1) = w*p_1D((rr-1).*ncol+cc,1) + 1./(A+B.*ant_current_path_val_mean);
        p = func_LexicoOrder_2D(p_1D, nrow, ncol);

    end % end of nMoveStep

    p = (1-rho).*p;
    p_1D = func_2D_LexicoOrder(p);

end % end of nIteration

clear h h_1D temp
clear ant_current_row ant_current_col ant_current_val
clear ant_current_path_val_mean
clear ant_search_range_row search_range_path_col ant_search_range_val
clear ant_search_range_transit_prob_h ant_search_range_transit_prob_v ant_search_range_transit_prob
clear ant_search_range_row_min ant_search_range_row_max ant_search_range_col_min ant_search_range_col_max

result = func_determine_class_fcm(wxin, p);

% ******************************************************************************
% **************************Inner Function *************************************
% ******************************************************************************

function idx = func_determine_class_fcm(wxin, p)
wx = abs(wxin);
p_1D = func_2D_LexicoOrder(p);
wx_1D = func_2D_LexicoOrder(wx);
[nrow, ncol] = size(wx);

nFeature(:,1) = p_1D(:)./(max2(p_1D));
nFeature(:,2) = wx_1D(:)./(max2(wx_1D));
[center,U,obj_fcn] = fcm(nFeature, 2,[2.0 100 1e-5 0]);

idx = zeros(nrow:ncol,1);
if sum(sum(U(1,:) >= U(2,:))) >= sum(sum(U(1,:) < U(2,:)))
    idx = (U(1,:) < U(2,:));
else
    idx = (U(1,:) >= U(2,:));
end
idx = func_LexicoOrder_2D(idx, nrow, ncol);
idx = double(idx);
idx((idx==1)&(wxin<0)) = -1;
idx((idx==1)&(wxin>=0)) = 1;
idx(idx==0) = 0;

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function result = func_LexicoOrder_2D(x, nRow, nColumn)
result = reshape(x, nColumn, nRow)';

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function result = func_2D_LexicoOrder(x)
[nRow, nColumn] = size(x);
temp = x';
result = temp(:);

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function result = func_psnr_gray(f, g)
f = double(f);
g = double(g);
Q=255;MSE=0;
[M,N]=size(f);
h = f - g;
MSE = sum(sum(h.*h));
MSE=MSE/M/N;
result=10*log10(Q*Q/MSE);

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function [result1, result2, result3] = func_proposed_three_state_estimation_aco_seg(I, I_mask, win_size)

thre = graythresh(abs(I));
[nrow, ncol] = size(I);
padnum = (win_size-1)/2;
A = padarray(I, [padnum padnum], 'replicate', 'bot');
B = padarray(I_mask, [padnum padnum], 'replicate', 'bot');
A = im2col(A, [win_size win_size],'sliding');
B = im2col(B, [win_size win_size],'sliding');

result1 = (sum(A.*(B==1),1).*(sum((B==1),1)~=0) + 0.*(sum((B==1),1)==0)) ./ (sum((B==1),1) + (sum((B==1),1)==0).*1);
result1 = reshape(result1, size(I));
result2 = (sum(A.*(B==-1),1).*(sum((B==-1),1)~=0) + 0.*(sum((B==-1),1)==0)) ./ (sum((B==-1),1) + (sum((B==-1),1)==0).*1);
result2 = reshape(result2, size(I));
result3 = (sum(A.*A.*(B==0),1).*(sum((B==0),1)~=0) + 0.*(sum((B==0),1)==0)) ./ (sum((B==0),1) + (sum((B==0),1)==0).*1);
result3 = reshape(result3, size(I));

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function ch0_var = func_proposed_par_propogation(ch1_var,ch2_var,nEnlargeFactor)

temp1 = imresize(ch1_var, size(ch1_var)*nEnlargeFactor, 'nearest').^2;
temp2 = imresize(ch2_var, size(ch2_var)*2*nEnlargeFactor, 'nearest');
ch0_var = ((temp2~=0).*temp1+(temp2==0).*0)./ ((temp2~=0).*temp2 + (temp2==0).*1);

% *************************************************************************
% **************************Inner Function ********************************
% *************************************************************************
function result = func_proposed_gen_subband(I_par1, I_par2, I_par3, I_mask)

rand('state',sum(100*clock));
result1 = exprnd(I_par1);
result2 = exprnd(I_par2);
result3 = randn(size(I_par3)).*sqrt(I_par3);
result1(isnan(result1))=0;
result2(isnan(result2))=0;
result3(isnan(result3))=0;
result = (I_mask==1).*result1 + (I_mask==-1).*result2 + (I_mask==0).*result3;


function [imgLabel, labels, labels_shift] = computeConnectedComponents(img, mode)
%
%
%       [imgLabel, labels, labels_shift] = computeConnectedComponents(img, mode)
%
%
%        Input:
%           -img: an integer grayscale image
%           -mode: 4 or 8 neighbors; this is the connetion type
%
%        Output:
%           -imgLabel: labelled image
%           -labels: all labels in imgLabel
%           -labels_shift:
%
%     Copyright (C) 2011-2020  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

r = size(img, 1);
c = size(img, 2);
imgLabel = zeros(r, c);

labels = unique(img);
n = length(labels);
totLabels = 0;

if(~exist('type', 'var'))

end

if(mode ~= 4 || mode ~= 8)
    mode = 4;
end

labels_shift = 0;

for i=1:n

    indx = find(img == labels(i));
    if(~isempty(indx))
        imbBin_i = zeros(r,c); %create a binary image
        imbBin_i(indx) = 1;
        segmented_i = bwlabel(logical(imbBin_i), mode);

        imgLabel = imgLabel + (segmented_i + totLabels);
        totLabels = totLabels + max(segmented_i(:)) + 1;

        labels_shift = [labels_shift, totLabels];
    end
end

end


function [aVal,aJacob,aHess,papt]=aWeave(x,t,A,alpha,theta0,uTurn)
%%AWEAVE The drift function for a somewhat sinusoidal weaving motion model
%        under a 3D flat-Earth approximation. The third example below shows
%        how this flat-Earth model can be used on a curved Earth.
%
%INPUTS: x The 6X1 target state at time t consisting of 3 position and 3
%          velocity components.
%        t The time at which the drift function is to be evaluated.
%        A A scalar term determining the magnitude of the deflection during
%          turns. It should be less than alpha*pi/2 to keep the target from
%          doubling back on its previous path.
%    alpha The positive scalar weave period in radians per second.
%   theta0 The scalar phase at time 0 of the weave. A cosine function is
%          used for the weave. The default if this parameter is omitted or
%          an empty matrix is passed is 0. To start a track going in the
%          overall direction of progression of the target, set theta=0 and
%          the initial velocity direction to the overall direction that you
%          want the target to go (weaves are orthogonal to that direction).
%    uTurn A 3X1 unit vector pointing in the direction of the turn axis
%          used for the weaves. It is orthogonal to the turn plane. For
%          example, if a target is going to weave in the x-y plane, then
%          this should be [0;0;1]. The default if this parameter is omitted
%          or an empty matrix is passed is [0;0;1].
%
%OUTPUTS: aVal The 6X1 flat-Earth time-derivative of the state.
%       aJacob The 6X6 matrix of partial derivatives of aVals such that
%              aJacob(:,k) is the partial derivative of aVals(:,k) with
%              respect to x(k).
%        aHess The 6X6X6 matrix of second partial derivatives of aVals such
%              that aHess(:,k1,k2) is the second partial derivative of
%              aVals with respect to x(k1) and x(k2).
%         papt The 6X1 partial derivative with resect to time of aVals.
%
%This model is derived in [1]. It is parameterized in a manner that allows
%one to create a target trajectory that performed weaves of a desired
%amplitude in a particular direction, progressing at a desired speed
%overall.
%
%Equation 78 and 79 in [1] are for a general turning model. Equation 88
%and 89 specifically show how the velocity is modulated, which in turn can
%lead to using a time-varying angular momentum vector modified from 79 to
%Omega=A*cos(alpha*t+theta0)*uTurn. Unlike Equation 79, this is generalized
%to allow uTurn to be any rotation axis, not just the z-axis. This is the
%model as implemented here.
%
%EXAMPLE 1:
%Here, we implement code that draws Figure 11 from [1]. We want the overall
%motion direction of the target to be along the x-axis. We also want the
%target to fly travel at a specified speed and go a specified distance
%along the x axis over the observation period. The one parameter that is
%left free is the magnitude of the weaving. Here, we plot two trajectories
%that have the same starting and ending points and make the same number of
%weaves, but have different weaving magnitudes. Since both targets are
%going the same speed, we need to determine the time duration of the
%trajectories so that we can show them advancing the same amount along the
%x-axis.
% Nw=6;%The number of weave cycles.
% speed=10;%Meters per second
% s=100;%The desired distance we want to cover along the x-axis (meters).
% uTurn=[0;0;1];%Turns are in the x-y plane.
% %The initial velocity of the target is along the x axis. By setting
% %theta0-0 and the velocity of xInit to point in the desired direction of
% %progression, we can easily set the direction of progression.
% theta0=0;
% xInit=[0;0;0;speed*[1;0;0]];
%
% %Parameters for the first target
% %beta determines the weave magnitude, as in Equation 101 of [1]. This value
% %can be from 0 to 1.
% betaVal1=1;
% %Solve Equation 102 in [1] to determine how long one must travel to go a
% %distance of s meters. Also determine the weave period in meters per second
% %as well as the amplitude parameter A of the weaves.
% [tEnd1,alphaVal1,A1]=determineWeaveTime(s,speed,Nw,betaVal1);
%
% %Parameters for the second target. This target does not weave as strongly.
% betaVal2=1/2;
% [tEnd2,alphaVal2,A2]=determineWeaveTime(s,speed,Nw,betaVal2);
%
% %Now, create the trajectories. We use a Runge-Kutta algorithm with a fixed
% %step to get the values. The step has been chosen to be sufficiently small.
% %However, if we only wanted a few samples we could use RungeKAtTimes for a
% %fixed-step size algorithm, or we could use RKAdaptiveAtTimes.
% numSteps=3000;
% deltaT1=tEnd1/numSteps;
% deltaT2=tEnd2/numSteps;
% aDyn1=@(x,t)aWeave(x,t,A1,alphaVal1,theta0,uTurn);
% xList1=RungeKSteps(xInit,0,aDyn1,deltaT1,numSteps);
% aDyn2=@(x,t)aWeave(x,t,A2,alphaVal2,theta0,uTurn);
% xList2=RungeKSteps(xInit,0,aDyn2,deltaT2,numSteps);
%
% figure(1)
% clf
% hold on
% plot(xList1(1,:),xList1(2,:),'-r','linewidth',6)
% plot(xList2(1,:),xList2(2,:),'--g','linewidth',6)
% axis([0 100 0 14])
% h1=xlabel('x');
% h2=ylabel('y');
%
% set(gca,'FontSize',18,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',20,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',20,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 2:
%In this example, we want a target to travel a distance of s meters in tMax
%seconds while weaving Nw times. As in the previous example, the target is
%moving along the x axis. We ned to use the function determineWeaveSpeed to
%determine the speed of the target that is required to make this happen.
% Nw=4;
% betaVal=0.5;
% tEnd=180;%The time over which the distance was traveled.
% theta0=0;
% uTurn=[0;0;1];%Turns are in the x-y plane.
% s=1.085e6;%The distance traveled.
%
% [speed,alphaVal,A]=determineWeaveSpeed(s,tEnd,Nw,betaVal);
% xInit=[0;0;0;speed*[1;0;0]];
% aDyn=@(x,t)aWeave(x,t,A,alphaVal,theta0,uTurn);
%
% numSteps=1000;
% deltaT=tEnd/numSteps;
% xList=RungeKSteps(xInit,0,aDyn,deltaT,numSteps);
% figure(2)
% clf
% hold on
% plot(xList(1,:),xList(2,:),'-r','linewidth',6)
% axis([0 s 0 8e4])
% h1=xlabel('x');
% h2=ylabel('y');
%
% set(gca,'FontSize',18,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',20,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',20,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 3:
%Here, we demonstrate how to use this flat-Earth model on a curved Earth.
%Some place in the ocean off of Hawaii.
% latLonStart=[20.3924;-155.4976]*(pi/180);
% %The location of the Ahihi-Kinau Natural Area Reserve
% latLonEnd=[20.6050;-156.4353]*(pi/180);
% %The altitude of the target-- it will remain constant.
% altitudeInit=20e3;
%
% tMax=180;
% times=0:tMax;
%
% %Get an initial heading and distance. We are ignoring the altitude of the
% %target, though it would be taken into account using the (slower) function
% %indirectGeodeticProbGen
% [azi1,s]=indirectGeodeticProb(latLonStart,latLonEnd);
%
% pStart=[phi0;lambda0;altitudeInit];
% uInit=getENUAxes(pStart);
%
% xyzStart=ellips2Cart(pStart);
% %The initial heading in the local tangent plane coordinates.
% uh=[sin(azi1);cos(azi1);0];
%
% %These two parameters make the sinusoidal deviations approximately
% %centered about the nominal trajectory
% theta0=0;
% Nw=4;%The number of weave cycles.
% beta=0.5;
% %We want the plane to travel a distance of about s in tMax seconds
% %while weaving Nw times. This means that the speed along the
% %trajectory is
% [speed,alphaVal,A]=determineWeaveSpeed(s,tMax,Nw,beta);
% xInit=[xyzStart;speed*uh];
%
% %To determine the realism of this being a manned maneuver, since
% %this is level flight, the maximum G-force felt can be easily found
% %to be
% %MaxGForce=sqrt((A)^2*norm(xInit(4:6,end))^2+9.81^2)/9.81
% %which is 7.4G and is within a reasonable tolerance for a pilot
% %wearing an "anti-G" suit.
%
% uTurn=[0;0;1];%The local vertical is the turn axis.
% aDyn=@(x,t)aWeave(x,t,A,alphaVal,theta0,uTurn);
%
% deltaTMax=0.05;
% [xList,uList]=RungeKCurvedAtTimes(xInit,uInit,times,aDyn,[],deltaTMax);
% xList=xList(1:6,:);
% %If one wanted to get the velocity information the global coordinates,
% %then the following line is necessary:
% xList(4:6,:)=getGlobalVectors(xList(4:6,:),uList);
%
% %Bounds for the axes on the map in case this trajectory is plotted.
% axisMapBounds=[-156-0.6,-156+0.6,20.5-0.6,20.5+0.6];
% axisHeightBounds=[0 times(end) 0 25e3];
%
% %Convert the Cartesian trajectory into latitude, longitude and
% %altitude.
% points=Cart2Ellipse(xList(1:3,:));
%
% figure(1)
% clf
% hold on
% axis(axisMapBounds)
% axis square
%
% %Plot the trajectory of the aircraft with the curved-Earth model.
% plot(points(2,:)*180/pi,points(1,:)*180/pi,'-r','linewidth',6);
%
% h1=xlabel('Longitude');
% h2=ylabel('Latitude');
% set(gca,'FontSize',18,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',20,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',20,'FontWeight','bold','FontName','Times')
%
% %Plot the altitude as a function of time.
% figure(2)
% clf
% plot(times,points(3,:),'-r','linewidth',6);
% axis(axisHeightBounds)
%
% h1=xlabel('Time (Seconds)');
% h2=ylabel('Altitude (Meters)');
% set(gca,'FontSize',18,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',20,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',20,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<6||isempty(uTurn))
        uTurn=[0;0;1];
    end

    if(nargin<5||isempty(theta0))
       theta0=0;
    end

    Omega=A*cos(alpha*t+theta0)*uTurn;
    aVal=[x(4:6);cross(Omega,x(4:6))];

    if(nargout>1)
        OmegaX=Omega(1);
        OmegaY=Omega(2);
        OmegaZ=Omega(3);

        aJacob=[0,0,0,1,        0,          0;
                0,0,0,0,        1,          0;
                0,0,0,0,        0,          1;
                0,0,0,0,        -OmegaZ,    OmegaY;
                0,0,0,OmegaZ,   0,          -OmegaX;
                0,0,0,-OmegaY,  OmegaX,     0];

        if(nargout>2)
            aHess=zeros(6,6,6);
            if(nargout>3)
                sinTerm=sin(alpha*t+theta0);

                uTurnX=uTurn(1);
                uTurnY=uTurn(2);
                uTurnZ=uTurn(3);

                vx=x(4);
                vy=x(5);
                vz=x(6);

                papt=[0;
                      0;
                      0;
                      A*alpha*(uTurnZ*vy-uTurnY*vz)*sinTerm;
                      A*alpha*(-uTurnZ*vx+uTurnX*vz)*sinTerm;
                      A*alpha*(uTurnY*vx-uTurnX*vy)*sinTerm];
            end
        end
    end
end

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


function varargout = fillSphericalTriangle(sphere, p1, p2, p3, varargin)
%FILLSPHERICALTRIANGLE Fill a triangle on a sphere.
%
%   fillSphericalTriangle(SPHERE, PT1, PT2, PT3);
%
%
%   See also
%   fillSphericalPolygon, drawSphericalTriangle, drawSphere
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 22/02/2005
%

%   HISTORY
%   27/06/2007 manage spheres other than origin
%   30/10/2008 replace intersectPlaneLine by intersectLinePlane
%   2012-02-09 rename as fillSphericalTriangle

% extract data of the sphere
ori = sphere(:, 1:3);
r   = sphere(4);

% extract direction vectors for each point
v1  = normalizeVector3d(p1 - ori);
v2  = normalizeVector3d(p2 - ori);
v3  = normalizeVector3d(p3 - ori);

% create a plane tangent to the sphere containing first point
plane = createPlane(v1, v1);

% position on the plane of the direction vectors
pp2 = planePosition(intersectLinePlane([ori v2], plane), plane);
pp3 = planePosition(intersectLinePlane([ori v3], plane), plane);

% create rough parametrization with 2 variables
nTri = 5;
s  = linspace(0, 1, nTri);
t  = linspace(0, 1, nTri);
ns = length(s);
nt = length(t);
s  = repmat(s, [nt, 1]);
t  = repmat(t', [1, ns]);

% convert to plane coordinates
xp = s * pp2(1) + t .* (1-s) * pp3(1);
yp = s * pp2(2) + t .* (1-s) * pp3(2);

% convert to 3D coordinates (still on the 3D plane)
x  = plane(1) * ones(size(xp)) + plane(4) * xp + plane(7) * yp - ori(1);
y  = plane(2) * ones(size(xp)) + plane(5) * xp + plane(8) * yp - ori(2);
z  = plane(3) * ones(size(xp)) + plane(6) * xp + plane(9) * yp - ori(3);

% project on the sphere
norm = hypot(hypot(x, y), z);
xn = x ./ norm * r + ori(1);
yn = y ./ norm * r + ori(2);
zn = z ./ norm * r + ori(3);

if nargout == 0
    % simply display the patch
    surf(xn, yn, zn, 'FaceColor', 'g', 'EdgeColor', 'none', varargin{:});

elseif nargout == 1
    % display the patch and return a handle
    h = surf(xn, yn, zn, 'FaceColor', 'g', 'EdgeColor', 'none', varargin{:});
    varargout = {h};

elseif nargout == 3
    % If 3 outputs are required, return patch vertex coordinates
    varargout = {x, y, z};
end


function [elem2face,face,elem2faceSign] = dof3face(elem)
%% DOF3FACE dof sturctrue for faces in 3-D.
%
% [elem2face,face,elem2faceSign] = DOF3FACE(elem) constructs data structure
% associated to faces in 3-D. elem is the connectivity matrix for a 3-D
% triangulation. elem2face is the elementwise pointer from elem to face
% indices. face is the face matrix using the ascend ordering. elem2faceSign
% records the consistency of the local faces with induced orientation and
% the global face with ascend orientation.
%
% DOF3FACE is used for elements using dof associated to faces like CR and
% WG elements in 3-D. For elements with orientation such as RT and BDM
% elements, when we use ascend ordering system, no elem2facSign is needed;
% see sc3doc for details.
%
%  See also dofedge, dof3edge
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

NT = size(elem,1);
totalFace = int32([elem(:,[2 3 4]); elem(:,[1 4 3]); ...
                   elem(:,[1 2 4]); elem(:,[1 3 2])]); % induced ordering
matlabversion = version;
if str2double(matlabversion(end-5:end-2)) > 2012
   [face, tempvar, j] = unique(sort(totalFace,2),'rows','legacy');
else
   [face, tempvar, j] = unique(sort(totalFace,2),'rows');
end
elem2face = uint32(reshape(j,NT,4));
elem2faceSign = int8(reshape(sum(sign(diff(totalFace(:,[1:3,1]),1,2)),2),NT,4));


%% Stream programming in Matlab
%  The stream programming is a computer programming methodology that is
% born in the field of mathematical languages using lambda calculus toole
% and exploiting the lazy evaluation methodology. Here a unefficient
% library, really useful to explain lazy evaluation and stream programming,
% is given. The set of tools is really wide to give examples of pure lazy
% evaluation, simple stream generation, the composition pattern, the
% accumulation pattern, the filtering pattern and a wide set of examples
% that allow the user to learn what stream programming is. This library
% isn't useful to create systems that must work with large ammounts of
% data, but can used where the number of data elements is small.. for
% example to generate streams of images (i.e. a stream of the
% approximations of a given image using a set of fourier armonics, this is
% a classical accumulation example).

%% What lazy evaluation is
%  The lazy evaluation is the technique of managing entities that aren't
% computed yet. For example we can think to the gradient of an image that
% can be taken as a variable and passed in functions and in workspaces, but
% that isn't computed yet, when we acces, for example, to a tail of the
% gradient that isn't computed automatically the lazy evaluation system
% computes that part of the matrix. This is usefull to optimize the code
% for execution time (parts of the gradient aren't computed) and for memory
% usage (because the parts of the gradient that aren't used arent allocated
% in the gradient matrix).
%
%  For example we can delay the evaluation of a function with a set of
% parameters:

% Function evaluation delayed:
dsin = delayEval(@sin,{2}),

%% Forcing the evaluation
%  After the delayed evaluation composition, the entity "dsin" can be thing
% to the promise of evaluation of the function "sin" with parameter "2".
% This isn't done yet, we can force the evaluation when we wish to get it.
% If we have tools to guess if an entity must be evaluated or not we can
% automatize the evaluation so that it is done only when an entity is
% accessed the first time, like the example of the gradient.
%
%  Here we force the evaluation exlpicitly:

% Forcing the evaluation:
res = forceEval(dsin),

%% The Stream Programming (SP)
%  The stream programming is a technique, based on lazy evaluation, to
% manage infinitely long sequence of elements, for examples the sequences
% of numbers, or the sequences of functions, series or other things. To
% manage an infinitely long sequence, that cannot be stored in a finite
% computer memory, we think to the sequence as a first element  followed
% with the promise to compute the other part of the sequence: this is a
% stream:
%
%     % A sample stream generating function generating the integers sequence:
%     function s = integersFrom(n)
%     s = {n, delayEval(@integersFrom,{n+1})};

% Generating an integers sequence:
integers = integersFrom(0),

% Getting another integer:
otherIntegers = forceEval(integers{2}),

% Computing the first ten integers:
firstTenInts = streamHead(integers,10),

%% Fibonacci numbers
%  The Fibonacci numbers can be constructed as a sequence where each number
% can be constructed as the sum of the previous two numbers. The first two
% numbers ar 0 and 1. With the following stream constructor the Fibonacci
% sequence can be constructed:
%
%     % The constructor of the Fibonacci sequence:
%     function s = fiboFrom(a1,a2)
%     s = {a1,delayEval(@fiboFrom,{a2,a1+a2})};
%
%  using this constructor we can obtain:

% Creating the sequence:
fib = fiboFrom(0,1);

% Getting the first ten values:
firstTenInts = streamHead(fib,10),

%% Functional list operators (from Schema language)
%  In the lisp and Schema languages the car and cdr operators are the bases
% for the management of lists. Here we have used "streamHead" to get the
% first elements of a stream, that is an infinite list; functions "head"
% and "tail" are defined as aliases of "streamCar" and "streamCdr", this
% two functions can be easily defined as follows:
%
%     % Head of a stream:
%     function v = streamCar(s)
%     v = s{1};
%
%     % Tail of a stream:
%     function so = streamCdr(si)
%     so = forceEval(si{2});
%
%  the streamHead function simply iterates with this two functions:
%
%     % Iteratively obtain a sequence of values:
%     function vals = streamHead(s,n)
%     s1 = s;
%     vals = cell(1,n);
%     for i=1:n
%         vals{i} = streamCar(s1);
%         s1 = streamCdr(s1);
%     end
%
%  other helper functions can be written with the same philosophy, like
% "streamRange" or "streamCons" to concatenate a value to a stream:
%
%     % Cut a range of a stream:
%     function vals = streamRange(s,start,nElem)
%     s1 = s;
%     for i=1:start-1
%         s1 = streamCdr(s1);
%     end
%     vals = cell(1,nElem);
%     for i=1:nElem
%         vals{i} = streamCar(s1);
%         s1 = streamCdr(s1);
%     end
%
%     % Concatenate a value with a stream:
%     function so = streamCons(v,si)
%     so = {v, delayEval(@fIdentity,{si})};

%% SP patterns: accumulate
%  In the software engeneering the design patterns are fundamental bricks
% for a correct and robust design of a complex system; generally desogn
% patterns are defined for OOP, but the same can be done for functional
% programming, procedural programming... and SP.
%  The accumulate pattern allows to define elements that operates over
% streams and that can be used as accumulators of data. Accumulators can be
% used to do a wide number of works like integration, sum of series...
% and for example the generation of streams containing successive
% approximations of series of what we wish to manage, for example the
% approximations of an image using its Fourier coefficients. Here the
% accumulator tool is defined as follows:
%
%     % Given a stream, a starting value and an accumulator function an
%     % accumulated stream is generated with the starting value as first
%     % element and the accumulator function that computes the next element.
%     function so = streamAccumulate(si,start,accumulator)
%     so = {start,delayEval(@streamAccumulate, ...
%             {streamCdr(si),
%              feval(accumulator,start,streamCar(si)),accumulator})};
%
%  Using this simple function (but take a deep look to it) we can define
% easily the integration:
%
%     % Integration of a sequence of numbers:
%     function so = integrateStream(si,dt,c)
%     so = scaleStream(streamAccumulate(si,c/dt,@plus),dt);
%
%  Let's try it:

% A discretization step:
dt = 0.1;

% A sequence of values from 0:
X = scaleStream(integers,dt);

% A sine function:
Y = filterStream(X,@sin);

% Getting sample values:
Xs = streamHead(X,10),
Ys = streamHead(Y,10),

% Computing the integral stream:
Yint = integrateStream(Y,dt,-1);
Yints = streamHead(Yint,10),
figure; hold on;
plotStream(X,Y,100,0),
plotStream(X,Yint,100,0),

%% SP patterns: accumulate
%  The composition of streams can be used to do different computations in
% different streams and then mixing them int a single stream. A simple
% example is the stream sum of other streams, we can try to sum the sin
% stream and it's integral.
%  A composition can be done given a sequence of streams and a composing
% function as follows:
%
%     % Composing n streams:
%     function so = streamCompose(streams,func)
%     argsv = {func};
%     argsf = {};
%     for i=1:size(streams,2);
%         argsv{i+1} = streamCar(streams{i});
%         argsf{i} = streamCdr(streams{i});
%     end
%     val = builtin('feval',argsv{:});
%     so = {val,delayEval(@streamCompose,{argsf,func})};
%
%  This tool allows to generate easily new composition operators:
%
%     % Adding the streams values:
%     function so = addStreams(varargin)
%     so = streamCompose(varargin,@sumVals);
%     function ris = sumVals(varargin)
%     ris = sum(cell2mat(varargin));
%
%     % Subtracting the stream values:
%     function so = subStreams(s1,s2)
%     so = streamCompose({s1,s2},@subVals);
%     function ris = subVals(v1,v2)
%     ris = v1-v2;
%
%     % Multiply streams:
%     function so = mulStreams(varargin)
%     so = streamCompose(varargin,@mulVals);
%     function ris = mulVals(varargin)
%     ris = prod(cell2mat(varargin));
%
%     % Divide streams:
%     function so = divStreams(s1,s2)
%     so = streamCompose({s1,s2},@divVals);
%     function ris = divVals(v1,v2)
%     ris = v1/v2;
%
%  Whit this tool all the composition of streams that we wish to do can be
% easily added to our operators set.

% Generating the sum of the two streams:
Ysum = addStreams(Y,Yint);

% Plotting:
figure; plotStream(X,Ysum,100,0),

%% SP patterns: filter
%  A stream can be filtered in a sense different from the previous one
% (that is the mapping of functional programming), a stream can be filtered
% with a filter that decides if an element must be discarded or not. For
% example immagine that you wish to get a sequence of prime numbers, you
% can get them filtering the stream of integers from 2 removing the
% integers that can be divided by the previous primes. We can design a
% stream filter function that recives another boolean function that can
% decide if an element can be mantained or must be rejected:
%
%     % A filtering element for streams:
%     function so = streamFilter(si,filter,varargin)
%     while ~builtin('feval',filter,head(si),varargin{:})
%         si = tail(si);
%     end
%     so = {head(si),delayEval(@streamFilter,{tail(si),filter,varargin{:}})};
%
%  This can filter element by element a stream (and can generate infinite
% loops). This can be usefull to generate streams from other discarding
% elements; the prime numbers stream is an extreme case because the filters
% chain grows of one for each element (prime) generated:
%
%     % Generation of all the primes:
%     function so = primes
%     so = PrimesFrom(integersFrom(2));
%
%     % The filtering function add a new filter to the stream:
%     function so = PrimesFrom(si)
%     % The first number is ok:
%     val=head(si); so=tail(si);
%     % The others are filtered:
%     so = streamFilter(so,@(v,p)(mod(v,p)~=0),val);
%     % Composing:
%     so = {val,delayEval(@PrimesFrom,{so})};
%
%  This is really inefficient, also because the use of the recursion is
% heavy, but it's an approach that generates the required results:

% Creating the primes stream:
pr = primes;

% Extracting values:
streamHead(pr,20),

%% Conclusions
%  The stream programming is a powerful technique to manage infinite
% sequences of elements, the lazy evaluation allows to manage this
% particular entities easily, this implementation of streams in Matlab does
% not look at computational complexity but an efficient version can be
% implemented. Tools like Simulink, Labview, HP-VEE gives the stream
% programming as a base programming tool and a graphical environment to
% generate programs connecting stream generators, operators and consumers.
% The stream programming can be an useful tool also in programs written in
% other procedural languages like Python or Java, the power is big.. but
% the diffusion of this technique is not :(


function a = clement1_inverse ( n )

%*****************************************************************************80
%
%% CLEMENT1_INVERSE returns the inverse of the CLEMENT1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.  N must not be odd%
%
%    Output, real A(N,N), the matrix.
%
  if ( mod ( n, 2 ) == 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CLEMENT1_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  The matrix is singular for odd N.\n' );
    error ( 'CLEMENT1_INVERSE - Fatal error!' );
  end

  a = zeros ( n, n );

  for i = 1 : n

    if ( mod ( i, 2 ) == 1 )

      for j = i : 2 : n-1

        if ( j == i )
          prod = 1.0 / sqrt ( ( j * ( n - j ) ) );
        else
          prod = - prod ...
            * sqrt ( ( j - 1 ) * ( n + 1 - j ) ) ...
            / sqrt ( j * ( n - j ) );
        end

        a(i,j+1) = prod;
        a(j+1,i) = prod;

      end

    end

  end

  return
end


function tnorm = tensor_array_norm( X )
% X is a cell array of tensors

tnorm = 0;
for i = 1:length(X)
    tnorm = tnorm + norm(X{i})^2;
end
tnorm = sqrt(tnorm);

end


function	[Wout,Weff] = make_test_weight(parm)
% Make output weigtht for test
% -Input
% parm.Ydim   =  Output dim
% parm.Xdim   =  Input dim
% parm.Meff   =  Effective Input dim
% parm.Tau    =  Lag time steps
% parm.Dtau   =  Number of embedding dimension
% parm.WXmax = maximum weight of X-dim
% parm.WYmax = bias weight for Y-dim
% parm.Wtau  = effctive time delay length of weight
% -Output
% Wout  : Weight for time embedded input [Ydim, Xdim*Dtau]
% Weff  : nonzero weight for [Ydim, Meff*Dtau]
%         effective input dimension : [Meff x Wtau]

% Input dim
if isfield(parm,'Xdim')
	Xdim = parm.Xdim ;
else
	Xdim = 100;
end
% Output dim
if isfield(parm,'Ydim')
	Ydim = parm.Ydim ;
else
	Ydim = 1;
end
% Effective Input dim
if isfield(parm,'Meff')
	Meff = parm.Meff ;
else
	Meff = 10; ;
end
% Number of embedding dimension
if isfield(parm,'Dtau')
	Dtau  = parm.Dtau  ;
else
	Dtau  = 1;
end

if Meff > Xdim, Meff = Xdim; end;
if Dtau==0, Dtau=1; end;

% effctive time delay length of weight
if isfield(parm,'Wtau')
	tau  = parm.Wtau  ;
else
	tau  = Dtau;
end
% maximum weight of X-dim
if isfield(parm,'WXmax')
	WXmax = parm.WXmax;
else
	WXmax = 100;
end
% maximum bias weight for Y-dim
if isfield(parm,'WYmax')
	WYmax = parm.WYmax;
else
	WYmax = 10;
end

%
% --- Make Output Weight matrix
%
% Output weight

% Spatial weight
Wbase = rand(Ydim,Meff)*WXmax + WYmax;

% Temporal filter
t = (0:tau-1)/max((tau-1)/2,1);
Wtau  = exp( - 2* t.^2);

ix_eff = 1:Meff;
Weff   = zeros(Ydim,Meff*Dtau);
Wout   = zeros(Ydim,Xdim*Dtau);

for n = 1:tau
	Weff(:,ix_eff + Meff*(n-1)) = Wtau(n) * Wbase;
	Wout(:,ix_eff + Xdim*(n-1)) = Wtau(n) * Wbase;
end


function scores = PascalOverlap(targetBox, testBoxes)
% scores = PascalOverlap(targetBox, testBoxes)
%
% Function obtains the pascal overlap scores between the targetBox and
% all testBoxes
%
% targetBox:            1 x 4 array containing target box
% testBoxes:            N x 4 array containing test boxes
%
% scores:               N x 1 array containing for each testBox the pascal
%                       overlap score.
%
%     Jasper Uijlings - 2013

intersectBoxes = BoxIntersection(targetBox, testBoxes);
overlapI = intersectBoxes(:,1) ~= -1; % Get which boxes overlap

% Intersection size
[nr nc intersectionSize] = BoxSize(intersectBoxes(overlapI,:));

% Union size
[nr nc testBoxSize] = BoxSize(testBoxes(overlapI,:));
[nr nc targetBoxSize] = BoxSize(targetBox);
unionSize = testBoxSize + targetBoxSize - intersectionSize;

scores = zeros(size(testBoxes,1),1);
scores(overlapI) = intersectionSize ./ unionSize;


function test_tutorial_timefrequencyanalysis(datadir)

% MEM 2gb
% WALLTIME 00:20:00
% DEPENDENCY ft_freqanalysis ft_preprocessing ft_multiplotTFR ft_singleplotTFR

if nargin==0
  datadir = dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/timefrequencyanalysis');
end

load(fullfile(datadir, 'dataFIC.mat'));

cfg              = [];
cfg.output       = 'pow';
cfg.channel      = 'MEG';
cfg.method       = 'mtmconvol';
cfg.taper        = 'hanning';
cfg.foi          = 2:2:30;                         % analysis 2 to 30 Hz in steps of 2 Hz
cfg.t_ftimwin    = ones(length(cfg.foi),1).*0.5;   % length of time window = 0.5 sec
cfg.toi          = -0.5:0.05:1.5;                  % time window "slides" from -0.5 to 1.5 sec in steps of 0.05 sec (50 ms)
TFRhann = ft_freqanalysis(cfg, dataFIC);

cfg = [];
cfg.baseline     = [-0.5 -0.1];
cfg.baselinetype = 'absolute';
cfg.zlim         = [-3e-27 3e-27];
cfg.showlabels   = 'yes';
cfg.layout       = 'CTF151.lay';
figure
ft_multiplotTFR(cfg, TFRhann);

cfg = [];
cfg.baseline     = [-0.5 -0.1];
cfg.baselinetype = 'absolute';
cfg.maskstyle    = 'saturation';
cfg.zlim         = [-3e-27 3e-27];
cfg.channel      = 'MRC15';
figure
ft_singleplotTFR(cfg, TFRhann);

cfg = [];
cfg.baseline     = [-0.5 -0.1];
cfg.baselinetype = 'absolute';
cfg.maskstyle    = 'saturation';
cfg.xlim         = [0.9 1.3];
cfg.zlim         = [-1.5e-27 1.5e-27];
cfg.ylim         = [15 20];
cfg.showlabels   = 'markers';
figure
ft_topoplotTFR(cfg, TFRhann);

cfg              = [];
cfg.output       = 'pow';
cfg.channel      = 'MRC15';
cfg.method       = 'mtmconvol';
cfg.taper        = 'hanning';
cfg.foi          = 2:1:30;
cfg.t_ftimwin    = 7./cfg.foi;  % 7 cycles per time window
cfg.toi          = -0.5:0.05:1.5;
TFRhann7 = ft_freqanalysis(cfg, dataFIC);

cfg              = [];
cfg.baseline     = [-0.5 -0.1];
cfg.baselinetype = 'absolute';
cfg.maskstyle    = 'saturation';
cfg.zlim         = [-3e-27 3e-27];
cfg.channel      = 'MRC15';
cfg.interactive  = 'no';
figure
ft_singleplotTFR(cfg, TFRhann7);

cfg              = [];
cfg.output       = 'pow';
cfg.channel      = 'MRC15';
cfg.method       = 'mtmconvol';
cfg.taper        = 'hanning';
cfg.foi          = 2:1:30;
cfg.t_ftimwin    = 4./cfg.foi;
cfg.toi          = -0.5:0.05:1.5;
TFRhann4 = ft_freqanalysis(cfg, dataFIC);

cfg.t_ftimwin    = 5./cfg.foi;
TFRhann5 = ft_freqanalysis(cfg, dataFIC);

cfg.t_ftimwin    = 10./cfg.foi;
TFRhann10 = ft_freqanalysis(cfg, dataFIC);

cfg = [];
cfg.output     = 'pow';
cfg.channel    = 'MEG';
cfg.method     = 'mtmconvol';
cfg.foi        = 1:2:30;
cfg.t_ftimwin  = 5./cfg.foi;
cfg.tapsmofrq  = 0.4 *cfg.foi;
cfg.toi        = -0.5:0.05:1.5;
cfg.pad        = 'maxperlen';
TFRmult = ft_freqanalysis(cfg, dataFIC);

cfg = [];
cfg.baseline     = [-0.5 -0.1];
cfg.baselinetype = 'absolute';
cfg.zlim         = [-3e-27 3e-27];
cfg.showlabels   = 'yes';
cfg.layout       = 'CTF151.lay';
figure
ft_multiplotTFR(cfg, TFRmult)

cfg = [];
cfg.channel    = 'MEG';
cfg.method     = 'wavelet';
cfg.width      = 7;
cfg.output     = 'pow';
cfg.foi        = 1:2:30;
cfg.toi        = -0.5:0.05:1.5;
TFRwave = ft_freqanalysis(cfg, dataFIC);

cfg = [];
cfg.baseline     = [-0.5 -0.1];
cfg.baselinetype = 'absolute';
cfg.zlim         = [-3e-25 3e-25];
cfg.showlabels   = 'yes';
cfg.layout       = 'CTF151.lay';
figure
ft_multiplotTFR(cfg, TFRwave)


function out = isPerp(v1,v2)
% check whether v1 and v2 are orthogonal

out = isnull(dot(v1,v2));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Comparison of different learning methods of Hawkes processes
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear

options.N = 200; % the number of sequences
options.Nmax = 100; % the maximum number of events per sequence
options.Tmax = 100; % the maximum size of time window
options.tstep = 0.1;
options.dt = 0.1;
options.M = 250;
options.GenerationNum = 5;
D = 3; % the dimension of Hawkes processes
nTest = 1;
nSeg = 5;
nNum = options.N/nSeg;

disp('Approximate simulation of Hawkes processes via branching process')
disp('Simple exponential kernel')
para1.kernel = 'exp';
para1.w = 1;
para1.landmark = 0;
L = length(para1.landmark);
para1.mu = rand(D,1)/D;
para1.A = zeros(D, D, L);
for l = 1:L
    para1.A(:,:,l) = (0.5^l)*(0.5+rand(D));
end
para1.A = 0.5*para1.A./max(abs(eig(sum(para1.A,3))));
para1.A = reshape(para1.A, [D, L, D]);
Seqs1 = Simulation_Branch_HP(para1, options);
%Seqs1 = SimulationFast_Thinning_ExpHP(para1, options);

%%
disp('Learning Hawkes processes via different methods')
alg.LowRank = 0;
alg.Sparse = 1;
alg.alphaS = 1;
alg.GroupSparse = 0;
alg.outer = 5;
alg.rho = 0.1;
alg.inner = 8;
alg.thres = 1e-5;
alg.Tmax = [];
alg.storeErr = 0;
alg.storeLL = 0;

Err = zeros(nTest, nSeg);

for n = 1:nTest
    for i = nSeg

        [A, Phi] = ImpactFunc( para1, options );

        disp('Maximum likelihood estimation and basis representation')
        model1 = Initialization_Basis(Seqs1);
        model1 = Learning_MLE_Basis( Seqs1(1:i*nNum), model1, alg );
        [A1, Phi1] = ImpactFunc( model1, options );

        disp('Least squares and discretization')
        model2.D = D;
        model2.h = 1;
        model2.k = floor(options.M * options.dt/model2.h);
        model2 = Initialization_Discrete(Seqs1);
        model2 = Learning_LS_Discrete( Seqs1(1:i*nNum), model2 );

        figure
        title('Simple exponetial kernels')
        for u = 1:D
            for v = 1:D
                subplot(D,D,D*(u-1)+v)
                hold on
                plot(options.dt*(0:(size(Phi,2)-1)), Phi(v,:,u), 'k-')
                plot(options.dt*(0:(size(Phi1,2)-1)), Phi1(v,:,u), 'r-')
                plot(model2.h*(0:(size(model2.A,2)-1)), model2.A(v,:,u), 'b-')
                hold off
                axis tight
                legend('Real', 'MLE', 'LS')%, 'LS2')
                xlabel('Time interval between events')
                ylabel(['\phi', sprintf('%d%d', u, v)])
            end
        end

    end
end


function Q = orth(A)
%ORTH   Array-valued CHEBFUN orthogonalization.
%   Q = ORTH(A) is an orthonormal basis for the range of the column CHEBFUN A.
%   That is, the columns of Q span the same space as the columns of A, Q'*Q = I,
%   and the number of columns of Q is the rank of A.
%
% See also NULL, SVD, RANK, QR.

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

if ( A(1).isTransposed )
	error('CHEBFUN:CHEBFUN:orth:row', ...
        'ORTH() only defined for column CHEBFUN objects.')
end

% Compute the SVD:
[U, S, V] = svd(A, 0);
s = diag(S);

% Choose a tolerance if none is given:
if ( nargin == 1 )
	tol = max(length(A)*eps(max(s)), vscale(A)*eps);
end

% Compute the rank:
r = sum(s > tol);

% Select these columns of U:
Q = extractColumns(U, 1:r);

end


function PL=PL_Hata(fc,d,htx,hrx,Etype)
% Hata Model
% Input
%       fc    : carrier frequency[Hz]
%       d     : between base station and mobile station[m]
%       htx   : height of transmitter[m]
%       hrx   : height of receiver[m]
%       Etype : Environment Type('urban','suburban','open')
% output
%       PL    : path loss[dB]
if nargin<5, Etype = 'URBAN'; end
fc=fc/(1e6);
if fc>=150&&fc<=200, C_Rx = 8.29*(log10(1.54*hrx))^2 - 1.1;
 elseif fc>200, C_Rx = 3.2*(log10(11.75*hrx))^2 - 4.97;
 else   C_Rx = 0.8+(1.1*log10(fc)-0.7)*hrx-1.56*log10(fc);
end
PL = 69.55 +26.16*log10(fc) -13.82*log10(htx) -C_Rx ...
     +(44.9-6.55*log10(htx))*log10(d/1000);
EType = upper(Etype);
if EType(1)=='S',  PL = PL -2*(log10(fc/28))^2 -5.4;
  elseif EType(1)=='O'
    PL=PL+(18.33-4.78*log10(fc))*log10(fc)-40.97;
end


%% <../index.html MATLAB xUnit Test Framework>: How to Test Using a Floating-Point Tolerance
% MATLAB performs arithmetic operations using the floating-point
% hardware instructions on your processor. Because
% almost all floating-point operations are subject to round-off
% error, arithmetic operations can sometimes produce surprising
% results. Here's an example.

a = 1 + 0.1 + 0.1 + 0.1

%%
a == 1.3

%%
% So why doesn't |a| equal 1.3? Because 0.1, 1.3, and most other
% decimal fractions do not have exact representations in the binary
% floating-point number representation your computer uses.  The
% first line above is doing an approximate addition of 1 plus an
% approximation of 0.1, plus an approximation of 0.1, plus an
% approximation of 0.1.  The second line compares the result of all
% that with an approximation of 1.3.
%
% If you subtract 1.3 from |a|, you can see that the computed result
% for |a| is _extremely close_ to the floating-point approximation
% of 1.3, but it is not exactly the same.

a - 1.3

%%
% As a general rule, when comparing the results of floating-point
% calculations for equality, it is necessary to use a tolerance
% value.  Two types of tolerance comparisons are commonly used: absolute
% tolerance and relative tolerance.  An absolute tolerance comparison of _a_ and _b_
% looks like:
%
% $$|a-b| \leq T$$
%
% A relative tolerance comparison looks like:
%
% $$|a-b| \leq T\max(|a|,|b|) + T_f$$
%
% where _Tf_ is called the _floor tolerance_. It acts as an absolute tolerance
% when _a_ and _b_ are very close to 0.
%
% For example, suppose that _a_ is 100, _b_ is 101, and T is 0.1.  Then _a_ and
% _b_ would not be considered equal using an absolute tolerance, because 1 >
% 0.1.  However, _a_ and _b_ would be considered equal using a relative
% tolerance, because they differ by only 1 part in 100.
%
% MATLAB xUnit provides the utility assertion functions called
% |assertElementsAlmostEqual| and |assertVectorAlmostEqual|. These functions
% make it easy to write tests involving floating-point tolerances.
%
% |assertElementsAlmostEqual(A,B)| applies the tolerance test independently to
% every element of |A| and |B|.  The function uses a relative tolerance test by
% default, but you make it use an absolute tolerance test, or change the
% tolerance values used, by passing additional arguments to it.
%
% |assertVectorsAlmostEqual(A,B)| applies the tolerance test to the vectors |A|
% and |B| in the L2-norm sense.  For example, suppose |A| is |[1 1e10|], |B|
% is |[2 1e10]|, and the tolerance is 1e-8.  Then |A| and |B| would fail an
% elementwise relative tolerance comparison, because the relative difference
% between the first elements is 0.5.  However, they would pass a vector relative
% tolerance comparison, because the relative vector difference between |A| and
% |B| is only about 1 part in 1e10.
%
% The |examples_general| directory contains a portion of a unit test for the
% |sin| function.  The output of |sin| can sometimes be a bit surprising because
% of floating-point issues.  For example:

sin(pi)

%%
% That's very close but not exactly equal to 0.  Here's how the
% |sin| unit test uses |assertElementsAlmostEqual| to write the |sin(pi)|
% test with a minimum of fuss.

cd examples_general
type testSin

%%
% Run the test using |runtests|.

runtests testSin

%%
% <../index.html Back to MATLAB xUnit Test Framework>

%%
% Copyright 2008-2010 The MathWorks, Inc.


function fv_fscore= proc_fisherScore(fv, varargin)
%PROC_FISHERSCORE - Computes the Fisher Score for each feature
%
%Synopsis:
% FV_SCORE= proc_fisherScore(FV, <OPT>)
%
%Arguments:
% FV  - data structure of feature vectors
% OPT - struct or property/value list of optional properties
%    .TolerateNans: observations with NaN value are skipped
%       (nanmean/nanstd are used instead of mean/std)
%
%Returns:
% FV_SCORE - data structute of Fisher scores (one sample only)
%
%See also:
% proc_t_scaled, proc_r_values, proc_r_square, proc_rocAreaValues
%
%Comment:
% Only standard case is tested.

% Author(s): Benjamin Blankertz
props= { 'TolerateNans'   0    '!BOOL'};

if nargin==0,
  fv_fscore = props; return
end

fv = misc_history(fv);
misc_checkType(fv, 'STRUCT(x clab y)');
opt= opt_proplistToStruct(varargin{:});
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

if size(fv.y,1)>2,  %% multi-class: TODO! now doing it pairwise:
  warning('calculating pairwise Fisher scores');
  combs= nchoosek(1:size(fv.y,1), 2);
  for ic= 1:length(combs),
    ep= proc_selectClasses(fv, combs(ic,:));
    fv0= proc_fisherScore(ep);
    if ic==1,
      fv_fscore= fv0;
    else
      fv_fscore= proc_appendEpochs(fv_fscore, fv0);
    end
  end
  return;
elseif size(fv.y,1)==1,
  warning('1 class only: calculating Fisher score against flat-line of same var');
  fv2= fv;
  szx= size(fv.x);
  fv2.x= fv2.x - repmat(mean(fv2.x,3), [1 1 size(fv2.x,3)]);
  fv2.className= {'flat'};
  fv= proc_appendEpochs(fv, fv2);
end

sz= size(fv.x);
fv.x= reshape(fv.x, [prod(sz(1:end-1)), sz(end)]);
cl1= find(fv.y(1,:));
cl2= find(fv.y(2,:));
if opt.TolerateNans,
  me= [nanmean(fv.x(:,cl1),2) nanmean(fv.x(:,cl2),2)];
  va= [nanstd(fv.x(:,cl1),0,2).^2 nanstd(fv.x(:,cl2),0,2).^2];
else
  me= [mean(fv.x(:,cl1),2) mean(fv.x(:,cl2),2)];
  va= [var(fv.x(:,cl1)')' var(fv.x(:,cl2)')'];
end
fscore= ((me(:,1)-me(:,2)).^2)./(va(:,1)+va(:,2)+eps);
fscore= reshape(fscore, [sz(1:end-1) 1]);

fv_fscore= rmfield(fv, intersect(fieldnames(fv),{'x','y','className'},'legacy') );
fv_fscore.x= fscore;
if isfield(fv, 'className'),
  fv_fscore.className= {sprintf('Fs( %s , %s )', fv.className{1:2})};
end
fv_fscore.y= 1;
fv_fscore.yUnit= 'au';