function value = r4_bide ( x )

%*****************************************************************************80
%
%% R4_BIDE: exponentially scaled derivative, Airy function Bi of an R4 argument.
%
%  Discussion:
%
%    if X < 0,
%      R4_BIDE ( X ) = R4_BID ( X )
%    else
%      R4_BIDE ( X ) = R4_BID ( X ) * exp ( - 2/3 * X^(3/2) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 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 derivative of
%    the Airy function Bi of X.
%
  persistent atr
  persistent bif2cs
  persistent bifcs
  persistent big2cs
  persistent bigcs
  persistent bip1cs
  persistent bip2cs
  persistent btr
  persistent nbif
  persistent nbif2
  persistent nbig
  persistent nbig2
  persistent nbip1
  persistent nbip2
  persistent x2sml
  persistent x3sml
  persistent x32sml
  persistent xbig

  atr =  8.7506905708484345;
  btr = -2.0938363213560543;

  if ( isempty ( nbif ) )

    bif2cs = [ ...
       0.323493987603522033521E+00, ...
       0.086297871535563559139E+00, ...
       0.002994025552655397426E+00, ...
       0.000051430528364661637E+00, ...
       0.000000525840250036811E+00, ...
       0.000000003561751373958E+00, ...
       0.000000000017146864007E+00, ...
       0.000000000000061663520E+00, ...
       0.000000000000000171911E+00, ...
       0.000000000000000000382E+00 ]';
    bifcs = [ ...
       0.1153536790828570243E+00, ...
       0.0205007894049192875E+00, ...
       0.0002135290278902876E+00, ...
       0.0000010783960614677E+00, ...
       0.0000000032094708833E+00, ...
       0.0000000000062930407E+00, ...
       0.0000000000000087403E+00, ...
       0.0000000000000000090E+00 ]';
    big2cs = [ ...
       1.6062999463621294578E+00, ...
       0.7449088819876088652E+00, ...
       0.0470138738610277380E+00, ...
       0.0012284422062548239E+00, ...
       0.0000173222412256624E+00, ...
       0.0000001521901652368E+00, ...
       0.0000000009113560249E+00, ...
       0.0000000000039547918E+00, ...
       0.0000000000000130017E+00, ...
       0.0000000000000000335E+00 ]';
    bigcs = [ ...
      -0.097196440416443537390E+00, ...
       0.149503576843167066571E+00, ...
       0.003113525387121326042E+00, ...
       0.000024708570579821297E+00, ...
       0.000000102949627731379E+00, ...
       0.000000000263970373987E+00, ...
       0.000000000000458279271E+00, ...
       0.000000000000000574283E+00, ...
       0.000000000000000000544E+00 ]';
    bip1cs = [ ...
      -0.1729187351079553719E+00, ...
      -0.0149358492984694364E+00, ...
      -0.0005471104951678566E+00, ...
       0.0001537966292958408E+00, ...
       0.0000154353476192179E+00, ...
      -0.0000065434113851906E+00, ...
       0.0000003728082407879E+00, ...
       0.0000002072078388189E+00, ...
      -0.0000000658173336470E+00, ...
       0.0000000074926746354E+00, ...
       0.0000000011101336884E+00, ...
      -0.0000000007265140553E+00, ...
       0.0000000001782723560E+00, ...
      -0.0000000000217346352E+00, ...
      -0.0000000000020302035E+00, ...
       0.0000000000019311827E+00, ...
      -0.0000000000006044953E+00, ...
       0.0000000000001209450E+00, ...
      -0.0000000000000125109E+00, ...
      -0.0000000000000019917E+00, ...
       0.0000000000000015154E+00, ...
      -0.0000000000000004977E+00, ...
       0.0000000000000001155E+00, ...
      -0.0000000000000000186E+00 ]';
    bip2cs = [ ...
      -0.13269705443526630495E+00, ...
      -0.00568443626045977481E+00, ...
      -0.00015643601119611610E+00, ...
      -0.00001136737203679562E+00, ...
      -0.00000143464350991284E+00, ...
      -0.00000018098531185164E+00, ...
       0.00000000926177343611E+00, ...
       0.00000001710005490721E+00, ...
       0.00000000476698163504E+00, ...
      -0.00000000035195022023E+00, ...
      -0.00000000058890614316E+00, ...
      -0.00000000006678499608E+00, ...
       0.00000000006395565102E+00, ...
       0.00000000001554529427E+00, ...
      -0.00000000000792397000E+00, ...
      -0.00000000000258326243E+00, ...
       0.00000000000121655048E+00, ...
       0.00000000000038707207E+00, ...
      -0.00000000000022487045E+00, ...
      -0.00000000000004953477E+00, ...
       0.00000000000004563782E+00, ...
       0.00000000000000332998E+00, ...
      -0.00000000000000921750E+00, ...
       0.00000000000000094157E+00, ...
       0.00000000000000167154E+00, ...
      -0.00000000000000055134E+00, ...
      -0.00000000000000022369E+00, ...
       0.00000000000000017487E+00, ...
       0.00000000000000000207E+00 ]';

    eta = 0.1 * r4_mach ( 3 );
    nbif = r4_inits ( bifcs, 8, eta );
    nbig = r4_inits ( bigcs, 9, eta );
    nbif2 = r4_inits ( bif2cs, 10, eta );
    nbig2 = r4_inits ( big2cs, 10, eta );
    nbip1 = r4_inits ( bip1cs, 24, eta );
    nbip2 = r4_inits ( bip2cs, 29, eta );
    x2sml = sqrt ( eta );
    x3sml = eta^0.3333;
    x32sml = 1.3104 * x3sml * x3sml;
    xbig = r4_mach ( 2 )^0.6666;

  end

  if ( x <= - 1.0 )
    [ xn, phi ] = r4_admp ( x );
    value = xn * sin ( phi );
  elseif ( 0.0 <= x && x <= x32sml )
    x2 = 0.0;
    x3 = 0.0;
    value = x2 * ( r4_csevl ( x3, bifcs, nbif ) + 0.25 ) ...
      + r4_csevl ( x3, bigcs, nbig ) + 0.5;
  elseif ( abs ( x ) <= x2sml )
    x2 = 0.0;
    x3 = 0.0;
    value = x2 * ( r4_csevl ( x3, bifcs, nbif ) + 0.25 ) ...
      + r4_csevl ( x3, bigcs, nbig ) + 0.5;
    value = value * exp ( - 2.0 * x * sqrt ( x ) / 3.0 );
  elseif ( x <= x3sml )
    x2 = x * x;
    x3 = 0.0;
    value = x2 * ( r4_csevl ( x3, bifcs, nbif ) + 0.25 ) ...
      + r4_csevl ( x3, bigcs, nbig ) + 0.5;
    value = value * exp ( - 2.0 * x * sqrt ( x ) / 3.0 );
  elseif ( x <= 1.0 )
    x2 = x * x;
    x3 = x * x * x;
    value = x2 * ( r4_csevl ( x3, bifcs, nbif ) + 0.25 ) ...
      + r4_csevl ( x3, bigcs, nbig ) + 0.5;
    value = value * exp ( - 2.0 * x * sqrt ( x ) / 3.0 );
  elseif ( x <= 2.0 )
    z = ( 2.0 * x * x * x - 9.0 ) / 7.0;
    value = exp ( - 2.0 * x * sqrt ( x ) / 3.0 ) ...
      * ( x * x * ( 0.25 + r4_csevl ( z, bif2cs, nbif2 ) ) ...
      + 0.5 + r4_csevl ( z, big2cs, nbig2 ) );
  elseif ( x <= 4.0 )
    sqrtx = sqrt ( x );
    z = atr / ( x * sqrtx ) + btr;
    value = ( 0.625 ...
      + r4_csevl ( z, bip1cs, nbip1 ) ) * sqrt ( sqrtx );
  elseif ( x < xbig )
    sqrtx = sqrt ( x );
    z = 16.0 / ( x * sqrtx ) - 1.0;
    value = ( 0.625 + r4_csevl ( z, bip2cs, nbip2 ) ) ...
      * sqrt ( sqrtx );
  else
    sqrtx = sqrt ( x );
    z = - 1.0;
    value = ( 0.625 + r4_csevl ( z, bip2cs, nbip2 ) ) * sqrt ( sqrtx );
  end

  return
end


function output = bicubic_interpolation_at(input,uu,vv,nx,ny,border_out,BOUNDARY_CONDITION)
output = 0.0;
sx = 0;
sy = 0;
if(uu < 0)
    sx = -1;
else
    sx = 1;
end
if(vv < 0)
    sy = -1;
else
    sy = 1;
end
out = 0;

switch(BOUNDARY_CONDITION)
    case 0
          [out,x]   = neumann_bc(uu, nx);
         [out,y]   = neumann_bc(vv, ny);
         [out,mx]  = neumann_bc(uu - sx, nx);
         [out,my]  = neumann_bc(vv - sx, ny);
         [out,dx]  = neumann_bc( uu + sx, nx);
         [out,dy]  = neumann_bc( vv + sy, ny);
         [out,ddx] = neumann_bc( uu + 2*sx, nx);
         [out,ddy] = neumann_bc( vv + 2*sy, ny);
    case 1
        [out,x]   =periodic_bc(uu, nx);
         [out,y]   = periodic_bc(vv, ny);
         [out,mx]  = periodic_bc(uu - sx, nx);
         [out,my]  = periodic_bc(vv - sx, ny);
         [out,dx]  = periodic_bc( uu + sx, nx);
         [out,dy]  = periodic_bc( vv + sy, ny);
         [out,ddx] = periodic_bc( uu + 2*sx, nx);
         [out,ddy] = periodic_bc( vv + 2*sy, ny);
    case 2
         [out,x]  = symmetric_bc(uu, nx);
         [out,y]   = symmetric_bc(vv, ny);
         [out,mx]  = symmetric_bc(uu - sx, nx);
         [out,my]  = symmetric_bc(vv - sx, ny);
         [out,dx]  = symmetric_bc( uu + sx, nx);
         [out,dy]  = symmetric_bc( vv + sy, ny);
         [out,ddx] = symmetric_bc( uu + 2*sx, nx);
         [out,ddy] = symmetric_bc( vv + 2*sy, ny);
    otherwise
        [out,x]   = neumann_bc(uu, nx);
         [out,y]   = neumann_bc(vv, ny);
         [out,mx]  = neumann_bc(uu - sx, nx);
         [out,my]  = neumann_bc(vv - sx, ny);
         [out,dx]  = neumann_bc( uu + sx, nx);
         [out,dy]  = neumann_bc( vv + sy, ny);
         [out,ddx] = neumann_bc( uu + 2*sx, nx);
         [out,ddy] = neumann_bc( vv + 2*sy, ny);
         if((out == 1) && (border_out == 0))

             output = 0.0;
         else
            p11 = input(mx  + nx * my);
         p12 = input(x   + nx * my);
          p13 = input(dx  + nx * my);
          p14 = input(ddx + nx * my);

          p21 = input(mx  + nx * y);
          p22 = input(x   + nx * y);
          p23 = input(dx  + nx * y);
          p24 = input(ddx + nx * y);

          p31 = input(mx  + nx * dy);
          p32 = input(x   + nx * dy);
          p33 = input(dx  + nx * dy);
          p34 = input(ddx + nx * dy);

          p41 = input(mx  + nx * ddy);
          p42 = input(x   + nx * ddy);
          p43 = input(dx  + nx * ddy);
          p44 = input(ddx + nx * ddy);
             pol = [p11, p21, p31, p41;
            p12, p22, p32, p42;
            p13, p23, p33, p43;
            p14, p24, p34, p44];
      output =   bicubic_interpolation_cell(pol, uu-x, vv-y);
         end

end


function x = strmv ( uplo, trans, diag, n, a, lda, x, incx )

%*****************************************************************************80
%
%% STRMV computes x: = A*x or x = A'*x for a triangular matrix A.
%
%  Discussion:
%
%    STRMV performs one of the matrix-vector operations
%
%      x := A*x,   or   x := A'*x,
%
%    where x is an n element vector and  A is an n by n unit, or non-unit,
%    upper or lower triangular matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 April 2014
%
%  Author:
%
%    This MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, character UPLO, specifies whether the matrix is an upper or
%    lower triangular matrix as follows:
%    'u' or 'U': A is an upper triangular matrix.
%    'l' or 'L': A is a lower triangular matrix.
%
%    Input, character TRANS, specifies the operation to be performed as
%    follows:
%    'n' or 'N': x := A*x.
%    't' or 'T': x := A'*x.
%    'c' or 'C': x := A'*x.
%
%    Input, character DIAG, specifies whether or not A is unit
%    triangular as follows:
%    'u' or 'U': A is assumed to be unit triangular.
%    'n' or 'N': A is not assumed to be unit triangular.
%
%    Input, integer N, the order of the matrix A.
%    0 <= N.
%
%    Input, real A(LDA,N).
%    Before entry with  UPLO = 'u' or 'U', the leading n by n
%    upper triangular part of the array A must contain the upper
%    triangular matrix and the strictly lower triangular part of
%    A is not referenced.
%    Before entry with UPLO = 'l' or 'L', the leading n by n
%    lower triangular part of the array A must contain the lower
%    triangular matrix and the strictly upper triangular part of
%    A is not referenced.
%    Note that when  DIAG = 'u' or 'U', the diagonal elements of
%    A are not referenced either, but are assumed to be unity.
%
%    Input, integer LDA, the first dimension of A as declared
%    in the calling program. max ( 1, N ) <= LDA.
%
%    Input/output, real X(1+(N-1)*abs( INCX)).
%    Before entry, the incremented array X must contain the n
%    element vector x. On exit, X is overwritten with the
%    tranformed vector x.
%
%    Input, integer INCX, the increment for the elements of
%    X.  INCX must not be zero.
%

%
%  Test the input parameters.
%
  info = 0;
  if  ( ~ lsame ( uplo , 'U' ) && ~ lsame ( uplo , 'L' ) )
    info = 1;
  elseif ( ~ lsame ( trans, 'N' ) && ~ lsame ( trans, 'T' ) && ...
            ~ lsame ( trans, 'C' ) )
    info = 2;
  elseif ( ~ lsame ( diag , 'U' ) && ~ lsame ( diag , 'N' ) )
    info = 3;
  elseif ( n < 0 )
    info = 4;
  elseif ( lda < max ( 1, n ) )
    info = 6;
  elseif ( incx == 0 )
    info = 8;
  end

  if ( info ~= 0 )
    xerbla ( 'DTRMV', info );
    return
  end
%
%  Quick return if possible.
%
  if ( n == 0 )
    return
  end

  nounit = lsame ( diag, 'N' );
%
%  Set up the start point in X if the increment is not unity. This
%  will be  ( N - 1 ) * INCX  too small for descending loops.
%
  if ( incx <= 0 )
    kx = 1 - ( n - 1 ) * incx;
  elseif ( incx ~= 1 )
    kx = 1;
  end
%
%  Start the operations. In this version the elements of A are
%  accessed sequentially with one pass through A.
%
  if ( lsame ( trans, 'N' ) )
%
%  Form x := A*x.
%
    if ( lsame ( uplo, 'U' ) )
      if ( incx == 1 )
        for j = 1 : n
          if ( x(j) ~= 0.0 )
            temp = x(j);
            for i = 1 : j - 1
              x(i) = x(i) + temp * a(i,j);
            end
            if ( nounit )
              x(j) = x(j) * a(j,j);
            end
          end
        end
      else
        jx = kx;
        for j = 1 : n
          if ( x(jx) ~= 0.0 )
            temp = x(jx);
            ix = kx;
            for i = 1 : j - 1
              x(ix) = x(ix) + temp * a(i,j);
              ix = ix + incx;
            end
            if ( nounit )
              x(jx) = x(jx) * a(j,j);
            end
          end
          jx = jx + incx;
        end
      end
    else
      if ( incx == 1 )
        for j = n : -1 : 1
          if ( x(j) ~= 0.0 )
            temp = x(j);
            for i = n : -1 : j + 1
              x(i) = x(i) + temp * a(i,j);
            end
            if ( nounit )
              x(j) = x(j) * a(j,j);
            end
          end
        end
      else
        kx = kx + ( n - 1 ) * incx;
        jx = kx;
        for j = n : -1 : 1
          if ( x(jx) ~= 0.0 )
            temp = x(jx);
            ix = kx;
            for i = n : -1 : j + 1
              x(ix) = x(ix) + temp * a(i,j);
              ix = ix - incx;
            end
            if ( nounit )
              x(jx) = x(jx) * a(j,j);
            end
          end
          jx = jx - incx;
        end
      end
    end
  else
%
%  Form x := A'*x.
%
    if ( lsame ( uplo, 'U' ) )
      if ( incx == 1 )
        for j = n : -1 : 1
          temp = x(j);
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j - 1 : -1 : 1
            temp = temp + a(i,j) * x(i);
          end
          x(j) = temp;
        end
      else
        jx = kx + ( n - 1 ) * incx;
        for j = n : -1 : 1
          temp = x(jx);
          ix = jx;
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j - 1 : -1 : 1
            ix = ix   - incx;
            temp = temp + a(i,j) * x(ix);
          end
          x(jx) = temp;
          jx = jx - incx;
        end
      end
    else
      if ( incx == 1 )
        for j = 1 : n
          temp = x(j);
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j + 1 : n
            temp = temp + a(i,j) * x(i);
          end
          x(j) = temp;
        end
      else
        jx = kx;
        for j = 1 : n
          temp = x(jx);
          ix = jx;
          if ( nounit )
            temp = temp * a(j,j);
          end
          for i = j + 1 : n
            ix = ix + incx;
            temp = temp + a(i,j) * x(ix);
          end
          x(jx) = temp;
          jx = jx + incx;
        end
      end
    end
  end

  return
end


function [K] = ku1u1(x, y, xp, yp, hyp, i)

logsigma = hyp(1);
logthetax = hyp(2);
logthetay = hyp(3);

n_x = size(x,1);
n_y = size(y,1);
n_xp = size(xp,1);
n_yp = size(yp,1);

x = repmat(x,1,n_xp);
y = repmat(y,1,n_yp);
xp = repmat(xp',n_x,1);
yp = repmat(yp',n_y,1);

switch i

case 0

K=exp(1).^(logsigma+(-2).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^logthetay+(-1).*(y+(-1).*yp).^2);

case 1 % logsigma

K=exp(1).^(logsigma+(-2).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^logthetay+(-1).*(y+(-1).*yp).^2);

case 2 % logthetax

K=(1/2).*exp(1).^(logsigma+(-1).*logthetax+(-2).*logthetay+(-1/2).*exp(1) ...
  .^((-1).*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).* ...
  (y+(-1).*yp).^2).*(x+(-1).*xp).^2.*(exp(1).^logthetay+(-1).*(y+(-1).*yp) ...
  .^2);

case 3 % logthetay

K=(-1/2).*exp(1).^(logsigma+(-3).*logthetay+(-1/2).*exp(1).^((-1).* ...
  logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).* ...
  yp).^2).*(2.*exp(1).^(2.*logthetay)+(-5).*exp(1).^logthetay.*(y+(-1).* ...
  yp).^2+(y+(-1).*yp).^4);

otherwise

        K = zeros(n_x, n_xp);
end

end


function [sCodebook, settings] = gmlvq(sCodebook, sData, varargin)

%GMLVQ trains a codebook using the Generalized Matrix LVQ (GMLVQ)
%algorithm.
%
% sM = gmlvq(sM, sD, [argID, value, ...])
%
%  sM = gmlvq(sM, sD)
%  sM = gmlvq(sM, sD, 'learningRatePrototypes', [0.01 0.001], 'relevanceStart', 10);
%
% Required Input Arguments:
%   sM                     (struct) map struct containing the labels as the
%                                   first column of .labels
%   sD                     (struct) data struct containing the labels as
%                                   the first column of .labels
%
% Optional Input Arguments:
%   PrototypesPerClass     (vector) number of prototypes per class used;
%                                   a number or a vector with a number
%                                   for each class (default=1)
%   initialMatrix          (matrix) initial Matrix omega
%   regularization         (scalar) regularization for relevances
%   dim                    (scalar) maximum rank or projection dimension
%   comparable             (scalar) a flag which resets the random
%                                   generator to produce comparable results
%                                   if set to 1
%   optimization           (string) choice for the optimization technique:
%                                   sgd or fminlbfgs (default=fminlbfgs)
%  Parameter for the stochastic gradient descent sgd:
%   nb_epochs              (scalar) the number of epochs (default=100)
%   learningRatePrototypes (vector) learning rate for the prototypes; could
%                                   be the start and end value or a vector
%                                   of length nb_epochs
%   learningRateMatrix     (vector) learning rate for the relevance matrix;
%                                   could be the start and end value or a
%                                   vector of length nb_epochs
%   MatrixStart            (scalar) epoch to start the matrix learning
%                                   (default=1)
%  Parameter for the build-in function fminlbfgs:
%   threshstop             (scalar) the training error for early stopping
%                                   (default=0)
%   nb_reiterations        (scalar) the number of optimization reiterations
%                                   performed (default=100)
%   useEarlyStopping       (scalar) use early stopping based on threshstop
%                                   (default=1)
%   Display                (string) the optimization output 'iter' or 'off'
%                                   (default='off')
%   GradObj                (string) turn the usage of gradient information
%                                   on or off (default='on')
%   HessUpdate             (string) the update can be 'lbfgs', 'bfgs' or
%                                   'steepdesc' (default='lbfgs')
%   TolFun                 (scalar) the tolerance (default=1e-6)
%   MaxIter                (scalar) the maximal number of iterations
%                                   (default=2500)
%   MaxFunEvals                     the maximal number of function
%                                   evaluations (default=1000000)
%   TolX                   (scalar) tolerance on the minimum
%                                   (default=1e-10)
%   DiffMinChange          (scalar) minimal change (default=1e-10)
%
% Output Arguments:
%   sM                     (struct) map struct containing the optimized
%                                   prototypes and the decomposed
%                                   relevances omega
%   settings               (struct) information on the settings of the
%                                   algorithm
%
% NOTE: does not take the vector mask into account but trains a relevance
%       matrix.
%
% For more help, try 'type gmlvq', or check out the online documentation.
% See also GRLVQ, GMLVQ_CORE, LVQ3, LVQ1.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% gmlvq
%
% PURPOSE
%
% Trains a codebook and a global metric with the Generalized Matrix LVQ
% (GMLVQ) algorithm.
%
% SYNTAX
%
%   sM = gmlvq(sM, sD)
%   sM = gmlvq(sM, sD, 'learningRatePrototypes', [0.01 0.001], 'MatrixStart', 10);
%   sM = gmlvq(sM, sD, argID, value, ...);
%
% DESCRIPTION
%
% GMLVQ constitutes an enhancement of the LVQ algorithm by minimizing an
% explicit error function and optimizing the metric of the space where the
% computation is performed. Distances are calculated by
%   d(x,y) = (x - y) * lambda * (x - y)', where lambda = omega' * omega.
% The matrix omega is optimized by the algorithm.
%
% Two optimization techniques are implemented:
%     - stochastic gradient descent (sgd)
%     - limited memory Quasi Newton Broyden-Fletcher-Goldfarb-Shanno
%       (L-BFGS)
%
% Classification is performed similarly to standard LVQ, with the only
% difference that the above specified formula is applied for the
% computation of distances.
%
% This function provides an interface in the style of the som-toolbox and
% is basically a wrapper for the method gmlvq_core. It deals with structs
% rather then directly with data matrices.
%
% REFERENCES
%
%   Petra Schneider, Michael Biehl, Barbara Hammer: Adaptive Relevance
%   Matrices in Learning Vector Quantization. Neural Computation 21(12):
%   3532-3561 (2009).
%
%   K. Bunte, P. Schneider, B. Hammer, F.-M. Schleif, T. Villmann and M.
%   Biehl: Limited Rank Matrix Learning - Discriminative Dimension
%   Reduction and Visualization, Neural Networks, vol. 26, nb. 4,
%   pp. 159-173, 2012.
%
%   P. Schneider, K. Bunte, B. Hammer and M. Biehl: Regularization in
%   Matrix Relevance Learning, IEEE Transactions on Neural Networks, vol.
%   21, nb. 5, pp. 831-840, 2010.
%
% REQUIRED INPUT ARGUMENTS
%   sM                     (struct) map struct containing the initialized
%                                   prototype vectors as rows of the
%                                   .codebooks matrix and the labels as the
%                                   first column of .labels
%   sD                     (struct) data struct containing the data vectors
%                                   as rows of .data and the labels as the
%                                   first column of .labels
%
% OPTIONAL INPUT ARGUMENTS
%   PrototypesPerClass     (vector) number of prototypes per class used;
%                                   a number or a vector with a number
%                                   for each class (default=1)
%   initialMatrix          (matrix) initial Matrix omega
%                                   (default: identity matrix)
%   regularization         (scalar) regularization for relevances
%                                   (detault=0)
%   dim                    (scalar) maximum rank or projection dimension,
%                                   i.e. of the matrix omega
%                                   (default=nb of features for training)
%   comparable             (scalar) a flag which resets the random
%                                   generator to produce comparable results
%                                   if set to 1 (default=0)
%   optimization           (string) choice for the optimization technique:
%                                   sgd (stochastic gradient descent) or
%                                   fminlbfgs (Limited memory Quasi Newton
%                                   Broyden-Fletcher-Goldfarb-Shanno)
%                                   (default=fminlbfgs)
%  Parameter for the stochastic gradient descent sgd:
%   nb_epochs              (scalar) the number of epochs (default=100)
%   learningRatePrototypes (vector) learning rate for the prototypes; could
%                                   be the value for the first and the last
%                                   epoch or a vector of length nb_epochs
%   learningRateMatrix     (vector) learning rate for the relevance matrix;
%                                   could be the value for the first and
%                                   the last epoch or a vector of length
%                                   nb_epochs
%   MatrixStart            (scalar) epoch to start the matrix learning
%                                   (default=1)
%  Parameter for the build-in function fminlbfgs:
%   threshstop             (scalar) the training error for early stopping
%                                   (default=0)
%   nb_reiterations        (scalar) the number of optimization reiterations
%                                   performed (default=100)
%   useEarlyStopping       (scalar) use early stopping based on threshstop
%                                   (default=1)
%   Display                (string) the optimization output 'iter' or 'off'
%                                   (default='off')
%   GradObj                (string) turn the usage of gradient information
%                                   on or off (default='on')
%   HessUpdate             (string) the update can be 'lbfgs', 'bfgs' or
%                                   'steepdesc' (default=lbfgs)
%   TolFun                 (scalar) the tolerance (default=1e-6)
%   MaxIter                (scalar) the maximal number of iterations
%                                   (default=2500)
%   MaxFunEvals                     the maximal number of function
%                                   evaluations (default=1000000)
%   TolX                   (scalar) tolerance on the minimum
%                                   (default=1e-10)
%   DiffMinChange          (scalar) minimal change (default=1e-10)
%
% OUTPUT ARGUMENTS
%   sM                     (struct) map struct containing the optimized
%                                   prototypes and the decomposed
%                                   relevances omega
%   settings               (struct) information on the settings of the
%                                   algorithm
%
% EXAMPLES
%
%   lab   = unique(sD.labels(:,1));           % different classes
%   nProt = length(lab)*5;                    % 5 prototypes for each
%   sM = som_randinit(sD,'msize',[nProt 1]);  % initial prototypes
%   sM.labels = [lab;lab;lab;lab;lab];        % and their classes
%   sM = gmlvq(sM,sD);                        % use GMLVQ to adjust the
%                                             % prototypes and the metric
%
% SEE ALSO
%
%   grlvq         Use the GRLVQ algorithm for training.
%   gmlvq_core    Access the GMLVQ functionality without using structs.
%   lvq3          Use LVQ3 algorithm for training.
%   lvq1          Use LVQ1 algorithm for training.

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% Copyright (c) Alexander Schulz
% http://www.cis.hut.fi/projects/somtoolbox/

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

cods      = sCodebook.codebook;
codLabels = sCodebook.labels;
data      = sData.data;
labels    = sData.labels;

% convert labels from cell to a vector
labelsV    = som_label2num(labels);
codLabelsV = som_label2num(codLabels);

% calculate the number of prototypes per class
n_prot_per_class = zeros(1, max(codLabelsV));
for i=1:max(codLabelsV)
    n_prot_per_class(i) = sum(codLabelsV == i);
end

% call the main function
[model, settings]  = gmlvq_core(data, labelsV, 'initialPrototypes', ...
    [cods, codLabelsV], 'PrototypesPerClass', n_prot_per_class, varargin{:});

% write the results in the output struct
sCodebook.codebook = model.w;
sCodebook.omega    = model.omega;
%sCodebook.lambda   = sCodebook.omega'*sCodebook.omega;

if strcmp(settings.optimization, 'sgd')
    trainlen   = settings.nb_epochs;
    alpha_ini  = settings.learningRatePrototypes(1);
    alpha_type = 'power';
else
    trainlen   = NaN;
    alpha_ini  = NaN;
    alpha_type = '';
end

sTrain = som_set('som_train','algorithm','GMLVQ',...
		 'data_name',sData.name,...
		 'neigh','',...
		 'mask',ones(size(model.w,2),1),...
		 'radius_ini',NaN,...
		 'radius_fin',NaN,...
		 'alpha_ini',alpha_ini,...
		 'alpha_type',alpha_type,...
		 'trainlen',trainlen,...
		 'time',datestr(now,0));
sCodebook.trainhist(end+1) = sTrain;


function [data] = iHOSVD(core, U)

site = size(core);
data = core;

for i=1:ndims(core)
    [m n] = size(U{i});
    site(i) = m;
    data = folding(U{i}*unfolding(data,i), i, site);
end


function [ r, z, rho, c, s, info ] = dchdd ( r, ldr, p, x, z, ldz, nz, y, rho )

%*****************************************************************************80
%
%% DCHDD downdates an augmented Cholesky decomposition.
%
%  Discussion:
%
%    DCHDD can also downdate the triangular factor of an augmented QR
%    decomposition.
%
%    Specifically, given an upper triangular matrix R of order P, a
%    row vector X, a column vector Z, and a scalar Y, DCHDD
%    determines an orthogonal matrix U and a scalar ZETA such that
%
%          (R   Z )     (RR  ZZ)
%      U * (      )  =  (      ),
%          (0 ZETA)     ( X   Y)
%
%    where RR is upper triangular.
%
%    If R and Z have been obtained from the factorization of a least squares
%    problem, then RR and ZZ are the factors corresponding to the problem
%    with the observation (X,Y) removed.  In this case, if RHO
%    is the norm of the residual vector, then the norm of
%    the residual vector of the downdated problem is
%    sqrt ( RHO * RHO - ZETA * ZETA ). DCHDD will simultaneously downdate
%    several triplets (Z, Y, RHO) along with R.
%
%    For a less terse description of what DCHDD does and how
%    it may be applied, see the LINPACK guide.
%
%    The matrix U is determined as the product U(1)*...*U(P)
%    where U(I) is a rotation in the (P+1,I)-plane of the form
%
%      ( C(I)      -S(I)    )
%      (                    ).
%      ( S(I)       C(I)    )
%
%    The rotations are chosen so that C(I) is real.
%
%    The user is warned that a given downdating problem may be impossible
%    to accomplish or may produce inaccurate results.  For example, this
%    can happen if X is near a vector whose removal will reduce the
%    rank of R.  Beware.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real R(LDR,P), the upper triangular matrix that
%    is to be downdated.  The part of R below the diagonal is not referenced.
%
%    Input, integer LDR, the leading dimension of the array R.
%    LDR must be at least P.
%
%    Input, integer P, the order of the matrix R.
%
%    Input, real X(P), the row vector that is to be removed from R.
%
%    Input, real Z(LDZ,NZ), an array of NZ P-vectors
%    which are to be downdated along with R.
%
%    Input, integer LDZ, the leading dimension of the array Z.
%    LDZ must be at least P.
%
%    Input, integer NZ, the number of vectors to be downdated.
%    NZ may be zero, in which case Z, Y, and RHO are not referenced.
%
%    Input, real Y(NZ), the scalars for the downdating of
%    the vectors Z.
%
%    Input, real RHO(NZ), the norms of the residual vectors.
%
%    Output, real R(LDR,P), the downdated upper triangular matrix.
%
%    Output, real Z(LDZ,NZ), the downdated vectors.
%
%    Output, real RHO(NZ), the norms of the residual vectors,
%    which have been changed along with R and Z.
%
%    Output, real C(P), S(P), the cosines and sines of the
%    transforming rotations.
%
%    Output, integer INFO, return flag.
%     0, the entire downdating was successful.
%    -1, if R could not be downdated.  In this case, all quantities
%        are left unaltered.
%     1, if some RHO could not be downdated.  The offending RHO's are
%        set to -1.
%

%
%  Solve R' * A = X, placing the result in the array S.
%
  info = 0;
  s(1) = x(1) / r(1,1);

  for j = 2 : p
    s(j) = x(j) - ddot ( j-1, r(1:j-1,j), 1, s(1:j-1), 1 );
    s(j) = s(j) / r(j,j);
  end

  norm = dnrm2 ( p, s(1:p), 1 );

  if ( 1.0 <= norm )
    info = -1;
    return
  end

  alpha = sqrt ( 1.0 - norm * norm );
%
%  Determine the transformations.
%
  for ii = 1 : p
    i = p - ii + 1;
    scale = alpha + abs ( s(i) );
    a = alpha / scale;
    b = s(i) / scale;
    norm = sqrt ( a * a + b * b );
    c(i) = a / norm;
    s(i) = b / norm;
    alpha = scale * norm;
  end
%
%  Apply the transformations to R.
%
  for j = 1 : p
    xx = 0.0;
    for ii = 1 : j
      i = j - ii + 1;
      t = c(i) * xx + s(i) * r(i,j);
      r(i,j) = c(i) * r(i,j) - s(i) * xx;
      xx = t;
    end
  end
%
%  If required, downdate Z and RHO.
%
  for j = 1 : nz

    zeta = y(j);
    for i = 1 : p
      z(i,j) = ( z(i,j) - s(i) * zeta ) / c(i);
      zeta = c(i) * zeta - s(i) * z(i,j);
    end

    azeta = abs ( zeta );

    if ( rho(j) < azeta )
      info = 1;
      rho(j) = -1.0;
    else
      rho(j) = rho(j) * sqrt ( 1.0 - ( azeta / rho(j) )^2 );
    end

  end

  return
end


%This Matlab script can be used to reproduce Figure 5.13 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Load SE simulation data, generated using the code from Section 4
load section5_Mvarying_K10_20;

%Number of UEs per BS
k_index = 2; %Selecting K = 10 from the loaded SE results
K = Krange(k_index);

%Fractions of data samples used for UL and DL
ULfraction = 1/3;
DLfraction = 2/3;

%Compute joint UL/DL sum SE using the fractions of UL/DL data
sumSE_MMMSE = ULfraction*sumSE_MMMSE_UL(:,k_index) + DLfraction*sumSE_MMMSE_DL(:,k_index);
sumSE_SMMSE = ULfraction*sumSE_SMMSE_UL(:,k_index) + DLfraction*sumSE_SMMSE_DL(:,k_index);
sumSE_RZF = ULfraction*sumSE_RZF_UL(:,k_index) + DLfraction*sumSE_RZF_DL(:,k_index);
sumSE_ZF = ULfraction*sumSE_ZF_UL(:,k_index) + DLfraction*sumSE_ZF_DL(:,k_index);
sumSE_MR = ULfraction*sumSE_MR_UL(:,k_index) + DLfraction*sumSE_MR_DL(:,k_index);

%Number of BSs
L = 16;

%Communication bandwidth
B = 20e6;

%PA efficiency UEs and BSs
mu_UE = 0.4;
mu_BS = 0.5;

%Define the pilot reuse factor
f = 1;

%Select length of coherence block
tau_c = 200;

%Compute length of pilot sequences
tau_p = f*K;

%Transmit power per UE in W
p = 0.1;

%Compute total effective transmit power
ETP_total =  K*p*(tau_p/mu_UE + (tau_c-tau_p)*(ULfraction/mu_UE + DLfraction/mu_BS))/tau_c;

%% Go through the two value sets of the CP model
for valueset = 1:2

    %Compute the total CP with different schemes
    [P_MR,P_RZF,P_MMMSE,P_ZF,P_SMMSE] = functionCPcomputation(Mrange,K,L,B,tau_c,tau_p,valueset,sumSE_MR,sumSE_RZF,sumSE_MMMSE,sumSE_ZF,sumSE_SMMSE);

    %Compute EE with M-MMSE
    EE_MMMSE = (B*sumSE_MMMSE)./(ETP_total + P_MMMSE);

    %Compute EE with S-MMSE
    EE_SMMSE = (B*sumSE_SMMSE)./(ETP_total + P_SMMSE);

    %Compute EE with RZF
    EE_RZF = (B*sumSE_RZF)./(ETP_total + P_RZF);

    %Compute EE with ZF
    EE_ZF = (B*sumSE_ZF)./(ETP_total + P_ZF);

    %Compute EE with MR
    EE_MR = (B*sumSE_MR)./(ETP_total + P_MR);

    %Plot simulation results
    figure;

    plot(B*sumSE_MMMSE/10^6,EE_MMMSE/10^6, 'rd-','LineWidth',1);hold on;
    plot(B*sumSE_SMMSE/10^6,EE_SMMSE/10^6,'b:','LineWidth',1);
    plot(B*sumSE_RZF/10^6,EE_RZF/10^6,'k-.','LineWidth',1);
    plot(B*sumSE_ZF/10^6,EE_ZF/10^6,'r--','LineWidth',1);
    plot(B*sumSE_MR/10^6,EE_MR/10^6,'bs-','LineWidth',1);

    xlabel('Throughput [Mbit/s/cell]');
    ylabel('EE [Mbit/Joule/cell]');
    legend('M-MMSE','S-MMSE','RZF','ZF','MR','Location','NorthWest');

end


% SP_PRECOMPUTE: compute the basis functions in the whole mesh. The output
%  can consume a lot of memory. Use with care.
%
%     space = sp_precompute (space, msh);
%     space = sp_precompute (space, msh, 'option', value);
%
% INPUT:
%
%    space: object representing the discrete function space (see sp_vector)
%    msh: mesh object containing the quadrature information (see msh_cartesian)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            divergence |      false      |  compute shape_function_divs
%            curl       |      false      |  compute shape_function_curls
%
% OUTPUT:
%
%    space: object containing the information of the input object, plus the
%            fields of the old structure, that are listed below. If no option
%            is given all the fields are computed. If an option is given,
%            only the selected fields will be computed.
%
%    FIELD_NAME      (SIZE)                               DESCRIPTION
%    ncomp           (scalar)                              number of components of the functions of the space
%    ndof            (scalar)                              total number of degrees of freedom
%    ndof_dir        (ncomp x ndim matrix)                 for each component, number of degrees of freedom along each direction
%    nsh_max         (scalar)                              maximum number of shape functions per element
%    nsh             (1 x msh.nel vector)                  actual number of shape functions per each element
%    connectivity    (nsh_max x msh.nel vector)            indices of basis functions that do not vanish in each element
%    shape_functions (ncomp x msh.nqn x nsh_max x msh.nel) basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%             (ncomp x rdim x msh.nqn x nsh_max x msh.nel) basis function gradients evaluated at each quadrature node in each element
%    shape_function_divs (msh.nqn x nsh_max x msh.nel)     basis function divergence evaluated at each quadrature node in each element
%    shape_function_curls
%         2D:  (msh.nqn x nsh_max x msh.nel)               basis function curl evaluated at each quadrature node in each element
%         3D:  (3 x msh.nqn x nsh_max x msh.nel)
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2015, 2019 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function sp_out = sp_precompute (sp, msh, varargin)

value = true;
gradient = false;
divergence = false;
curl = false;
hessian = false;

if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_precompute: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'curl'))
      curl = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'divergence'))
      divergence = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

if (~isstruct (msh))
  msh = msh_precompute (msh);
end

grad_param = gradient || divergence || curl || hessian;
value_param = value || grad_param;
div_param = false; curl_param = false;
switch (lower (sp.transform))
  case {'curl-preserving'}
    curl_param = curl;
  case {'div-preserving'}
    div_param = divergence;
end

sp_out = sp_precompute_param (sp, msh, 'value', value_param, 'gradient', grad_param, 'divergence', div_param, 'curl', curl_param, 'hessian', hessian);

switch (lower (sp.transform))
  case {'grad-preserving'}
    sp_out = sp_vector_grad_preserving_transform (sp_out, msh, value, gradient, curl, divergence, hessian);
  case {'curl-preserving'}
    sp_out = sp_vector_curl_preserving_transform (sp_out, msh, value, curl);
    if (gradient || divergence || hessian)
      warning ('Gradient, divergence and hessian not implemented for curl-preserving transformation')
    end
  case {'div-preserving'}
    sp_out = sp_vector_div_preserving_transform (sp_out, msh, value, gradient, curl, divergence);
    if (hessian)
      warning ('Hessian not implemented for div-preserving transformation')
    end
end

end


% PTrc2sp()

% from emuflight

function [RCcommand_dps] = PTrc2sp(RCcommand, rateSystem)
%function [RCcommand_dps] = PTrc2sp(RCcommand)
%   converts rc command to deg/s

BetaflightRates = 1;
BFActualRates = 2;

EmuflightRates = 2;

    rcCommandf    = RCcommand / 500.0;
    rcCommandfAbs = abs(rcCommandf);

          %  if rc_expo
                expof = rc_expo / 100;
                rcCommandf = rcCommandf * Math.pow(rcCommandfAbs, RC_EXPO_POWER) * expof + rcCommandf * (1-expof);
          %  end

            rcRate = rc_rates / 100.0;
          %  if (rcRate > 2.0)
                rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0);
          %  end

            angleRate = 200.0 * rcRate * rcCommandf;
         %   if rates
                rcSuperfactor = 1.0 / ((1.0 - (rcCommandfAbs * rates / 100.0)));
                angleRate = angleRate * rcSuperfactor;
         %   end

 return calculateSetpointRate(axis, value);

%%%%% From betaflight configurator RC3

 const minRc = 1000;
const midRc = 1500;
const maxRc = 2000;

const RateCurve = function (useLegacyCurve) {
    this.useLegacyCurve = useLegacyCurve;
    this.maxAngularVel = null;

    this.constrain = function (value, min, max) {
        return Math.max(min, Math.min(value, max));
    };

    this.rcCommand = function (rcData, rcRate, deadband) {
        const tmp = Math.min(Math.max(Math.abs(rcData - midRc) - deadband, 0), 500);

        let result = tmp * rcRate;

        if (rcData < midRc) {
            result = -result;
        }

        return result;
    };

    this.getBetaflightRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo, superExpoActive, limit) {
        let angularVel;

        if (rcRate > 2) {
            rcRate = rcRate + (rcRate - 2) * 14.54;
        }

        let expoPower;
        let rcRateConstant;

        if (semver.gte(FC.CONFIG.apiVersion, "1.20.0")) {
            expoPower = 3;
            rcRateConstant = 200;
        } else {
            expoPower = 2;
            rcRateConstant = 205.85;
        }

        if (rcExpo > 0) {
            rcCommandf =  rcCommandf * Math.pow(rcCommandfAbs, expoPower) * rcExpo + rcCommandf * (1-rcExpo);
        }

        if (superExpoActive) {
            const rcFactor = 1 / this.constrain(1 - rcCommandfAbs * rate, 0.01, 1);
            angularVel = rcRateConstant * rcRate * rcCommandf; // 200 should be variable checked on version (older versions it's 205,9)
            angularVel = angularVel * rcFactor;
        } else {
            angularVel = (((rate * 100) + 27) * rcCommandf / 16) / 4.1; // Only applies to old versions ?
        }

        angularVel = this.constrain(angularVel, -1 * limit, limit); // Rate limit from profile

        return angularVel;
    };

    this.getRaceflightRates = function (rcCommandf, rate, rcRate, rcExpo) {
        let angularVel = ((1 + 0.01 * rcExpo * (rcCommandf * rcCommandf - 1.0)) * rcCommandf);
        angularVel = (angularVel * (rcRate + (Math.abs(angularVel) * rcRate * rate * 0.01)));
        return angularVel;
    };

    this.getKISSRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo) {
        const kissRpy = 1 - rcCommandfAbs * rate;
        const kissTempCurve = rcCommandf * rcCommandf;
        rcCommandf = ((rcCommandf * kissTempCurve) * rcExpo + rcCommandf * (1 - rcExpo)) * (rcRate / 10);
        return ((2000.0 * (1.0 / kissRpy)) * rcCommandf);
    };

    this.getActualRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo) {
        let angularVel;
        const expof = rcCommandfAbs * ((Math.pow(rcCommandf, 5) * rcExpo) + (rcCommandf * (1 - rcExpo)));

        angularVel = Math.max(0, rate-rcRate);
        angularVel = (rcCommandf * rcRate) + (angularVel * expof);

        return angularVel;
    };

    this.getQuickRates = function (rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo) {
        rcRate = rcRate * 200;
        rate = Math.max(rate, rcRate);

        let angularVel;
        const superExpoConfig = (((rate / rcRate) - 1) / (rate / rcRate));
        const curve = Math.pow(rcCommandfAbs, 3) * rcExpo + rcCommandfAbs * (1 - rcExpo);

        angularVel = 1.0 / (1.0 - (curve * superExpoConfig));
        angularVel = rcCommandf * rcRate * angularVel;

        return angularVel;
    };

};

RateCurve.prototype.rcCommandRawToDegreesPerSecond = function (rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit) {
    let angleRate;

    if (rate !== undefined && rcRate !== undefined && rcExpo !== undefined) {
        let rcCommandf = this.rcCommand(rcData, 1, deadband);
        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_43)) {
            rcCommandf = rcCommandf / (500 - deadband);
        } else {
            rcCommandf = rcCommandf / 500;
        }

        const rcCommandfAbs = Math.abs(rcCommandf);

        switch(TABS.pid_tuning.currentRatesType) {
            case TABS.pid_tuning.RATES_TYPE.RACEFLIGHT:
                angleRate=this.getRaceflightRates(rcCommandf, rate, rcRate, rcExpo);

                break;

            case TABS.pid_tuning.RATES_TYPE.KISS:
                angleRate=this.getKISSRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo);

                break;

            case TABS.pid_tuning.RATES_TYPE.ACTUAL:
                angleRate=this.getActualRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo);

                break;

            case TABS.pid_tuning.RATES_TYPE.QUICKRATES:
                angleRate=this.getQuickRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo);

                break;

            // add future rates types here
            default: // BetaFlight
                angleRate=this.getBetaflightRates(rcCommandf, rcCommandfAbs, rate, rcRate, rcExpo, superExpoActive, limit);

                break;
        }
    }

    return angleRate;
};

RateCurve.prototype.getMaxAngularVel = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit) {
    let maxAngularVel;
    if (!this.useLegacyCurve) {
        maxAngularVel = this.rcCommandRawToDegreesPerSecond(maxRc, rate, rcRate, rcExpo, superExpoActive, deadband, limit);
    }

    return maxAngularVel;
};

RateCurve.prototype.setMaxAngularVel = function (value) {
    this.maxAngularVel = Math.ceil(value/200) * 200;
    return this.maxAngularVel;

};

RateCurve.prototype.draw = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context) {
    if (rate !== undefined && rcRate !== undefined && rcExpo !== undefined) {
        const height = context.canvas.height;
        const width = context.canvas.width;

        if (this.useLegacyCurve) {
            this.drawLegacyRateCurve(rate, rcRate, rcExpo, context, width, height);
        } else {
            this.drawRateCurve(rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context, width, height);
        }
    }
};


function [rc,var,ASAcontrol] = burg_s(f,b,max_order,last)
%BURG_S Burg type AR estimator for multiple segments
%   [RC,VAR] = BURG_S(SIG_MTX,MAX_ORDER) estimates a single vector of AR-
%   reflectioncoefficients RC up to order MAX_ORDER, using data from
%   multiple segments simultaneously. The data segments must be of equal
%   length and arranged columnwise in SIG_MTX. VAR is the estimated
%   variance based on all elements of SIG_MTX.
%
%   RC = BURG_S(F,B,ADD_ORDER) estimates ADD_ORDER additional
%   reflectioncoefficients from matrices of forward and backward
%   residuals F and B, that have been internally used in a previous Burg
%   estimation procedure.
%
%   Since forward and backward residual vectors are internally assigned
%   to the ASAglob variables 'ASAglob_final_f' and 'ASAglob_final_b', an
%   appropriate F and B can be retrieved after the program has finished
%   execution. This is most easily done by typing:
%     F = ASAglobretr('ASAglob_final_f');
%     B = ASAglobretr('ASAglob_final_b');
%
%   BURG_S is an ARMASA main function.
%
%   See also: BURG, ASAGLOB, ASAGLOBRETR.

%   References: S. de Waele and P. M. T. Broersen, The Burg Algorithm for
%               Segments, IEEE Transactions on Signal Processing,
%               vol. 48, no. 10, pp. 2876-2880, October 2000.
%               W. Wunderink, The Introduction of a Computationally
%               Efficient Burg Estimator Algorithm,
%               Tech. Report SSC-March00-1, Delft University of
%               Technology, Department of Applied Physics, Systems
%               Signals and Control Group, the Netherlands, March 2000.

%Header
%==============================================================================

%Declaration of variables
%------------------------

%Declare and assign values to local variables
%according to the input argument pattern
switch nargin
case 1
   if isa(f,'struct'), ASAcontrol=f;
   else, error(ASAerr(39))
   end
   f=[]; b=[]; max_order=[];
case 2
   if isa(b,'struct'), error(ASAerr(2,mfilename))
   end
   max_order=b; b=[]; ASAcontrol=[];
case 3
   if isa(max_order,'struct'), ASAcontrol=max_order; max_order=b; b=[];
   else, ASAcontrol=[];
   end
case 4
   if isa(last,'struct'), ASAcontrol=last;
   else, error(ASAerr(39))
   end
otherwise
   error(ASAerr(1,mfilename))
end

state = 0;
if isequal(nargin,1) & ~isempty(ASAcontrol)
      %ASAcontrol is the only input argument
   ASAcontrol.error_chk = 0;
   ASAcontrol.run = 0;
elseif (isequal(nargin,3) & isempty(ASAcontrol)) | ...
      isequal(nargin,4) %f and b have been provided
      %as input arguments (empty b not excluded)
   state = 1;
end

%Declare ASAglob variables
ASAglob = {'ASAglob_final_f';'ASAglob_final_b'};

%Assign values to ASAglob variables by screening the
%caller workspace
for ASAcounter = 1:length(ASAglob)
   ASAvar = ASAglob{ASAcounter};
   eval(['global ' ASAvar]);
   if evalin('caller',['exist(''' ASAvar ''',''var'')'])
      eval([ASAvar '=evalin(''caller'',ASAvar);']);
   else
      eval([ASAvar '=[];']);
   end
end

%ARMASA-function version information
%-----------------------------------

%This ARMASA-function is characterized by
%its current version,
ASAcontrol.is_version = [2000 12 30 20 0 0];
%and its compatability with versions down to,
ASAcontrol.comp_version = [2000 12 30 20 0 0];

%This function calls other functions of the ARMASA
%toolbox. The versions of these other functions must
%be greater than or equal to:
ASAcontrol.req_version.convolrev = [2000 12 6 12 17 20];

%Checks
%------

if ~any(strcmp(fieldnames(ASAcontrol),'error_chk')) | ASAcontrol.error_chk
      %Perform standard error checks
   %Input argument format checks
   ASAcontrol.error_chk = 1;
   if ~isnum(f)
      error(ASAerr(11,'f'))
   elseif isavector(f) & size(f,2)>1
      f = f(:);
      warning(ASAwarn(25,{'row';'f';'column'},ASAcontrol))
   elseif ~(length(size(f))==2)
      error(ASAerr(35,'f'))
   end
   if ~isavector(f)
      warning(ASAwarn(36,ASAcontrol))
   end
   if ~isempty(b)
      if ~isnum(b)
         error(ASAerr(11,'b'))
      elseif isavector(b) & size(b,2)>1
         b = b(:);
         warning(ASAwarn(25,{'row';'b';'column'},ASAcontrol))
      elseif ~(length(size(b))==2)
         error(ASAerr(35,'b'))
      end
   end
   if ~isnum(max_order) | ~isintscalar(max_order) | ...
         max_order<0
      error(ASAerr(17,'max_order'))
   end

   %Input argument value checks
   if ~isreal(f) | (~isempty(b) & ~isreal(b))
      error(ASAerr(13))
   end
   if isempty(b)
      if max_order>size(f,1)-1
         error(ASAerr(21))
      end
   elseif ~isequal(size(f),size(b))
      error(ASAerr(28,{'f','b'}))
   elseif max_order>size(f,1)-2
      error(ASAerr(29,'max_order'))
   end
end

if ~any(strcmp(fieldnames(ASAcontrol),'version_chk')) | ASAcontrol.version_chk
      %Perform version check
   ASAcontrol.version_chk = 1;

   %Make sure the requested version of this function
   %complies with its actual version
   ASAversionchk(ASAcontrol);

   %Make sure the requested versions of the called
   %functions comply with their actual versions
   convolrev(ASAcontrol);
end

if ~any(strcmp(fieldnames(ASAcontrol),'run')) | ASAcontrol.run
      %Run the computational kernel
   ASAcontrol.run = 1;
   ASAcontrol.version_chk = 0;
   ASAcontrol.error_chk = 0;

%Main
%==========================================================================

[N,n] = size(f);
P = max_order;

if P>0
   %The protocol is tuned using Matlab R12,
   %but also works fine on R11.
   if ~isempty(b)
      P = P+1;
      protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
   else
      if N<128
         protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
      elseif N<12000 %C_7 (& C_13)
         if P<(0.1+6.7e-006*N)*N %C_14 & S
            protocol = [0 1 0;0 0 0;0 0 0;0 P 0];
         else
            protocol = [0 3 0;0 0 0;0 0 0;0 P 0];
         end
      else
         if P<0.18*N %S
            protocol = [0 1 0;0 0 0;0 0 0;0 P 0];
         else
            protocol = [0 1 3 0;0 0 0 0;0 1 0 0;0 round(0.03*N) P 0]; %C_5
         end
      end
   end
else
   protocol = zeros(4,2);
end

p = 0;
p_max = 0;
i2 = 2;
while protocol(4,i2)~=0;
   p = protocol(4,i2)-protocol(4,i2-1);
   if p>p_max
      p_max = p;
   end
   i2 = i2+1;
end
i9 = 1;
i10 = 2;
if n>N
   i9 = 2;
   i10 = 1;
end

v = zeros(N+p_max,n,2);
a = zeros(N+p_max,n,2);
if isempty(b)
   v(1:N,:,2) = f;
   a(1+p_max:N+p_max,:,2) = f;
   if protocol(1,2)==3 | protocol(1,2)==0
      var = sum(sum(f(1:N,:).^2,i9),i10)/(n*N);
      den = 2*var*n*N;
   end
else
   v(1:N,:,2) = f;
   a(1+p_max:N+p_max,:,2) = b;
   den = sum(sum(f(1:N,:).^2+b(1:N,:).^2,i9),i10);
   var = [];
end

r1 = p_max+1;
v1 = [1 1];
v2 = [N N];
v3 = [N N];
a1 = [r1 r1];
a2 = [r1 r1];
a3 = [r1+N-1 r1+N-1];
rc = zeros(1,P+1);
i1 = 0;
i2 = 2;
i4 = 0;
c = 0;

while protocol(4,i2)~=0
   i1 = i1+1;
   p = protocol(4,i2);

   if protocol(1,i2)==3 %SP
      r2 = a2(1)+1;
      r3 = v2(1)-1;
      r4 = a2(2)+1;
      r5 = v2(2)-1;
      i5 = i1;
      for i1 = i1:p
         i5 = i5+1;
         if c, c0 = 2; c1 = 1;
         else c0 = 1; c1 = 2;
         end
         v1(c0) = v1(c1)+1;
         a3(c0) = a3(c1)-1;
         v(v1(c0):v2(c0),:,c0) = v(v1(c0):v2(c1),:,c1)+...
            rc(i1)*a(r4:a3(c1),:,c1);
         a(a2(c0):a3(c0),:,c0) = a(a2(c1):a3(c0),:,c1)+...
            rc(i1)*v(v1(c1):r5,:,c1);
         den = (1-rc(i1)^2)*den-...
            sum((v(v1(c1),:,c1)+rc(i1)*a(a2(c1),:,c1)).^2)-...
            sum((a(a3(c1),:,c1)+rc(i1)*v(v2(c1),:,c1)).^2);
         rc(i5) = -2*sum(sum(v(v1(c0):v2(c0),:,c0).*...
            a(a2(c0):a3(c0),:,c0),i9),i10)/den;
         c = ~c;
      end
      v(v1(c1),:,c0) = v(v1(c1),:,c1)+rc(i1)*a(a2(c1),:,c1);
      a(a3(c1),:,c0) = a(a3(c1),:,c1)+rc(i1)*v(v2(c1),:,c1);

   elseif protocol(1,i2)==1 %ACC
      i13 = 2*p;
      i14 = p+1;
      i15 = 0;
      i16 = 0;
      i17 = 0;
      jj = zeros(2,i13);
      jjm_h = zeros(2,i14);
      jj_h = zeros(2,i14);
      jj(2,1) = 1;
      jjm_h(2,1) = 1;
      jj_h(2,2) = 1;
      for i4 = 1:n
         acov_x(:,i4) = convolrev(f(:,i4),p,ASAcontrol);
      end
      i1 = i1+1;
      var = sum(acov_x(1,:));
      den = (2*var-sum(v(v1(2),:,2).^2)-sum(a(a3(2),:,2).^2));
      rc(i1) = -2*sum(acov_x(2,:))/den;
      var = var/(n*N);
      v1(2) = v1(2)+1;
      a3(2) = a3(2)-1;
      t = 0;
      r5 = v1(2)+p-2;
      r6 = a3(2)-p+2;
      if protocol(3,i2)==1
         r11 = r5+1;
         r12 = r6-1;
         i33 = 0;
      else
         r11 = v2(2)-p+2;
         r12 = a2(2)+p-2;
         i33 = 1;
      end
      r7 = v2(2)+1;
      r8 = a2(2)-1;
      r9 = v1(2);
      r10 = a3(2);
      r13 = r5-r9;
      r14 = r10-r6;
      r15 = r11-r9;
      r16 = r10-r12;

      for i1 = i1:p
         i13 = i1+1;
         i14 = i1-1;
         i15 = 2*i1-1;
         i16 = 2*i1-2;
         i17 = 2*i1-3;
         rc_h1 = rc(i1)^2;
         rc_h2 = 2*rc(i1);
         if c, c0 = 2; c1 = 1;
         else c0 = 1; c1 = 2;
         end
         jj(c0,1:2) = jj(c1,1:2);
         jj(c0,3:i15) = jj(c1,3:i15)+rc_h1*jj(c1,i17:-1:1);
         jj(c0,i1:i16) = jj(c0,i1:i16)+rc_h2*jjm_h(c1,1:i14);
         jj(c0,2:i14) = jj(c0,2:i14)+rc_h2*jjm_h(c1,i14:-1:2);
         jjm_h(c0,1:i1) = rc(i1)*jj_h(c1,1:i1);
         jjm_h(c0,1:i14) = jjm_h(c0,1:i14)+(1+rc(i1)^2)*jjm_h(c1,1:i14);
         jj_h(c0,1:i13) = jj(c0,i13:-1:1);
         jj_h(c0,1:i14) = jj_h(c0,1:i14)+jj(c0,i13:i15);
         va_t(1,:) = jj(c0,1:i1)*acov_x(i13:-1:2,:);
         va_t(2,:) = jj(c0,i13:i15)*acov_x(1:i14,:);
         v1(c0) = v1(c1)+1; v3(c0) = v3(c1)+1;
         a1(c0) = a1(c1)-1; a3(c0) = a3(c1)-1;
         t = t+i33;
         v(r9:r5,:,c0) = v(r9:r5,:,c1)+...
            rc(i1)*a(a1(c1):a1(c1)+r13,:,c1);
         v(r11+t:v3(c1),:,c0) = v(r11+t:v3(c1),:,c1)+...
            rc(i1)*a(a1(c1)+r15+t:r10,:,c1);
         v(v3(c0),:,c0) = rc(i1)*f(N,:);
         a(r6:r10,:,c0) = a(r6:r10,:,c1)+...
            rc(i1)*v(v3(c1)-r14:v3(c1),:,c1);
         a(a1(c1):r12-t,:,c0) = a(a1(c1):r12-t,:,c1)+...
            rc(i1)*v(r9:v3(c1)-r16-t,:,c1);
         a(a1(c0),:,c0) = rc(i1)*f(1,:);
         omega_va(c0) = sum(sum(v(r9:v1(c1),:,c0).*a(a1(c0):r8,:,c0)+...
            v(r7:v3(c0),:,c0).*a(a3(c1):r10,:,c0),1));
         den = den*(1-rc_h1)-sum(v(v1(c1),:,c0).^2)-sum(a(a3(c1),:,c0).^2);
         rc(i13) = -2*(sum(sum(va_t,2),1)-omega_va(c0))/den;
         c=~c;
      end
      i1 = p;
   end
   i2 = i2+1;
end

if state %f and b have been provided as input
      %arguments (empty b not excluded)
      if isempty(b)
         rc = rc(2:end);
      else
         rc = rc(3:end);
      end
else
   rc(1) = 1;
end

i2 = i2-1;
ASAglob_final_f = [];
ASAglob_final_b = [];
if protocol(3,i2)==1 | protocol(1,i2)==3
   ASAglob_final_f = v(v1(c1):v2(c0),:,c0);
   ASAglob_final_b = a(a2(c0):a3(c1),:,c0);
elseif isequal(P,0)
   ASAglob_final_f = f;
end

%Footer
%=====================================================

else %Skip the computational kernel
   %Return ASAcontrol as the first output argument
   if nargout>1
      warning(ASAwarn(9,mfilename,ASAcontrol))
   end
   rc = ASAcontrol;
   ASAcontrol = [];
end

%Program history
%======================================================================
%
% Version                Programmer(s)          E-mail address
% -------                -------------          --------------
% former version         S. de Waele            waele@tn.tudelft.nl
% [2000 12 30 20 0 0]    W. Wunderink           wwunderink01@freeler.nl


function SR = superresolve_dbrsr(slidingWindows, magFactor)

numberOfFrames = size(slidingWindows.frames,3);

% Set PSF kernels for the different binning factors.
psf{1} = fspecial('gaussian', 5, 0.8); % PSF for binning factor 2
psf{2} = fspecial('gaussian', 7, 1.2); % PSF for binning factor 3
psf{3} = fspecial('gaussian', 9, 1.6); % PSF for binning factor 4

% Specific preprocessing steps for the non-uniform interpolation based multi-frame methods.
weighting_struct = struct('mec_weight_flag',4,'dis_weight_flag',1,'qps_weight_flag',0,'mec_confMapN',[],...
    'mec_thr_scaler',2,'mec_weight_exp',2,'mec_weighting',[],'dis_rho',0.7,'dis_scaler',magFactor*10,'dis_weighting',[],...
    'qps_map',[],'qps_rho',0.7,'qps_weighting',[],'alpha',1,'beta',1,'gamma',1,'whi_mode',2);
warped_meshXN = zeros(size(slidingWindows.referenceFrame,1),size(slidingWindows.referenceFrame,2),numberOfFrames-1);
warped_meshYN = zeros(size(slidingWindows.referenceFrame,1),size(slidingWindows.referenceFrame,2),numberOfFrames-1);

lr_seq_sorted = slidingWindows.frames;
lr_seq_tmp = lr_seq_sorted(:,:,(numberOfFrames+1)/2);
lr_seq_sorted(:,:,(numberOfFrames+1)/2) = lr_seq_sorted(:,:,1);
lr_seq_sorted(:,:,1) = lr_seq_tmp;

lr_seq_sorted = permute(lr_seq_sorted,[1 2 4 3]);

permutation_vec = 1:numberOfFrames-1;
permutation_vec((numberOfFrames+1)/2:end) = permutation_vec((numberOfFrames+1)/2:end) + 1;
permutation_vec(1:(numberOfFrames-1)/2)   = circshift(permutation_vec(1:(numberOfFrames-1)/2),[0 -1]);

fw_optFlowN_xc = zeros(size(lr_seq_sorted,1),size(lr_seq_sorted,2),2,numberOfFrames-1);

for itera = 1:numberOfFrames-1
    [warped_meshXN(:,:,itera),warped_meshYN(:,:,itera)] = lmsSR_warpImgGridLocalTrans(size(slidingWindows.referenceFrame,1),size(slidingWindows.referenceFrame,2),slidingWindows.flowToReference{permutation_vec(itera)}.mvs_xc);

    % Storing the MEC-SSD-Confidence into the Weighting Struct
    weighting_struct.mec_confMapN(:,:,itera) = slidingWindows.mec_confMap{permutation_vec(itera)};

    % Forward MVF for WDBR
    fw_optFlowN_xc(:,:,:,itera) = slidingWindows.flowToReference{permutation_vec(itera)}.mvs_xc;
end

% Actual SR call
[~,wnuisr_nodeconv,~,~] = lmsSR_generateSRusingNUIv4weighted(lr_seq_sorted,warped_meshXN,warped_meshYN,[magFactor, magFactor],psf{magFactor-1},2,weighting_struct);
[SR,~] = lmsSR_generateSRusingWDBR(wnuisr_nodeconv,fw_optFlowN_xc,magFactor,numberOfFrames,psf{magFactor-1},2,weighting_struct);


function dvh = matRad_calcDVH(cst,doseCube,dvhType,doseGrid)
% matRad dvh calculation
%
% call
%   dvh = matRad_calcDVH(cst,doseCube)
%   dvh = matRad_calcDVH(cst,doseCube,dvhType)
%   dvh = matRad_calcDVH(cst,doseCube,doseGrid)
%   dvh = matRad_calcDVH(cst,doseCube,dvhType,doseGrid)
%
% input
%   cst:        matRad cst struct
%   doseCube:   arbitrary doseCube (e.g. physicalDose)
%   dvhType:    (optional) string, 'cum' for cumulative, 'diff' for differential
%               dvh
%   doseGrid:   (optional) use predefined evaluation points. Useful when
%               comparing multiple realizations
%
% output
%   dose volume histogram
%
% References
%   van't Riet et. al., IJROBP, 1997 Feb 1;37(3):731-6.
%   Kataria et. al., J Med Phys. 2012 Oct-Dec; 37(4): 207�213.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2016 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('dvhType','var') || isempty(dvhType)
    dvhType = 'cum';
end

if ~exist('doseGrid', 'var') || isempty(doseGrid)
    maxDose = max(doseCube(:));
    minDose = min(doseCube(:));

    % get dvhPoints for every structure and every scenario the same
    n = 1000;
    if strcmp(dvhType, 'cum')
        doseGrid = linspace(0,maxDose*1.05,n);
    elseif strcmp(dvhType, 'diff')
        doseGrid = linspace(0.95*minDose,maxDose*1.05,n);
    end
end

numOfVois = size(cst,1);
dvh = struct;
for i = 1:numOfVois
    dvh(i).doseGrid     = doseGrid;
    dvh(i).volumePoints = getDVHPoints(cst, i, doseCube, doseGrid, dvhType);
    dvh(i).name         = cst{i,2};
end

end %eof

function dvh = getDVHPoints(cst, sIx, doseCube, dvhPoints, dvhType)
n = numel(dvhPoints);
dvh         = NaN * ones(1,n);
indices     = cst{sIx,4}{1};
numOfVoxels = numel(indices);

doseInVoi   = doseCube(indices);

switch dvhType
    case 'cum' % cummulative DVH
        for j = 1:n
            dvh(j) = sum(doseInVoi >= dvhPoints(j));
        end

    case 'diff' % differential DVH
        binning = (dvhPoints(2) - dvhPoints(1))/2;
        for j = 1:n % differential DVH
            dvh(j) = sum(dvhPoints(j) + binning > doseInVoi & doseInVoi > dvhPoints(j) - binning);
        end

end
dvh = dvh ./ numOfVoxels * 100;
end %eof getDVHPoints


function [parameters, ll, ht, intercept, VCV, scores, diagnostics] = scalar_vt_vech(data,dataAsym,p,o,q,composite,startingvals,options)
% Estimation of symmetric and asymmetric scalar multivariate vech ARCH models using variance
% targeting to reduce the number of parameters needing to be estimated simultaneously
%
% USAGE:
%  [PARAMETERS,LL,HT,INTERCEPT,VCV,SCORES,DIAGNOSTICS] = scalar_vt_vech(DATA,DATAASYM,P,O,Q,COMPOSITE,STARTINGVALS,OPTIONS)
%
% INPUTS:
%   DATA         - A T by K matrix of zero mean residuals -OR-
%                    K by K by T array of covariance estimators (e.g. realized covariance)
%   DATAASYM     - [OPTIONAL] K by K by T array of asymmetric covariance estimators
%   P            - Positive, scalar integer representing the number of lags of the innovation process
%   O            - Non-negative scalar integer representing the number of asymmetric lags to include
%   Q            - Non-negative scalar integer representing the number of lags of conditional covariance
%   COMPOSUTE    - [OPTIONAL] String, one of:
%                    'None' - Standard K-dimensional QMLE (Default)
%                    'Diagonal' - Use only pairwise likelihoode for i,i+1, i=1,2,...,K-1
%                    'Full' - Use all pairwise likelihoods
%   STARTINGVALS - [OPTIONAL] (p+o+q) x 1 vector of starting values
%   OPTIONS      - [OPTIONAL] Options to use in the optimization (fminunc)
%
% OUTPUTS:
%   PARAMETERS   - A p+o+q column vector of parameters.  The intercept is reported in DIAGNOSTICS
%   LL           - The log likelihood at the optimum
%   HT           - A [K K T] dimension matrix of conditional covariances
%   INTERCEPT    - K by K matrix containing the intercept computed from the unconditional variance
%                    and parameters
%   VCV          - A numParams^2 square matrix of robust parameter covariances (A^(-1)*B*A^(-1)*t^(-1))
%   SCORES       - A T by numParams matrix of individual scores
%
% COMMENTS:
%    The conditional variance, H(t), of a scalar variance-targeting vech is modeled
%    as follows:
%
%      H(t) = (1-alpha(1)+...+alpha(p)-beta(1)-...-beta(q))*C + ...
%              alpha(1)*r_{t-1}'*r_{t-1} + ... + alpha(p)*r_{t-p}'*r_{t-p}+...
%              gamma(1)*n_{t-1}'*n_{t-1} + ... + gamma(o)*n_{t-p}'*n_{t-p}+...
%              beta(1)*H(t-1) +...+ beta(q)*H(t-q)
%
%    where n_{t} = r_{t} .* (r_{t}<0)
%
% EXAMPLES:
%     Estimation of a Scalar VECH(1,0,1)
%       [simulatedData, Ht, pseudoRC] = scalar_vt_vech_simulate(1000, [.02 .04 .95], .01*eye(2), 1, 1, 1, 72);
%       parameters = scalar_vt_vech(simulatedData,[],1,0,1)
%     Estimation of an asymmetric Scalar VECH(1,1,1)
%       parameters = scalar_vt_vech(simulatedData,[],1,1,1)
%     Estimation of an asymmetric Scalar VECH(1,1,1) using realized-type data
%       asymPseudoRC = zeros(size(pseudoRC));
%       for i = 1:1000
%         asymPseudoRC(:,:,i) = pseudoRC(:,:,i).*(double(simulatedData(i,:)<0)'*double(simulatedData(i,:)<0));
%       end
%       parameters = scalar_vt_vech(pseudoRC,asymPseudoRC,1,1,1);

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 10/28/2009

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 3
        o=0;
        q=0;
        composite = 'None';
        startingvals=[];
        options = [];
    case 4
        q=0;
        composite = 'None';
        startingvals=[];
        options = [];
    case 5
        composite = 'None';
        startingvals=[];
        options = [];
    case 6
        startingvals=[];
        options = [];
    case 7
        options = [];
    case 8
        % Nothing
    otherwise
        error('Between 3 and 8 arguments required.')
end

%data should be TxK, T>K
if ndims(data)==2
    [t,k]=size(data);
    if ~isempty(dataAsym)
        error('If DATA is a T by K matrix, DATAASYM must be empty.');
    end
    temp = zeros(k,k,t);
    dataAsym = zeros(k,k,t);
    for i=1:t
        temp(:,:,i) = data(i,:)'*data(i,:);
        dataAsym(:,:,i) = (data(i,:).*(data(i,:)<0))'*(data(i,:).*(data(i,:)<0));
    end
    data = temp;
elseif ndims(data)==3
    [k,m,t] = size(data);
    if m~=k
        error('DATA must be K by K by T is a 3D array.');
    end
    if ~isempty(dataAsym)
        if ndims(dataAsym)~=3
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
        [k2,m2,t2]=size(dataAsym);
        if any([k m t]~=[k2 m2 t2])
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
    end
end

if min(t,k)<2 || t<k
    error('DATA must be a T by K matrix or a K by K by T 3D array, T>K>1');
end

%p, o, q much be non-negative scalars
if length(p)>1 || any(p<1) || floor(p)~=p
    error('P must be a positive scalar');
end
if isempty(o)
    o = 0;
end
if length(o)>1 || any(o<0) || floor(o)~=o
    error('O must be a non-negative scalar');
end
if o>0 && isempty(dataAsym)
    error('DATAASYM must be non-empty if O>0.')
end

if isempty(q)
    q = 0;
end
if length(q)>1 || any(q<0) || floor(q)~=q
    error('Q must be a non-negative scalar');
end

if isempty(composite)
    composite = 'None';
end
switch lower(composite)
    case {'none'}
        useComposite = 0;
    case {'diagonal'}
        useComposite = 1;
    case {'full'}
        useComposite = 2;
end

% Startingvals must have p+o+q parameters sum(alpha)+kappa*sum(gamma)+sum(beta)<1
C = mean(data,3);
if o>0
    Casym = mean(dataAsym,3);
    kappa = 1 / (max(eig(C^(-0.5)*Casym*C^(-0.5))) + eps);
else
    Casym = zeros(k);
    kappa = 2;
end
if  ~isempty(startingvals)
    if size(startingvals,2)>size(startingvals,1)
        startingvals = startingvals';
    end
    if length(startingvals)<(p+o+q)
        error('STARTINGVALS should be a P+O+Q by 1 vector');
    end
    %Only validate if provided
    A=startingvals(1:p);
    G=startingvals(p+1:p+o);
    B=startingvals(p+o+1:p+o+q);
    if (sum(A)+sum(G)/kappa+sum(B))>=.999998
        error('Weighted sum of STATINGVALUES must be less than 1. See Comments.');
    end
    if any(A<0) || any(B<0) || any(G<0)
        error('STARTINGVALS must all be nonnegative.');
    end
end

%Make sure options is a valid option structure
if isempty(options)
    options=optimset('fminunc');
    options.Display='iter';
    options.Diagnostics='on';
    options.LargeScale='off';
end
try
    optimset(options);
catch ME
    error('OPTIONS is not a valid options structure');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Compute the backCast
backCast = zeros(k);
backCastAsym = zeros(k);
tau = max(ceil(sqrt(t)),k);
weights = .06 * .94.^(0:tau);
weights = weights / sum(weights);
for i=1:tau
    backCast = backCast + weights(i) * data(:,:,i);
    if o>0
        backCastAsym = backCastAsym + weights(i) * dataAsym(:,:,i);
    end
end

%Augment the data with backcasts
[startingvals,lls,output_parameters] = scalar_vt_vech_starting_values(startingvals,data,dataAsym,p,o,q,C,Casym,kappa,useComposite,backCast,backCastAsym); %#ok<NASGU>

% finally to transform the parameters to the unrestricted equivalents
startingvals=scalar_vt_vech_transform(startingvals,p,o,q,kappa);

[parameters,ll,exitflag,output]=fminunc('scalar_vt_vech_likelihood',startingvals,options,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,true);
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if exitflag<=0 &&  ll<lls(1)
    %Did not converge, but function improved
    options.MaxFunEvals=4*100*(p+q);
    options.MaxIter=2*100*(p+q);
    parameters=fminunc('scalar_vt_vech_likelihood',parameters,options,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,true);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%

parameters=scalar_vt_vech_itransform(parameters,p,o,q,kappa);
if nargout>1
    [ll,~,ht]=scalar_vt_vech_likelihood(parameters,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,false);
    ll=-ll;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout>4
    k2 = k*(k+1)/2;
    CVech=vech(C);
    if o==0
        momentCount = k2;
        CasymVech = [];
    else
        momentCount = 2*k2;
        CasymVech=vech(Casym);
    end

    A = zeros(momentCount + p + o + q);
    A(1:momentCount, 1:momentCount) = - t * eye(momentCount);
    jointParameters=[CVech;CasymVech;parameters];

    % A will be k2 + p + q square
    scores = zeros(t,momentCount+p+q);
    scoreCount = 1;
    for i=1:k
        for j=i:k
            scores(:,scoreCount) = squeeze(data(j,i,:));
            if o>0
                scores(:,k2+scoreCount) = squeeze(dataAsym(j,i,:));
            end
            scoreCount = scoreCount  + 1;
        end
    end
    [~,gt] = gradient_2sided(@scalar_vt_vech_likelihood,parameters,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,false);
    scores(:,momentCount+1:momentCount+p+o+q) = gt;
    A(momentCount+1:momentCount+p+o+q,:) = hessian_2sided_nrows(@scalar_vt_vech_likelihood,jointParameters,p+o+q,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,true,useComposite,false);

    A = A/t;
    B = covnw(scores);
    Ainv = inv(A);
    VCV = Ainv*B*Ainv'/t; %#ok<MINV>
    VCV = VCV(momentCount+1:momentCount+p+o+q,momentCount+1:momentCount+p+o+q);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
diagnostics.EXITFLAG=exitflag;
diagnostics.ITERATIONS=output.iterations;
diagnostics.FUNCCOUNT=output.funcCount;
diagnostics.MESSAGE=output.message;
diagnostics.kappa=kappa;
alpha = sum(parameters(1:p));
gamma = sum(parameters(p+1:p+o));
beta = sum(parameters(p+o+1:p+o+q));
diagnostics.intercept = C*(1-alpha-beta);
diagnostics.composite = composite;
if o>0
    diagnostics.intercept  = diagnostics.intercept  - gamma*Casym;
end
intercept = diagnostics.intercept;


function [ point_num2, edge_num2, face_num2, face_order_max2 ] = ...
  dual_size_3d ( point_num, edge_num, face_num, face_order_max, ...
  point_coord, face_order, face_point )

%*****************************************************************************80
%
%% DUAL_SIZE_3D determines sizes for a dual of a shape in 3D.
%
%  Discussion:
%
%    We don't actually need FACE_POINT as input here.  But since the
%    three arrays occur together everywhere else, it seems unnecessarily
%    user-confusing to vary the usage here!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer EDGE_NUM, the number of edges.
%
%    Input, integer FACE_NUM, the number of faces.
%
%    Input, integer FACE_ORDER_MAX, the maximum number of vertices per face.
%
%    Input, real POINT_COORD(3,POINT_NUM); POINT_COORD(*,J) is
%    the X, Y and Z coordinates of point J.
%
%    Input, integer FACE_ORDER(FACE_NUM), the number of vertices per face.
%
%    Input, integer FACE_POINT(FACE_ORDER_MAX,FACE_NUM); FACE_POINT(I,J)
%    is the index of the I-th point in the J-th face.  The
%    points are listed in the counter-clockwise direction defined
%    by the outward normal at the face.
%
%    Output, integer POINT_NUM2, the number of points in the dual.
%
%    Output, integer EDGE_NUM2, the number of edges in the dual.
%
%    Output, integer FACE_NUM2, the number of faces in the dual.
%
%    Output, integer FACE_ORDER_MAX2, the maximum number of vertices per face
%    in the dual.
%

%
%  These values are easy to compute:
%
  point_num2 = face_num;
  edge_num2 = edge_num;
  face_num2 = point_num;
%
%  To determine FACE_ORDER_MAX2 is not so easy.
%  You have to construct the FACE_ORDER array for the dual shape.
%  The order of a dual face is the number of edges that the vertex occurs in.
%  But then all we have to do is count how many times each item shows up
%  in the FACE_POINT array.
%
  face_order_max2 = 0;
  face_order2(1:face_num2) = 0;

  for face = 1 : face_num
    for i = 1 : face_order(face)
      face2 = face_point(i,face);
      face_order2(face2) = face_order2(face2) + 1;
    end
  end

  face_order_max2 = max ( face_order2(1:face_num2) );

  return
end


function g = glmderiv(net, x)
%GLMDERIV Evaluate derivatives of GLM outputs with respect to weights.
%
%	Description
%	G = GLMDERIV(NET, X) takes a network data structure NET and a matrix
%	of input vectors X and returns a three-index matrix mat{g} whose  I,
%	J, K element contains the derivative of network output K with respect
%	to weight or bias parameter J for input pattern I. The ordering of
%	the weight and bias parameters is defined by GLMUNPAK.
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check arguments for consistency
errstring = consist(net, 'glm', x);
if ~isempty(errstring)
    error(errstring);
end

ndata = size(x, 1);
if isfield(net, 'mask')
  nwts = size(find(net.mask), 1);
  mask_array = logical(net.mask)*ones(1, net.nout);
else
  nwts = net.nwts;
end
g = zeros(ndata, nwts, net.nout);

temp = zeros(net.nwts, net.nout);
for n = 1:ndata
    % Weight matrix w1
    temp(1:(net.nin*net.nout), :) = kron(eye(net.nout), (x(n, :))');
    % Bias term b1
    temp(net.nin*net.nout+1:end, :) = eye(net.nout);
    if isfield(net, 'mask')
	g(n, :, :) = reshape(temp(find(mask_array)), nwts, net.nout);
    else
	g(n, :, :) = temp;
    end
end


function p=v_psychofunc(m,q,x,r)
%V_PSYCHOFUNC Calculate psychometric functions: trial success probability versus SNR
%
% Usage: p=v_psychofunc('',q,x)       % calculate probabilities
%        b=v_psychofunc('r',q,x)       % generate boolean variables with success prob p
%        p=v_psychofunc(m,q,x,r)     % Calculate likelihoods for observations r
%        x=v_psychofunc([m 'i'],q,p) % Calculate inverse
%
% Inputs:
%          m        mode string [may be omitted if not required]
%                      'n'   do not normalize likelihoods
%                      'f'   do not squeeze output arrays to remove singleton dimensions
%                      'i'   calculate inverse function
%                      'r'   calculate binary random variables with probability p
%                      ['s'   calculate sweet points for threshold and slope]
%                      ['d'   calculate partial derivatives with respect to q(1:5)]
%                      'g'   plot graph
%                      'G'   plot image
%                      'c'   include colourbar
%          q        model parameters. Either a column vector with a single model,
%                   a matrix with one model per column or a cell array with multiple values for
%                   some or all of the parameters
%                      1  probability at threshold [0.5]
%                      2  threshhold [0 dB]
%                      3  slope at threshold [0.1 prob/dB ]
%                      4  miss or lapse probability [0]
%                      5  guess probability   [0]
%                      6  psychometric function type [1]
%                          1 = logistic
%                          2 = cumulative Gaussian
%                          3 = Weibull
%                          [4 = reversed Weibull]
%                          [5 = Gumbell]
%                          [6 = reversed Gumbell]
%          x        vector of SNR values
%          r        test results (0 or 1) corresponding to x
%          p        vector of probabilities
%
% Outputs:
%          p        array of probabilities or random variates ('r' option).
%                   p is a squeezed 7-dimensional array
%                   whose dimensions correspond to x followed by the 6 model parameter entries.
%                   if q is a cell array, singleton dimensions are removed unless the 'f' option is given.
%          x        Inverse function gives SNR, x, as a function of p
%          b        array of boolean variables

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

% first sort out input arguments
minp=0.01;          % minimum probability to use for inverse function by default
qq=[0.5 0 0.1 0 0 1]';  % default values for q
if nargin<4
    r=[];
    if nargin<3
        x=[];
        if nargin<2
            q=[];
            if ~nargin
                m='';
            end
        end
    end
end
if ~ischar(m);      % mode argument is optional
    r=x;
    x=q;
    q=m;
    m='';
end
sq=size(q);
ckmod=0;
if iscell(q)
    nq=ones(1,6);
    qax=num2cell([0; qq]);  % used for plotting
    for i=1:min(numel(q),6)
        nq(i)=numel(q{i});
        if nq(i)>=1
            nr=size(qq,2);
            qax{i+1}=q{i};
            if i<=5             % do not replicate for multiple models
                qq=repmat(qq,1,nq(i));
                qq(i,:)=reshape(repmat(q{i}(:)',nr,1),1,nr*nq(i));
            else
                qq(i,:)=repmat(q{i}(1),1,nr);
            end
        end
    end
    nq=max(nq,1);
    nmod=nq(6);
    if nmod>1      % list of models to use
        modlist=q{6};
    else
        modlist=qq(6,1);  % default model
    end
else
    nq=sq(2);
    if nq
        ql=repmat(qq,1,nq);
        ql(1:sq(1),:)=q;
    else
        ql=qq;
        nq=1;
    end
    modlist=unique(ql(6,:));
    nmod=length(modlist);
    ckmod=nmod>1;   % need to check model list
    qq=ql;
end
% now perform the calculation
nx=numel(x);
npt=50;       % number of points
if any(m=='i') % doing inverse
    if ~nx
        nx=npt;
        xlim=[max(qq(5,:)),1-max(qq(4,:))]*[1-minp minp; minp 1-minp];
        x=linspace(xlim(1),xlim(2),nx)';
    end
    p=zeros([nx nq]);  % space for SNRs
    ia=0;
    for i=1:nmod % loop for each model type
        mod=modlist(i);
        if ckmod
            qq=ql(:,ql(6,:)==mod);
        end
        pscale=1-qq(4,:)-qq(5,:);
        pstd=(qq(1,:)-qq(5,:))./pscale; % prob target compensating for miss and lapse probs
        sstd=qq(3,:)./pscale; % slope compensating for miss and lapse probs
        px=x(:)*pscale.^(-1)-repmat(qq(5,:)./pscale,nx,1);  % adjust for miss and lapse probs
        switch mod
            case 1
                beta=sstd./(pstd.*(1-pstd));
%                 alpha=qq(2,:)+log((1-pstd)./pstd)./beta;
                px=repmat(qq(2,:)+log((1-pstd)./pstd)./beta,nx,1)-log(px.^(-1)-1).*repmat(beta.^(-1),nx,1);
            case 2   % cumulative Gaussian function
                xtstd=norminv(pstd); % x position of target in std measure
                sig=normpdf(xtstd)./sstd;
                px= repmat(qq(2,:)-sig.*xtstd,nx,1) + repmat(sig,nx,1).*norminv(px);
            case 3
                wlog=log(1-pstd);
                kbeta=sstd./((pstd-1).*wlog);
                alpha=qq(2,:)-log(-wlog)./kbeta;
                px=repmat(alpha,nx,1)+log(-log(1-px)).*repmat(kbeta.^(-1),nx,1);
            otherwise
                error('Invalid psychometric model index');
        end
        if ckmod
            p(:,ql(6,:)==i)=px;
        else
            ib=ia+numel(p)/nmod;
            p(ia+1:ib)=px(:);
            ia=ib;
        end
    end
else % doing forward mapping
    if ~nx
        ef=2;         % expansion factor
        nx=npt;
        x=linspace(min(qq(2,:)-ef*(qq(1,:)-qq(5,:))./qq(3,:)), ...
            max(qq(2,:)+ef*(1-qq(1,:)-qq(4,:))./qq(3,:)),nx)';
    end
    p=zeros([nx nq]);  % space for probabilities
    ia=0;
    for i=1:nmod % loop for each model type
        mod=modlist(i);
        if ckmod
            qq=ql(:,ql(6,:)==mod);
        end
        pscale=1-qq(4,:)-qq(5,:);  % prob range excluding miss and lapse probs
        pstd=(qq(1,:)-qq(5,:))./pscale; % prob target compensating for miss and lapse probs
        sstd=qq(3,:)./pscale; % slope compensating for miss and lapse probs
        switch mod
            case 1   % logistic function
                beta=sstd./(pstd.*(1-pstd));
%                 alpha=qq(2,:)+log((1-pstd)./pstd)./beta;
                px=(1+exp(repmat(beta.*qq(2,:)+log((1-pstd)./pstd),nx,1)-x(:)*beta)).^(-1);
            case 2   % cumulative Gaussian function
                xtstd=norminv(pstd); % x position of target in std measure
                sigi=sstd./normpdf(xtstd);
                px=normcdf(x(:)*sigi-repmat(qq(2,:).*sigi-xtstd,nx,1));
            case 3
                wlog=log(1-pstd);
                kbeta=sstd./((pstd-1).*wlog);
                alpha=qq(2,:)-log(-wlog)./kbeta;
                px=1-exp(-exp(x(:)*kbeta-repmat(alpha.*kbeta,nx,1)));
            otherwise
                error('Invalid psychometric model index');
        end
        px=repmat(qq(5,:),nx,1)+repmat(pscale,nx,1).*px;  % adjust for miss and lapse probs
        if ckmod
            p(:,ql(6,:)==i)=px;
        else
            ib=ia+numel(p)/nmod;
            p(ia+1:ib)=px(:);
            ia=ib;
        end
    end
    if numel(r)                 % we are calculating likelihoods
        mk=r(:)==0;
        p(mk,:)=1-p(mk,:);      % invert probability for results that are zero
        if nx>1
            if any(m=='n')
                p=prod(p,1);
            else
                p=sum(log(p),1);
                p=exp(p-max(p(:)));
                p=p/sum(p(:));     % normalize to equal 1
            end
            nx=1;
        end
    end

end
pg=p;       % save unsqueezed p for plotting
if ~any(m=='f') && iscell(q) % remove all singleton dimensions
    szp=size(p);
    szq=szp(szp>1);
    szq=[szq ones(1,max(0,2-numel(szq)))];
    p=reshape(p,szq);
end
if any(m=='r') && ~any(m=='i');
    p=rand(size(p))<p;
end

if ~nargout || any(lower(m)=='g')
    clf;
    szp=[nx nq];
    czp=sum(szp>1);
    if czp>0  % check if there is anything to plot
        if iscell(q)
            axlab={'Input SNR','Threshold prob','Threshold SNR','Threshold slope','Lapse prob','Guess prob','Sigmoid type'};
            [szs,izs]=sort(szp,'descend');
            pg=permute(pg,izs);
            qax{1}=x;
            if any(m=='G') || czp>2 % image
                ngr=prod(szs(3:end));
                ncol=ceil(sqrt(ngr));
                nrow=ceil(ngr/ncol);
                npix=szs(1)*szs(2);
                ia=0;
                for i=1:ngr
                    subplot(nrow,ncol,i);
                    ib=ia+npix;
                    imagesc(qax{izs(1)},qax{izs(2)},reshape(pg(ia+1:ib),szs(1:2))');
                    axis 'xy'
                    if any(m=='c')
                        colorbar;
                    end
                    if nrow*ncol-i<ncol
                        xlabel(axlab(izs(1)));
                    end
                    if rem(i-1,ncol)==0
                        ylabel(axlab(izs(2)));
                    end
                    ia=ib;
                end
            else                    % graph
                plot(qax{izs(1)},reshape(permute(pg,izs),szs(1:2)),'-');
                xlabel(axlab{izs(1)});
            end
        else
            if any(m=='G')  % image
                imagesc(pg');
                axis 'xy'
                if any(m=='c')
                    colorbar;
                end
                xlabel('Input SNR (dB)');
                ylabel('Model Index');
            else            % graph
                if nx>=nq
                    plot(x,pg,'-');
                    xlabel('Input SNR (dB)');
                else
                    plot(1:nq,pg','-');
                    xlabel('Model Index');
                end
            end
        end
    end
end


% MP_SOLVE_STOKES_DIV_CONFORMING: Solve a Stokes flow problem on a multipatch domain with
% divergence-conforming spaces, using an interior penalty method between patches.
%
% The function solves the Stokes problem
%
%   -div(mu(x) grad(vel)) + grad(press) = f    in Omega
%                              div(vel) = 0    in Omega
%                                   vel = h    on Gamma_D
%
% USAGE:
%
%  [geometry, msh, space_v, vel, space_p, press] = ...
%                        mp_solve_stokes_div_conforming (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - f:            force term
%    - h:            function for Dirichlet boundary condition
%    - viscosity:    viscosity coefficient (mu in the equation)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:       degree of the spline functions for pressure
%    - regularity:   continuity of the spline functions for pressure
%    - nsub:       number of subelements with respect to the geometry mesh
%                   for the pressure space (nsub=1 leaves the mesh unchanged)
%    - nquad:        number of points for Gaussian quadrature rule
%    - element_name: one of {RT,NDL}, specify how to build the velocity
%                    space from the data for the pressure space
%                     +RT  is the generalized Raviart-Thomas element
%                     +NDL is the generalized Nedelec element of the second family
% OUTPUT:
%
%  geometry: array of geometry structures (see geo_load)
%  msh:      multipatch mesh, consisting of several Cartesian meshes (see msh_multipatch)
%  space_v:  multipatch space, formed by several tensor product spaces plus the connectivity (see sp_multipatch).
%              Only the normal component is continuous at the interfaces.
%  vel:      the computed degrees of freedom for the velocity
%  space_p:  multipatch space for the pressure (see sp_multipatch). The functions are discountinuous at the interfaces.
%  press:    the computed degrees of freedom for the pressure
%
%  See also EX_STOKES_BIFURCATION_2D_RT_MP for an example
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2010, 2011, 2015, 2020 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [geometry, msh, space_v, vel, space_p, press] = ...
              mp_solve_stokes_div_conforming (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

if (strcmpi ('element_name', 'TH') || strcmpi ('element_name', 'SG'))
  error ('For TH and SG spaces, use mp_solve_stokes');
end

% Construct geometry structure, and information for interfaces and boundaries
[geometry, boundaries, interfaces, ~, boundary_interfaces] = mp_geo_load (geo_name);
npatch = numel (geometry);

msh = cell (1, npatch);
spv = cell (1, npatch);
spp = cell (1, npatch);
for iptc = 1:npatch
% Construct msh structure using the finest mesh
  msh_breaks = msh_set_breaks (element_name, ...
                                       geometry(iptc).nurbs.knots, nsub);
  rule      = msh_gauss_nodes (nquad);
  [qn, qw]  = msh_set_quad_nodes (msh_breaks, rule);
  msh{iptc} = msh_cartesian (msh_breaks, qn, qw, geometry(iptc));

% Construct space structure
  [spv{iptc}, spp{iptc}] = sp_bspline_fluid (element_name, ...
               geometry(iptc).nurbs.knots, nsub, degree, regularity, msh{iptc});
end

msh = msh_multipatch (msh, boundaries);
space_v = sp_multipatch (spv, msh, interfaces, boundary_interfaces);
space_p = sp_multipatch (spp, msh, interfaces, boundary_interfaces);
clear spv spp

% Compute and assemble the matrices
if (msh.rdim == 2)
  fun_one = @(x, y) ones (size(x));
elseif (msh.rdim == 3)
  fun_one = @(x, y, z) ones (size(x));
end

A = op_gradu_gradv_mp (space_v, space_v, msh, viscosity);
B = op_div_v_q_mp (space_v, space_p, msh);
E = (op_f_v_mp (space_p, msh, fun_one)).';
F = op_f_v_mp (space_v, msh, f);

vel   = zeros (space_v.ndof, 1);
press = zeros (space_p.ndof, 1);

% Apply DG techniques on the interfaces
A = A + mp_dg_penalty (space_v, msh, interfaces, viscosity, Cpen);

% Apply Neumann boundary conditions
rhs_nmnn = zeros(space_v.ndof,1);
Nbnd = cumsum ([0, boundaries.nsides]);
for iref = nmnn_sides
  iref_patch_list = Nbnd(iref)+1:Nbnd(iref+1);
  gref = @(varargin) g(varargin{:},iref);

  for bnd_side = 1:msh.boundaries(iref).nsides
    iptc = msh.boundaries(iref).patches(bnd_side);
    iside = msh.boundaries(iref).faces(bnd_side);

    msh_side = msh_eval_boundary_side (msh.msh_patch{iptc}, iside);
    msh_side_from_interior = msh_boundary_side_from_interior (msh.msh_patch{iptc}, iside);

    sp_bnd = space_v.sp_patch{iptc}.constructor (msh_side_from_interior);
    sp_bnd = sp_precompute (sp_bnd, msh_side_from_interior, 'value', true);
%     sp_bnd.dofs = 1:sp_bnd.ndof;

    x = cell (msh_side.rdim, 1);
    for idim = 1:msh_side.rdim
      x{idim} = reshape (msh_side.geo_map(idim,:,:), msh_side.nqn, msh_side.nel);
    end
    gval = reshape (gref(x{:}), msh.rdim, msh_side.nqn, msh_side.nel);
    rhs_nmnn(space_v.gnum{iptc}) = rhs_nmnn(space_v.gnum{iptc}) + ...
      op_f_v (sp_bnd, msh_side, gval);
  end
end

% Apply Dirichlet boundary conditions
[N_mat, N_rhs] = sp_weak_drchlt_bc_stokes (space_v, msh, drchlt_sides, h, viscosity, Cpen);
A = A - N_mat; F = F + N_rhs;
[vel_drchlt, drchlt_dofs] = sp_drchlt_l2_proj_udotn (space_v, msh, drchlt_sides, h);
vel(drchlt_dofs) = vel_drchlt;

int_dofs = setdiff (1:space_v.ndof, drchlt_dofs);
nintdofs = numel (int_dofs);
rhs_dir  = -A(int_dofs, drchlt_dofs)*vel(drchlt_dofs);

% Solve the linear system
if (isempty (nmnn_sides))
  mat = [A(int_dofs, int_dofs), -B(:,int_dofs).', sparse(nintdofs, 1);
         -B(:,int_dofs),        sparse(space_p.ndof, space_p.ndof), E.';
         sparse(1, nintdofs),   E, 0];
  rhs = [F(int_dofs) + rhs_dir;
         B(:, drchlt_dofs)*vel(drchlt_dofs);
         0];
  sol = mat \ rhs;
  vel(int_dofs) = sol(1:nintdofs);
  press = sol(1+nintdofs:end-1);
else
% With natural boundary condition, the constraint on the pressure is not needed.
  mat = [ A(int_dofs, int_dofs), -B(:,int_dofs).';
         -B(:,int_dofs),         sparse(size (B,1), size (B,1))];
  rhs = [F(int_dofs) + rhs_dir + rhs_nmnn(int_dofs);
         B(:, drchlt_dofs)*vel(drchlt_dofs)];
  sol = mat \ rhs;
  vel(int_dofs) = sol(1:nintdofs);
  press = sol(1+nintdofs:end);
end

end