function [ a_lu, pivot, info ] = r8gb_trf ( m, n, ml, mu, a )

%*****************************************************************************80
%
%% R8GB_TRF performs a LAPACK-style PLU factorization of a R8GB matrix.
%
%  Discussion:
%
%    The R8GB storage format is for an M by N banded matrix, with lower
%    bandwidth ML and upper bandwidth MU.  Storage includes room for ML
%    extra superdiagonals, which may be required to store nonzero entries
%    generated during Gaussian elimination.
%
%    The original M by N matrix is "collapsed" downward, so that diagonals
%    become rows of the storage array, while columns are preserved.  The
%    collapsed array is logically 2*ML+MU+1 by N.
%
%    This is a simplified, standalone version of the LAPACK
%    routine R8GBTRF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Anderson, Bai, Bischof, Demmel, Dongarra, Du Croz, Greenbaum,
%    Hammarling, McKenney, Ostrouchov, Sorensen,
%    LAPACK User's Guide,
%    Second Edition,
%    SIAM, 1995.
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix A.  0 <= M.
%
%    Input, integer N, the number of columns of the matrix A.  0 <= N.
%
%    Input, integer ML, the number of subdiagonals within the band of A.
%    0 <= ML.
%
%    Input, integer MU, the number of superdiagonals within the band of A.
%    0 <= MU.
%
%    Input, real A(2*ML+MU+1,N), the matrix A in band storage.
%
%    Output, real A_LU(2*ML+MU+1,N), information about the PLU factorization.
%
%    Output, integer PIVOT(min(M,N)), the pivot indices;
%    for 1 <= i <= min(M,N), row i of the matrix was interchanged with
%    row IPIV(i).
%
%    Output, integer INFO, error flag.
%    = 0: successful exit;
%    < 0: an input argument was illegal;
%    > 0: if INFO = +i, U(i,i) is exactly zero. The factorization
%         has been completed, but the factor U is exactly
%         singular, and division by zero will occur if it is used
%         to solve a system of equations.
%
  info = 0;
  a_lu(1:2*ml+mu+1,1:n) = a(1:2*ml+mu+1,1:n);
%
%  KV is the number of superdiagonals in the factor U, allowing for fill-in.
%
  kv = mu + ml;
%
%  Set fill-in elements in columns MU+2 to KV to zero.
%
  for j = mu + 2 : min ( kv, n )
    for i = kv - j + 2 : ml
      a_lu(i,j) = 0.0E+00;
    end
  end
%
%  JU is the index of the last column affected by the current stage
%  of the factorization.
%
  ju = 1;

  for j = 1 : min ( m, n )
%
%  Set the fill-in elements in column J+KV to zero.
%
    if ( j + kv <= n )
      a_lu(1:ml,j+kv) = 0.0;
    end
%
%  Find the pivot and test for singularity.
%  KM is the number of subdiagonal elements in the current column.
%
    km = min ( ml, m-j );

    piv = abs ( a_lu(kv+1,j) );
    jp = kv + 1;

    for i = kv + 2 : kv + km + 1
      if ( piv < abs ( a_lu(i,j) ) )
        piv = abs ( a_lu(i,j) );
        jp = i;
      end
    end

    jp = jp - kv;

    pivot(j) = jp + j - 1;

    if ( a_lu(kv+jp,j) ~= 0.0E+00 )

      ju = max ( ju, min ( j+mu+jp-1, n ) );
%
%  Apply interchange to columns J to JU.
%
      if ( jp ~= 1 )
        for i = 0 : ju - j
          t = a_lu(kv+jp-i,j+i);
          a_lu(kv+jp-i,j+i) = a_lu(kv+1-i,j+i);
          a_lu(kv+1-i,j+i) = t;
        end
      end
%
%  Compute the multipliers.
%
      if ( 0 < km )

        a_lu(kv+2:kv+km+1,j) = a_lu(kv+2:kv+km+1,j) / a_lu(kv+1,j);
%
%  Update the trailing submatrix within the band.
%
        if ( j < ju )

          for k = 1 : ju-j

            if ( a_lu(kv+1-k,j+k) ~= 0.0E+00 )

              for i = 1 : km
                a_lu(kv+i+1-k,j+k) = a_lu(kv+i+1-k,j+k) ...
                  - a_lu(kv+i+1,j) * a_lu(kv+1-k,j+k);
              end
            end
          end
        end
      end

    else
%
%  If pivot is zero, set INFO to the index of the pivot
%  unless a zero pivot has already been found.
%
      if ( info == 0 )
        info = j
      end

    end

  end

  return
end


function [gx,dG_dX,dG_dPhi,d2G_dXdPhi] = g_ERP(x,Phi,u,inG)
% neural mass observation function (DCM for ERPs)

n = size(x,1);          % should be 9
nPhi = length(Phi);     % should be 1

g = exp(Phi(1)).*[0  0   0   0   0   0   0   0   1
                  0  0   0   0   0   0   0   0   0.3
                  0  0   0   0   0   0   0   0   -0.2
                  0  0   0   0   0   0   0   0   -0.7];

% state observation
gx = g*x;

%------ gradients evaluations ------%

% wrt the hidden states
dG_dX = g';

% wrt the observation parameters
dG_dPhi = (g*x)';

% mixed partial derivatives
d2G_dXdPhi(:,:,1) = zeros(n,nPhi);
d2G_dXdPhi(9,1,1) = exp(Phi(1));
d2G_dXdPhi(:,:,2) = 0.3*d2G_dXdPhi(:,:,1);
d2G_dXdPhi(:,:,3) = -0.2*d2G_dXdPhi(:,:,1);
d2G_dXdPhi(:,:,4) = -0.7*d2G_dXdPhi(:,:,1);


function [fAphi] = calc_FaultStyle(rStress)
    % Determine the faulting style based on Quantifying Anderson's fault type
% [fAphi] = calc_FaultStyle(rStress)
% ------------------------------------------
% Determining the faulting style based on Quantifying Anderson's fault type
% by B. Simpson, 1997, JGR
%
% Incoming:
% rStress : Structure from script gui_NodeCalcStressInv giving stress
%           tensor inversion by Michael
% Output:
% fAphi   : 0 <= fAphi < 1 Normal faulting
%           1 <= fAphi < 2 Strike slip
%           2 <= fAphi < 3 Thrust faulting
%
% jowoe@gps.caltech.edu
% 19.05.2006

% Stress tensor from Michael inversion
mS=[rStress.fS11 rStress.fS12 rStress.fS13; rStress.fS12 rStress.fS22 rStress.fS23; rStress.fS13 rStress.fS23 rStress.fS33];

% Eigenvector; sorted from max. compressive to minimum (S1, S2, S3)
% Maximum compressive is the most negative value
vEig = eig(mS);

% Check for "most" vertical axis
vPlunge = [rStress.fS1Plunge; rStress.fS2Plunge; rStress.fS3Plunge];

vSel = (vPlunge == max(vPlunge));
fSigvert = vEig(vSel);

% This is because maximum compressive is negative
fSighmax = min(vEig(~vSel,:));
fSighmin = max(vEig(~vSel,:));

if fSighmax > fSigvert
    n = 0;
elseif fSigvert > fSighmin
    n = 2;
else
    n = 1;
end
% Formula 2, Simpson, JGR, 1997
fAphi = (n+0.5)+(-1)^n*(rStress.fPhi-0.5);


function CPerm=permuteMatrix(C,order)
%%PERMUTEMATRIX Given an n1Xn2X...nS matrix, rearrange the dimensions of C
%      to be in the order specified by order. All elements of order must be
%      unique, real, positive, integer values from 1 to S. This functions
%      in the same manner as Matlab's permute function. The permutation
%      performed by this function is not done in-place. This type of
%      permutation is a type of tensor matrix transpose.
%
%INPUTS: C An n1Xn2X...XnS matrix.
%    order The length S order vector. This should hold integer values from
%          1 to S with no repeats.
%
%OUTPUTS: CPerm C with its dimensions permuted according to order.
%
%The algorithm is essentially brute force. It goes through each element in
%C and find the tuple of the associated element in CPerm to which the
%assignment must be performed.
%
%EXAMPLE:
% C=100*rand(3,4,5);
% order=[3,2,1];
% CPerm=permuteMatrix(C,order);
%One will ntoe that size(CPerm) is note [5,4,3].
%
%March 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

S=ndims(C);
numOrderEls=length(order);

if(isempty(order)&&~isempty(C))
   error('order cannot be empty with a non-empty C.')
end

if(numOrderEls<S)
    error('The ordering vector must be >=ndims(C).')
end

if(any(order>numOrderEls)||any(order<1))
    error('')
end

if(isempty(C))
    CPerm=[];
    return;
end

nVals=size(C);

if(numOrderEls>S)
   nVals=[nVals,ones(1,numOrderEls-S)];
   S=numOrderEls;
end

totalEls=numel(C);

cumProdNew=zeros(S,1);
nValsNew=zeros(S,1);
invPerm=zeros(S,1);
newIdx=zeros(S,1);%Initialization
idx=zeros(S,1);%Initialization

iNew=order(1);
nValsNew(1)=nVals(iNew);

invPerm(iNew)=1;
cumProdNew(1)=1;

for i=2:S
    iNew=order(i);

    nValsNew(i)=nVals(iNew);
    invPerm(iNew)=i;

    cumProdNew(i)=cumProdNew(i-1)*nValsNew(i-1);
end

CPerm=zeros(nValsNew(:)');

newLinIdx=0;
linIdx=0;
while(1)
    %Assign the current tuple.
    CPerm(newLinIdx+1)=C(linIdx+1);

    %Move on to the next tuple.
    linIdx=linIdx+1;

    if(linIdx>=totalEls)
        break;
    end

    %The code below is similar to that in getNextTuple, because we are
    %updating the tuples for newIdx.
    curLevel=1;
    while(1)
        %Try incrementing the order at this level.
        idx(curLevel)=idx(curLevel)+1;
        if(idx(curLevel)<nVals(curLevel))
            iNew=invPerm(curLevel);
            newIdx(iNew)=newIdx(iNew)+1;
            newLinIdx=newLinIdx+cumProdNew(iNew);

            idx(1:(curLevel-1))=0;
            break;
        else
            %If the value is invalid, then just keep ascending and
            %adjust newLinIdx.
            iNew=invPerm(curLevel);

            newLinIdx=newLinIdx-newIdx(iNew)*cumProdNew(iNew);
            newIdx(iNew)=0;

            curLevel=curLevel+1;
            continue;
        end
    end
end
end

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


function [ a, seed ] = r8vec_uniform ( n, b, c, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM returns a scaled pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real B, C, the range of the pseudorandom values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    a(i) = b + ( c - b ) * seed * 4.656612875E-10;

  end

  return
end


% Example for the algorithms described in
%
%       D. Kressner, M. Steinlechner, A. Uschmajew:
%		Low-rank tensor methods with subspace correction for symmetric eigenvalue problems
%		SIAM J. Sci. Comput., 36(5):A2346-A2368, 2014.
%
% Code to produce Figure 4.4: Three Eigenvalues for Henon-Heiles with
% with n=28, d=10

%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt
% =========================================================================
clear all
close all

n = 28;
d = 10;
A = TTeMPS_op_NN_hermite(n, d);
p = 3;
r = 1;

% Run block eigenvalue procedure:
% =========================================================================

if ~exist('hh_3_blk_hermite.mat','file')
    opts = struct( 'maxiter', 3, ...
                   'maxrank', 40, ...
                   'tol', 1e-8, ...
                   'tolOP', 1e-3, ...
                   'tolLOBPCG', 1e-6, ...
                   'maxiterLOBPCG', 500, ...
                   'verbose', true , ...
                   'precInner', true);

    rng(11)
    rr = [1, 1 * ones(1, d-1), 1];
    [X_blk_hermite, C_blk_hermite, evalue_blk_hermite, residuums_blk_hermite, micro_res_blk_hermite, objective_blk_hermite, t_blk_hermite] = block_eigenvalue( A, p, rr, opts);
    save('hh_3_blk_hermite', 'X_blk_hermite', 'C_blk_hermite', 'evalue_blk_hermite', 'residuums_blk_hermite', 'micro_res_blk_hermite', 'objective_blk_hermite','t_blk_hermite');
else
    load('hh_3_blk_hermite.mat')
end

% Run EVAMEn:
% =========================================================================

if ~exist('hh_3_evamen_hermite.mat','file')
    opts = struct( 'maxiter', 3, ...
                   'maxrank', 40, ...
                   'maxrankRes', 2, ...
                   'tol', 1e-8, ...
                   'tolOP', 1e-3, ...
                   'tolLOBPCG', 1e-6, ...
                   'maxiterLOBPCG', 500, ...
                   'verbose', true , ...
                   'precInner', true);
    rng(11)
    rr = [1, 1 * ones(1, d-1), 1];
    [X_evamen_hermite, C_evamen_hermite, evalue_evamen_hermite, residuums_evamen_hermite, micro_res_evamen_hermite, objective_evamen_hermite, t_evamen_hermite] = amen_eigenvalue( A, 1, p, rr, opts);
    save('hh_3_evamen_hermite', 'X_evamen_hermite', 'C_evamen_hermite', 'evalue_evamen_hermite', 'residuums_evamen_hermite', 'micro_res_evamen_hermite', 'objective_evamen_hermite','t_evamen_hermite');
else
    load('hh_3_evamen_hermite.mat')
end

% Prepare data for plotting:
% =========================================================================

evalue_end = repmat(evalue_blk_hermite(:,end), [1,size(evalue_blk_hermite,2)-1]);
ev_blk_hermite = abs(evalue_blk_hermite(:,1:end-1) - evalue_end);
ev_evamen_hermite = abs(evalue_evamen_hermite(:,1:end-1) - evalue_end);

% Plot vs. Iterations
% =========================================================================
f = figure
set(0,'defaultlinelinewidth',2)
subplot(1,2,1)

semilogy( sqrt(sum(micro_res_blk_hermite.^2, 1)), '-b' )
hold on
semilogy( sqrt(sum(micro_res_evamen_hermite.^2, 1)), '-k' )

semilogy( sum(ev_blk_hermite,1), '--b' )
semilogy( sum(ev_evamen_hermite,1), '--k' )

res_blk_hermite         = sqrt(sum(micro_res_blk_hermite.^2, 1))
res_evamen_hermite  = sqrt(sum(micro_res_evamen_hermite.^2, 1))

semilogy((d-1):(d-1):length(micro_res_blk_hermite),res_blk_hermite(:,(d-1):(d-1):end),'ob')
semilogy((d-1):(d-1):length(micro_res_evamen_hermite),res_evamen_hermite(:,(d-1):(d-1):end),'ok')

semilogy((d-1):(d-1):length(ev_blk_hermite),sum(ev_blk_hermite(:,(d-1):(d-1):end),1),'ob')
semilogy((d-1):(d-1):length(ev_evamen_hermite),sum(ev_evamen_hermite(:,(d-1):(d-1):end),1),'ok')

set(gca,'fontsize',20)
xlabel('Microiterations')
ylabel('Residual and eigenvalue error')

% Plot vs. Time
% =========================================================================

subplot(1,2,2)
semilogy( t_blk_hermite, sqrt(sum(micro_res_blk_hermite.^2, 1)), '-b' )
hold on
semilogy( t_evamen_hermite, sqrt(sum(micro_res_evamen_hermite.^2, 1)), '-k' )

semilogy( t_blk_hermite, sum(ev_blk_hermite,1), '--b' )
semilogy( t_evamen_hermite, sum(ev_evamen_hermite,1), '--k' )

semilogy(t_blk_hermite((d-1):(d-1):end),res_blk_hermite(:,(d-1):(d-1):end),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end),res_evamen_hermite(:,(d-1):(d-1):end),'ok')

semilogy(t_blk_hermite((d-1):(d-1):end),sum(ev_blk_hermite(:,(d-1):(d-1):end),1),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end),sum(ev_evamen_hermite(:,(d-1):(d-1):end),1),'ok')

semilogy(t_blk_hermite((d-1):(d-1):end),        res_blk_hermite(:,(d-1):(d-1):end),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end), res_evamen_hermite(:,(d-1):(d-1):end),'ok')

semilogy(t_blk_hermite((d-1):(d-1):end),        sum(ev_blk_hermite(:,(d-1):(d-1):end),1),'ob')
semilogy(t_evamen_hermite((d-1):(d-1):end), sum(ev_evamen_hermite(:,(d-1):(d-1):end),1),'ok')

h_leg = legend('Res. err., Block-ALS',...
       'Res. err. EVAMEn, prec.',...
       'EV. err., Block-ALS',...
       'EV. err. EVAMEn, prec.')
set(gca,'fontsize',20)
set(h_leg, 'fontsize',16)
xlabel('Time [s]')
ylabel('Residual and eigenvalue error')

set(f, 'Position', [0 0 1200 700])


function [ n_data, nu, x, fx ] = bessel_in_values ( n_data )

%*****************************************************************************80
%
%% BESSEL_IN_VALUES returns some values of the In Bessel function.
%
%  Discussion:
%
%    The modified Bessel functions In(Z) and Kn(Z) are solutions of
%    the differential equation
%
%      Z^2 W'' + Z * W' - ( Z^2 + N^2 ) * W = 0.
%
%    In Mathematica, the function can be evaluated by:
%
%      BesselI[n,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer NU, the order of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 28;

  fx_vec = [ ...
     0.5016687513894678E-02, ...
     0.1357476697670383E+00, ...
     0.6889484476987382E+00, ...
     0.1276466147819164E+01, ...
     0.2245212440929951E+01, ...
     0.1750561496662424E+02, ...
     0.2281518967726004E+04, ...
     0.3931278522104076E+08, ...
     0.2216842492433190E-01, ...
     0.2127399592398527E+00, ...
     0.1033115016915114E+02, ...
     0.1758380716610853E+04, ...
     0.2677764138883941E+21, ...
     0.2714631559569719E-03, ...
     0.9825679323131702E-02, ...
     0.2157974547322546E+01, ...
     0.7771882864032600E+03, ...
     0.2278548307911282E+21, ...
     0.2752948039836874E-09, ...
     0.3016963879350684E-06, ...
     0.4580044419176051E-02, ...
     0.2189170616372337E+02, ...
     0.1071597159477637E+21, ...
     0.3966835985819020E-24, ...
     0.4310560576109548E-18, ...
     0.5024239357971806E-10, ...
     0.1250799735644948E-03, ...
     0.5442008402752998E+19 ];

  nu_vec = [ ...
     2,  2,  2,  2, ...
     2,  2,  2,  2, ...
     3,  3,  3,  3, ...
     3,  5,  5,  5, ...
     5,  5, 10, 10, ...
    10, 10, 10, 20, ...
    20, 20, 20, 20 ];

  x_vec = [ ...
      0.2E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      2.5E+00, ...
      3.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     20.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    nu = 0;
    x = 0.0;
    fx = 0.0;
  else
    nu = nu_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function value = r4_besi1e ( x )

%*****************************************************************************80
%
%% R4_BESI1E: exponentially scaled Bessel function I of order 1 of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the exponentially scaled Bessel function I
%    of order 1 of X.
%
  persistent ai1cs
  persistent ai12cs
  persistent bi1cs
  persistent ntai1
  persistent ntai12
  persistent nti1
  persistent xmin
  persistent xsml

  if ( isempty ( nti1 ) )

    ai1cs = [ ...
     -0.02846744181881479E+00, ...
     -0.01922953231443221E+00, ...
     -0.00061151858579437E+00, ...
     -0.00002069971253350E+00, ...
      0.00000858561914581E+00, ...
      0.00000104949824671E+00, ...
     -0.00000029183389184E+00, ...
     -0.00000001559378146E+00, ...
      0.00000001318012367E+00, ...
     -0.00000000144842341E+00, ...
     -0.00000000029085122E+00, ...
      0.00000000012663889E+00, ...
     -0.00000000001664947E+00, ...
     -0.00000000000166665E+00, ...
      0.00000000000124260E+00, ...
     -0.00000000000027315E+00, ...
      0.00000000000002023E+00, ...
      0.00000000000000730E+00, ...
     -0.00000000000000333E+00, ...
      0.00000000000000071E+00, ...
     -0.00000000000000006E+00 ]';
    ai12cs = [ ...
      0.02857623501828014E+00, ...
     -0.00976109749136147E+00, ...
     -0.00011058893876263E+00, ...
     -0.00000388256480887E+00, ...
     -0.00000025122362377E+00, ...
     -0.00000002631468847E+00, ...
     -0.00000000383538039E+00, ...
     -0.00000000055897433E+00, ...
     -0.00000000001897495E+00, ...
      0.00000000003252602E+00, ...
      0.00000000001412580E+00, ...
      0.00000000000203564E+00, ...
     -0.00000000000071985E+00, ...
     -0.00000000000040836E+00, ...
     -0.00000000000002101E+00, ...
      0.00000000000004273E+00, ...
      0.00000000000001041E+00, ...
     -0.00000000000000382E+00, ...
     -0.00000000000000186E+00, ...
      0.00000000000000033E+00, ...
      0.00000000000000028E+00, ...
     -0.00000000000000003E+00 ]';
    bi1cs = [ ...
     -0.001971713261099859E+00, ...
      0.40734887667546481E+00, ...
      0.034838994299959456E+00, ...
      0.001545394556300123E+00, ...
      0.000041888521098377E+00, ...
      0.000000764902676483E+00, ...
      0.000000010042493924E+00, ...
      0.000000000099322077E+00, ...
      0.000000000000766380E+00, ...
      0.000000000000004741E+00, ...
      0.000000000000000024E+00 ]';

    nti1 = r4_inits ( bi1cs, 11, 0.1 * r4_mach ( 3 ) );
    ntai1 = r4_inits ( ai1cs, 21, 0.1 * r4_mach ( 3 ) );
    ntai12 = r4_inits ( ai12cs, 22, 0.1 * r4_mach ( 3 ) );
    xmin = 2.0 * r4_mach ( 1 );
    xsml = sqrt ( 8.0 * r4_mach ( 3 ) );

  end

  y = abs ( x );

  if ( x == 0.0 )
    value = 0.0;
  elseif ( y <= xmin )
    value = 0.0;
  elseif ( y <= xsml )
    value = 0.5 * x;
    value = exp ( - y ) * value;
  elseif ( y <= 3.0 )
    value = x * ( 0.875 ...
      + r4_csevl ( y * y / 4.5 - 1.0, bi1cs, nti1 ) );
    value = exp ( - y ) * value;
  elseif ( y <= 8.0 )
    value = ( 0.375 + r4_csevl ( ( 48.0 / y - 11.0 ) / 5.0, ...
      ai1cs, ntai1) ) / sqrt ( y );
    if ( x < 0.0 )
      value = - value;
    end
  else
    value = ( 0.375 + r4_csevl ( 16.0 / y - 1.0, ai12cs, ntai12 ) ) ...
      / sqrt ( y );
    if ( x < 0.0 )
      value = - value;
    end
  end

  return
end


% NaN    Not-a-Number.
%    NaN is the IEEE arithmetic representation for Not-a-Number.
%    A NaN is obtained as a result of mathematically undefined
%    operations like 0.0/0.0  and inf-inf.
%
%    NaN('double') is the same as NaN with no inputs.
%
%    NaN('single') is the single precision representation of NaN.
%
%    NaN(N) is an N-by-N matrix of NaNs.
%
%    NaN(M,N) or NaN([M,N]) is an M-by-N matrix of NaNs.
%
%    NaN(M,N,P,...) or NaN([M,N,P,...]) is an M-by-N-by-P-by-... array of NaNs.
%
%    NaN(..., CLASSNAME) is an array of NaNs of class specified by the
%    string CLASSNAME. CLASSNAME can be either 'single' or 'double'.
%
%    NaN(..., 'like', Y) is an array of NaNs with the same data type, sparsity,
%    and complexity (real or complex) as the single or double precision numeric
%    variable Y.
%
%    Note: The size inputs M, N, and P... should be nonnegative integers.
%    Negative integers are treated as 0.
%
%    See also INF, ISNAN, ISFINITE, ISFLOAT.
%
%    Reference page in Doc Center
%       doc nan
%
%    Other functions named nan
%
%       codistributed/nan      codistributor2dbc/nan    gpuArray/nan
%       codistributor1d/nan    distributed/nan          ts/nan
%


function writeDoublePendulumDynamics(f,M)

%This function writes the dynamics file for the double pendulum.

filename = 'doublePendulumDynamics';

comments{1} = ['DZ = ' upper(filename) '(T,Z,U,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the dynamics of a double';
comments{4} = '    pendulum, and is designed to be called from ode45. The';
comments{5} = '    model allows for arbitrary mass and inertia for each';
comments{6} = '    link, but no friction or actuation';
comments{7} = ' ';
comments{8}  = 'INPUTS: ';
comments{9}  = '    t = time. Dummy input for ode45. Not used.';
comments{10} = '    z = [4xn] matrix of states.';
comments{11} = '    u = [2xn] matrix of inputs';
comments{12} = '    P = struct of parameters';
comments{13} = 'OUTPUTS: ';
comments{14} = '    dz = [4xn] matrix of state derivatives';
comments{15} = ' ';
comments{16} = 'NOTES:';
comments{17} = ['    This file was automatically generated by ' mfilename];

params{1} = {'m1','link one mass'};
params{2} = {'m2','link two mass'};
params{3} = {'g ','gravity'};
params{4} = {'l1','link one length'};
params{5} = {'l2','link two length'};
params{6} = {'I1','link one moment of inertia about its center of mass'};
params{7} = {'I2','link two moment of inertia about its center of mass'};
params{8} = {'d1','distance between link one center of mass and parent joint'};
params{9} = {'d2','distance between link two center of mass and parent joint'};

states{1} = {'th1','link one absolute angle'};
states{2} = {'dth1','link one angular rate'};
states{3} = {'th2','link two absolute angle'};
states{4} = {'dth2','link two angular rate'};

input{1} = {'u1','torque acting on link 1 wrt ground'};
input{2} = {'u2','torque acting on link 2 wrt ground'};

dstates{1} = {'dth1','derivative of link one absolute angle'};
dstates{2} = {'ddth1','derivative of link one angular rate'};
dstates{3} = {'dth2','derivative of link two absolute angle'};
dstates{4} = {'ddth2','derivative of link two angular rate'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function dz = ' filename '(~,z,u,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(states)
    fprintf(fid,[states{i}{1} ' = z(' num2str(i) ',:); %%' states{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(input)
    fprintf(fid,[input{i}{1} ' = u(' num2str(i) ',:); %%' input{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:2;
    fprintf(fid,['f' num2str(i) ' = ' vectorize(char(f(i))) ';\n']);
end
fprintf(fid,'\n');

for i=1:2
    for j=1:2
        fprintf(fid,['M' num2str(i) num2str(j) ' = ' vectorize(char(M(i,j))) ';\n']);
    end
end
fprintf(fid,'\n');

fprintf(fid,'D = M11.*M22 - M12.*M21;\n\n');

fprintf(fid,'ddth1 = (f2.*M12 - f1.*M22)./D;\n');
fprintf(fid,'ddth2 = -(f2.*M11 - f1.*M21)./D;\n');
fprintf(fid,'\n');

fprintf(fid,'dz = [...\n');
for i=1:length(dstates)
    fprintf(fid,['    ' dstates{i}{1} '; %%' dstates{i}{2} '\n']);
end
fprintf(fid,'];\n');

fprintf(fid,'end \n');

fclose(fid);
end


%Sensor Refs=[use (0,1), val, obs noise std, ini std]
%u is assumed to be a random constant (with infinite initial variance)
function [u, ximu, Pimu, Pu, Pux]=selfcalib1(imu_data, Aimu, Qimu, Cimu, Rimu, PimuInv, M, sensor_ref)

DBG=0;  %Debug

nu=size(M,2); %# of kinematic variables
ns=size(M,1); %# of sensors
nimu=size(Aimu,1);    %total # of imu states

if (isempty(sensor_ref)) %not a compulsory argument
    sensor_ref=zeros(size(M,2),4);
end

%System model (augment with kv)
A=diagmat_v000(Aimu, eye(nu),[]);
Q=diagmat_v000(Qimu, zeros(nu), []);
C=[M Cimu];
R=Rimu;
Pinv=diagmat_v000(PimuInv, zeros(nu), []);

%External body acc/rot rate observations
next=0;
Cext=[];
Rext=[];
yext=[];
for in=1:nu
    if (sensor_ref(in,1)==1)     %there is an external obs
        next=next+1;
        Cext=[Cext;zeros(1,size(A,1))];
        Cext(end,in)=1;
        Rext=diagmat_v000(sensor_ref(in,3)^2,Rext,[]);
        yext=[yext;sensor_ref(in,2)];
        if (sensor_ref(in,4)~=0)    %external obs have random bias
            A=diagmat_v000(1, A, []);
            Q=diagmat_v000(0, Q, []);
            Pinv=diagmat_v000(1/sensor_ref(in,4)^2, Pinv, []);
            C=[C zeros(ns,1)];
            Cext=[Cext zeros(next,1)];
            Cext(end,end)=1;
        end
    end
end
nst=size(A,1);

%%%%% Start the estimation routine
%%Sensor outputs
%%For the first sensor data use the innovation form (Pinv is probably singular)
y=imu_data(:,1);
Rinv=inv(R);
P=inv(Pinv+C'*Rinv*C);
xest=P*(C'*Rinv*y);
%%External obs
if (next>0)
    K=P*Cext'*inv(Cext*P*Cext'+Rext);
    xest=xest+K*(yext-Cext*xest);
    P=(eye(nst)-K*Cext)*P*(eye(nst)-K*Cext)'+K*Rext*K';
end

if (DBG)
    ndat=size(imu_data,2);
    debmn=zeros(nu,ndat);
    debop=zeros(size(A,1),ndat);

    WLS=lsmat_v001(M,R,0);
    mean_dat=WLS*y;
    debmn(:,1)=mean_dat;
    debop(:,1)=xest;
end

%For the rest use standard KF
for in=2:size(imu_data,2)
    %%%prediction
    xest=A*xest;
    P=A*P*A'+Q;

    %%%filter
    %%Sensor data
    y=imu_data(:,in);
    K=P*C'*inv(C*P*C'+R);
    %K(1:nimu,:)=0;
    xest=xest+K*(y-C*xest);
    P=(eye(nst)-K*C)*P*(eye(nst)-K*C)'+K*R*K';
    %%External obs
    if (next>0)
        K=P*Cext'*inv(Cext*P*Cext'+Rext);
        xest=xest+K*(yext-Cext*xest);
        P=(eye(nst)-K*Cext)*P*(eye(nst)-K*Cext)'+K*Rext*K';
    end

    if (DBG)
        mean_dat=((in-1)*mean_dat+(WLS*y))/in;
        debmn(:,in)=mean_dat;
        debop(:,in)=xest;
    end
end

%Outputs (combine deterministic and stochastic results)
ximu=xest(nu+1:nu+nimu,1);
Pimu=P(nu+1:nu+nimu,nu+1:nu+nimu);
u=xest(1:nu,:);
Pu=P(1:nu,1:nu);
Pux=P(1:nu,nu+1:nu+nimu);

if (DBG)
    in=1;
    figure;
    plot(imu_data(in,:))
    hold on;
    plot(debmn(in,:),'r')
    plot(debop(in,:),'g')
end


function [csd,Hz] = spm_ccf2csd(ccf,Hz)
% Converts cross covariance function to cross spectral density
% FORMAT [csd,Hz] = spm_ccf2csd(ccf,Hz)
%
% ccf  (N,:,:)          - cross covariance functions
% Hz   (n x 1)          - vector of frequencies (Hz)
%
% csd  (n,:,:)          - cross spectral density (cf, mar.P)
%
% See also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m and spm_Q
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ccf2csd.m 5895 2014-02-26 14:28:23Z karl $

% unpack cells
%--------------------------------------------------------------------------
if iscell(ccf)
    for i = 1:length(ccf)
       csd{i}    = spm_ccf2csd(ccf{i},Hz);
    end
    return
end

% unpack time bins (for time-frequency responses)
%--------------------------------------------------------------------------
if ndims(ccf) == 4
    for i = 1:size(ccf,1)
       csd(i,:,:,:) = spm_ccf2csd(squeeze(ccf(i,:,:,:)),Hz);
    end
    return
end

% Frequencies
%--------------------------------------------------------------------------
ds    = Hz(2) - Hz(1);
gi    = ceil(Hz/ds) + 1;
N     = Hz(end);

% Fourier transform cross-spectral density
%==========================================================================
for i = 1:size(ccf,2)
    if ismatrix(ccf)
        g          = fft(ccf(:,i));
        csd(:,i)   = g(gi)/N/ds;
    else
        for j = 1:size(ccf,3)
            g          = fft(ccf(:,i));
            csd(:,i,j) = g(gi)/N/ds;
        end
    end
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (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                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxLowrank.m                               |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   : Compare compressor with total pivoting        |
%|  `---'  |                                                              |
%+========================================================================+

clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Dimensions
Nx  = 100;
Ny  = 101;
rk  = 10;
tol = 1e-3;

% Compress zeros
M     = zeros(Nx,Ny);
[A,B] = hmxACA(M,tol);
norm(A*B,'inf')

% Compress ones
M     = ones(Nx,Ny);
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress eye
M          = eye(Nx,Ny);
[~,~,flag] = hmxACA(M,tol);
flag

% Compress circular permutation for eye
M = circshift(eye(Nx,Ny),3,2);
[~,~,flag] = hmxACA(M,tol);
flag

% Compress 1-eye
M          = 1-eye(Nx,Ny);
[~,~,flag] = hmxACA(M,tol);
flag

% Compress random
M     = rand(Nx,rk) * rand(rk,Ny);
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress random complex
M     = (-1+2*rand(Nx,rk)+1i*(-1+2*rand(Nx,rk))) * (rand(rk,Ny) + 1i*rand(rk,Ny));
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 1x2 block with zeros
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,10);
M     = [M Z];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 2x1 block with zeros
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(10,Ny);
M     = [M ; Z];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 2x2 block random
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,Ny);
M     = [M Z ; Z M];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 4x4 block random
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,Ny);
M     = [M Z ; Z M];
Z     = zeros(size(M));
M     = [M Z ; Z M];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 2x2 inegal block random (to be hardly improved...)
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,1);
M     = [M zeros(Nx,1) ; zeros(1,Ny) 1];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 4x4 inegal block random (to be hardly improved...)
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,1);
M     = [M zeros(Nx,3) ; zeros(3,Ny) eye(3)];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Build orthogonal mesh
vtx   = [0 0 0 ; eye(3)];
elt   = [1 3 2 ; 1 2 4];
col   = [1 ; 2];
mesh  = msh(vtx,elt,col);
mesh  = refine(mesh,0.2);
[~,I] = sort(mesh.col);
mesh  = mesh.sub(I);

% Translate
mesh2          = mesh;
mesh2.vtx(:,1) = mesh2.vtx(:,1) + 2;

% Graphical representation
figure
plot(mesh)
hold on
plotNrm(mesh)
plot(mesh2)
axis equal
view(-180,30)

% Build double layer matrix
Gxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',1);
Gxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',1);
Gxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',1);
D      = integral(dom(mesh,3),dom(mesh2,3),fem(mesh,'P0'),Gxy,ntimes(fem(mesh2,'P0')));

% Graphical represenation
figure
imagesc(abs(D))
colorbar

% Compression
[A,B] = hmxACA(D,tol);
norm(A*B-D,'inf')/norm(D,'inf')

disp('~~> Michto gypsilab !')


function [rDisambig,sourceInfo]=disambiguateClust1D(rMeas,ampMeas,deltaR,rMax,clustDist,twoWayDisambig,threshold)
%%DISAMBIGUATECLUST1D This function uses a clustering algorithm to
%                   disambiguate measurements in one dimension. It can
%                   handle measurements from multiple simultaneous targets.
%                   Information on which detections went into the
%                   disambiguated measurements is also returned. The
%                   snapshots all have a different number of bins; the
%                   lengths in deltaR should be relatively prime (coprime)
%                   integers (possibly all times a scaling constant). When
%                   the "true" value of a detection is outside of the range
%                   0<=rMeas<deltaR(i) for the ith interval, it is aliased
%                   back in. This function tries to dealias the
%                   measurements and determine how many detections are
%                   present. This function only considers aliasing in a
%                   positive (+deltaR) direction and is thus best suited
%                   for dealiasing ranges.
%
%INPUTS: rMeas A numIntX1 or 1XnumInt cell matrix with the detections in
%              each interval. Each cell contains a numMeasX1 or 1XnumMeas
%              vector of the detections in that interval. Detections should
%      ampMeas A numIntX1 or 1XnumInt cell matrix with the positive real
%              amplitudes associated with the detections in each interval.
%              These are used to perform a weighted average to determine
%              the true target location.
%       deltaR The numIntX1 or 1XnumInt vector of the unambiguous range in
%              each interval. These values must be relatively prime
%              integers (times a common possibly non-integer scale factor)
%              for the algorithm to work.
%         rMax The maximum unambiguous range to consider for the
%              completely unaliased measurements (all dealiased values
%              are less than this. If this value is omitted or an empty
%              matrix is passed, then the default of prod(deltaR) is used,
%              which is the maximum from the Chinese remainder theorem.
%              However, as demonstrated below, the presence of multiple
%              targets can cause extra distant "ghost" detections to appear
%              due to the interference of the targets. Thus, it is often
%              good to design the maximum unambiguous value to be very big
%              and to limit the maximum number of bins actually considered.
%              Put another way, use more coprime intervals.
%    clustDist This is the distance to determine that two aliased-out
%              values are in the same cluster. This depends on the scaling
%              of the intervals. The default if this parameter is omitted
%              or an empty matrix is passed is 0.5, which is good if deltaR
%              are all integers.
% twoWayDisambig If this is false, then the integers over which aliasing
%              are performed are only positive. Otherwise, they can be
%              positive and negative, which can be more convenient for
%              Doppler disambiguation.
%    threshold An optional threshold for declaring a detection. At least
%              this many common intervals must cluster together. The
%              default if this parameter is omitted or an empty matrix is
%              passed is numInt.
%
%OUTPUTS: rDisambig The values of the detections, a numDetectX1 vector.
%              These are weighted averages of the values in rMeas that went
%              into the detections. If there are no detections, then this
%              will be an empty matrix.
%        sourceInfo Information allowing one to reconstruct which values in
%              rMeas went into forming each detection. This is a
%              numDetectX1 cell array. Each cell ion the array has a
%              numMeasX2 matrix providing information on the measurements
%              in that cluster. The first column gives the number of the
%              interval that produced the measurement (index of rMeas) and
%              the second column provides information on which measurement
%              in that interval led to the detection.
%
%The general idea of such clustering is described in [1] and [2]. However,
%it is done differently here. The authors in [2] described a
%disambiguiation algorithm that is unusable if one of the intervals does
%not produce detections and the basic algorithm in [1] is only focussed on
%a single target. Here, we alias out the measurement across all ambiguity
%intervals and then determine which aliased out measurements gate. Two
%values gate if they are from different intervals and are within clustDist
%of each other. A DisjointSet data structure is used to take the gating
%ifnromation and cluster the aliased out measurements. Clusters of a
%sufficient size are counted as detections. Note that this clustering is
%not globally optimal as it is still possible for two detections from a
%single PRI to end up in one cluster, which is only particularly likely
%with closely spaced targets. An optimal clustering algorithm would be a
%multiframe assignment problem.
%
%Note that because linear averaging is performed, points very near the
%edges will not average to the correct values. However, since measurement
%noises are typically small and unambiguous intervals large, this is seldom
%a problem.
%
%EXAMPLE 1:
%This is similar to the toy example given in [2], except we add some noise
%to the measurements In this instance, the algorithm will produce results
%that are close to the true unaliased bins.
% deltaR=[7;8;11];
% maxR=24;
%
% trueBins=[6;13];
%
% rMeas=cell(3,1);
% for curInt=1:3
%     rMeas{curInt}=unique(wrapRange(trueBins,0,deltaR(curInt)));
%     %Add noise.
%     rMeas{curInt}=rMeas{curInt}+0.1*randn(size(rMeas{curInt}));
% end
%
%[rDisambig,sourceInfo]=disambiguateClust1D(rMeas,[],deltaR,maxR)
%
%EXAMPLE 2:
%This example is better suited for Doppler disambiguation Here, positive
%and negaitve values are present, which could correspond to positive and
%negative range rates. The disambiguation is thus two-sided and the maximum
%number of bins used is limited so that irrelevant extra-fast targets are
%not detected.
% deltaR=[7;8;11;13];
% maxR=100;
% %-50 to 49
% trueBins=[6;-13];
%
% rMeas=cell(3,1);
% for curInt=1:4
%     rMeas{curInt}=unique(wrapRange(trueBins,0,deltaR(curInt)));
%     %Add noise.
%     rMeas{curInt}=rMeas{curInt}+0.1*randn(size(rMeas{curInt}));
% end
% disambiguateClust1D(rMeas,[],deltaR,maxR,[],true)
%
%REFERENCES:
%[1] G. Trunk and S. Brockett, "Range and velocity ambiguity resolution,"
%    in Record of the IEEE National Radar Conference, Lynnfield, MA, 20-22
%    Apr. 1993, pp. 146-149.
%[2] P. Stinco, M. Greco, F. Gini, A. Farina, and L. Timmoneri, "Analysis
%    and comparison of two disambiguity algorithms: The modified CA and
%    CRT," in Proceedings of the International Radar Conference -
%    Surveillance for a Safer World, Bordeaux, France, 12-16 Oct. 2009.
%
%December 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(rMax))
   rMax=prod(deltaR);
end

numPRI=length(rMeas);
if(nargin<5||isempty(clustDist))
   clustDist=0.5;
end

if(nargin<6||isempty(twoWayDisambig))
   twoWayDisambig=false;
end

if(nargin<7||isempty(threshold))
   threshold=numPRI;
end

%Determine the maximum number of aliased out measurements in both
%directions.
numAliased=0;
for curPRI=1:numPRI
    aliasedMax=ceil(rMax/deltaR(curPRI));
    numAliased=numAliased+length(rMeas{curPRI})*aliasedMax;
end

%We will now alias out all of the measurements
measList=zeros(numAliased,1);
%souceData(i,1) holds the PRI that produced the aliased-out measurement.
%sourceData(i,2) holds the index of the measurement in the PRI.
sourceData=zeros(numAliased,2);
numAdded=0;
for curPRI=1:numPRI
    curMeas=rMeas{curPRI};
    numMeas=length(curMeas);

    aliasedMax=ceil(rMax/deltaR(curPRI));
    if(twoWayDisambig)
        valRange=(-ceil((aliasedMax)/2)):fix((aliasedMax)/2);
    else
        valRange=(0:(aliasedMax-1));
    end

    aliasedVals=bsxfun(@plus,curMeas(:),deltaR(curPRI)*valRange);
    origIdx=repmat(1:numMeas,1,aliasedMax+1);
    aliasedVals=aliasedVals(:);
    origIdx=origIdx(:);
    if(twoWayDisambig)
        %Get rid of values that aliased to values that are too low.
        %Get rid of values that aliased too low or too high
        sel=(aliasedVals>=-rMax/2)&(aliasedVals<=rMax/2);
        aliasedVals=aliasedVals(sel);
    else
        %Get rid of values that aliased to values that are too high.
        sel=aliasedVals<=rMax;
        aliasedVals=aliasedVals(sel);
    end

    origIdx=origIdx(sel);

    num2Add=length(aliasedVals);
    idx2Add=(numAdded+1):(numAdded+num2Add);

    measList(idx2Add)=aliasedVals;

    sourceData(idx2Add,1)=curPRI;
    sourceData(idx2Add,2)=origIdx;

    numAdded=numAdded+num2Add;
end

%Shrink to fit.
measList=measList(1:numAdded);
sourceData=sourceData(1:numAdded,:);

%Sort the values.
[measList,idx]=sort(measList,'ascend');
sourceData=sourceData(idx,:);

%Create a DisjointSet and determine which measurements gate with each other.
%The DisjointSet will cluster them. To gate, they must be sufficiently
%close and also not within the same interval. The loops go through all
%pairs of aliased-out measurements. We take advantage of having sorted the
%measurements so that once a distance comparison fails, no measurements are
%farther ranges are considered.
gateSet=DisjointSet(numAdded);
for idx1=1:(numAdded-1)
    for idx2=(idx1+1):numAdded
        %If they originate in different PRIs.
        if(sourceData(idx1,1)~=sourceData(idx2,1))
            if(abs(measList(idx1)-measList(idx2))<clustDist)
                %The two values gate together.
                gateSet.unionFromList([idx1;idx2]);
            else
                break;
            end
        end
    end
end

%Form clusters.
theClust=gateSet.createClusterSet();

%Count how many clusteres are large enough to keep.
numClusters=theClust.numClusters();

numPastThresh=sum(theClust.clustSize>=threshold);
rDisambig=zeros(numPastThresh,1);
sourceInfo=cell(numPastThresh,1);

curDetectClust=0;
for curClust=1:numClusters
    numMeasCur=theClust.clustSize(curClust);
    if(numMeasCur>=threshold)
        sourceDataCur=sourceData(theClust(curClust,:),:);

        rAmpCur=zeros(numMeasCur,1);
        if(~isempty(ampMeas))
            for curMeas=1:numMeasCur
                PRI=sourceDataCur(curMeas,1);
                idx=sourceDataCur(curMeas,2);
                idxR=theClust(curClust,curMeas);

                rAmpCur(curMeas,1)=measList(idxR);
                rAmpCur(curMeas,2)=ampMeas{PRI}(idx);
            end
        else
             for curMeas=1:numMeasCur
                idxR=theClust(curClust,curMeas);

                rAmpCur(curMeas,1)=measList(idxR);
                rAmpCur(curMeas,2)=1;
            end
        end

        curDetectClust=curDetectClust+1;

        %A simple weighted average.
        rDisambig(curDetectClust)=sum(rAmpCur(:,1).*rAmpCur(:,2))/sum(rAmpCur(:,2));
        sourceInfo{curDetectClust}=sourceDataCur;
    end
end
end

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


%% FUNCTION nchoose
%   How many unique combinations of 1 to N elements are there. This
%   function is not intended to directly called by users.
%   See the Logistic_iMSF function.
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Lei Yuan, Jiayu Zhou, and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 12, 2012.
%
%% RELATED FUNCTIONS
%   init_opts, Construct_iMSF, Logistic_iMSF, MultiSource_LogisticR

function W = nchoose(S)

N = numel(S) ;

% How many unique combinations of 1 to N elements are there
M = (2^N)-1 ;
if N > 18,
    warning('Nchoose:LargeOutput', ...
        'There are %d unique combinations. Please be patient ...',M) ;
end

S = S(:).' ;   % make the set a row vector, for uniform output

W = cell(M,1) ;    % Pre-allocation of output
p2=2.^(N-1:-1:0) ; % This part of the formula can be taken out of the loop

for i=1:M,
    % calculate the (reversed) binary representation of i
    % select the elements of the set based on this representation
    W{i} = S(bitget(i*p2,N) > 0) ;
end


function [ won bet happy tt ] = auction( w, iter, e, verbose )
% Subroutine AUCTION is a implemetation of the auction Algorithm
% presented in Turing's Invisible Hand. Although well discribed the
% pseudo-algorithm presented there contained an error which was corrected
% by line 62-65 replacing
%           if ((w(ii,j) - p(j)) >= 0)
%
% [ won bet happiness ] = auction( w, iter, e, verbose )
% Inputs:
%   w (g x b)
%     the rows is the bidders
%     the cols is the goods
%     the values are every bidder i's percieved value of good j
%   iter
%     iterations iterations to do
%   e
%     e is the epsilon - the minumum bet increase (default e = 1/(ngoods+1)
%     )
%
% Copyright (C) 2012 - Pieter V. Reyneke, South Africa
%
% Open Source licence, use whereever you like but at own risk, keep this
% complete copyright notice in the code and please send me updates and
% report errors to pieter.reyneke@gmail.com. Will give recognition, if
% requested, after any valid comment leading to an update was recieved.
%
% Author: PV Reyneke

if (nargin<1)
    w = rand(3)*10;
end

if (nargin<2)
    iter = Inf;
end

if (nargin<4)
    verbose = 1;
end

p   = zeros(1,size(w,2));     % reset all goods' current bets (top - tracks)
won = zeros(1,size(w,2));     % reset cuurent winners for each good
q   = 1:size(w,1);            % create queue for bidders (left - obs)

if (nargin<3)
    e = 1/(length(p)+1);
end

if (verbose)
    display('The percieved value of goods (top) by each bidder (left):');
    display([num2str(w, '%3.0f') ]);
end

i = find(q>0,1);              % take first non-committed bidder
ii = q(i);
tdead = 0;
tic
while (~isempty(ii) && (iter > 0))
    q(q==ii) = 0;             % out of queue
    [v, j] = max(w(ii,:) - p); % search for good w greatest value
                              % above current bid for bidder i
    if ( ((w(ii,j) - (p(j) + e)) >= 0) && ...
         ...                  % if our current bidder i values good j
         ...                  % more than previous bidder plus epsilon
         (won(j) ~= ii) )     % and the bot is not on himself already
        if (won(j)>0)
            k = find(q==0,1); % find first open bidder space
            q(k:(end-1)) = q((k+1):end);
                              % shift left and put
            q(end) = won(j);  % previous highest bidder back in the queue
        end
        won(j) = ii;          % assign current bidder i to good j
        p(j) = p(j) + e;      % and save the new bet
    end

    if (verbose)
        tsss = toc;
        display(['Bets(in epsilons)  : ' num2str(p/e, '%3.0f') ]);
        display(['Bidder             : ' num2str(won, '%3.0f') ]);
        display(['Left todo          : ' num2str(q, '%3.0f') ]);
        display(['-----------------------------------------' ]);
        tdead = toc - tsss;
    end
    i = find(q>0,1);          % take next non-committed bidder
    ii = q(i);
    iter = iter-1;
end
bet = p;
tt = toc;

% measuring each bidder's happiness (the invariant of above loop)
happy = won;
iown = find(won > 0);
for (ii = won(iown))
    iii = won(ii);
    if (~isempty(iii))
        jjj = find(iii==won,1);
        if (~isempty(jjj))
            happy(iii) = (sum(e + w(iii,jjj) - p < w(iii,:) - p) == 0);
        end
    end
end
iown = find(won > 0);
for (ii = won(iown))
    iii = won(ii);
    happy(iii) = happy(iii) | (sum(w(iii,:) - p > 0));
end

j = 1;
cost = 0;
for (i=won)
    cost = cost + w(i,j);
    j = j+1;
end

if (verbose)
    display(['Assignment Cost    : ' num2str(cost, '%5.3f') ]);
    display(['Happiness          : ' num2str(happy, '%3.0f') ]);
    display(['Calc took          : ' num2str((tt - tdead) * 1000, '%5.3f') ' ms' ]);
end


%PEAK2  Find peaks in a matrix
%
% ZP = PEAK2(Z, OPTIONS) are the peak values in the 2-dimensional signal Z.
%
% [ZP,IJ] = PEAK2(Z, OPTIONS) as above but also returns the indices of the
% maxima in the matrix Z.  Use SUB2IND to convert these to row and column
% coordinates
%
% Options::
% 'npeaks',N    Number of peaks to return (default all)
% 'scale',S     Only consider as peaks the largest value in the horizontal
%               and vertical range +/- S points.
% 'interp'      Interpolate peak (default no interpolation)
% 'plot'        Display the interpolation polynomial overlaid on the point data
%
% Notes::
% - A maxima is defined as an element that larger than its eight neighbours.
%   Edges elements will never be returned as maxima.
% - To find minima, use PEAK2(-V).
% - The interp options fits points in the neighbourhood about the peak with
%   a paraboloid and its peak position is returned.  In this case IJ will
%   be non-integer.
%
% See also PEAK, SUB2IND.

% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% Copyright (c) Peter Corke 1/96

function [zp,xypout, aout] = peak2(z, varargin)

    % process input options
    opt.npeaks = 2;
    opt.scale = 1;
    opt.interp = false;

    [opt,args] = tb_optparse(opt, varargin);

    % create a neighbourhood mask for non-local maxima
    % suppression
    h = opt.scale;
    w = 2*h+1;
    M = ones(w,w);
    M(h+1,h+1) = 0;

    % compute the neighbourhood maximum
    znh = iwindow(double(z), M, 'max', 'wrap');

    % find all pixels greater than their neighbourhood
    k = find(z > znh);

    % sort these local maxima into descending order
    [zpk,ks] = sort(z(k), 'descend');

    k = k(ks);

    npks = min(length(k), opt.npeaks);
    k = k(1:npks);

    [y,x] = ind2sub(size(z), k);
    xy = [x y]';

    % interpolate the peaks if required
    if opt.interp

        xyp = [];
        zp = [];
        ap = [];

        % for each previously identified peak x(i), y(i)
        for xyt=xy
            % fit a polynomial to the local neighbourhood
            try

                x = xyt(1); y = xyt(2);

                % now try to interpolate the peak over a 3x3 window

                zc = z(x,   y);
                zn = z(x,   y-1);
                zs = z(x,   y+1);
                ze = z(x+1, y);
                zw = z(x-1, y);

                dx = (ze - zw)/(2*(2*zc - ze - zw));
                dy = (zn - zs)/(2*(zn - 2*zc + zs));

                zest = zc - (ze - zw)^2/(8*(ze - 2*zc + zw)) - (zn - zs)^2/(8*(zn - 2*zc + zs));

                aest = min(abs([ze/2 - zc + zw/2, zn/2 - zc + zs/2]));

            catch
                % handle situation where neighbourhood falls off the data
                % vector
                warning('Peak at %f too close to edge of image, skipping', x(i));
                continue;
            end
            %

            % store x, y for the refined peak
            xyp = [xyp [x+dx; y+dy]];
            zp = [zp zest];
            ap = [ap aest];

        end
    else
        % no interpolation case
        xyp = xy;
        zp = z(k)';
        ap = [];

    end

    % return values
    if nargout > 1
        xypout = xyp;
    end
    if nargout > 2
        aout = ap;
    end


% the basic RIM quantifier: Q(r)=r^alpha
function re=weight1(n,m) % n-length of weighting vector, m - is alpha
re=[];
for h=1:n
    re=[re ((h/n).^m-((h-1)/n).^m)];
end
end


%==============================================================================
% 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/
%==============================================================================
%
% function [yc,y1,y2,y3] = grid2grid(yc,m,in,out)
%
% transfers input grid (c,s,n) to output grid (c,s,n) using inter- and extrapolation
% central operation are reduce/expand, acting on each dimension individually
%
% Input:
%   yc        input grid
%   m         number of discretization points
%   in        type of input grid
%   out       type of output grid
% Output:
%   yc        output grid
%   y1,y2,y3  components of yc
%
%==============================================================================

function [yc,y1,y2,y3] = grid2grid(yc,m,in,out)

if nargin == 0, % help and minimal example
  runMinmalExample;
  return;
end;

dim = length(m);  y{1} = []; y{3} = []; y{3} = [];
if ~exist('out','var'), out = in;  end;

switch in,
  case {'centered','cell-centered'}, n = prod(m);
    % decompose Yin
    for j=1:dim, y{j} = reshape(yc((j-1)*n+(1:n)),m); end;
    switch out,
      case {'centered','cell-centered'},
      case 'staggered', for j=1:dim, y{j} = extend(y{j},j); end;
      case 'nodal',
        for j=1:dim,
          for k=1:dim, y{j} = extend(y{j},k); end;
        end;
    end;

  case 'staggered',
    % decompose Yin, get dimensions right
    E = eye(dim) + m'*ones(1,dim); n = cumsum([0,prod(E)]);
    for j=1:dim, y{j} = reshape(yc(n(j)+1:n(j+1)),E(:,j)'); end;
    switch out,
      case {'centered','cell-centered'}, for j=1:dim, y{j} = reduce(y{j},j); end;
      case 'staggered',
      case 'nodal',
        for j=1:dim,
          for k=setdiff(1:dim,j), y{j} = extend(y{j},k); end;
        end;
    end;

  case 'nodal', n = prod(m+1);
    % decompose Yin
    for j=1:dim, y{j} = reshape(yc((j-1)*n+(1:n)),m+1); end;
    switch out,
      case {'centered','cell-centered'},
        for j=1:dim, for k=1:dim, y{j} = reduce(y{j},k); end; end;
      case 'staggered',
        for j=1:dim, for k=setdiff(1:dim,j) y{j} = reduce(y{j},k); end; end;
      case 'nodal',
    end;

  otherwise,
    error('can not interpret flags')
end;
y1 = y{1}; y2 = y{2}; y3 = y{3}; yc = [y1(:);y2(:);y3(:)];

%------------------------------------------------------------------------------

% the following operators act on x=y{k} in the j-th direction,
% permuting x makes j the first direction, averaging (extend creates two
% additional outside points based on linear BC) and permute back

function x = reduce(x,j)
m = size(x); J = [j,setdiff(1:length(size(x)),j)];
x = permute(x,J); x = (x(1:end-1,:,:)+x(2:end,:,:))/2; x = ipermute(x,J);

function x = extend(x,j)
m = size(x); J = [j,setdiff(1:length(size(x)),j)]; x = permute(x,J);
x = [1.5*x(1,:,:)-0.5*x(2,:,:);(x(1:end-1,:,:)+x(2:end,:,:))/2;...
  1.5*x(end,:,:)-0.5*x(end-1,:,:)];
x = ipermute(x,J);

%------------------------------------------------------------------------------

function runMinmalExample
help(mfilename);
omega = [0,6,0,4]; m = [4 3];

h   = (omega(2:2:end)-omega(1:2:end))./m;
eps = 0.2*min(h);

yC  = getCellCenteredGrid(omega,m); %yC = yC + eps*randn(size(yC));
yN  = reshape(getNodalGrid(omega,m),[],2);
yS  = getStaggeredGrid(omega,m);

yCN = grid2grid(yC,m,'centered','nodal');
yCS = grid2grid(yC,m,'centered','staggered');
ySN = grid2grid(yS,m,'staggered','nodal');
ySC = grid2grid(yS,m,'staggered','centered');
yNC = grid2grid(yN,m,'nodal','centered');
yNS = grid2grid(yN,m,'nodal','staggered');

yC  = reshape(yC, [],2);
yN  = reshape(yN, [],2);
yCN = reshape(yCN,[],2);
ySN = reshape(ySN,[],2);
ySC = reshape(ySC,[],2);
yNC = reshape(yNC,[],2);

FAIRfigure(1); clf;

subplot(3,2,1);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yC(:,1),yC(:,2),'bs');
plot(yCN(:,1),yCN(:,2),'m*');
title(sprintf('%s: example cell-centered to nodel',mfilename));

subplot(3,2,2);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yC(:,1),yC(:,2),'bs');
[y11,y12,y21,y22] = splittStaggeredGrid(yCS,omega,m);
plot(y11,y12,'m>',y21,y22,'m^');
title(sprintf('%s: example cell-centered to staggered',mfilename));

[y11,y12,y21,y22] = splittStaggeredGrid(yS,omega,m);
subplot(3,2,3);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(y11,y12,'b>',y21,y22,'b^');
plot(ySC(:,1),ySC(:,2),'ms');
title(sprintf('%s: example staggered to cell-centered',mfilename));

subplot(3,2,4);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(y11,y12,'b>',y21,y22,'b^');
plot(ySN(:,1),ySN(:,2),'m*');
title(sprintf('%s: example staggered to nodal',mfilename));

subplot(3,2,5);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yN(:,1),yN(:,2),'b*');
plot(yNC(:,1),yNC(:,2),'ms');
title(sprintf('%s: example nodal to cell-centered',mfilename));

subplot(3,2,6);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yN(:,1),yN(:,2),'b*');
[y11,y12,y21,y22] = splittStaggeredGrid(yNS,omega,m);
plot(y11,y12,'m>',y21,y22,'m^');

title(sprintf('%s: example nodal to staggered',mfilename));

%------------------------------------------------------------------------------

function [y11,y12,y21,y22] = splittStaggeredGrid(yS,omega,m);
dim = length(omega)/2;
e  = @(i) (1:dim == i); % i-th unit vector
ns = cumsum([0;prod(ones(dim,1)*m+eye(dim),2)]);
yN = reshape(grid2grid(yS,m,'staggered','nodal'),[m+1,2]);
y11 = reshape(yS(ns(1)+1:ns(2)),m+e(1));
y12 = (yN(:,1:end-1,2)+yN(:,2:end,2))/2;
y21 = (yN(1:end-1,:,1)+yN(2:end,:,1))/2;
y22 = reshape(yS(ns(2)+1:ns(3)),m+e(2));

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


% Originally Coded By: Niall Mangan
% Copyright 2017, All Rights Reserved
% Code by Niall Mangan for paper "Inferring biological networks by sparse
% identification of nonlinear dynamics"
% by N. M. Mangan S. L. Brunton, J. L. Proctor, and J. N. Kutz
% Original Code: https://github.com/niallmm/iSINDy
%%
% This file is modified to compare the performance of i-SINDy on
% insufficient data.
% Modified by:K, 2019/07/16
% Find equations for yeast glycolysis state variable 6
%%
clc;clear all;close all;

addpath('./utils');
addpath('./bioutils');
FolderName='Results2';

% define libarary parameters
laurentorder = 0;
polyorder = 6;
usesine = 0;
dyorder = 1;

% clear variables from other solutions.
clear Theta Thetastring Xi indTheta lambdavec numterms errorv indopt
clear indTheta1 Xi1 numterms1 nT

% Here we load the previously simulated data
load('TrainingData.mat')

%% Define some parameters for the implicit SINDy
% Initial lambda value, which is the value used for soft thresholding in ADM
% To make implicit SINDy work, you need to carefully tune these two
% parameters. The below values are the best one that I found.
tol = 1e-4;
lambda = 3e-4;

% counter
jj = 1;

% initialize the number of nonzero terms found for the lambda
num= 1;
errorvec= 0;

% Define maximum iteration you need
MaxIter = 1000;

% Plot option, we set it to zero
plottag=0;

% Define the for loop
percent_start=0.3;
percent_end=1;
d_percent=0.1;

% Define how many results you want to get for each percentage
N_Iter=20;

for iter=1:N_Iter
    for percent=percent_start:d_percent:percent_end
        fprintf('\n\n\t Using %i percent of the data...\n',percent*100)

        fprintf('\n\t\t Calculating for the %i time...\n',iter)
        % Get the data length
        new_length=round(percent*length(xt));

        % Shuffel the original data
        Sequence=randperm(size(xt,1));
        xt_dum=xt(Sequence,:);
        dxt_dum=dxt(Sequence,:);

        % Assign the value to the new variables
        Data=xt_dum(1:new_length,:);dData=dxt_dum(1:new_length,:);

        % Define the number of states
        n=size(Data,2);

        % pool Data  (i.e., build library of nonlinear time series)
        [Theta, Thetastring] = poolDatady(Data,n,polyorder,usesine, laurentorder, dData(:,6), dyorder);

        % for now calculate null space using null function
        nT = null(Theta);

        % Get the sparse vector
        tic
        [indTheta1, Xi1, numterms1] = ADMinitvary(nT,lambda,MaxIter,tol, plottag);
        fprintf('\t\t\t Calculation finished! Used %d seconds.\n',toc)

        % Save the calculation result of current iteration
        fprintf('\n\t\t\t\t Saving the result...\n')
        tic
        cc=clock;
        ResultName=strcat(FolderName,'/implicit_SINDY_Data_Length_',num2str(percent*100),'_','Iter',num2str(iter),'_',num2str(cc(3)),'_',num2str(cc(4)),'_',num2str(cc(5)),'_',num2str(round(cc(6))),'.mat');
        save(ResultName,'Xi1','indTheta1','percent')
        fprintf('\n\t\t\t\t Saving finished! Using %i seconds...\n',toc)

    end
end


function [pd, labels] = imPerimeterDensity(img, varargin)
% Perimeter density of a 2D binary structure, using Crofton formula.
%
%   Pv = imPerimeterDensity(IMG)
%
%   Example
%   imPerimeterDensity
%
%   See also
%     imPerimeter

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-01-21,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Pre-processing

% check image dimension
if ndims(img) ~= 2 %#ok<ISMAT>
    error('first argument should be a 2D image');
end

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    % extract labels (considers 0 as background)
    labels = unique(img);
    labels(labels==0) = [];

    % allocate result array
    nLabels = length(labels);
    pd = zeros(nLabels, 1);

    props = regionprops(img, 'BoundingBox');

    % compute perimeter of each label considered as binary image
    for i = 1:nLabels
        label = labels(i);
        bin = imcrop(img, props(label).BoundingBox) == label;
        pd(i) = imPerimeterDensity(bin, varargin{:});
    end

    return;
end

%% Extract input arguments

% in case of binary image, compute only one label
labels = 1;

% default number of directions
nDirs = 4;

% default image resolution
delta = [1 1];

% parse parameter name-value pairs
while ~isempty(varargin)
    var = varargin{1};

    if isnumeric(var)
        % option is either number of directions or resolution
        if isscalar(var)
            nDirs = var;
        else
            delta = var;
        end
        varargin(1) = [];

    elseif ischar(var)
        if length(varargin) < 2
            error('Parameter name must be followed by parameter value');
        end

        if strcmpi(var, 'ndirs')
            nDirs = varargin{2};
        elseif strcmpi(var, 'resolution')
            delta = varargin{2};
        else
            error(['Unknown parameter name: ' var]);
        end

        varargin(1:2) = [];
    end
end

%% Compute perimeter within image, and normalize by area

% component area in image
p = imPerimeterEstimate(img, nDirs, delta);

% total area of image, without borders
totalArea = prod(size(img)-1) * prod(delta);

% compute perimeter density
pd = p / totalArea;


classdef RWMOP8 < PROBLEM
% <multi> <real> <constrained>
% Car side impact design problem

%------------------------------- Reference --------------------------------
% A. Kumar, G. Wu, M. Ali, Q. Luo, R. Mallipeddi, P. Suganthan, and S. Das,
% A benchmark-suite of real-world constrained multi-objective optimization
% problems and some baseline results, Swarm and Evolutionary Computation,
% 2021, 67: 100961.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Initialization
        function Setting(obj)
            obj.M        = 3;
            obj.D        = 7;
            obj.lower    = [0.5,0.45,0.5,0.5,0.875,0.4,0.4];
            obj.upper    = [1.5,1.35,1.5,1.5,2.625,1.2,1.2];
            obj.encoding = ones(1,obj.D);
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            x  = varargin{1};
            x1 = x(:,1);
            x2 = x(:,2);
            x3 = x(:,3);
            x4 = x(:,4);
            x5 = x(:,5);
            x6 = x(:,6);
            x7 = x(:,7);
            VMBP = 10.58-0.674.*x1.*x2-0.67275.*x2;
            VFD  = 16.45-0.489.*x3.*x7-0.843.*x5.*x6;
            % Objective function
            f(:,1) = 1.98+4.9.*x1.*6.67.*x2+6.98.*x3+4.01.*x4+1.78.*x5+1e-5.*x6+2.73.*x7;
            f(:,2) = 4.72-0.5.*x4-0.19.*x2.*x3;
            f(:,3) = 0.5.*(VMBP+VFD);
            % Constraints
            g(:,1) = -1+1.16-0.3717.*x2.*x4-0.0092928.*x3;
            g(:,2) = -0.32+0.261-0.0159.*x1.*x2-0.06486.*x1-0.019.*x2.*x7+0.0144.*x2.*x5+0.0154464.*x6;
            g(:,3) = -0.32+0.74-0.61.*x2-0.031296.*x3-0.031872.*x7+0.227.*x2.^2;
            g(:,4) = -0.32+0.214+0.00817.*x5-0.045195.*x1-0.0135168.*x1+0.03099.*x2.*x6-0.018.*x2.*x7+0.007176.*x3+0.023232.*x3-0.00364.*x5.*x6-0.018.*x2.^2;
            g(:,5) = -32+33.86+2.95.*x3-5.057.*x1.*x2-3.795.*x2-3.4431.*x7+1.45728;
            g(:,6) = -32+28.98+3.818.*x3-4.2.*x1.*x2+1.27296.*x6-2.68065.*x7;
            g(:,7) = -32+46.36-9.9.*x2-4.4505.*x1;
            g(:,8) = f(:,2)-4;
            g(:,9) = VMBP - 9.9;
            Population = SOLUTION(varargin{1},f,g,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
         %% Generate a point for hypervolume calculation
        function R = GetOptimum(obj,~)
            R = [9.2596587e+01   4.0000000e+00   1.2699733e+01];
        end
    end
end


function [traj, infStates] = tapas_hgf_whichworld(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf_whichworld(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf_whichworld(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Check whether we have a configuration structure
if ~isfield(r,'c_prc')
    error('tapas:hgf:ConfigRequired', 'Configuration required: before calling tapas_hgf_whichworld, tapas_hgf_whichworld_config has to be called.');
end

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_whichworld_transp(r, p);
end

% Number of worlds
nw = r.c_prc.nw;

% Bernoulli parameters that characterize
% worlds (column vector)
bp = [0.85; 0.15];

% Unpack parameters
mu2_0 = p(1:nw);
sa2_0 = p(nw+1:2*nw);
mu3_0 = p(2*nw+1);
sa3_0 = p(2*nw+2);
ka    = p(2*nw+3);
om    = p(2*nw+4);
th    = p(2*nw+5);
m     = p(2*nw+6);
phi   = p(2*nw+7);

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

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

% Initialize updated quantities

% Representations
mu1 = NaN(n,nw);
pi1 = NaN(n,nw);
mu2 = NaN(n,nw);
pi2 = NaN(n,nw);
mu3 = NaN(n,1);
pi3 = NaN(n,1);

% Other quantities
mu1hat = NaN(n,nw);
pi1hat = NaN(n,nw);
mu2hat = NaN(n,nw);
pi2hat = NaN(n,nw);
mu3hat = NaN(n,1);
pi3hat = NaN(n,1);
v2     = NaN(n,1);
w2     = NaN(n,nw);
da1    = NaN(n,nw);
da2    = NaN(n,nw);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu1(1,:) = tapas_sgm(mu2_0, 1);
pi1(1,:) = 1./(mu1(1,:).*(1-mu1(1,:)));
mu2(1,:) = mu2_0;
pi2(1,:) = 1./sa2_0;
mu3(1)   = mu3_0;
pi3(1)   = 1/sa3_0;

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

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

        % 1st level
        % ~~~~~~~~~
        % Predictions
        mu1hat(k,:) = tapas_sgm(mu2(k-1,:), 1);

        % Precisions of predictions
        pi1hat(k,:) = 1./(mu1hat(k,:).*(1 -mu1hat(k,:)));

        % Updates (simply applying Bayes' theorem)

        % Likelihood of outcome u(k)
        llh = bp.^u(k).*(1-bp).^(1-u(k));

        % Marginal likelihood of outcome
        mllh = mu1hat(k,:)*llh;

        % Posterior for each world
        mu1(k,:) = mu1hat(k,:).*llh'./mllh;

        % Precision of posterior
        pi1(k,:) = 1./(mu1(k,:).*(1-mu1(k,:)));

        % Prediction errors
        da1(k,:) = mu1(k,:) -mu1hat(k,:);

        % 2nd level
        % ~~~~~~~~~
        % Predictions
        mu2hat(k,:) = mu2(k-1,:);

        % Precisions of predictions
        pi2hat(k,:) = 1./(1./pi2(k-1,:) +exp(ka *mu3(k-1) +om));

        % Updates
        pi2(k,:) = pi2hat(k,:) +1./pi1hat(k,:);

        mu2(k,:) = mu2hat(k,:) +1./pi2(k,:) .*da1(k,:);

        % Volatility prediction errors
        da2(k,:) = (1./pi2(k,:) +(mu2(k,:) -mu2hat(k,:)).^2) .*pi2hat(k,:) -1;

        % 3rd level
        % ~~~~~~~~~
        % Predictions
        mu3hat(k) = mu3(k-1) +phi *(m -mu3(k-1));

        % Precision of prediction
        pi3hat(k) = 1/(1/pi3(k-1) +th);

        % Weighting factors
        v2(k)   = exp(ka *mu3(k-1) +om);
        w2(k,:) = v2(k) *pi2hat(k,:);

        % Updates
        pi3(k) = pi3hat(k) +1/nw*sum(1/2 *ka^2 *w2(k,:) .*(w2(k,:) +(2 *w2(k,:) -1) .*da2(k,:)));

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

        mu3(k) = mu3hat(k) +sum(1/2 *1/pi3(k) *ka *w2(k,:) .*da2(k,:));

    else
        mu1(k,:) = mu1(k-1,:);
        pi1(k,:) = pi1(k-1,:);
        mu2(k,:) = mu2(k-1,:);
        pi2(k,:) = pi2(k-1,:);
        mu3(k)   = mu3(k-1);
        pi3(k)   = pi3(k-1);

        mu1hat(k,:) = mu1hat(k-1,:);
        pi1hat(k,:) = pi1hat(k-1,:);
        mu2hat(k,:) = mu2hat(k-1,:);
        pi2hat(k,:) = pi2hat(k-1,:);
        mu3hat(k)   = mu3hat(k-1);
        pi3hat(k)   = pi3hat(k-1);
        v2(k)       = v2(k-1);
        w2(k,:)     = w2(k-1,:);
        da1(k,:)    = da1(k-1,:);
        da2(k,:)    = da2(k-1,:);
    end
end

% Implied learning rates at the first level
sgmmu2 = tapas_sgm(mu2, 1);
lr1    = diff(sgmmu2)./da1(2:n,:);
lr1(da(2:n,1)==0) = 0;

% Remove representation priors
mu1(1,:)  = [];
pi1(1,:)  = [];
mu2(1,:)  = [];
pi2(1,:)  = [];
mu3(1)    = [];
pi3(1)    = [];

% Remove other dummy initial values
mu1hat(1,:) = [];
pi1hat(1,:) = [];
mu2hat(1,:) = [];
pi2hat(1,:) = [];
mu3hat(1,:) = [];
pi3hat(1)   = [];
v2(1)       = [];
w2(1,:)     = [];
da1(1,:)    = [];
da2(1,:)    = [];

% Create result data structure
traj = struct;

traj.mu = NaN(n-1,3,nw);
traj.mu(:,1,:) = mu1;
traj.mu(:,2,:) = mu2;
traj.mu(:,3,1) = mu3;

traj.sa = NaN(n-1,3,nw);
traj.sa(:,1,:) = 1./pi1;
traj.sa(:,2,:) = 1./pi2;
traj.sa(:,3,1) = 1./pi3;

traj.muhat = NaN(n-1,3,nw);
traj.muhat(:,1,:) = mu1hat;
traj.muhat(:,2,:) = mu2hat;
traj.muhat(:,3,1) = mu3hat;

traj.sahat = NaN(n-1,3,nw);
traj.sahat(:,1,:) = 1./pi1hat;
traj.sahat(:,2,:) = 1./pi2hat;
traj.sahat(:,3,1) = 1./pi3hat;

traj.v       = v2;
traj.w       = w2;

traj.da = NaN(n-1,2,nw);
traj.da(:,1,:) = da1;
traj.da(:,2,:) = da2;

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

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,3,nw);
psi(:,2,:) = 1./pi2;
psi(:,3,:) = diag(1./pi3) *pi2hat;
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,3,nw);
epsi(:,2,:) = squeeze(psi(:,2,:)) .*da1;
epsi(:,3,:) = squeeze(psi(:,3,:)) .*da2;
traj.epsi   = epsi;

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

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

return;


function pde = constant_fun(bm,bp,r,x0,y0,z0,coef)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one);

pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = ((x-x0).^2 + (y-y0).^2 + (z-z0).^2).^(1/2)/r-1;
    end

%% exact solution
    function u = exactu1(x,y,z)
        u = um1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id));
    end
    function u = exactu2(x,y,z)
        u = um2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id));
    end
    function u = exactu3(x,y,z)
        u = um3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id));
    end
    function u = um1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = um2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = um3(x,y,z)
        u = coef*ones(size(x));
    end
    function u = up1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = up2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = up3(x,y,z)
        u = coef*ones(size(x));
    end

%% Boundary Function
    function u = gD1(x,y,z)
        u = exactu1(x,y,z);
    end
    function u = gD2(x,y,z)
        u = exactu2(x,y,z);
    end
    function u = gD3(x,y,z)
        u = exactu3(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dyum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dzum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dxup(x,y,z)
        u = zeros(size(x));
    end
    function u = Dyup(x,y,z)
        u = zeros(size(x));
    end
    function u = Dzup(x,y,z)
        u = zeros(size(x));
    end

    function u = Duker(x,y,z)
        u = zeros(size(x));
    end

%% right hand side function
    function u = f1(x,y,z)
        u = fm1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id));
    end
    function u = f2(x,y,z)
        u = fm2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id));
    end
    function u = f3(x,y,z)
        u = fm3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id));
    end

    function u = fm1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fm2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fm3(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fp1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fp2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fp3(x,y,z)
        u = coef*ones(size(x));
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function A10 = synA10min(A11, A00, cmax)
%-----------------------------------------------------------------------------
%
% For each point of colour 10 this function assigns the minimum value at the
% neighbouring gridpoints of colours 11 and 00.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 7, 2001.
% (c) 1998-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
[n00, m00]=size(A00);
[n11, m11]=size(A11);
n10=n11;
m10=m00;
%[n10, m10]=size(A10);
if     m10 == m11
  S=min(A11, stripR(extL(A11, cmax)));
elseif m10 == m11+1
  S=min(extL(A11, cmax), extR(A11, cmax));
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str(size(A00))]);
  error(' synA10min - A11 and A00 do not match ');
end
if     n10 == n00
  T=min(A00, stripU(extD(A00, cmax)));
elseif n10 == n00-1
  T=min(stripD(A00), stripU(A00));
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str(size(A00))]);
  error(' synA10min - A11 and A00 do not match ');
end
%Note: all(size(S) == size(T)) & all(size(S) == [n10 m10]) always holds.
A10=min(S, T);
%-----------------------------------------------------------------------------


%% FACE_POOL runs the example interactively with MATLABPOOL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 June 2010
%
%  Author:
%
%    Gene Cliff, John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FACE_POOL\n' );
  fprintf ( 1, '  Evaluate FACE_FUN directly with MATLABPOOL.\n' );
%
%  Load the MAT file containing the grid coordinates as the variable
%  "Grid" and the element connectivity as the variable "e_conn".
%
  load GRID
%
%  We will be looking for tetrahedrons which have one face lying
%  on a plane defining one side of the box.  To define this box
%  we must specify:
%
%  II, the index of the "I" variable which is constant on the plane.
%  If the constant variable is X, then II is 1, and so on.
%
%  VAL_II, the value of the constant "I" variable.
%
%  XJ_LB, XJ_UB, the upper and lower bounds for the "J" nonconstant variable.
%
%  XK_LB, XK_UB, the upper and lower bounds for the "K" nonconstant variable.
%
%  The "J" and "K" variables are the first and second variables to follow
%  the "I" variable, assuming that we are counting with wrap around.
%  There are thus three possibilities for I, J and K:
%
%    I  J  K
%   -- -- --
%    1  2  3
%    2  3  1
%    3  1  2
%
%  Our choices below specify that we are looking at the plane Z = 3.0,
%  and more specifically, the box in that plane for which
%  -1.0 <= X <= +1.0, and -1.0 <= Y <= +1.0.
%
  ii = 3;
  val_ii = 3.0;
  xj_lb = -1.0;
  xj_ub = +1.0;
  xk_lb = -1.0;
  xk_ub = +1.0;
%
%  Call FACE_FUN to compute a list of the triangular faces of the
%  tetrahedral elements which lie on the boundary plane.
%
%  Get 4 "labs" to work on the job.
%
  matlabpool open local 4
  F_conn = face_fun ( Grid, e_conn, ii, val_ii, xj_lb, xj_ub, xk_lb, xk_ub );
  matlabpool close

  face_num = size ( F_conn, 2 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of triangles on boundary is %d\n', face_num );
%
%  Display the surface triangulation.
%
  i_pl = setdiff ( 1:3, ii );

  triplot ( F_conn', Grid(:, i_pl(1)), Grid(:, i_pl(2)) );

  if ( ii == 1 )
    title ( 'Triangular faces on X boundary plane' );
    xlabel ( '- Y axis -' );
    ylabel ( '- Z axis -' );
  elseif ( ii == 2 )
    xlabel ( '- X axis -' );
    title ( 'Triangular faces on Y boundary plane' );
    ylabel ( '- Z axis -' );
  elseif ( ii == 3 )
    xlabel ( '- X axis -' );
    ylabel ( '- Y axis -' );
    title ( 'Triangular faces on Z boundary plane' );
  end

  axis equal
  axis tight


function s=shuffle(x,c)
%Syntax: s=shuffle(x,c)
%______________________
%
% Makes c shuffled surrogates of a time series x.
%
% s is the shuffled time series.
% x is the original time series.
% c is the number of surrogates.
%
%
% References:
%
% Theiler J, Galdrikian B, Longtin A, Eubank S, Farmer D J (1992): Using
% Surrogate Data to Detect Nonlinearity in Time Series. In Nonlinear Modeling
% and Forecasting, eds. Casdagli M & Eubank S. 163-188. Addison-Wesley
%
% Theiler J, Eubank S,Galdrikian B, Longtin A,  Farmer D J (1992): Testing
% for nonlinearity in time series: the method of surrogate data. Physica D
% 58: 77-94
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 12 Apr 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % N is the time series length
   N=length(x);
end

if nargin<2 | isempty(c)==1
   c=1;
else
   % c must be scalar
   if sum(size(c))>2
      error('c must be scalar.');
   end
   % c must be greater or equal than 1
   if c<1
      error('c must be greater or equal than 1.');
   end
end

for i=1:c

    % Make a random vector with indices up to n
    s1=randperm(N);

    % Shuffle the original x with respect to s1
    s(:,i)=x(s1);
end


%% brute_force_tune
% Code to test the performance of various tuning parameters
% Works sorta like RANSAC I guess?
% Adam Werries 2016, see Apache 2.0 license.

k_max = 50;
% Specify ranges
accel_bias_PSD = logspace(-10,-4,100);
gyro_bias_PSD = logspace(-10,-4,100);
% Repeat arrays
accel_bias_PSD = repmat(accel_bias_PSD, [1 k_max]);
gyro_bias_PSD = repmat(gyro_bias_PSD, [1 k_max]);
% Generate random selections of each vector
num_items = length(accel_bias_PSD);
accel_bias_i = randperm(num_items);
gyro_bias_i = randperm(num_items);

rms_error_filter = Inf*ones(1,num_items);
max_error_filter = Inf*ones(1,num_items);
parfor i = 1:num_items
    fprintf('Iteration: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));
    temp_conf = LC_KF_config;
    temp_conf.accel_bias_PSD = accel_bias_PSD(accel_bias_i(i));
    temp_conf.gyro_bias_PSD = gyro_bias_PSD(gyro_bias_i(i));
    [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
    xyz = out_profile(:,2:4);
    if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
        llh = ecef2lla(xyz);
        [x,y] = deg2utm(llh(:,1),llh(:,2));
        x = x-min_x;
        y = y-min_y;
%         h = -llh(:,3);
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2 + (ground_truth_full(:,3)-h).^2).^0.5;
        distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
        rms_error_filter(i) = rms(distance);
        max_error_filter(i) = max(distance);
    end
end

[minmax, i] = min(max_error_filter);
fprintf('\nBest max: %08.4f, rms is %08.4f\n', minmax, rms_error_filter(i));
fprintf('Best iteration for max: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));
[minrms, i] = min(rms_error_filter);
fprintf('Best rms: %08.4f, max is %08.4f\n', minrms, max_error_filter(i));
fprintf('Best iteration for rms: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));
[minrms, i] = min((rms_error_filter+max_error_filter)/2);
fprintf('Best average of RMS and max: %08.4f, rms is  %08.4f, max is %08.4f\n', minrms, rms_error_filter(i), max_error_filter(i));
fprintf('Best iteration for rms: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));


function [x,e,obj,err,iter] = tracelassoR(A,b,lambda,opts)

% Solve the trace Lasso regularized minimization problem by M-ADMM
%
% min_{x,e} loss(e)+lambda*||A*Diag(x)||_*, s.t. Ax+e=b
% loss(e) = ||e||_1 or 0.5*||e||_2^2
% ---------------------------------------------
% Input:
%       A       -    d*n matrix
%       b       -    d*1 vector
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(e) = ||e||_1
%                               'l2': loss(e) = 0.5*||e||_2^2
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       x       -    n*1 vector
%       e       -    d*1 vector
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 18/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,n] = size(A);
x = zeros(n,1);
Z = zeros(d,n);
e = zeros(d,1);
Y1 = e;
Y2 = Z;

Atb = A'*b;
AtA = A'*A;
invAtA = (AtA+diag(diag(AtA)))\eye(n);

iter = 0;
for iter = 1 : max_iter
    xk = x;
    ek = e;
    Zk = Z;
    % first super block {Z,e}
    [Z,nuclearnorm] = prox_nuclear(A*diag(x)-Y2/mu,lambda/mu);
    if strcmp(loss,'l1')
        e = prox_l1(b-A*x-Y1/mu,1/mu);
    elseif strcmp(loss,'l2')
        e = mu*(b-A*x-Y1/mu)/(1+mu);
    else
        error('not supported loss function');
    end
    % second super block {x}
    x = invAtA*(-A'*(Y1/mu+e)+Atb+diagAtB(A,Y2/mu+Z));
    dY1 = A*x+e-b;
    dY2 = Z-A*diag(x);
    chgx = max(abs(xk-x));
    chge = max(abs(ek-e));
    chgZ = max(max(abs(Zk-Z)));
    chg = max([chgx chge chgZ max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = comp_loss(e,loss)+lambda*nuclearnorm;
            err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = comp_loss(e,loss)+lambda*nuclearnorm;
err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);

function v = diagAtB(A,B)
% A, B - d*n matrices
% v = diag(A'*B), n*1 vector

n = size(A,2);
v = zeros(n,1);
for i = 1 : n
   v(i) = A(:,i)'*B(:,i);
end


clear all
close all

disp('Synthetic data from nonlinear ODE model');
disp('defined in Ramsay et al. (2007)');
disp('based on Van der Pol oscillator and which');
disp('reduces to Fitzhugh-Nagumo for certain parameters');

sigma_e=0.01;
[M,U] = mci_ramsay_struct(sigma_e);

% Use higher integration tolerances than by default
M.reltol=1e-3;
M.abstol=1e-5;

rand_init=0;
if rand_init
    P=spm_normrnd(M.vpE,M.pC,1);
else
    P=[log(0.2) log(0.2)]';
end

tic;
Y = mci_ramsay_gen(P,M,U);
toc

mci_plot_outputs(M,Y);

%mcmc.inference='vl';
%mcmc.inference='langevin';
%mcmc.maxits=16;
%mcmc.verbose=1;

mcmc.inference='ais';
mcmc.anneal='geometric';
mcmc.prop='lmc';
mcmc.nprop=1;
mcmc.J=16;
mcmc.maxits=8;
mcmc.rec_traj=1;

post = spm_mci_post (mcmc,M,U,Y,P);

if ~strcmp(mcmc.inference,'vl')
    spm_mci_diag(post);

    stats = spm_mci_mvnpost (post,'ESS')
    stats = spm_mci_mvnpost (post,'thinning')

    for j=1:length(P),
        spm_mci_quantiles (post,j,0);
    end
end

load ramsay-surface
figure
surf(S.x,S.y,L);
xlabel(S.name{1});
ylabel(S.name{2});

figure;
imagesc(S.pxy(1,:),S.pxy(2,:),L);
axis xy
hold on
xlabel(S.name{1});
ylabel(S.name{2});
hold on
ms=10;
plot(M.pE(1),M.pE(2),'wo','MarkerSize',ms);
plot(P(1),P(2),'w+','MarkerSize',ms);
if ~strcmp(mcmc.inference,'vl')
    j=post.ind;
    plot(post.P(1,j),post.P(2,j),'w.');
end
plot(post.Ep(1),post.Ep(2),'kx','MarkerSize',ms);

if strcmp(mcmc.inference,'ais')
    % plot individual trajectories
    figure
    xlim([-2 0]);
    ylim([-2 0]);
    hold on
    for i=1:mcmc.maxits,
        xx=squeeze(post.traj(i,1,:));
        yy=squeeze(post.traj(i,2,:));
        plot(xx,yy,'k-');
        plot(xx(end),yy(end),'kx','MarkerSize',ms);
        xlabel(S.name{1});
        ylabel(S.name{2});
        disp('Press space to see more trajectories');
        pause

    end
end