function ps = getSplitMergeFeatChoiceProbs( F, stateSeq, data_struct, model, jj, ki )
%function ps = getSplitMergeFeatChoiceProbs( F, jj, ki )
ps = F(jj,:);

availFeatIDs = find( F(jj,:) );
relFeatIDs = union( availFeatIDs, ki);

%Ustats  = getStateSeqSuffStats_C1( stateSeq, F, data_struct, model, 1:size(F,1), 1:size(F,2) );
%Ustats.Nkv = Ustats.Nkv(relFeatIDs,:);
%myTheta = getTheta_PosteriorMean( myTheta, Ustats, model.obsModel, relFeatIDs );

Xstats = getXSuffStats( F, stateSeq, data_struct, model, 1:size(F,1),  relFeatIDs );
myTheta = setThetaToPosteriorMean(  [], Xstats, model.obsModel, relFeatIDs );

EPS = 1e-9;
switch model.obsModel.type
    case 'Multinomial'
        pHat = exp( myTheta.logp )';
        pHat = bsxfun( @rdivide, pHat, sum(pHat,1) );
        D = calcChiSqDistanceBetter( pHat(:,ki), pHat );
    case 'Gaussian'
        if size( myTheta.Mu, 1) > 1
            D = pdist( myTheta.Mu, 'euclidean');
            D = squareform(D);
            D = D(ki,:);
        else
            D = 0; % Default distance from self is zero.
        end
        D = D./(EPS+max(max(D)) ); % Ensure D varies from 0 to 1.

    case 'AR-Gaussian'
        [D DR K] = size( myTheta.A );

        %Avec = -100*ones(K,D);
        Avec = zeros(K, D*DR);
        for kk = relFeatIDs
            %Sig = myTheta.invSigma(:,:,kk) \ eye(D);
            %Avec(kk,:) = diag( Sig );
            Avec(kk,:) = reshape( myTheta.A(:,:,kk), 1, D*DR );
        end
        if K > 1
            D = pdist( Avec, 'euclidean');
            D = squareform(D);
            D = D(ki,:);
        else
            D = 0; % Default distance from self is zero.
        end
        D = D./(EPS+max(max(D)) );
        % Ensure D varies from 0 to 1.
        %  but is never quite 0 (must be at least EPS)
end

D( ~F(jj,:)  ) = Inf;

ps = 1 - D;
ps( ~F(jj,:) ) = 0;

ps = ps./sum( ps );

if F( jj, ki ) && length( ps( ps>0 ) ) > 1
    % If object jj possesses feature ki,
    %    then let prob kj =ki = 2/3
    %                  kj~=ki = 1/3 (unif. distrib. over all jj's feats)
    ps( ki ) = 2*( sum( ps( [1:ki-1 ki+1:end] ) ) );
end

ps = ps./sum(ps);

% NOTE: Alternate distance metric
%sig = eps+min(D(availFeatIDs));
%ps = exp(-D./sig );

end


function Gxy = femGreenKernel(X,Y,green,k)
%+========================================================================+
%|                                                                        |
%|              OPENFEM - LIBRARY FOR FINITE ELEMENT METHOD               |
%|           openFem is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : femGreenKernel.m                              |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : M. Aussal & F. Alouges & M. Averseng          |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.05.2019                                    |
%| ( === ) |   SYNOPSIS   : Usefull green kernel functions                |
%|  `---'  |                                                              |
%+========================================================================+

% Security
if (size(X,2) ~= 3) || (size(Y,2) ~= 3)
    error('femGreenKernel.m : unavailable case')
end
if isempty(k)
    k = 0;
end

% Distances between particles
Rxy = sqrt( ...
    (X(:,1) - Y(:,1)).^2 + ...
    (X(:,2) - Y(:,2)).^2 + ...
    (X(:,3) - Y(:,3)).^2 );

% For empty wave-number
if isempty(k)
    k = 0;
end

% Green kernel definition
if strcmp(green,'[1/r]')
    Gxy = 1./Rxy;

elseif strcmp(green,'dx[1/r]')
    Gxy = - 1 ./ (Rxy.^3);

elseif strcmp(green,'dy[1/r]')
    Gxy = 1 ./ (Rxy.^3);

elseif strcmp(green,'[exp(ikr)/r]')
    Gxy = exp(1i*k*Rxy)./Rxy;

elseif strcmp(green,'dx[exp(ikr)/r]')
    Gxy = (1i*k - 1./Rxy) .* exp(1i*k.*Rxy) ./ (Rxy.^2);

elseif strcmp(green,'dy[exp(ikr)/r]')
    Gxy = - (1i*k - 1./Rxy) .* exp(1i*k.*Rxy) ./ (Rxy.^2);

elseif strcmp(green(1:end-1),'gradx[1/r]')
    j = str2double(green(end));
    Gxy = - (X(:,j)-Y(:,j)) ./ (Rxy.^3);

elseif strcmp(green(1:end-1),'grady[1/r]')
    j = str2double(green(end));
    Gxy = (X(:,j)-Y(:,j)) ./ (Rxy.^3);

elseif strcmp(green(1:end-1),'gradx[exp(ikr)/r]')
    j = str2double(green(end));
    Gxy = (1i*k - 1./Rxy) .* exp(1i*k.*Rxy) .* ...
        (X(:,j)-Y(:,j)) ./ (Rxy.^2);

elseif strcmp(green(1:end-1),'grady[exp(ikr)/r]')
    j = str2double(green(end));
    Gxy = - (1i*k - 1./Rxy) .* exp(1i*k.*Rxy) .* ...
        (X(:,j)-Y(:,j)) ./ (Rxy.^2);

elseif strcmp(green,'[log(r)]')
    Gxy = log(Rxy);

elseif strcmp(green,'[H0(kr)]')
    Gxy = besselh(0,k*Rxy);

elseif strcmp(green(1:end-1),'gradx[log(r)]')
    j = str2double(green(end));
    Gxy = (X(:,j)-Y(:,j)) ./ (Rxy.^2);

elseif strcmp(green(1:end-1),'grady[log(r)]')
    j = str2double(green(end));
    Gxy = - (X(:,j)-Y(:,j)) ./ (Rxy.^2);

elseif strcmp(green(1:end-1),'gradx[H0(kr)]')
    j   = str2double(green(end));
    Gxy = - k * besselh(1,k*Rxy) .* (X(:,j)-Y(:,j)) ./ Rxy;

elseif strcmp(green(1:end-1),'grady[H0(kr)]')
    j   = str2double(green(end));
    Gxy = k * besselh(1,k*Rxy) .* (X(:,j)-Y(:,j)) ./ Rxy;

elseif strcmp(green(1:end-2),'[ij/r+rirj/r^3]')
    i = str2double(green(end-1));
    j = str2double(green(end));
    Gxy = (i==j)./Rxy + (X(:,i)-Y(:,i)).*(X(:,j)-Y(:,j))./(Rxy.^3);

elseif strcmp(green(1:end-3),'[rirjrk/r^5]')
    i = str2double(green(end-2));
    j = str2double(green(end-1));
    k = str2double(green(end));
    Gxy = (X(:,i)-Y(:,i)).*(X(:,j)-Y(:,j)).*(X(:,k)-Y(:,k))./(Rxy.^5);

else
    error('Error in femGreenKernel.m : unknown green kernel')
end

% Singularity
if strcmp(green,'[exp(ikr)/r]')
    Gxy(Rxy<1e-12) = 0 + 1i*k;
elseif strcmp(green,'[H0(kr)]')
    gamma         = 0.5772156649;
    Gxy(Rxy<1e-12) = 1 + 1i*(2/pi*(gamma+log(k/2)));
else
    Gxy(Rxy<1e-12) = 0;
end
end


function pval = lme_mass_LR(statsfull,statsred,q)
% pval = lme_mass_LR(statsfull,statsred,q)
%
% Likelihood ratio test for the random effects. It can be used to test if a
% model with q+1 random effects is significantly better than a model with q
% random effects.
%
% Input
% statsfull: Structure array containing statistiscs for every voxel/vertex
% (see lme_FSfit for more details on these statistics) for the full
% model (the one with q+1 random effects).
% statsred: Structure array containing statistiscs for every voxel/vertex
% for the reduced model (the one with q random effects).
% q: Number of random effects in the reduced model.
%
% Output
% pval: P-value of the test at each voxel/vertex (based on a 50:50 mixture
% of chi-squared distributions with q and q+1 degrees of freedom).
%
% $Revision: 1.2 $  $Date: 2015/01/06 17:14:55 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:14:55 $
%    $Revision: 1.2 $
% References: Bernal-Rusiel J.L., Greve D.N., Reuter M., Fischl B., Sabuncu
% M.R., 2012. Statistical Analysis of Longitudinal Neuroimage Data with Linear
% Mixed Effects Models, NeuroImage, doi:10.1016/j.neuroimage.2012.10.065.
%

if nargin < 3
    error('Too few inputs');
end;

nv = length(statsfull);
pval = zeros(1,nv);
for i=1:nv
    lrstats = lme_LR(statsfull(i).lreml,statsred(i).lreml,q);
    pval(i) = lrstats.pval;
end;


function r8col_unique_count_test ( )

%*****************************************************************************80
%
%% R8COL_UNIQUE_COUNT_TEST tests R8COL_UNIQUE_COUNT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2010
%
%  Author:
%
%    John Burkardt
%
  m = 3;
  n = 15;

  a = [ ...
    2.0,  6.0, 10.0; ...
    4.0,  8.0, 12.0; ...
    1.0,  5.0,  9.0; ...
    3.0,  7.0, 11.0; ...
    2.0,  6.0,  0.0; ...
    3.0,  4.0, 18.0; ...
    0.0,  0.0,  0.0; ...
    0.0,  6.0, 10.0; ...
    2.0,  6.0, 10.0; ...
    3.0,  7.0, 11.0; ...
    2.0,  0.0, 10.0; ...
    2.0,  6.0, 10.0; ...
    1.0,  5.0,  9.0; ...
    1.0,  5.0,  9.1; ...
    1.0,  5.1,  9.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8COL_UNIQUE_COUNT_TEST\n' );
  fprintf ( 1, '  For an R8COL;\n' );
  fprintf ( 1, '  R8COL_UNIQUE_COUNT counts unique columns.\n' );
  fprintf ( 1, '\n' );

  r8mat_transpose_print ( m, n, a, '  The R8COL (transposed):' );

  unique_num = r8col_unique_count ( m, n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of unique columns is %d\n', unique_num );

  return
end


%
%CA driver
%
%forest fire

clf
clear all

n=100;

Plightning = .000005;
Pgrowth = .01; %.01

z=zeros(n,n);
o=ones(n,n);
veg=z;
sum=z;

imh = image(cat(3,z,veg*.02,z));
set(imh, 'erasemode', 'none')
axis equal
axis tight

% burning -> empty
% green -> burning if one neigbor burning or with prob=f (lightning)
% empty -> green with prob=p (growth)
% veg = {empty=0 burning=1 green=2}
for i=1:3000
    %nearby fires?

     sum = (veg(1:n,[n 1:n-1])==1) + (veg(1:n,[2:n 1])==1) + ...
           (veg([n 1:n-1], 1:n)==1) + (veg([2:n 1],1:n)==1) ;

    veg = ...
         2*(veg==2) - ((veg==2) & (sum>0 | (rand(n,n)<Plightning))) + ...
         2*((veg==0) & rand(n,n)<Pgrowth) ;

    set(imh, 'cdata', cat(3,(veg==1),(veg==2),z) )
    drawnow
end


%%*******************************************************************
%% randinfsdp.m : creates random infeasible SDP problems with various block
%%                diagonal structures.
%%
%% [blk,Avec,C,b,X0,y0,Z0] =
%%      randinfsdp(dense_blk,sparse_blk,diag_blk,m,infeas,solve);
%%
%% E.g.
%%      randinfsdp([32 20],[10 5],100,10,infeas,solve);
%%
%%  dense_blk : for generating dense blocks, where
%%              dense_blk(i) is the dimension of the ith block
%%  sparse_blk: for generating a sparse block of small subblocks, where
%%              sparse_blk(i) is the size of the ith subblock.
%%  diag_blk: for generating a column vector of length specified by
%%            diag_blk (this corresponds to a diagonal block).
%%
%%  infeas = 1 if want primal infeasible pair of problems
%%         = 2 if want dual infeasible pair of problems
%%
%%  solve = 0 just to initialize
%%        = 1 if want to solve the problem.
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%******************************************************************

   function  [blk,Avec,C,b,X0,y0,Z0] = ...
                randinfsdp(dense_blk,sparse_blk,diag_blk,m,infeas,solve);

   if nargin < 6; solve = 0; end;
   if nargin < 5; error(' insufficient number of inputs '); end;
   if all(infeas-[1 2]);
      error(' infeas must be 1 or 2 ');
   end

   blk = [];
   if ~isempty(dense_blk);
      for k = 1:length(dense_blk);
          blk{k,1} = 's';  blk{k,2} = dense_blk(k);
      end;
   end;
   if ~isempty(sparse_blk);
      if size(sparse_blk,1) > size(sparse_blk,2); sparse_blk = sparse_blk'; end;
      tmp = size(blk,1);
      blk{tmp+1,1} = 's';
      blk{tmp+1,2} = sparse_blk;
   end;
   if ~isempty(diag_blk);
      tmp = size(blk,1);
      blk{tmp+1,1} = 'l';
      blk{tmp+1,2} = diag_blk;
   end;
   N = size(blk,1);

   if (infeas == 1),
%%
%% primal infeasible
%%
%% generate infeasibility certificate Zi and Z0
%%
   tmp_sp = sparse(sum(sparse_blk));
   for L = 1:2,
       T = [];
       if ~isempty(dense_blk);
          for k = 1:length(dense_blk);
              n = dense_blk(k);
              tmp = randn(n);
              tmp = tmp*tmp';
              tmp = 0.5*(tmp + tmp');
              %mineig = min(real(eig(tmp)));
              %if (mineig < 0); tmp = tmp - 1.1*mineig*eye(n); end;
              T{k,1} = tmp;
          end;
       end;
       if ~isempty(sparse_blk);
          for k = 1:length(sparse_blk);
              n = sparse_blk(k);
              pos = [sum(sparse_blk(1:k-1))+1 : sum(sparse_blk(1:k))];
              tmp = randn(n);
              tmp = tmp*tmp';
              tmp = 0.5*(tmp + tmp');
              %mineig = min(real(eig(tmp)));
              %if (mineig < 0); tmp = tmp - 1.1*mineig*eye(n); end;
              tmp_sp(pos,pos) = sparse(tmp);
          end;
          T{size(T,1)+1,1} = tmp_sp;
       end;
       if ~isempty(diag_blk);
          tmp = randn(diag_blk,1);
          tmp = tmp.*tmp;
          %mineig = min(tmp);
          %if (mineig < 0); tmp = tmp - 1.1*mineig*ones(diag_blk,1); end;
          T{size(T,1)+1,1} = tmp;
       end;
       if (L == 1);  Zi = T; else; Z0 = T; end;
   end;
%%
%% set up the matrices Ak and b
%%
   A  = cell(N,m);
   Ak_sp = sparse(sum(sparse_blk),sum(sparse_blk));
   for k = 1:m;
       Ak = [];
       if ~isempty(dense_blk);
          for j = 1:length(dense_blk);
              tmp = randn(dense_blk(j)); tmp = 0.5*(tmp+tmp');
              Ak{j,1} = tmp;
          end;
       end;
       if ~isempty(sparse_blk);
          for j = 1:length(sparse_blk);
              n = sparse_blk(j);
              tmp = randn(n); tmp = 0.5*(tmp+tmp');
              pos = [sum(sparse_blk(1:j-1))+1 : sum(sparse_blk(1:j))];
              Ak_sp(pos,pos) = tmp;
          end;
          Ak{size(Ak,1)+1,1} = Ak_sp;
       end;
       if ~isempty(diag_blk);
          Ak{size(Ak,1)+1,1} = randn(diag_blk,1);
       end;
       A(:,k) = Ak;
   end;

   y = ones(m,1);
   SAZm = ops(ops(Zi,'+',Asum(blk,A,y)),'/',m);
   SAZm = ops(ops(SAZm,'+',ops(SAZm,'transpose')),'*',0.5);
   yi  = randn(m,1);
   for k = 1:m,
      Ak = A(:,k);
      Ak = ops(ops(Ak,'-',SAZm),'/',yi(k));
      Ak = ops(ops(Ak,'+',ops(Ak,'transpose')),'*',0.5);
      A(:,k) = Ak;
   end;
   b = randn(m,1);
   if (b'*yi < 0), b = -b; end;
   y0  = randn(m,1);
   C = ops(Z0,'+',Asum(blk,A,y0));
   C = ops(ops(C,'+',ops(C,'transpose')),'*',0.5);
%%
%% (yi,Zi) is a primal infeasibility certificate
%%
   elseif (infeas == 2),
%%
%% dual infeasible
%%
%% generate infeasibility certificate Xi, positive definite X0, and C
%%
   tmp_sp_Xi  = sparse(sum(sparse_blk));
   tmp_sp_X0 = sparse(sum(sparse_blk));
   tmp_sp_C   = sparse(sum(sparse_blk));
   Xi = [];
   X0 = [];
   C  = [];
       if ~isempty(dense_blk);
          for k = 1:length(dense_blk);
              n = dense_blk(k);
              tmp = randn(n);
              tmp = tmp*tmp';
              tmp = 0.5*(tmp + tmp');
              %mineig = min(real(eig(tmp)));
              %if (mineig < 0); tmp = tmp - 1.1*mineig*eye(n); end;
              Xi{k,1} = tmp;
              tmp = randn(n);
              tmp = tmp*tmp';
              tmp = 0.5*(tmp + tmp');
              %mineig = min(real(eig(tmp)));
              %if (mineig < 0); tmp = tmp - 1.1*mineig*eye(n); end;
              X0{k,1} = tmp;
              tmp = randn(n); tmp = 0.5*(tmp + tmp');
              C{k,1} = tmp;
          end;
       end;
       if ~isempty(sparse_blk);
          for k = 1:length(sparse_blk);
              n = sparse_blk(k);
              pos = [sum(sparse_blk(1:k-1))+1 : sum(sparse_blk(1:k))];
              tmp = randn(n);
              tmp = tmp*tmp';
              tmp = 0.5*(tmp + tmp');
              %mineig = min(real(eig(tmp)));
              %if (mineig < 0); tmp = tmp - 1.1*mineig*eye(n); end;
              tmp_sp_Xi(pos,pos) = sparse(tmp);
              tmp = randn(n);
              tmp = tmp*tmp';
              tmp = 0.5*(tmp + tmp');
              %mineig = min(real(eig(tmp)));
              %if (mineig < 0); tmp = tmp - 1.1*mineig*eye(n); end;
              tmp_sp_X0(pos,pos) = sparse(tmp);
              tmp = randn(n);  tmp = 0.5*(tmp + tmp');
              tmp_sp_C(pos,pos) = sparse(tmp);
          end;
          Xi{size(Xi,1)+1,1} = tmp_sp_Xi;
          X0{size(X0,1)+1,1} = tmp_sp_X0;
          C{size(C,1)+1,1}   = tmp_sp_C;
       end;
       if ~isempty(diag_blk);
          n = diag_blk;
          tmp = randn(n,1);
          tmp = tmp.*tmp;
          %mineig = min(tmp);
          %if (mineig < 0); tmp = tmp - 1.1*mineig*ones(n,1); end;
          Xi{size(X0,1)+1,1} = tmp;
          tmp = randn(n,1);
          tmp = tmp.*tmp;
          %mineig = min(tmp);
          %if (mineig < 0); tmp = tmp - 1.1*mineig*ones(n,1); end;
          X0{size(X0,1)+1,1} = tmp;
          tmp = randn(n,1);
          C{size(C,1)+1,1} = tmp;
       end;
%%
%% set up the matrices Ak and b
%%
   trXX = blktrace(blk,Xi,Xi);
   A  = cell(N,m);
   Ak_sp = sparse(sum(sparse_blk),sum(sparse_blk));
   b = ones(m,1);
   for k = 1:m;
       Ak = [];
       if ~isempty(dense_blk);
          for j = 1:length(dense_blk);
              n = dense_blk(j);
              Aj  = randn(n);
              Ak{j,1} = (Aj + Aj')/2;
          end;
       end;
       if ~isempty(sparse_blk);
          for j = 1:length(sparse_blk);
              pos = [sum(sparse_blk(1:j-1))+1 : sum(sparse_blk(1:j))];
              n = sparse_blk(j);
              Aj  = randn(n);
              Ak_sp(pos,pos) = (Aj + Aj')/2;
          end;
          Ak{size(Ak,1)+1,1} = Ak_sp;
       end;
       if ~isempty(diag_blk);
          n = diag_blk;
          Ak{size(Ak,1)+1,1} = randn(n,1);
       end;
       trAX = blktrace(blk,Ak,Xi);
       Ak = ops(Ak,'-',ops(Xi,'*',(trAX/trXX)));
       Ak = ops(ops(Ak,'+',ops(Ak,'transpose')),'*',0.5);
       A(:,k) = Ak;
       b(k) = blktrace(blk,Ak,X0);
   end;
   if (blktrace(blk,C,Xi) > 0), C = ops(C,'*',-1); end;
%%
%% Xi is a dual infeasibility certificate
%%
   end;

%%
%% infeasible initial iterate
%%
   Avec = svec(blk,A,ones(size(blk,1),1));
   [X0,y0,Z0] = infeaspt(blk,Avec,C,b);
%%
   if solve;
      [obj,X,y,Z] = sqlp(blk,Avec,C,b,[],X0,y0,Z0);
   end;

%%=================================================


function [yn,en,S] = LMSadapt_dec(un,S)

% LMSadapt_dec      LMS Algorithm for Decision-Directed Channel Equalization
%
%                   The history of output, squared error and coefficients
%                   of FIR filter are passed to calling function.
%                   Only for decision-directed mode channel equalization
%
% Arguments:
% un                Input signal
% S                 Adptive filter parameters as defined in LMSinit.m
% yn                History of output signal
% en                History of error signal
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

M = length(S.coeffs);               % Length of FIR filter
mu = S.step;                        % Step size mu
leak = S.leakage;                   % Leaky factor
w = S.coeffs;                       % Weight vector of FIR filter
u = zeros(M,1);                     % Input signal vector

ITER = length(un);                  % Length of input sequence
yn = zeros(1,ITER);                 % Initialize output sequence to zero
en = zeros(1,ITER);                 % Initialize error sequence to zero

if isfield(S,'unknownsys')
    b = S.unknownsys;
    norm_b = norm(b);
    eml = zeros(1,ITER);
    ComputeEML = 1;
else
    ComputeEML = 0;
end

for n = 1:ITER
    u = [un(n); u(1:end-1)];         % Input signal vector [u(n),u(n-1),...,u(n-M+1)]'
    yn(n) = w'*u;                    % Output signal
    dn(n) = sign(yn(n));             % Only for decision directed mode channel equalizer
    en(n) = dn(n) - yn(n);           % Estimation error
    w = (1-mu*leak)*w + (mu*en(n))*u;% Tap-weight adaptation
    S.iter = S.iter + 1;
end

S.coeffs = w;                        % Coefficient values at the final iteration
if ComputeEML == 1;
    S.eml = eml;
end


% This script provides a tutorial application of the Gaussian-Copula Mutual
% Information (GCMI) estimator with a 2 category discrete stimulus (face
% vs noise images) event-related EEG data set.

% This script uses cell mode - cells are delimited by %% lines and can
% be run with:
% ctrl-enter (windows, linux) or cmd-enter (mac)
% or "Run Section" button from the toolbar
% or right click -> Evaluate currect section

% You need the gcmi/matlab directory on your matlab path.
% If you are running from the gcmi project tree you can add this with the
% following command:
% addpath('../matlab')

% Questions / comments : robince@gmail.com

%% Download and load data

% this will attempt to download the eeg data from the internet (~210MB)
% alternatively you can download the file eeg_face_vs_noise.mat manually
% and place it in the same directory as this script
fname = 'eeg_face_vs_noise.mat';
data_url = ['https://www.robince.net/data/gcmi/data/' fname];

% checks for data in current working directory
if ~exist(fname)
    disp(sprintf('Downloading %s ...', data_url))
    websave(fname,data_url);
    disp('Done.')
end

load(fname);
% csddat : [channels x time x trials]
% permute to trials first axis for looping
csddat = permute(csddat, [3 1 2]);
[Ntrl, Nch, Nt] = size(csddat);

% stim : stimulus class on each trial (0, 1)

%% Calculate GCMI at a specific sensor and time point

% this is the strongest effect
chi = 41;
ti = 168;

% for a t-test we contrast the data from the two classes
faceeeg = csddat(stim==0,chi,ti);
noiseeeg = csddat(stim==1,chi,ti);
[h,p,ci,stats] = ttest2(faceeeg, noiseeeg, 'VarType', 'unequal');
t = stats.tstat

% for MI we use the stimulus labels for each trial
% this works in the same way if there are more than 2 classes
% Reminder: stim must take values 0 or 1
I = gcmi_model_cd(csddat(:,chi,ti), stim, 2)

%% Calculate GCMI across all sensors and time point

% following the commonly used mass-univeriate approach we repeat
% the calculation above for each sensor and time point
% we could call gcmi_cd as above inside the loop, but here we first
% normalise the data separately so we can reuse it for the permutation
% testing

% Gaussian-copula normalisation, works along first axis, applied to each
% other dimension independently
ceeg = copnorm(csddat);
Ieeg = zeros(Nch,Nt);
for ti=1:Nt
    for chi=1:Nch
        % as the data has been copula-normalised we can use the
        % Gaussian parametric estimator (whatever the distribution was
        % originally)
        Ieeg(chi,ti) = mi_model_gd(ceeg(:,chi,ti), stim, 2, true, true);
    end
end

figure
imagesc(time,[],Ieeg)
xlabel('Time')
ylabel('Channels')

%% Permutation test for signifiance with the method of maximum statistics
Nperm = 100;

% to reduce computation time, we consider a single time point here - the
% same method should normally be applied over all sensors and time points
ti = 168;
I = Ieeg(:,ti);
Iperm = zeros(Nch,Nperm);
for pi=1:Nperm
    idx = randperm(Ntrl);
    % randomly permute stimulus labels
    pstim = stim(idx);
    % repeat mass-univariate MI calculation
    for chi=1:Nch
        Iperm(chi,pi) = mi_model_gd(ceeg(:,chi,ti), pstim, 2, true, true);
    end
end

% method of maximum statistics
% maximum values across permutations
Imax = max(Iperm,[],1);
thresh = prctile(Imax, 99);
Isig = I>thresh;

% if you have eeglab we can plot the topologies
if exist('topoplot')
    figure
    subplot(1,2,1)
    topoplot(I, chanlocs, 'maplimits', [0 max(I)]);
    title(sprintf('MI, t = %d ms', time(ti)))
    colorbar

    subplot(1,2,2)
    topoplot(Isig, chanlocs, 'maplimits', [0 1])
    title(sprintf('MI, p<0.01, t = %d ms', time(ti)));
    colorbar

    colormap parula
end


% DURATION_FEATURE Calculate the log-duration of an object
%
% Usage
%    duration = DURATION_FEATURE(x, object)
%
% Input
%    x (numeric): The file data (not used).
%    object (struct): The objects contained in the data.
%
% Output
%    duration (numeric): The log-duration of the objects.
%
% See also
%    PREPARE_DATABASE

function t = duration_feature(x, object)
	t = permute(log([object.u2]-[object.u1]+1),[1 3 2]);
end


function FtI = warped_img_inv(I,u,v)

[M,N] = size(I);

% Stack the motion field into vector
vectors = zeros(length(xPos),2);
vectors(:,1) = u(sub2ind(size(u),yPos,xPos));
vectors(:,2) = v(sub2ind(size(u),yPos,xPos));

% Calculate frame2to1
[xPosv, yPosv] = meshgrid(1:1:n_size(2),1:1:n_size(1));
% vectors_full is the full size motion field
vectors_full = zeros(n_size(1),n_size(2),2);
vectors_full(:,:,1) = u;
vectors_full(:,:,2) = v;

xPosv = xPosv+vectors_full(:,:,1);
yPosv = yPosv+vectors_full(:,:,2);
xPosv = reshape(xPosv,n_size(1),n_size(2));
yPosv = reshape(yPosv,n_size(1),n_size(2));

xPosv(xPosv <= 1) = 1;
yPosv(yPosv <= 1) = 1;
xPosv(xPosv >= n_size(2)) = n_size(2);
yPosv(yPosv >= n_size(1)) = n_size(1);

FtI = interp2(I,xPosv,yPosv,'cubic');


function [trl] = ft_trialfun_emgdetect(cfg)

% Note that there are some parameters, like the EMG channel name and the
% processing that is done on the EMG channel data, which are hardcoded in
% this trial function. You should change these parameters if necessary.

% read the header and determine the channel number corresponding with the EMG
hdr         = ft_read_header(cfg.headerfile);
chanindx    = strmatch('EMGlft', hdr.label);

if length(chanindx)>1
  error('only one EMG channel supported');
end

% read all data of the EMG channel, assume continuous file format
emg = ft_read_data(cfg.datafile, 'header', hdr, ...
              'begsample', 1, 'endsample', hdr.nSamples*hdr.nTrials, ...
              'chanindx', chanindx, 'checkboundary', false);

% apply filtering, hilbert transformation and boxcar convolution (for smoothing)
emgflt      = ft_preproc_highpassfilter(emg, hdr.Fs, 10); % highpassfilter
emghlb      = abs(hilbert(emgflt')');                     % hilbert transform
emgcnv      = conv2([1], ones(1,hdr.Fs), emghlb, 'same'); % smooth using convolution
emgstd      = ft_preproc_standardize(emgcnv, 2);          % z-transform, i.e. mean=0 and stdev=1
emgtrl      = emgstd>0;                                   % detect the muscle activity
emgtrl      = diff(emgtrl, [], 2);

emgon       = find(emgtrl(:)== 1);
emgoff      = find(emgtrl(:)==-1);

trl(:,1) = emgon (:) + hdr.Fs*0.5;  % as a consequence of the convolution with a one-second boxcar
trl(:,2) = emgoff(:) - hdr.Fs*0.5;  % as a consequence of the convolution with a one-second boxcar
trl(:,3) = 0;


function plot_bootfitloglike_a2(mycat,time,timef,bootloops,maepi)
    % Plots Ncum (observed vs. modeled) for specified time windows (choose model for the learning period)
    %
    % plot_bootfitloglike_a2(mycat,time,timef,bootloops,ZG.maepi);
    % --------------------------------------------------
    % Plots Ncum observed vs. Ncum modeled for specified time windows
    % with choosing the model for the learning period
    % Input variables:
    % mycat         : earthquake catalog
    % time      : learning period fo fit Omori parameters [days]
    % timef     : forecast period [days]
    % bootloops : Number of bootstraps
    % ZG.maepi     : mainshock
    %
    % J.Woessner, S. Wiemer

%FIXME mCat ZG.maepi still treated as arrays
report_this_filefun();
    % Surpress warnings from fmincon
    % warning off;

    if ~ensure_mainshock()
        return
    end
    date_matlab = datenum(mycat.Date);
    date_main = datenum(ZG.maepi.Date);
    time_aftershock = date_matlab-date_main;

% Select biggest aftershock earliest in time, but more than 1 day after mainshock
    fDay = 1; %days
    vSel = (mycat.Date > ZG.maepi.Date + days(fDay)) & mycat.Date<= ZG.maepi.Date+days(time);
    mCat = mycat.subset(vSel);
    vSel = mCat.Magnitude == max(mCat.Magnitude);
    vBigAf = mCat(vSel,:);
    if sum(vSel) > 1
        vBigAf.sort('Date')
        vBigAf = vBigAf.subset(1);
    end
    date_biga = datenum(vBigAf.Date);
    fT1 = date_biga - date_main; % Time of big aftershock

    % Aftershock times
    l = time_aftershock(:) > 0;
    tas = time_aftershock(l);
    eqcatalogue = mycat.subset(l);

    % time_as: Learning period
    l = tas <= time;
    time_as=tas(l);

    % Times up to the forecast time
    lf = tas <= time+timef ;
    time_asf= [tas(lf) ];
    time_asf=sort(time_asf);

    % % Calculate p,c,k for dataset
    % prompt  = {'Enter model number (1:pck, 2:pckk, 3:ppckk, 4:ppcckk:'};
    % title   = 'Model selection for fitting aftershock sequence';
    % lines= 1;
    % def     = {'1'};
    % answer  = inputdlg(prompt,title,lines,def);
    % nMod = str2double(answer{1});
    % Calculate fits of different models
    % see OmoriModel for enumaration details

    MOL_models = sort(enumeration('OmoriModel'));

    mRes = [];
    % Modified Omori law (pck)

    for nMod=1:numel(MOL_models) % do this for each model
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as, fT1, MOL_models(nMod));
        mRes(nMod,:) = [nMod, pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL];
    end

    % Select best fitting model by AIC
    vSel = (mRes(:,8)==min(mRes(:,8)));
    mRes = mRes(vSel,:);
    if length(mRes(:,1)) > 1
        vSel = (mRes(:,1)==min(mRes(:,1)));
        mRes = mRes(vSel,:);
    end
    % Model to use for bootstrapping as of lowest AIC to observed data
    nMod = OmoriModel(mRes(1,1));
    pval1= mRes(1,2);
    pval2= mRes(1,3);
    cval1= mRes(1,4);
    cval2= mRes(1,5);
    kval1= mRes(1,6);
    kval2= mRes(1,7);

    % Calculate goodness of fit with KS-Test and RMS
    [H,P,KSSTAT,fRMS] = calc_llkstest_a2(time_as,fT1,pval1, pval2, cval1, cval2, kval1, kval2, nMod);

    % Calculate uncertainty and mean values of p,c,and k
    [mMedModF, mStdL, loopout] = brutebootloglike_a2(time_as, time_asf, bootloops,fT1,nMod);
    pmed1 = mMedModF(1,1);
    pmed2 = mMedModF(1,3);
    cmed1 = mMedModF(1,5);
    cmed2 = mMedModF(1,7);
    kmed1 = mMedModF(1,9);
    kmed2 = mMedModF(1,11);

    % Start plotting
    if (~isnan(pval1) && ~isnan(pval2))

        figure_w_normalized_uicontrolunits('Numbertitle','off','Name','Forecast aftershock occurence')
        loopout(:,end+1) = 0; % add a column

        pval1s = loopout(:,1);
        pval2s = loopout(:,2);
        cval1s = loopout(:,3);
        cval2s = loopout(:,4);
        kval1s = loopout(:,5);
        kval2s = loopout(:,6);

        cumnr_model = OmoriModel.doForecast(nMod, time_asf, pval1s, cval1s, kval1s, fT1, kval2s, pval2s, cval2s);

        loopout(:,9)=max(cumnr_model);

        pfloop = plot(time_asf,cumnr_model,'color',[0.8 0.8 0.8]);
        set(gca,'NextPlot','add')

        warning('This uses only the last values')
        % 2nd moment of bootstrap number of forecasted number of events
        fStdBst = std(loopout(:,9),1,'omitnan');
        %
        % Plot the forecast ...
        cumnrf = (1:length(time_asf))';

        cumnr_modelf = OmoriModel.doForecast(nMod, time_asf, pval1s(end), cval1s(end), kval1s(end), fT1, kval2s(end), pval2s(end), cval2s(end));

        time_asf=sort(time_asf);
        cumnr_modelf=sort(cumnr_modelf);

        pf1 =  plot(time_asf,cumnr_modelf,'g-.','Linewidth',2);
        set(gca,'NextPlot','add')
        %pf2 =  plot(time_asf,cumnrf, 'b-','Linewidth',2);
        %
        % Plot the  fit to the observed data
        % Cumulative number of observed events
        cumnr = (1:length(time_as))';
        cumnr_model = [];
        cumnr_model= OmoriModel.doForecast(nMod, time_as, pval1s(end), cval1s(end), kval1s(end), fT1, kval2s(end), pval2s(end), cval2s(end));

        time_as=sort(time_as);
        cumnr_model=sort(cumnr_model);
        p1 = plot(time_as,cumnr_model,'r','Linewidth',2,'Linestyle','--');
        set(gca,'NextPlot','add');
        p2 = plot(time_as,cumnr,'b','Linewidth',2,'Linestyle','--');

        % Plot the forecast from median value
        cumnr_modelmed = [];
        cumnr_modelmed= OmoriModel.doForecast(nMod, time_asf, pmed1, cmed1, kmed1, fT1, kmed2, pmed2, cmed2);

        time_asf=sort(time_asf);
        cumnr_modelmed=sort(cumnr_modelmed);
        pmedmod =  plot(time_asf,cumnr_modelmed,'y-.','Linewidth',2);

        % Plot observed events in forecast period from endpoint of modeled events in learning period
        vSel = time_asf >= max(time_as);
        vCumnr_forecast = cumnrf(vSel,:);
        vTime_forecast = time_asf(vSel,:);
        % Difference of modelled and observed number of events at time_as
        fDiff_timeas = cumnr_modelmed(length(time_as))-cumnrf(length(time_as));
        vCumnr_forecast = vCumnr_forecast+fDiff_timeas;
        pf3 = plot(vTime_forecast, vCumnr_forecast,'m-.','Linewidth',2);

        xlabel('Time [days]')
        ylabel('Cumulative number of aftershocks')
        xlim([0 max(time_asf)]);

        % Plot standard deviation from bootstrap
        ps2=errorbar(max(time_asf),max(cumnr_modelmed),fStdBst,fStdBst);
        set(ps2,'Linewidth',2,'Color',[1 0 0])
        %
        %     % Title
        % Find amount of events in forecast period for modeled data
        nummod2 = max(cumnr_modelf)-cumnr_modelf(length(time_as));
        nummod = max(cumnr_modelmed)-cumnr_modelmed(length(time_as));
        %     % Find amount of  events in forecast period for observed data
        l = time_asf <=time+timef & time_asf > time;
        numreal = sum(l); % observed number of aftershocks
        %     fRc_Flaw = (numreal-nummod)/sigma;
        fRc_Bst2 = (numreal-nummod2)/fStdBst;
        fRc_Bst = (numreal-nummod)/fStdBst;

        % Round values for output
        pval1 = round(pval1,2);
        pval2 = round(pval2,2);
        cval1 = round(cval1,3);
        cval2 = round(cval2,3);
        kval1 = round(kval1,1);
        kval2 = round(kval2,1);
        pmed1 = round(pmed1,2); mStdL(1,1) = round(mStdL(1,1),2);
        pmed2 = round(pmed2,2); mStdL(1,2) = round(mStdL(1,2),2);
        cmed1 = round(cmed1,3); mStdL(1,3) = round(mStdL(1,3),3);
        cmed2 = round(cmed2,3); mStdL(1,4) = round(mStdL(1,4),3);
        kmed1 = round(kmed1,1); mStdL(1,5) = round(mStdL(1,5),2);
        kmed2 = round(kmed2,1); mStdL(1,6)= round(mStdL(1,6),2);
        fAIC = round(fAIC,2);
        fStdBst = round(fStdBst,2);
        fRc_Bst = round(fRc_Bst,2);

        % Set line for learning period
        yy = get(gca,'ylim');
        plot([max(time_as) max(time_as)],[0 yy(2)],'k-.')
        textX = max(time_asf)*0.1;
        textY = @(z) yy(2)* z;

        switch nMod
            case OmoriModel.pck
                string1 = sprintf('p = %g; c = %g; k = %g', pval1, cval1, kval1);
                string3 = sprintf('pm = %g+-%g; cm = %g+-%g; km = %g+-%g', mped1, mStdL(1,1), cmed1, mStdL(1,3), kmed1, mStdL(1,5));
                % string3=['pm = ' num2str(pmed1) '+-' num2str(mStdL(1,1)) '; cm = ' num2str(cmed1) '+-' num2str(mStdL(1,3)) '; km = ' num2str(kmed1) '+-' num2str(mStdL(1,5))];
                text(textX,textY(0.9),string1,'FontSize',8);
                text(textX,textY(0.8),string3,'FontSize',8);
            case  OmoriModel.pckk
                string1 = sprintf('p = %g; c = %g; k1 = %g; k2 = %g', pval1, cval1, kval1, kval2);
                string3 = sprintf('pm = %g+-%g; cm = %g+-%g; km1 = %g+-%g; km2 = %g+-%g',...
                    mped1, mStdL(1,1), cmed1, mStdL(1,3), kmed1, mStdL(1,5), kmed2, mStdL(1,6));
                text(textX,textY(0.9),string1,'FontSize',8);
                text(textX,textY(0.8),string3,'FontSize',8);
            case OmoriModel.ppckk
                string1 = sprintf('p1 = %g; c = %g; k1 = %g', pval1, cval1, kval1);
                string2 = sprintf('p2 = %g; k2 = %g', pval2, kval2);
                string3 = sprintf('pm1 = %g+-%g; cm = %g+-%g; km1 = %g+-%g', mped1, mStdL(1,1), cmed1, mStdL(1,3), kmed1, mStdL(1,5));
                string4 = sprintf('pm2 = %g+-%g; km2 = %g+-%g', mped2, mStdL(1,2), kmed2, mStdL(1,6));
                text(textX,textY(0.9),string1,'FontSize',8);
                text(textX,textY(0.85),string2,'FontSize',8);
                text(textX,textY(0.8),string3,'FontSize',8);
                text(textX,textY(0.75),string4,'FontSize',8);
            case OmoriModel.ppcckk
                string1 = sprintf('p1 = %g; c1 = %g; k1 = %g', pval1, cval1, kval1);
                string2 = sprintf('p2 = %g; c2 = %g; k2 = %g', pval2, cval2, kval2);
                string3 = sprintf('pm1 = %g+-%g; cm1 = %g+-%g; km1 = %g+-%g', mped1, mStdL(1,1), cmed1, mStdL(1,3), kmed1, mStdL(1,5));
                string4 = sprintf('pm2 = %g+-%g; cm2 = %g+-%g; km2 = %g+-%g', mped2, mStdL(1,2), cmed2, mStdL(1,4), kmed2, mStdL(1,6));
                text(textX,textY(0.9),string1,'FontSize',8);
                text(textX,textY(0.85),string2,'FontSize',8);
                text(textX,textY(0.8),string3,'FontSize',8);
                text(textX,textY(0.75),string4,'FontSize',8);
        end
        string=sprintf('\sigma(Bst) = %g Rc = %g',fStdBst, fRc_Bst);%' Rc(Obfit) = ' num2str(fRc_Bst2)];
        text(textX,yy(2)*0.1,string,'FontSize',8);

        sAIC = sprintf('AIC = %g',fAIC);
        text(textX,yy(2)*0.05,sAIC,'FontSize',8);

        paf = plot(fT1, 0,'h','MarkerFaceColor',[1 1 0],'MarkerSize',12,'MarkerEdgeColor',[0 0 0] );
        sGoodfit = sprintf('KS Test: H = %g, KS statistic = %g P value = %g; RMS = %g', H, KSSTAG, P,fRMS);
        text(textX,yy(2)*0.15,sGoodfit,'FontSize',8);
        % Legend
        legend([p2 p1 pf1 pf3 pmedmod min(ps2) paf],'data','model to data','forecast','observed', 'Mean Bst-model', '\sigma (Bst)','Sec. AF', 'location', 'Best');
    else
        disp('no result')
    end


function y = efilter2(x, f, extmod, shift)
% EFILTER2   2D Filtering with edge handling (via extension)
%
%	y = efilter2(x, f, [extmod], [shift])
%
% Input:
%	x:	input image
%	f:	2D filter
%	extmod:	[optional] extension mode (default is 'per')
%	shift:	[optional] specify the window over which the
%		convolution occurs. By default shift = [0; 0].
%
% Output:
%	y:	filtered image that has:
%		Y(z1,z2) = X(z1,z2)*F(z1,z2)*z1^shift(1)*z2^shift(2)
%
% Note:
%	The origin of filter f is assumed to be floor(size(f)/2) + 1.
%	Amount of shift should be no more than floor((size(f)-1)/2).
%	The output image has the same size with the input image.
%
% See also:	EXTEND2, SEFILTER2

if ~exist('extmod', 'var')
    extmod = 'per';
end

if ~exist('shift', 'var')
    shift = [0; 0];
end

% Periodized extension
sf = (size(f) - 1) / 2;

xext = extend2(x, floor(sf(1)) + shift(1), ceil(sf(1)) - shift(1), ...
	       floor(sf(2)) + shift(2), ceil(sf(2)) - shift(2), extmod);

% Convolution and keep the central part that has the size as the input
y = conv2(xext, f, 'valid');


function area = sphericalCapsAreaC6(varargin)
%SPHERICALCAPSAREAC6 compute area of spherical caps on the unit sphere
%
%   AREA = sphericalCapsAreaC6
%   Compute the area of the spherical caps associated to the voronoi
%   diagram on the unit sphere, when the germs correspond to the 26
%   discrete directions in the unit cube.
%   Result is a 1x3 array, containing fraction of area of each type of
%   spherical cap.
%   area(1) concerns the direction [1 0 0]
%   area(2) concerns the direction [0 1 0]
%   area(3) concerns the direction [0 0 1]
%   Result is formatted such that 2*(area(1)+area(2)+area(3))=1.
%   For homogeneous lattices, all areas are equal.
%
%   AREA = sphericalCapsAreaC6(DELTA)
%   where DELTA = [DELTA1 DELTA2 DELTA3]  specifies the resolution of unit
%   voxel in each direction.
%
%
%   Algorithm :
%   - separate 3 basic case, corresponding on the type of directions
%   - for each cases, specify manually germ and neighbours
%   - compute great circle between each couple of germ
%   - compute intersection points of these circles
%   - sort intersection points around the central germ, giving a spherical
%      polygon
%   - compute triangulation of the resulting polygon
%   - compute spherical area of each triangle
%   - and area of each polygon
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY
%   27/04/2007 extends to non uniform grid spacing

%% Initializations

% grid resolution
delta = [1 1 1];
if ~isempty(varargin)
    delta = varargin{1};
end

% poins in the 26 discrete directions
%pt000 = normalize([-1 -1 -1].*delta);
%pt100 = normalize([ 0 -1 -1].*delta);
%pt200 = normalize([+1 -1 -1].*delta);
%pt010 = normalize([-1  0 -1].*delta);
pt110 = normalize([ 0  0 -1].*delta);
% pt210 = normalize([+1  0 -1].*delta);
% pt020 = normalize([-1 +1 -1].*delta);
% pt120 = normalize([ 0 +1 -1].*delta);
% pt220 = normalize([+1 +1 -1].*delta);

%pt001 = normalize([-1 -1  0].*delta);
pt101 = normalize([ 0 -1  0].*delta);
% pt201 = normalize([+1 -1  0].*delta);
pt011 = normalize([-1  0  0].*delta);
%pt111 = [ 0  0  0];             % origin point
pt211 = normalize([+1  0  0].*delta);
% pt021 = normalize([-1 +1  0].*delta);
pt121 = normalize([ 0 +1  0].*delta);
% pt221 = normalize([+1 +1  0].*delta);

% pt002 = normalize([-1 -1 +1].*delta);
% pt102 = normalize([ 0 -1 +1].*delta);
% pt202 = normalize([+1 -1 +1].*delta);
% pt012 = normalize([-1  0 +1].*delta);
pt112 = normalize([ 0  0 +1].*delta);
% pt212 = normalize([+1  0 +1].*delta);
% pt022 = normalize([-1 +1 +1].*delta);
% pt122 = normalize([ 0 +1 +1].*delta);
% pt222 = normalize([+1 +1 +1].*delta);

% basic unit sphere
sphere = [0 0 0  1];

%% Spherical cap type 1 direction [1 0 0]
% ----------------------------------------------------------------------

% Compute area of voronoi cell for a point on the Ox axis, i.e. a point
% in the 6-neighboorhoud of the center.
centre1 = pt211;
% neighbours of chosen point, sorted by angle
%voisins1 = [pt200;pt201;pt202;pt212;pt222;pt221;pt220;pt210];
voisins1 = [pt112;pt121;pt110;pt101];
n1 = size(voisins1, 1);

% compute separating circles
planes1 = zeros(n1, 9);
for i=1:n1
    planes1(i,1:9) = normalizePlane(medianPlane(centre1, voisins1(i,:)));
%    circles1(i,1:7) = intersectPlaneSphere(planes1(i,:), sphere);
end

% compute circle intersections
lines1 = zeros(n1, 6);
for i=1:n1
    lines1(i,1:6) = intersectPlanes(planes1(i,:), ...
        planes1(mod(i,n1)+1,:));
    points1(2*i-1:2*i,1:3) = intersectLineSphere(lines1(i,:), sphere);
end

% keep only points with x>0
ind = dot(points1, repmat(centre1, [2*n1 1]), 2)>0;
points1 = points1(ind,:);
n1 = size(points1, 1);

% compute spherical area of each triangle [center  pt[i+1]%4   pt[i] ]
angles1 = zeros(n1, 1);
for i=1:n1
    pt1 = points1(i, :);
    pt2 = points1(mod(i  , n1)+1, :);
    pt3 = points1(mod(i+1, n1)+1, :);

    angles1(i) = sphericalAngle(pt1, pt2, pt3);
end

% compute area of spherical polygon
area1 = sum(angles1)-pi*(n1-2);

%% Spherical cap type 1 direction [0 1 0]
% ----------------------------------------------------------------------

% Compute area of voronoi cell for a point on the Oy axis, i.e. a point
% in the 6-neighboorhoud of the center.
centre1 = pt121;
% neighbours of chosen point, sorted by angle
%voisins1 = [pt200;pt201;pt202;pt212;pt222;pt221;pt220;pt210];
%voisins1 = [pt221;pt222;pt122;pt022;pt021;pt020;pt120;pt220];
voisins1 = [pt211;pt112;pt011;pt110];
n1 = size(voisins1, 1);

% compute separating circles
planes1 = zeros(n1, 9);
for i=1:n1
    planes1(i,1:9) = normalizePlane(medianPlane(centre1, voisins1(i,:)));
%    circles1(i,1:7) = intersectPlaneSphere(planes1(i,:), sphere);
end

% compute circle intersections
lines1 = zeros(n1, 6);
for i=1:n1
    lines1(i,1:6) = intersectPlanes(planes1(i,:), ...
        planes1(mod(i,n1)+1,:));
    points1(2*i-1:2*i,1:3) = intersectLineSphere(lines1(i,:), sphere);
end

% keep only points with x>0
ind = dot(points1, repmat(centre1, [2*n1 1]), 2)>0;
points1 = points1(ind,:);
n1 = size(points1, 1);

% compute spherical area of each triangle [center  pt[i+1]%4   pt[i] ]
angles1 = zeros(n1, 1);
for i=1:n1
    pt1 = points1(i, :);
    pt2 = points1(mod(i  , n1)+1, :);
    pt3 = points1(mod(i+1, n1)+1, :);

    angles1(i) = sphericalAngle(pt1, pt2, pt3);
end

% compute area of spherical polygon
area2 = sum(angles1)-pi*(n1-2);

%% Spherical cap type 1 direction [0 0 1]
% ----------------------------------------------------------------------

% Compute area of voronoi cell for a point on the Oz axis, i.e. a point
% in the 6-neighboorhoud of the center.
centre1 = pt112;
% neighbours of chosen point, sorted by angle
%voisins1 = [pt200;pt201;pt202;pt212;pt222;pt221;pt220;pt210];
%voisins1 = [pt212;pt222;pt122;pt022;pt012;pt002;pt102;pt202];
voisins1 = [pt121;pt011;pt101;pt211];

n1 = size(voisins1, 1);

% compute separating circles
planes1 = zeros(n1, 9);
for i=1:n1
    planes1(i,1:9) = normalizePlane(medianPlane(centre1, voisins1(i,:)));
%    circles1(i,1:7) = intersectPlaneSphere(planes1(i,:), sphere);
end

% compute circle intersections
lines1 = zeros(n1, 6);
for i=1:n1
    lines1(i,1:6) = intersectPlanes(planes1(i,:), ...
        planes1(mod(i,n1)+1,:));
    points1(2*i-1:2*i,1:3) = intersectLineSphere(lines1(i,:), sphere);
end

% keep only points with z>0
ind = dot(points1, repmat(centre1, [2*n1 1]), 2)>0;
points1 = points1(ind,:);
n1 = size(points1, 1);

% compute spherical area of each triangle [center  pt[i+1]%4   pt[i] ]
angles1 = zeros(n1, 1);
for i=1:n1
    pt1 = points1(i, :);
    pt2 = points1(mod(i  , n1)+1, :);
    pt3 = points1(mod(i+1, n1)+1, :);

    angles1(i) = sphericalAngle(pt1, pt2, pt3);
    angles1(i) = min(angles1(i), 2*pi-angles1(i));
end

% compute area of spherical polygon
area3 = sum(angles1)-pi*(n1-2);

%% Results
% -------------------------------------------------------------------

% display some results
disp('results : ')
pattern = 'area of cell in direction %d: %12.10f sr, = %12.10f %%';
disp(sprintf(pattern, 1, area1, area1*100/4/pi));
disp(sprintf(pattern, 2, area2, area2*100/4/pi));
disp(sprintf(pattern, 3, area3, area3*100/4/pi));

% return computed areas, formatted as fraction of sphere surface
area = [area1 area2 area3]/4/pi;

%% Internal functions
% -------------------------------------------------------------------
% This functions are part of a more general geometric library, but are
% included here for avoiding dependencies

function theta = angle3Points(varargin)
%ANGLE3POINTS return oriented angle made by 3 points
%
%   ALPHA = ANGLE3POINTS(P1, P2, P3).
%   Pi are either [1*2] arrays, or [N*2] arrays, in this case ALPHA is a
%   [N*1] array. The angle computed is the directed angle between line
%   (P2P1) and line (P2P3).
%   Result is always given in radians, between 0 and 2*pi.
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 23/02/2004.
%

%   HISTORY :
%   25/09/2005 : enable single parameter

if length(varargin)==3
    p1 = varargin{1};
    p2 = varargin{2};
    p3 = varargin{3};
elseif length(varargin)==1
    var = varargin{1};
    p1 = var(1,:);
    p2 = var(2,:);
    p3 = var(3,:);
end

% angle line (P2 P1)
theta = lineAngle(createLine(p2, p1), createLine(p2, p3));

function line = createLine(varargin)
%CREATELINE create a line with various inputs.
%
%   Line is represented in a parametric form : [x0 y0 dx dy]
%   x = x0 + t*dx
%   y = y0 + t*dy;
%
%
%   l = CREATELINE(p1, p2) return the line going through the two given
%   points.
%
%   l = CREATELINE(x0, y0, dx, dy) the line going through point (x0, y0)
%   and with direction vector(dx, dy).
%
%   l = CREATELINE(param) where param is an array of 4 values, create the
%   line oing through the point (param(1) param(2)), and with direction
%   vector (param(3) param(4)).
%
%   l = CREATELINE(theta) create a line originated at (0,0) and
%   with angle theta.
%
%   l = CREATELINE(rho, theta) create a line with normal theta, and with
%   min distance to origin equal to rho. rho can be negative, in this case,
%   the line is the same as with CREATELINE(-rho, theta+pi), but the
%   orientation is different.
%
%
%   Note : in all cases, parameters can be vertical arrays of the same
%   dimension. The result is then an array of lines, of dimensions [N*4].
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 31/10/2003.
%

if length(varargin)==1
    % Only one input parameter. It can be :
    % - line angle
    % - array of four parameters
    var = varargin{1};

    if size(var, 2)==4
        % 4 parameters of the line in a single array.
        line = var;
    elseif size(var, 2)==1
        % 1 parameter : angle of the line, going through origin.
        line = [zeros(size(var)) zeros(size(var)) cos(var) sin(var)];
    else
        error('wrong number of dimension for arg1 : can be 1 or 4');
    end

elseif length(varargin)==2
    % 2 input parameters. They can be :
    % - line angle and signed distance to origin.
    % - 2 points, then 2 arrays of 1*2 double.
    v1 = varargin{1};
    v2 = varargin{2};
    if size(v1, 2)==1
        % first param is angle of line, and second param is signed distance
        % to origin.
        line = [v1.*cos(v2) v1.*sin(v2) -sin(v2) cos(v2)];
    else
        % first input parameter is first point, and second input is the
        % second point.
        line = [v1(:,1), v1(:,2), v2(:,1)-v1(:,1), v2(:,2)-v1(:,2)];
    end

elseif length(varargin)==3
    % 3 input parameters :
    % first one is a point belonging to the line,
    % second and third ones are direction vector of the line (dx and dy).
    p = varargin{1};
    line = [p(:,1) p(:,2) varargin{2} varargin{3}];

elseif length(varargin)==4
    % 4 input parameters :
    % they are x0, y0 (point belongng to line) and dx, dy (direction vector
    % of the line).
    % All parameters should have the same size.
    line = [varargin{1} varargin{2} varargin{3} varargin{4}];
else
    error('Wrong number of arguments in ''createLine'' ');
end

function plane = createPlane(varargin)
%CREATEPLANE create a plane in parametrized form
%
%   Create a plane in the following format :
%   PLANE = [X0 Y0 Z0  DX1 DY1 DZ1  DX2 DY2 DZ2], where :
%   - (X0, Y0, Z0) is a point belonging to the plane
%   - (DX1, DY1, DZ1) is a first direction vector
%   - (DX2, DY2, DZ2) is a second direction vector
%
%
%
%   PLANE = createPlane(P1, P2, P3)
%   create a plane containing the 3 points
%
%   PLANE = createPlane(PTS)
%   The 3 points are packed into a single 3x3 array.
%
%   PLANE = createPlane(P0, N);
%   create a plane from a point and from a normal to the plane.
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

%   HISTORY :
%   24/11/2005 : add possibility to pack points for plane creation

if length(varargin)==1
    var = varargin{1};

    if iscell(var)
        plane = zeros(length(var), 9);
        for i=1:length(var)
            plane(i,:) = createPlane(var{i});
        end
    elseif size(var, 1)==3
        % 3 points in a single array
        p1 = var(1,:);
        p2 = var(2,:);
        p3 = var(3,:);

        % create direction vectors
        v1 = p2-p1;
        v2 = p3-p1;

        % create plane
        plane = [p1 v1 v2];
        return;
    end

elseif length(varargin)==2

    p0 = varargin{1};

    var = varargin{2};
    if size(var, 2)==2
        n = sph2cart2([var repmat(1, [size(var, 1) 1])]);
    elseif size(var, 2)==3
        n  = normalize3d(var);
    else
        error ('wrong number of parameters in createPlane');
    end

    % find a vector not colinear to the normal
    v0 = repmat([1 0 0], [size(p0, 1) 1]);
    if abs(cross(n, v0, 2))<1e-14
        v0 = repmat([0 1 0], [size(p0, 1) 1]);
    end

    % create direction vectors
    v1 = normalize3d(cross(n, v0, 2));
    v2 = -normalize3d(cross(v1, n, 2));

    plane = [p0 v1 v2];
    return;

elseif length(varargin)==3
    p1 = varargin{1};
    p2 = varargin{2};
    p3 = varargin{3};

    % create direction vectors
    v1 = p2-p1;
    v2 = p3-p1;

    plane = [p1 v1 v2];
    return;

else
    error('wrong number of arguments in "createPlane".');
end

function point = intersectLineSphere(line, sphere)
%INTERSECTLINESPHERE return intersection between a line and a sphere
%
%   GC = intersectLineSphere(LINE, SPHERE) return the two points which are
%   the intersection of the given line and sphere.
%   LINE   : [x0 y0 z0  dx dy dz]
%   SPHERE : [xc yc zc  R]
%   GC     : [x1 y1 z1 ; x2 y2 z2]
%
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

%   HISTORY

% difference between centers
dc = line(1:3)-sphere(1:3);

a = sum(line(:, 4:6).*line(:, 4:6), 2);
b = 2*sum(dc.*line(4:6), 2);
c = sum(dc.*dc, 2) - sphere(:,4).*sphere(:,4);

delta = b.*b -4*a.*c;

if delta>1e-14
    % find two roots of second order equation
    u1 = (-b -sqrt(delta))/2/a;
    u2 = (-b +sqrt(delta))/2/a;

    % convert into 3D coordinate
    point = [line(1:3)+u1*line(4:6) ; line(1:3)+u2*line(4:6)];

elseif abs(delta) > 1e-14
    % find unique root, and convert to 3D coord.
    u = -b/2./a;
    point = line(1:3) + u*line(4:6);

else
    point = zeros(0, 3);
    return;
end

function point = intersectPlaneLine(plane, line)
%INTERSECTPLANELINE return intersection between a plane and a line
%
%   PT = intersectPlaneSphere(PLANE, LINE) return the intersection point of
%   the given line and the given plane.
%   PLANE : [x0 y0 z0 dx1 dy1 dz1 dx2 dy2 dz2]
%   LINE :  [x0 y0 z0 dx dy dz]
%   PT :    [XI YI ZI]
%
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 17/02/2005.
%

%   HISTORY
%   24/11/2005 add support for multiple input

% unify sizes of data
if size(plane, 1)~=size(line, 1)
    if size(plane, 1)==1
        plane = repmat(plane, [size(line, 1) 1]);
    elseif size(line,1)==1
        line = repmat(line, [size(plane, 1) 1]);
    else
        error('line and plane do not have the same dimension');
    end
end

% plane normal
n = cross(plane(:,4:6), plane(:, 7:9), 2);

% test if line and plane are parallel
if abs(dot(n, line(:,4:6), 2))<1e-14
    point = [NaN NaN NaN];
    return;
end

% difference between origins of plane and line
dp = plane(:,1:3) - line(:,1:3);

% relative position of intersection on line
t = dot(n, dp, 2)/dot(n, line(:,4:6), 2);

% compute coord of intersection point
point = line(:,1:3) + t*line(:,4:6);

function line = intersectPlanes(plane1, plane2)
%INTERSECTPLANES return intersection between 2 planes in space
%
%   PT = intersectPlanes(PLANE1, PLANE2) return the straight line belonging
%   to both planes
%   PLANE : [x0 y0 z0 dx1 dy1 dz1 dx2 dy2 dz2]
%   LINE :  [x0 y0 z0 dx dy dz]
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 17/02/2005.
%

%   HISTORY

% plane normal
n1 = normalize3d(cross(plane1(:,4:6), plane1(:, 7:9), 2));
n2 = normalize3d(cross(plane2(:,4:6), plane2(:, 7:9), 2));

% test if planes are parallel
if abs(cross(n1, n2, 2))<1e-14
    line = [NaN NaN NaN NaN NaN NaN];
    return;
end

% Uses hessian form, ie : N.p = d
% I this case, d can be found as : -N.p0, when N is normalized
d1 = dot(n1, plane1(:,1:3), 2);
d2 = dot(n2, plane2(:,1:3), 2);

% compute dot products
dot1 = dot(n1, n1, 2);
dot2 = dot(n2, n2, 2);
dot12 = dot(n1, n2, 2);

det = dot1*dot2 - dot12*dot12;
c1 = (d1*dot2 - d2*dot12)./det;
c2 = (d2*dot1 - d1*dot12)./det;

p0 = c1*n1 + c2*n2;
dp = cross(n1, n2, 2);

line = [p0 dp];

function theta = lineAngle(varargin)
%LINEANGLE return angle between lines
%
%   a = LINEANGLE(line) return the angle between horizontal, right-axis
%   and the given line. Angle is fiven in radians, between 0 and 2*pi,
%   in counter-clockwise direction.
%
%   a = LINEANGLE(line1, line2) return the directed angle between the
%   two lines. Angle is given in radians between 0 and 2*pi.
%
%   see createLine for more details on line representation.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 31/10/2003.
%

%   HISTORY :
%   19/02/2004 : added support for multiple lines.

nargs = length(varargin);
if nargs == 1
    % one line
    line = varargin{1};
    theta = mod(atan2(line(:,4), line(:,3)) + 2*pi, 2*pi);
elseif nargs==2
    % two lines
    theta1 = lineAngle(varargin{1});
    theta2 = lineAngle(varargin{2});
    theta = mod(theta2-theta1+2*pi, 2*pi);
end

function plane = medianPlane(p1, p2)
%MEDIANPLANE create a plane in the middle of 2 points
%
%   plane = medianPlane(P1, P2)
%   plane is perpendicular to line (P1 P2) and contains the midpoint of p1
%   and p2.
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

p0 = (p1 + p2)/2;
n = p2-p1;

plane = createPlane(p0, n);

function vn = normalize(v)
%NORMALIZE normalize a vector
%
%   V2 = normalize(V);
%   return the normalization of vector V, such that ||V|| = 1. V can be
%   either a row or a column vector.
%
%   When V is a MxN array, normalization is performed for each row of the
%   array.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 29/11/2004.
%

dim = size(v);
if dim(1)==1 || dim(2)==1
    vn = v/sqrt(sum(v.*v));
else
    vn = v./repmat(sqrt(sum(v.*v, 2)), [1 dim(2)]);
end

function vn = normalize3d(v)
%NORMALIZE3D normalize a 3D vector
%
%   V2 = normalize3d(V);
%   return the normalization of vector V, such that ||V|| = 1. Vector V is
%   given in vertical form.
%
%   When V is a Nx3 array, normalization is performed for each row of the
%   array.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 29/11/2004.
%

%   HISTORY
%   30/11/2005  : correct a bug

n = sqrt(v(:,1).*v(:,1) + v(:,2).*v(:,2) + v(:,3).*v(:,3));
vn = v./[n n n];

function plane2 = normalizePlane(plane1)
%NORMALIZEPLANE normalize parametric form of a plane
%
%   plane2 = normalizePlane(plane1);
%   transform the plane PANE1 in the following format :
%   [X0 Y0 Z0  DX1 DY1 DZ1  DX2 DY2 DZ2], where :
%   - (X0, Y0, Z0) is a point belonging to the plane
%   - (DX1, DY1, DZ1) is a first direction vector
%   - (DX2, DY2, DZ2) is a second direction vector
%   into another plane, with the same format, but with :
%   - (x0 y0 z0) is the closest point of plane to origin
%   - (DX1 DY1 DZ1) has norm equal to 1
%   - (DX2 DY2 DZ2) has norm equal to 1 and is orthogonal to (DX1 DY1 DZ1)
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY :

% compute origin point of the plane
p0 = projPointOnPlane([0 0 0], plane1);

% compute first direction vector
d1 = normalize3d(plane1(:,4:6));

% compute second direction vector
n = normalize3d(planeNormal(plane1));
d2 = -normalize3d(cross(d1, n));

% create the resulting plane
plane2 = [p0 d1 d2];

function n = planeNormal(plane)
%PLANENORMAL compute the normal to a plane
%
%   N = planeNormal(PLANE)
%   compute the normal of the given plane
%   PLANE : [x0 y0 z0 dx1 dy1 dz1 dx2 dy2 dz2]
%   N : [dx dy dz]
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 17/02/2005.
%

%   HISTORY

% plane normal
n = cross(plane(:,4:6), plane(:, 7:9), 2);

function pos = planePosition(point, plane)
%PLANEPOSITION compute position of a point on a plane
%
%   PT2 = PLANEPOSITION(POINT, PLANE)
%   POINT has format [X Y Z], and plane has format
%   [X0 Y0 Z0  DX1 DY1 DZ1  DX2 DY2 DZ2], where :
%   - (X0, Y0, Z0) is a point belonging to the plane
%   - (DX1, DY1, DZ1) is a first direction vector
%   - (DX2, DY2, DZ2) is a second direction vector
%
%   Result PT2 has the form [XP YP], with [XP YP] coordinate of the point
%   in the coordinate system of the plane.
%
%
%   CAUTION :
%   WORKS ONLY FOR PLANES WITH ORTHOGONAL DIRECTION VECTORS
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY :
%   24/11/2005 add support for multiple input

% unify size of data
if size(point, 1)~=size(plane, 1)
    if size(point, 1)==1
        point = repmat(point, [size(plane, 1) 1]);
    elseif size(plane, 1)==1
        plane = repmat(plane, [size(point, 1) 1]);
    else
        error('point and plane do not have the same dimension');
    end
end

p0 = plane(:,1:3);
d1 = plane(:,4:6);
d2 = plane(:,7:9);

s = dot(point-p0, d1, 2)./vecnorm3d(d1);
t = dot(point-p0, d2, 2)./vecnorm3d(d2);

pos = [s t];

function point = projPointOnPlane(point, plane)
%PROJPOINTONPLANE return the projection of a point on a plane
%
%   PT2 = PROJECTEDPOINT(PT1, PLANE).
%   Compute the (orthogonal) projection of point PT1 onto the line PLANE.
%
%   Function works also for multiple points and planes. In this case, it
%   returns multiple points.
%   Point PT1 is a [N*3] array, and PLANE is a [N*9] array (see createPlane
%   for details). Result PT2 is a [N*3] array, containing coordinates of
%   orthogonal projections of PT1 onto planes PLANE.
%
%   See also planePosition
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

n = planeNormal(plane);
line = [point repmat(n, [size(point, 1) 1])];
point = intersectPlaneLine(plane, line);

function varargout = sph2cart2(varargin)
%SPH2CART2 convert spherical coordinate to cartesian coordinate
%
%   usage :
%   C = SPH2CART2(S)
%   C = SPH2CART2(PHI, THETA)       (assume rho = 1)
%   C = SPH2CART2(PHI, THETA, RHO)
%   [X, Y, Z] = SPH2CART2(PHI, THETA, RHO);
%
%   S = [phi theta rho] (sphercial coordiante).
%   C = [X Y Z]  (cartesian coordinate)
%
%   Math convention is used : theta is angle with vertical, 0 for north
%   pole, +pi for south pole, pi/2 for points with z=0.
%   phi is the same as matlab cart2sph : angle from Ox axis, counted
%   counter-clockwise.
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

%   HISTORY
%   22/03/2005 : make test for 2 args, and add radius if not specified for
%       1 arg.

if length(varargin)==1
    var = varargin{1};
    if size(var, 2)==2
        var = [var ones(size(var, 1), 1)];
    end
elseif length(varargin)==2
    var = [varargin{1} varargin{2} ones(size(varargin{1}))];
elseif length(varargin)==3
    var = [varargin{1} varargin{2} varargin{3}];
end

[x y z] = sph2cart(var(:,1), pi/2-var(:,2), var(:,3));

if nargout == 1 || nargout == 0
    varargout{1} = [x, y, z];
else
    varargout{1} = x;
    varargout{2} = y;
    varargout{3} = z;
end

function alpha = sphericalAngle(p1, p2, p3)
%SPHERICALANGLE compute angle on the sphere
%
%   ALPHA = sphericalAngle(P1, P2, P3)
%   compute angle (P1, P2, P2), in radians, between 0 and 2*PI.
%
%   Points are given either as [x y z] (there will be normalized to lie on
%   the unit sphere), or as [phi theta], with phi being the longitude in [0
%   2*PI] and theta being the elevation on horizontal [-pi/2 pi/2].
%
%
%   NOTE :
%   this is an 'oriented' version of the angle computation, that is, the
%   result of sphericalAngle(P1, P2, P3) equals
%   2*pi-sphericalAngle(P3,P2,P1). To have the more classical relation
%   (with results given betwen 0 and PI), it suffices to take the minimum
%   of angle and 2*pi-angle.
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY

% test if points are given as matlab spherical coordinate
if size(p1, 2) ==2
    [x y z] = sph2cart(p1(:,1), p1(:,2));
    p1 = [x y z];
    [x y z] = sph2cart(p2(:,1), p2(:,2));
    p2 = [x y z];
    [x y z] = sph2cart(p3(:,1), p3(:,2));
    p3 = [x y z];
end

% normalize points
p1 = normalize3d(p1);
p2 = normalize3d(p2);
p3 = normalize3d(p3);

% create the plane tangent to the unit sphere and containing central point
plane = createPlane(p2, p2);

% project the two other points on the plane
pi1 = planePosition(intersectPlaneLine(plane, [0 0 0 p1]), plane);
pi3 = planePosition(intersectPlaneLine(plane, [0 0 0 p3]), plane);

% compute angle on the tangent plane
alpha = angle3Points(pi1, [0 0], pi3);

function n = vecnorm3d(v)
%VECNORM3D compute euclidean norm of vector or of set of 3D vectors
%
%   n = vecnorm(V);
%   return euclidean norm of vector V.
%
%   When V is a Nx3 array, compute norm for each vector of the array.
%   Vector are given as rows. Result is then a [N*1] array.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY

n = sqrt(sum(v.*v, 2));


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: demo for 2D data setup and visualization
%
% load images                        (hands-?.jpg)
% visualize ij and omega based data  (viewImage)
% creates multi-lebel representation (getMultilevel)
%
%==============================================================================

clear, close all, help(mfilename);

% load data
Tij = imread('hands-T.jpg'); Tdata = double(flipud(Tij))';
Rij = imread('hands-R.jpg'); Rdata = double(flipud(Rij))';

% specify domain Omega = [omega(1),omega(2)]x[omega(3),omega(3)];
omega = [0,20,0,25];
m     = size(Tdata);

% setup image viewer
viewImage('reset','viewImage','viewImage2D','colormap','gray(256)');

% visualize
FAIRfigure(1);  colormap(gray(256));
subplot(2,2,1); imagesc(Tij);              title('original T data, uint8, ij');
subplot(2,2,2); viewImage(Tdata,omega,m);  title('FAIR T data on \Omega, xy');
subplot(2,2,3); imagesc(Rij);              title('original R data, uint8, ij');
subplot(2,2,4); viewImage(Rdata,omega,m);  title('FAIR R data on \Omega, xy');

% create multi-level representaion
ML = getMultilevel({Tdata,Rdata},omega,m,'fig',2);

%==============================================================================


function [km] = A2km(A)
% Convert length from angstroms to kilometers.
% Chad A. Greene 2012
km = A*1e-13;


function [surfDice,addedPathLen,addedPathLenNorm] = ...
    calc_surfDice_addPathLen(structNum1,structNum2,margin,planC)
% function [surfDice,addedPathLen,addedPathLenNorm] = ...
%     calc_surfDice_addPathLen(structNum1,structNum2,margin,planC)

mask1M = getSurfaceRing(structNum1,margin,planC);
mask2M = getSurfaceRing(structNum2,margin,planC);
avgMask = (sum(mask1M(:))+sum(mask2M(:)))/2;
intrsctM = mask1M & mask2M;
surfDice = sum(intrsctM(:))/avgMask;

% addedPathLenNorm = sum((mask1M(:) | mask2M(:)) & ~intrsctM(:))/(avgMask*2);
% addedPathLen = sum((mask1M(:) | mask2M(:)) & ~intrsctM(:));

scanNum = getStructureAssociatedScan(structNum1,planC);
% [~,~,numSlcs] = size(intrsctM);

indexS = planC{end};

[xScanV,yScanV,zScanV] = getScanXYZVals(planC{indexS.scan}(scanNum));
[~,~,zUnifScanV] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));
numSlcs = length(zScanV);

structLen = 0;
intersectStructLen = 0;
for slc = 1:numSlcs
    if length(planC{indexS.structures}(structNum1).contour(slc).segments) ~= 1
        continue
    end
    xV = planC{indexS.structures}(structNum1).contour(slc).segments(1).points(:,1);
    yV = planC{indexS.structures}(structNum1).contour(slc).segments(1).points(:,2);
    %[rowV, colV] = xytom(xV, yV, slc, planC,scanNum);
    siz = size(intrsctM(:,:,slc));
    %indV = sub2ind(siz, round(rowV), round(colV));

    structXYv = [xV';yV'];

    d = hypot(diff(xV), diff(yV)); % Distance Of Each Segment
    contourLen = sum(d);

    structLen = structLen + contourLen;

    unifSlc = findnearest(zUnifScanV,zScanV(slc));
    cc = bwconncomp(intrsctM(:,:,unifSlc));
    intersectLen = 0;
    for comp = 1:cc.NumObjects
        compIndV = cc.PixelIdxList{comp};
        [compiV,compjV] = ind2sub(siz,compIndV);
        compXYv = [xScanV(compjV);yScanV(compiV)];

        distM = sepsq(structXYv,compXYv);
        [~,indMinV] = min(distM,[],1);
        indMinV = unique(indMinV);
        segX = xV(indMinV);
        segY = yV(indMinV);

        d = hypot(diff(segX), diff(segY)); % Distance Of Segment between points
        segDist = sum(d);

        intersectLen = intersectLen + segDist;

    end

    intersectStructLen = intersectStructLen + intersectLen;

end

addedPathLen = structLen - intersectStructLen;
addedPathLenNorm = addedPathLen / structLen;


%PROBLEM SPECIFICATION
% Specification of parameters, grid and boundary conditions

global Re

tic
if geval == 1	% Horizontal Poiseuille flow to the right
  dt = 0.1;	% Time step
  tend = 10;	% End time
  Re = 100;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 1/2;  % Omega-scheme for time-stepping
  umax = 0.75;	% Estimates of max. velocity components required for stability
  vmax = 0;	% condition for Adams-Bashforth-crank-Nicolson scheme

  xseglen = [1,1];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1,1];	% Similar to xseglen in y-direction.
  nx = [4,4];	% Number of cells along x-segments.
  ny = [4,4];	% Number of cells along y-segments.

  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [1,1; 1,1];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [2,2; 3,3];

elseif geval == 2	% Vertical Poiseuille flow upward
  dt = 0.5;	% Time step
  tend = 10;	% End time
  Re = 100;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 1/2;  % Omega-scheme for time-stepping
  umax = 0;	% Estimates of max. velocity components required for stability
  vmax = 0.75;	% condition for Adams-Bashforth-crank-Nicolson scheme

  xseglen = [1,1];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1,1];	% Similar to xseglen in y-direction.
  nx = [8,16];	% Number of cells along x-segments.
  ny = [8,8];	% Number of cells along y-segments.

  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [2,2; 3,3];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [1,1; 1,1];

elseif geval == 3	% Backward facing step
  dt = 3;	% Time step
  tend = 140;	% End time
  Re = 100;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 1/2;  % Omega-scheme for time-stepping
  umax = 1.25;	% Estimates of max. velocity components required for stability
  vmax = 0.25;	% condition for Adams-Bashforth-Crank-Nicolson scheme
  		% (not used in program ns1)

  xseglen = [5,5];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1,1];	% Similar to xseglen in y-direction.
  nx = [20,10];	% Number of cells along x-segments.
  ny = [25,25];	% Number of cells along y-segments.

  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [1,1; 1,1];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [1,2; 3,3];

elseif geval == 4	% Driven cavity
  dt = 0.5;	% Time step
  tend = 25;	% End time
  Re = 200;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 0.55;  % Omega-scheme for time-stepping
  umax = 1;	% Estimates of max. velocity components required for stability
  vmax = 0;	% condition for Adams-Bashforth-crank-Nicolson scheme

  xseglen = [1,1];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1,1];	% Similar to xseglen in y-direction.
  nx = [15,15];	% Number of cells along x-segments.
  ny = [15,15];	% Number of cells along y-segments.
  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [1,1; 2,2];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [1,1; 1,1];

elseif geval == 5	% Uniform flow under angle alpha in [0,pi/2] without
			% segmentation
  alpha = pi/4;
  dt = 3;	% Time step
  tend = 60;	% End time
  Re = 100;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 1/2;  % Omega-scheme for time-stepping
  umax = abs(cos(alpha));% Estimates of max. velocity components required for stability
  vmax = abs(sin(alpha));% condition for Adams-Bashforth-crank-Nicolson scheme

  xseglen = [1];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1];	% Similar to xseglen in y-direction.
  nx = [2];	% Number of cells along x-segments.
  ny = [2];	% Number of cells along y-segments.

  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [2; 3];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [2; 3];

elseif geval == 6	% Uniform flow under angle alpha in [0,pi/2] with
			% segmentation
  alpha = pi/4;
  dt = 3;	% Time step
  tend = 60;	% End time
  Re = 100;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 1/2;  % Omega-scheme for time-stepping
  umax = abs(cos(alpha));% Estimates of max. velocity components required for stability
  vmax = abs(sin(alpha));% condition for Adams-Bashforth-crank-Nicolson scheme

  xseglen = [1,1];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1,1];	% Similar to xseglen in y-direction.
  nx = [2,6];	% Number of cells along x-segments.
  ny = [2,3];	% Number of cells along y-segments.

  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [2,2; 3,3];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [2,2; 3,3];

elseif geval == 7	% Horizontal Poiseuille flow to the left
  dt = 0.5;	% Time step
  tend = 60;	% End time
  Re = 100;	% Reynolds number based on unit length and unit velocity
  central = 1;	% Enter 1 for central scheme or something else for upwind scheme
  omega = 1/2;  % Omega-scheme for time-stepping
  umax = 0.75;	% Estimates of max. velocity components required for stability
  vmax = 0;	% condition for Adams-Bashforth-crank-Nicolson scheme

  xseglen = [1,1];	% Enter segment lengths of horizontal boundary in
		% order of increasing x. Number of segments is arbitrary.
		% These segments are used for grid generation and boundary
		% conditions.
  yseglen = [1,1];	% Similar to xseglen in y-direction.
  nx = [4,4];	% Number of cells along x-segments.
  ny = [4,4];	% Number of cells along y-segments.

  % Type of boundary condition: 1: no-slip
  %				2: inflow
  %				3: outflow
  % Give type of boundary condition along lower (first row) and upper
  %(second row) horizontal segments in order of increasing x:

  xbc = [1,1; 1,1];

  % Give type of boundary condition along left (first row) and right
  % (second row) vertical segments in order of increasing y:

  ybc = [3,3; 2,2];

else
  error('Wrong value in input for parameter geval')
end

if (size(xseglen)~=size(nx))|(size(yseglen)~=size(ny))
  error('Wrong correspondence between xseglen,nx or yseglen,ny in problem_specification')
end
if (size(xbc(1,:))~=size(nx))|(size(ybc(1,:))~=size(nx))
  error('Wrong correspondence between nx,xbc or ny,ybc in problem_specification')
end

tijd = toc; disp(['problem_specification time = ',num2str(tijd)])


clear,clc
% compute the background image
Imzero = zeros(240,320,3);
for i = 1:5
Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback);

% Kalman filter initialization
R=[[0.2845,0.0045]',[0.0045,0.0455]'];
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
P = 100*eye(4);
dt=1;
A=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]'];
g = 6; % pixels^2/time step
Bu = [0,0,0,g]';
kfinit=0;
x=zeros(100,4);

% loop over all images
for i = 1 : 60
  % load image
  Im = (imread(['DATA/',int2str(i), '.jpg']));
  imshow(Im)
  imshow(Im)
  Imwork = double(Im);

  %extract ball
  [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);
  if flag==0
    continue
  end

  hold on
    for c = -1*radius: radius/20 : 1*radius
      r = sqrt(radius^2-c^2);
      plot(cc(i)+c,cr(i)+r,'g.')
      plot(cc(i)+c,cr(i)-r,'g.')
    end
  % Kalman update
i
  if kfinit==0
    xp = [MC/2,MR/2,0,0]'
  else
    xp=A*x(i-1,:)' + Bu
  end
  kfinit=1;
  PP = A*P*A' + Q
  K = PP*H'*inv(H*PP*H'+R)
  x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
  x(i,:)
  [cc(i),cr(i)]
  P = (eye(4)-K*H)*PP

  hold on
    for c = -1*radius: radius/20 : 1*radius
      r = sqrt(radius^2-c^2);
      plot(x(i,1)+c,x(i,2)+r,'r.')
      plot(x(i,1)+c,x(i,2)-r,'r.')
    end
      pause(0.3)
end

% show positions
  figure
  plot(cc,'r*')
  hold on
  plot(cr,'g*')
%end

%estimate image noise (R) from stationary ball
  posn = [cc(55:60)',cr(55:60)'];
  mp = mean(posn);
  diffp = posn - ones(6,1)*mp;
  Rnew = (diffp'*diffp)/5;


 % Copyright 2001, Brown University, Providence, Rhode Island.
 %
 % All Rights Reserved
 %
 % Permission to use this software for noncommercial research and
 % educational purposes is hereby granted without fee.
 % Redistribution, sale, or incorporation of this software into a
 % commercial product is prohibited.
 %
 % BROWN UNIVERSITY DISCLAIMS ANY AND ALL WARRANTIES WITH REGARD TO
 % THIS SOFTWARE,INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
 % AND FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL BROWN
 % UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
 % DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
 % DATA OR PROFITS.

  function [a,b] = am282ab(x,y)

   dims = size(x);
   Np = dims(1)*dims(2);

   a = zeros(dims);
   b = zeros(dims);
   for n=1:Np  % hack
    if(y(n) ~= 1)
     a(n) = 2*(1+x(n))/(1-y(n))-1;
    else
     a(n) = -1;
    end
   end

   b = y;


function[newPop] = normGeomSelect(oldPop,options)
% NormGeomSelect is a ranking selection function based on the normalized
% geometric distribution.
%
% function[newPop] = normGeomSelect(oldPop,options)
% newPop  - the new population selected from the oldPop
% oldPop  - the current population
% options - options to normGeomSelect [gen probability_of_selecting_best]

% Binary and Real-Valued Simulation Evolution for Matlab
% Copyright (C) 1996 C.R. Houck, J.A. Joines, M.G. Kay
%
% C.R. Houck, J.Joines, and M.Kay. A genetic algorithm for function
% optimization: A Matlab implementation. ACM Transactions on Mathmatical
% Software, Submitted 1996.
%
% 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 1, 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. A copy of the GNU
% General Public License can be obtained from the
% Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

q=options(2); 				% Probability of selecting the best
e = size(oldPop,2); 			% Length of xZome, i.e. numvars+fit
n = size(oldPop,1); 			% Number of individuals in pop
newPop = zeros(n,e); 			% Allocate space for return pop
fit = zeros(n,1); 			% Allocates space for prob of select
x=zeros(n,2); 			        % Sorted list of rank and id
x(:,1) =[n:-1:1]'; 			% To know what element it was
[y x(:,2)] = sort(oldPop(:,e)); 	% Get the index after a sort
r = q/(1-(1-q)^n); 			% Normalize the distribution, q prime
fit(x(:,2))=r*(1-q).^(x(:,1)-1); 	% Generates Prob of selection
fit = cumsum(fit); 			% Calculate the cumulative prob. func
rNums=sort(rand(n,1)); 			% Generate n sorted random numbers
fitIn=1; newIn=1; 			% Initialize loop control
while newIn<=n 				% Get n new individuals
  if(rNums(newIn)<fit(fitIn))
    newPop(newIn,:) = oldPop(fitIn,:); 	% Select the fitIn individual
    newIn = newIn+1; 			% Looking for next new individual
  else
    fitIn = fitIn + 1; 			% Looking at next potential selection
  end
end


function y=fst(x,a,b)
if x>b
    y=1;
elseif x<a
    y=0;
else
    y=(x-a)/(b-a);
end


%MINC Minimum combining classifier
%
% 	W = MINC(V)
% 	W = V*MINC
%
% INPUT
%   V     Set of classifiers
%
% OUTPUT
%   W    Minimum combining classifier on V
%
% DESCRIPTION
% If V = [V1,V2,V3, ... ] is a set of classifiers trained on the
% same classes and W is the minimum combiner: it selects the class
% with the minimum of the outputs of the input classifiers. This
% might also be used as A*[V1,V2,V3]*MINC in which A is a dataset to
% be classified. Consequently, if S is a dissimilarity matrix with
% class feature labels (e.g. S = A*PROXM(A,'d')) then S*MINC*LABELD
% is the nearest neighbor classifier.
%
% If it is desired to operate on posterior probabilities then the
% input classifiers should be extended like V = V*CLASSC;
%
% The base classifiers may be combined in a stacked way (operating
% in the same feature space by V = [V1,V2,V3, ... ] or in a parallel
% way (operating in different feature spaces) by V = [V1;V2;V3; ... ]
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, VOTEC, MAXC, MEANC, MEDIANC, PRODC,
% AVERAGEC, STACKED, PARALLEL
%
% EXAMPLES
% See PREX_COMBINING

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

% $Id: minc.m,v 1.2 2006/03/08 22:06:58 duin Exp $

function w = minc(p1)

	type = 'min'; % define the operation processed by FIXEDCC.

	% define the name of the combiner.
	% this is the general procedure for all possible calls of fixed combiners
	% handled by FIXEDCC
	name = 'Minimum combiner';

	if nargin == 0
		w = prmapping('fixedcc','combiner',{[],type,name});
	else
		w = fixedcc(p1,[],type,name);
	end

	if isa(w,'prmapping')
		w = setname(w,name);
	end

return


function s = resampdet(p,m,n);
%RESAMPDET Deterministic resampling
%
%   Description
%   S = RESAMPDET(P) returns a new set of indices according to the
%   probabilities P. P is array of probabilities, which are not
%   necessarily normalized, though they must be non-negative, and
%   not all zero. The size of S is the size of P.
%
%   S = RESAMPDET(P,M,N) returns M by N matrix.
%
%   S = RESAMPDET(P,M) returns M by M matrix.
%
%   Default is to use no-sort resampling. For sorted resampling use
%    [PS,PI]=SORT(P);
%    S=PI(RESAMPDET(PS));
%   Sorted re-sampling is slower but has smaller variance. Note
%   that deterministic resampling is not unbiased. Stratified
%   resampling (RESAMPSTR) is unbiased, almost as fast as
%   deterministic resampling, and has only slightly larger
%   variance.
%
%   In deterministic resampling indices are sampled using
%   deterministic numbers u_j~(j-a)/n, for fixed a in [0,1) and
%   n is length of P. Compare this to simple random resampling
%   where u_j~U[0,n]. See, Kitagawa, G., Monte Carlo Filter and
%   Smoother for Non-Gaussian Nonlinear State Space Models,
%   Journal of Computational and Graphical Statistics, 5(1):1-25,
%   1996.
%
%   See also RESAMPSIM, RESAMPRES, RESAMPSTR
%
% Copyright (c) 2003-2004 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

if nargin<2
    [m,n]=size(p);
elseif nargin==2
    n=m;
end
mn=m.*n;
pn=p./sum(p(:)).*mn;
fpn=floor(pn);
s=zeros(m,n);
k=0;
c=0.5;
for i=1:numel(p)
  if pn(i)>=1
    a=fpn(i);
    pn(i)=pn(i)-a;
    s(k+[1:a])=i;
    k=k+a;
  end
  c=c+pn(i);
  if c>=1
    k=k+1;
    s(k)=i;
    c=c-1;
  end
end


% NANMIN Minimum value, ignoring NaNs.
%    M = NANMIN(A) returns the minimum of A with NaNs treated as missing.
%    For vectors, M is the smallest non-NaN element in A.  For matrices, M
%    is a row vector containing the minimum non-NaN element from each
%    column.  For N-D arrays, NANMIN operates along the first non-singleton
%    dimension.
%
%    [M,NDX] = NANMIN(A) returns the indices of the minimum values in A.  If
%    the values along the first non-singleton dimension contain more than
%    one minimal element, the index of the first one is returned.
%
%    M = NANMIN(A,B) returns an array the same size as A and B with the
%    smallest elements taken from A or B.  Either one can be a scalar.
%
%    [M,NDX] = NANMIN(A,[],DIM) operates along the dimension DIM.
%
%    See also MIN, NANMAX, NANMEAN, NANMEDIAN, NANVAR, NANSTD.
%
%    Reference page in Doc Center
%       doc stats/nanmin
%
%    Other functions named nanmin
%
%       distributed/nanmin    fints/nanmin
%


function f = prolong(f, nOut)
%PROLONG   Manually adjust the number of points used in a TRIGTECH.
%   G = PROLONG(F, N) returns a TRIGTECH G where LENGTH(G) = N and G represents
%   the same function as F but using more or less coefficients than F.
%
%   If N < LENGTH(F) the representation is compressed by chopping
%   coefficients, which may result in a loss of accuracy.
%
%   If N > LENGTH(F) the coefficients are padded with zeros.
%
% See also ALIAS.

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

% Get the number of coefficients.
n = length(f);

% Return if nOut == n
if ( nOut == n )
    return
end

% Get coefficients
coeffs = f.coeffs;

% If n is even extend to coeffs to n+1
if ( mod(n,2) == 0 )
    coeffs = [.5*coeffs(1,:);coeffs(2:end,:);.5*coeffs(1,:)];
    n = n + 1;
end

% Return if nOut == n
if ( nOut == n )
    f.coeffs = coeffs;
    f.values = f.coeffs2vals(f.coeffs);
    f.values(:,f.isReal) = real(f.values(:,f.isReal));
    return
end

% Pad with zeros when nOut > n:
if ( nOut > n )
    kup = ceil((nOut-n)/2);
    kdown = floor((nOut-n)/2);
    coeffs = [zeros(kup, size(coeffs, 2)); coeffs; zeros(kdown, size(coeffs,2))];
    f.coeffs = coeffs;
    f.values = f.coeffs2vals(f.coeffs);
    f.values(:,f.isReal) = real(f.values(:,f.isReal));
    return
end

% Chop coefficients when nOut < n:
if ( nOut < n )
    kup = floor((n-nOut)/2);
    kdown = ceil((n-nOut)/2);
    coeffs(end-kdown+1:end,:) = [];
    coeffs(1:kup,:) = [];
    if ( kup < kdown )
        coeffs(1,:) = 2*coeffs(1,:); % scale coefficients in even case
    end
    f.coeffs = coeffs;
    f.values = f.coeffs2vals(f.coeffs);
    f.values(:,f.isReal) = real(f.values(:,f.isReal));
    return
end

end


function e = rmModelSearchFit_twoGaussiansDoGSigmasOnly(p,Y,XvYv,stim,t)
% rmModelSearchFit_twoGaussians - actual fit function of rmSearchFit
%
% error = rmModelSearchFit_twoGaussians(p,Y,trends,Gx,Gy,stim,rawrss);
%
% Basic barebones fit of a single time-series. Error is returned in
% percentage: 100% is RSS of unfitted time-series. This way we can quantify
% the improvement of the fit independend of the variation in the raw
% time-series.
%
% 2006/06 SOD: wrote it.
% 2006/12 SOD: modifications for fmincon, this is litterally called >>10000
% times so we cut every corner possible.

% make RF (taken from rfGaussian2d)
p = -2.*(p.^2);
RF = zeros(numel(XvYv),2);
RF(:,1) = exp( (XvYv) ./ p(1) );
RF(:,2) = exp( (XvYv) ./ p(2) );

% make prediction (taken from rfMakePrediction)
X = [stim * RF t];

% fit
%b = pinv(X)*Y;
[U,S,V] = svd(X,0);
s = diag(S);
tol = numel(X) * eps(max(s));
r = sum(s > tol);
if (r == 0)
    pinvX = zeros(size(X'));
else
    s = diag(ones(r,1)./s(1:r));
    pinvX = V(:,1:r)*s*U(:,1:r)';
end
b = pinvX*Y;

% force positive fit
b(1) = abs(b(1));

%force negative b2 fit
b(2) = -(abs(b(2)));

% The center of the pRF should be positive. Thus, b(1)+b(2)>=0, or
% b(2) should be larger than -b(1) (implemented).
b(2) = max(b(2),-b(1));

% force second Gaussian to be negative
b(2) = -abs(b(2));

% compute residual sum of squares (e)
e = norm(Y - X*b);

return;


function val= froNormMatn(x,y)

val =sqrt(sum( (x(:)-y(:)).^2));


function L=computeLap(X)
% Compute graph Laplacian

optL=[];
optL.NeighborMode='KNN';
optL.k=5;
optL.WeightMode='Cosine';   % 'Cosine' or 'HeatKernel'
optL.t=1;
L=constructW(X,optL);


function E = compose(disc, y)
%FEVAL   Evaluation functional for CHEBCOLLOC.
%   FEVAL(DISC, LOC, DIRN) returns a functional that evaluates the Chebyshev
%   polynomial represented by a COLLOC discretization at the given point LOC as
%   approached from the direction DIRN (either +1 or -1).
%
% See also CHEBCOLLOC.FEVAL

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

n = disc.dimension;

% Find the collocation points and create an empty functional.
[x, ~, v] = functionPoints(disc);
offset = cumsum([0 ; n(:)]);
N = offset(end);
E = zeros(1, N);

% Evaluate the given input at x:
y = y(x);
% Find which point is in which subinterval
intnum = chebfun.whichInterval(disc.domain, y);
intnum = max(intnum, 1); intnum = min(intnum,numel(n));
% Loop over each interval:
for i = unique(intnum)'
    j = i == intnum;
    active = offset(i) + (1:n(i));
    E(j,active) = barymat(y(j), x(active), v(active));
end

end


function DCV=plsldadcv(X,y,A,K,method,OPT,order)
%+++ K-fold double cross validation Cross-validation for PLS-LDA
%+++ Input:  X: m x n  (Sample matrix)
%            y: m x 1  (measured property)
%            A: The max PC for cross-validation
%            K: fold. when K = m, it is leave-one-out CV
%       method: pretreatment method. Contains: autoscaling, center etc.
%          OPT: =1 Print process.
%               =0 No print.
%               pareto,minmax,center or none.
%+++ Order: =1  sorted,default. For CV partition.
%           =0  random.
%+++ Output: Structural data: CV
%+++ Hongdong Li, Oct. 16, 2008.
%+++ Revised in Jan.12, 2009.

if nargin<7;order=1;end;
if nargin<6;OPT=1;end;
if nargin<5;method='autoscaling';end;
if nargin<4;K=10;end;
if nargin<3;A=2;end;

check=0; %+++ status variable:  1: Inf

if order==1
  [y,indexyy]=sort(y);
  X=X(indexyy,:);
else
  indexyy=randperm(length(y));
  X=X(indexyy,:);
  y=y(indexyy);
end

A=min([size(X,1)-ceil(length(y)/K) size(X,2) A]);
yytest=[];YR=[];
[Mx,Nx]=size(X);
groups = 1+rem(0:Mx-1,K);
yytest=[];yp=[];nLV=zeros(K,1);
for group=1:K
    testk = find(groups==group);  calk = find(groups~=group);
    Xcal=X(calk,:);ycal=y(calk);
    Xtest=X(testk,:);ytest=y(testk);

    CV=plsldacv(Xcal,ycal,A,K,method,0,order);
    if CV.check==1;check==1;break;end;
    LDA=plslda(Xcal,ycal,CV.optPC,method);
    ypred=plsldaval(LDA,Xtest,ytest);
    yytest=[yytest;ytest];
    yp=[yp;ypred;];
    nLV(group)=CV.optPC;
    if OPT==1;fprintf('The %dth outer loop finished.\n',group);end;
end

%+++ Find the most frequently chosen nLV.
uniLV=unique(nLV);
for j=1:length(uniLV); freq(j)=length(find(nLV==uniLV(j)));end
[maxf,maxindex]=max(freq);
optPC=uniLV(maxindex(1));

%+++ output
if check==0
  F=roccurve(yytest,yp,0);
  error=sum(sign(yp)~=yytest)/Mx;
  DCV.method=method;
  DCV.check=check;
  DCV.error=error;
  DCV.Sensitivity=F.sensitivity;
  DCV.Specificity=F.specificity;
  DCV.nLV=nLV;
  DCV.optPC=optPC;
elseif check==1
  DCV.method=method;
  DCV.check=check;
end