function pic_serial ( )

%*****************************************************************************80
%
%% PIC_SERIAL applies the Particle in Cell method to an electrostatic problem.
%
%  Discussion:
%
%    Simple Electrostatic Particle In Cell (PIC) code in MATLAB
%    Flow of solar wind around a charged plate
%
%    For more, visit http://www.particleincell.com/2010/es-pic-method/
%    and http://www.particleincell.com/2011/particle-in-cell-example/
%
%  Modified:
%
%    27 July 2013
%
%  Parameters:
%
%    Local, real A(NX*NY,NX*NY), the finite difference stencil matrix.
%
%    Local, real ACC, the acceleration experienced by a particle.
%
%    Local, real AMU, the atomic mass unit.
%
%    Local, integer BOX(2,2), the indices of the lower left and upper right
%    corners of the internal obstruction.
%
%    Local, real CHG(NX,NY), the charge distribution.
%
%    Local, real DEBYE, the Debye length.
%
%    Local, real DEN(NX,NY), the charge density.
%
%    Local, real DH, the "diameter" of a single cell.
%
%    Local, real DT, the time step.
%
%    Local, real E(2), the electric field experienced by a particle.
%
%    Local, real EFX(NX,NY), the X component of the electric field.
%
%    Local, real EFY(NX,NY), the Y component of the electric field.
%
%    Local, real EPS0, the permittivity of free space.
%
%    Local, real F, the Lorentz force experienced by a particle.
%
%    Local, real FLUX, the flux of entering particles.
%
%    Local, real K, the Boltzmann constant.
%    Oddly enough, this value seems never to be used.
%
%    Local, real M, the ion mass for molecular oxygen.
%
%    Local, real MP_Q, the macro-particle charge.
%
%    Local, integer N0, the average particle density per cubic meter.
%
%    Local, integer NN, the total number of nodes.
%
%    Local, integer NP, the number of particles.
%
%    Local, integer NP_INSERT, the number of (computational) particles inserted
%    per time step.
%
%    Local, real NPT, the number of real particles created per time step.
%
%    Local, integer NX, the number of nodes in the X direction.
%
%    Local, integer NY, the number of nodes in the Y direction.
%
%    Local, integer PART_MAX, the maximum number of particles.
%    Here, this is set to 20,000.
%
%    Local, real PART_V(NP,2), the X and Y velocities for each particle.
%
%    Local, real PART_X(NP,2), the X and Y coordinates for each particle.
%
%    Local, real PHI(NX,NY), the potential.
%
%    Local, real PHI_P, the wall potential.
%
%    Local, real PHI0, the reference potential.
%
%    Local, real QE, the elementary charge.
%
%    Local, integer SEED, a seed for the random number generator.
%
%    Local, real SPWT, the specific weight, real particles per macroparticle.
%
%    Local, integer STEP, the current time step.
%
%    Local, integer STEP_NUM, the number of time steps.
%
%    Local, real TE, the electron temperature in eV.
%
%    Local, real TI, the ion velocity in eV.
%
%    Local, real V_DRIFT, the ion injection velocity, 7 km/s.
%
%    Local, real VTH, the thermal velocity.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PIC_SERIAL\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Apply the Particle in Cell (PIC) method to\n' );
  fprintf ( 1, '  an electrostatic problem that models the flow of\n' )
  fprintf ( 1, '  the "solar wind" around an electrically charged plate.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  It is worth considering how to turn this serial program\n' );
  fprintf ( 1, '  into one that uses parallel programming techniques.\n' );
%
%  Initialize the random number generator.
%
  seed = 123456789;
% rng ( seed );
%
%  Set physical constants.
%
  eps0 = 8.854e-12;
  qe = 1.602e-19;
  k = 1.381e-23;
  amu = 1.661e-27;
  m = 32.0 * amu;
%
%  Set problem parameters.
%
  n0 = 1.0e12;
  phi0 = 0.0;
  te = 1.0;
  ti = 0.1;
  v_drift = 7000.0;
  phi_p = -5.0;
%
%  Calculate plasma parameters.
%
  debye = sqrt ( eps0 * te / ( n0 * qe ) );
  vth = sqrt ( 2.0 * qe * ti / m );
%
%  Set the simulation domain.
%
  nx = 16;
  ny = 10;
  nn = nx * ny;
  step_num = 200;
  dh = debye;
  np_insert = ( ny - 1 ) * 15;
%
%  Compute other quantities.
%
  dt = 0.1 * dh / v_drift;
  width = ( nx - 1 ) * dh;
  height = ( ny - 1 ) * dh;
%
%  Indices of the corners of the obstruction.
%
  ox1 = floor ( nx / 3 );
  ox2 = floor ( nx / 3 ) + 2;
  oy1 = 1;
  oy2 = floor ( ny / 2 );
%
%  Indices of the corners of the obstruction, in an array.
%
  box(1,1) = floor ( nx / 3 );
  box(1,2) = floor ( nx / 3 ) + 2;
  box(2,1) = 1;
  box(2,2) = floor ( ny / 2 );
%
%  Calculate the specific weight.
%
  flux = n0 * v_drift * height;
  npt = flux * dt;
  spwt = npt / np_insert;
  mp_q = 1.0;
  part_max = 20000;
%
%  Allocate particle arrays.
%
  part_x = zeros(part_max,2);
  part_v = zeros(part_max,2);
%
%  Set the finite difference stencil matrix.
%
  A = stencil ( nx, ny, dh, box );

  phi = ones ( nx, ny ) * phi0;

  np = 0;
%
%   Time loop
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Time  Particles\n' );
  fprintf ( 1, '\n' );

  for step = 1 : step_num
%
%  1. CALCULATE THE CHARGE DENSITY.
%
    chg = zeros(nx,ny);

    for p = 1 : np

      fi = 1.0 + part_x(p,1) / dh;
      i = floor ( fi );
      hx = fi - i;

      fj = 1.0 + part_x(p,2) / dh;
      j = floor ( fj );
      hy = fj - j;

      chg(i,  j)   = chg(i,  j)   + ( 1.0 - hx ) * ( 1.0 - hy );
      chg(i+1,j)   = chg(i+1,j)   +         hx   * ( 1.0 - hy );
      chg(i,  j+1) = chg(i,  j+1) + ( 1.0 - hx ) *         hy;
      chg(i+1,j+1) = chg(i+1,j+1) +         hx   *         hy;

    end
%
%  Calculate the density.
%  Along the boundaries, the calculated density must be doubled.
%  Add a density floor for plotting, and to help the solver.
%
    den(1:nx,1:ny) = spwt * mp_q * chg(1:nx,1:ny) / ( dh * dh );

    den(1,:)  = 2.0 * den(1,:);
    den(nx,:) = 2.0 * den(nx,:);
    den(:,1)  = 2.0 * den(:,1);
    den(:,ny) = 2.0 * den(:,ny);

    den = den + 10000.0;
%
%  2. CALCULATE THE POTENTIAL.
%
    phi = eval_2dpot_gs ( nx, ny, phi, A, den, n0, phi0, te, phi_p, box, ...
      eps0, qe );
%
%  3. CALCULATE THE ELECTRIC FIELD.
%  Use central differences at internal nodes,
%  forward or backward differences at boundaries.
%
    efx = zeros(nx,ny);
    efy = zeros(nx,ny);

    efx(2:nx-1,:) = ( phi(1:nx-2,:) - phi(3:nx,:) ) / ( 2.0 * dh );
    efy(:,2:ny-1) = ( phi(:,1:ny-2) - phi(:,3:ny) ) / ( 2.0 * dh );

    efx(1,:)  = ( phi(1,:)    - phi(2,:) )  / dh;
    efx(nx,:) = ( phi(nx-1,:) - phi(nx,:) ) / dh;
    efy(:,1)  = ( phi(:,1)    - phi(:,2) )  / dh;
    efy(:,ny) = ( phi(:,ny-1) - phi(:,ny) ) / dh;
%
%  4. GENERATE NEW PARTICLES
%
    if ( part_max - np <= np_insert  )
%     np_insert = part_max - np;
    end
%
%  The new particles have coordinates randomly chosen within the first X layer,
%  and any Y layer.
%
    part_x(np+1:np+np_insert,1) = rand ( np_insert, 1 ) * dh;
    part_x(np+1:np+np_insert,2) = rand ( np_insert, 1 ) * height;
%
%  Sample Maxwellian in x and y, add drift velocity in x.
%
    part_v(np+1:np+np_insert,1) = v_drift ...
      + ( - 1.5 + rand(np_insert,1) + rand(np_insert,1) ...
      + rand(np_insert,1) ) * vth;

    part_v(np+1:np+np_insert,2) = 0.5 ...
      * ( - 1.5 + rand(np_insert,1) + rand(np_insert,1) ...
      + rand(np_insert,1) ) * vth;

    np = np + np_insert;
%
%  5. MOVE ALL THE PARTICLES
%
    p = 1;

    while ( p <= np )

      fi = 1.0 + part_x(p) / dh;
      i = floor ( fi );
      hx = fi - i;

      fj = 1.0 + part_x(p,2) / dh;
      j = floor ( fj );
      hy = fj - j;

      e = [ 0.0, 0.0 ];
      e =     [ efx(i,j),     efy(i,j)     ] * ( 1.0 - hx ) * ( 1.0 - hy );
      e = e + [ efx(i+1,j),   efy(i+1,j)   ] *         hx   * ( 1.0 - hy );
      e = e + [ efx(i,j+1),   efy(i+1,j)   ] * ( 1.0 - hx ) *         hy;
      e = e + [ efx(i+1,j+1), efy(i+1,j+1) ] *         hx   *         hy;
%
%  Compute the Lorentz force and the corresponding acceleration.
%  Then update the particle velocity and position.
%
      f = qe * e;
      acc = f / m;
      part_v(p,:) = part_v(p,:) + acc * dt;
      part_x(p,:) = part_x(p,:) + part_v(p,:) * dt;
%
%  The bottom boundary is reflective.
%
      if ( part_x(p,2) < 0 )
        part_x(p,2) = - part_x(p,2);
        part_v(p,2) = - part_v(p,2);
      end
%
%  Is the particle inside the plate?
%
      in_box = ( box(1,1) <= i && i < box(1,2) && ...
                 box(2,1) <= j && j < box(2,2) );
%
%  Particle is absorbed if it passes left, right or top boundaries,
%  or is inside the plate.
%  Kill the particle by replacing it with the last particle.
%
      if ( part_x(p,1) < 0.0 || width <= part_x(p,1) || height <= part_x(p,2) || in_box )
        part_x(p,:) = part_x(np,:);
        part_v(p,:) = part_v(np,:);
        np = np - 1;
        p = p - 1;
      end

      p = p + 1;

    end
%
%  6. PLOT RESULTS
%
    if ( mod ( step, 25 ) == 0 || step == step_num )

      figure ( 1 );
      clf
      hold on
      contourf ( den' );
      colorbar;
      patch ( [ ox1 ox2 ox2 ox1 ], [ oy1, oy1, oy2, oy2 ], 'w' )
      title ( sprintf ( 'Density on step %i', step ), 'Fontsize', 16 );
      hold off;

      figure ( 2 );
      clf
      hold on
      contourf ( phi' );
      patch ( [ ox1 ox2 ox2 ox1 ], [ oy1, oy1, oy2, oy2 ], 'w' )
      title ( sprintf ( 'Potential on step %i', step ), 'Fontsize', 16 );
      colorbar;
      hold off

      drawnow;

    end

    fprintf ( 1, '  %6i  %9i\n', step, np );

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PIC_SERIAL\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function phi = eval_2dpot_gs ( nx, ny, phi, A, den, n0, phi0, te, phi_p, ...
  box, eps0, qe )

%*****************************************************************************80
%
%% EVAL_2DPOT_GS computes the 2D potential using Gauss-Seidel iteration.
%
%  Discussion:
%
%    For more information, refer to
%      http://www.particleincell.com/2010/es-pic-method/
%    and
%      http://www.particleincell.com/2011/particle-in-cell-example/
%
%  Modified:
%
%    05 July 2013
%
%  Parameters:
%
%    Input, integer NX, NY, the number of nodes in the X and Y directions.
%
%    Input, real PHI(NX,NY), the current potential function.
%
%    Input, real A(NX*NY,NX*NY), the finite difference stencil matrix.
%
%    Input, real DEN(NX,NY), the charge density.
%
%    Input, real PHI0, the reference potential.
%
%    Input, real TE, the electron temperature in eV.
%
%    Input, real EPS0, the permittivity of free space.
%
%    Input, real QE, the elementary charge.
%
%    Output, real PHI(NX,NY), the updated potential function.
%
  nn = nx * ny;
%
%  Solver tolerance.
%
  tol = 0.1;
%
%  Convert the density and potential into column vectors.
%
  den = reshape ( den, numel(den), 1 );
  phi = reshape ( phi, numel(phi), 1 );
%
%  Carry out the Gauss-Seidel iteration.
%
  for it = 1 : 2000
%
%  Recalculate the right hand side, adding Boltzmann term for the electrons.
%
    b = den - n0 * exp ( ( phi - phi0 ) / te );
    b = - b * qe / eps0;
%
%  Set the boundaries.
%  Zero electric field on y = 0, y = L, x = L.
%  Fixed potential on X = 0.
%
    b(1:nx) = 0.0;
    b(nn-nx+1:nn) = 0.0;
    b(nx:nx:nn) = 0.0;
    b(1:nx:nn) = phi0;
%
%  Set the potential on the fixed nodes.
%
    for j = box(2,1) : box(2,2)
      b([box(1,1):box(1,2)]+(j-1)*nx) = ones ( box(1,2)-box(1,1)+1, 1 ) * phi_p;
    end
%
%  Apply the Gauss-Seidel update to the current solution estimate.
%
    for i = 1 : nn
      phi(i) = ( b(i) - A(i,1:i-1)  * phi(1:i-1) ...
                      - A(i,i+1:nn) * phi(i+1:nn) ) / A(i,i);
    end
%
%  Compute the residual.
%
    if ( mod ( it, 10 ) == 0 )
      res = norm ( b - A * phi );
      if ( res <= tol )
        phi = reshape ( phi, nx, ny );
        return;
      end
    end

  end
%
%  Check if the solver converged.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EVAL_2DPOT_GS - Warning\n' );
  fprintf ( 1, '  The Gauss-Seidel iteration did not converge.\n' );
  fprintf ( 1, '  Residual norm = %g\n', res );

  phi = reshape ( phi, nx, ny );

  return
end
function A = stencil ( nx, ny, dh, box )

%*****************************************************************************80
%
%% STENCIL sets up the finite difference stencil matrix.
%
%  Modified:
%
%    05 July 2013
%
%  Parameters:
%
%    Input, integer NX, NY, the number of nodes in the X and Y directions.
%
%    Local, real DH, the "diameter" of a single cell.
%
%    Input, integer BOX(2,2), contains the coordinates of the lower left
%    and upper right corners of the box.
%
%    Output, real A(NX*NY,NX*NY), the finite difference stencil matrix.
%
  A = zeros ( nx*ny, nx*ny );
%
%  For internal nodes, here are the node numberings for the
%  north, west, central, east, and south locations:
%
%             u-nx
%      u-1    u      u+1
%             u+nx
%
  for j = 2 : ny - 1
    for i = 2 : nx - 1

      u = ( j - 1 ) * nx + i;

      A(u,u)    = -4.0 / ( dh * dh );
      A(u,u-1)  =  1.0 / ( dh * dh );
      A(u,u+1)  =  1.0 / ( dh * dh );
      A(u,u-nx) =  1.0 / ( dh * dh );
      A(u,u+nx) =  1.0 / ( dh * dh );

    end
  end
%
%  Neumann boundary on y=0
%
  j = 1;
  for i = 1 : nx
    u = ( j - 1 ) * nx + i;
    A(u,u)    = -1.0 / dh;
    A(u,u+nx) =  1.0 / dh;
  end
%
%  Neumann boundary on y=height
%
  j = ny;
  for i = 1 : nx
    u = ( j - 1 ) * nx + i;
    A(u,u-nx) =  1.0 / dh;
    A(u,u)    = -1.0 / dh;
  end
%
%  Neumann boundary on x=width
%
  i = nx;
  for j = 1 : ny
    u = ( j - 1 ) * nx + i;
    A(u,:) = zeros ( 1, nx * ny );
    A(u,u-1) =  1.0 / dh;
    A(u,u)   = -1.0 / dh;
  end
%
%  Dirichlet boundary on x=0
%
  i = 1;
  for j = 1 : ny
    u = ( j - 1 ) * nx + i;
    A(u,:) = zeros ( 1, nx * ny );
    A(u,u) = 1.0;
  end
%
%  Dirichlet boundary on nodes corresponding to the plate
%
  for j = box(2,1) : box(2,2)
    for i = box(1,1) : box(1,2)
      u = ( j - 1 ) * nx + i;
      A(u,:) = zeros ( 1, nx * ny );
      A(u,u) = 1.0;
    end
  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


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

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

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

for i=1:c
    % Initialize
    y=x;
    % Make n normal random devaiates
    normal=sort(randn(size(y)));
    % Sort y and extract the ranks
    [y,T]=sort(y);
    [T,r]=sort(T);
    % Assign the ranks of y to the normal deviates and apply the phase
    %  randomization
    normal=phaseran(normal(r));
    % Extract the ranks of the phase randomized normal deviates
    [normal,T]=sort(normal);
    [T,r]=sort(T);
    % Assign the ranks of the phase randomized normal deviates to y and
    %  obtain the AAFT surrogates
    s(:,i)=y(r);
end


function [ xy, w ] = triangle_nco_rule ( rule, order_num )

%*****************************************************************************80
%
%% TRIANGLE_NCO_RULE returns the points and weights of an NCO rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer ORDER_NUM, the order (number of points) of the rule.
%
%    Output, real XY(2,ORDER_NUM), the points of the rule.
%
%    Output, real W(ORDER_NUM), the weights of the rule.
%

%
%  Get the suborder information.
%
  suborder_num = triangle_nco_suborder_num ( rule );

  suborder = triangle_nco_suborder ( rule, suborder_num );

  [ suborder_xyz, suborder_w ] = triangle_nco_subrule ( rule, suborder_num );
%
%  Expand the suborder information to a full order rule.
%
  o = 0;

  for s = 1 : suborder_num

    if ( suborder(s) == 1 )

      o = o + 1;
      xy(1:2,o) = suborder_xyz(1:2,s);
      w(o) = suborder_w(s);

    elseif ( suborder(s) == 3 )

      for k = 1 : 3
        o = o + 1;
        xy(1,o) = suborder_xyz ( i4_wrap(k,  1,3), s );
        xy(2,o) = suborder_xyz ( i4_wrap(k+1,1,3), s );
        w(o) = suborder_w(s);
      end

    elseif ( suborder(s) == 6 )

      for k = 1 : 3
        o = o + 1;
        xy(1,o) = suborder_xyz ( i4_wrap(k,  1,3), s );
        xy(2,o) = suborder_xyz ( i4_wrap(k+1,1,3), s );
        w(o) = suborder_w(s);
      end

      for k = 1 : 3
        o = o + 1;
        xy(1,o) = suborder_xyz ( i4_wrap(k+1,1,3), s );
        xy(2,o) = suborder_xyz ( i4_wrap(k,  1,3), s );
        w(o) = suborder_w(s);
      end

    else

      fprintf ( 1, '\n' );
      fprintf ( 1, 'TRIANGLE_NCO_RULE - Fatal error!\n' );
      fprintf ( 1, '  Illegal SUBORDER(%d) = %d\n', s, suborder(s) );
      error ( 'TRIANGLE_NCO_RULE - Fatal error!' );

    end

  end

  return
end


function value = imDynamicRange(img, bRobust, type)
%
%
%        value = imDynamicRange(img, bRobust, type)
%
%
%        Input:
%           -img: the input image
%           -bRobust: if bRobust > 0 --> robust statistics for min and max
%                     luminance values. bRboust becomes the percentile!
%           -type: 'Classic', 'Michelson', and 'Weber'
%
%        Output:
%           -value(1): dynamic range of img
%           -value(2): dynamic range of img in f-stops (if 'Classic')
%           -value(3): dynamic range of img in log10 space space (if
%           'Classic')
%
%     Copyright (C) 2011-20  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('bRobust', 'var'))
    bRobust = 0;
end

if(~exist('type', 'var'))
    type = 'Classic';
end

if(bRobust >= 0.5)
   bRobust = 0.01;
end

L = lum(img);

if(bRobust > 0.0)
    minL = MaxQuart(L, bRobust);
    maxL = MaxQuart(L, 1 - bRobust);
else
    minL = min(L(:));
    maxL = max(L(:));
end

if(minL < 1e-6)
    warning('minL is less than 1e-6 cd/m^2');
end

if(minL <= 0.0)
    warning('minL is 0.0 is set to the first value greater than zero.');

    minL = min(min(L(L > 0)));
end

switch type

    case 'Classic'
        value(1) = maxL / minL;
        value(2) = log2(value(1));
        value(3) = log10(value(1));

    case 'Michelson'
        value = (maxL - minL) / (maxL + minL);

    case 'Weber'
        value = (maxL - minL) / minL;
end

end


function [Mc, Mc95, Mc90] = calc_McBest(magnitudes, binInterval)
    % CALC_MCBEST calculate best Magnitude of Completion
    % [fMc, fMc95, fMc90] = calc_McBest(magnitudes, binInterval)
    % each row of magitudes gets its own calculation

    % report_this_filefun();

    % Magnitude increment
    if ~exist('binInterval','var')
        binInterval = 0.1;
    end
    magnitudes = sort(magnitudes);

    half_bin   = binInterval / 2;
    % First estimation of magnitude of completeness (maximum curvature)
    McStarts = maxCurvature(magnitudes, [-2 : binInterval : 6] , half_bin); % vectorized

    % cheat to allow this to handle multiple rows of magnitudes (independently)
    for jj = 1:size(magnitudes,2)
        valid = ~isnan(magnitudes(:,jj));
        [Mc(jj), Mc95(jj), Mc90(jj)] = calc_McBest_unvectorized(McStarts(jj), magnitudes(valid,jj), binInterval, half_bin);
    end
end

function  [fMc, fMc95, fMc90] = calc_McBest_unvectorized(McStart, magnitudes, binInterval, half_fBin)
    magCenters  = (McStart - 0.9) : binInterval : (McStart + 1.5);
    eachedge    = [magCenters - half_fBin , magCenters(end) + half_fBin];

    magnitudes = flipud(magnitudes); % from biggest to smallest
    nGtEdge = sum(magnitudes > eachedge((end-1) : -1 : 1)); % magnitudes(nGtEdge) gives last event above threshhold
    nGtEdge = fliplr(nGtEdge);
    too_few = nGtEdge < 25;

    results=nan(numel(magCenters),1);
    for idx = 1:numel(magCenters)
        if too_few(idx)
            continue
        end
        hypotheticalMc  = magCenters(idx);
        results(idx) = doCalculation(magnitudes(1:nGtEdge(idx)), binInterval, hypotheticalMc);
    end

    % Evaluation of results

    % Is fMc90 available
    nSel = find(results < 10, 1, 'first' );
    if isempty(nSel)
        fMc90 = NaN;
    else
        fMc90 = magCenters(nSel);
    end

    % Is fMc95 available
    nSel = find(results < 5, 1 );
    if isempty(nSel)
        fMc95 = NaN;
    else
        fMc95 = magCenters(nSel);
    end

    % take results from bins. (I tested against discretize, and this was faster -CGR)

    j =  find(results < 10 , 1, 'first');
    if isempty(j)
        j =  find(results < 15 , 1, 'first' );
    end
    if isempty(j)
        j =  find(results < 20 , 1, 'first' );
    end
    if isempty(j)
        j =  find(results < 25 , 1, 'first' );
    end
    fMc = magCenters(j);
    if isempty(fMc)
        fMc = NaN;
    end
end

function [result] = doCalculation(theseMags, binInterval, hypotheticalMc)
        fBValue   = calc_bmemag(theseMags, binInterval);
        half_fBin = binInterval ./ 2;

        % log10(N)=A-B*M
        vMag    = hypotheticalMc:binInterval:15; % Ending magnitude must be sufficiently high (???what is "sufficently high")
        vNumber = 10.^(log10(numel(theseMags)) - fBValue*(vMag - hypotheticalMc));
        vNumber = round(vNumber);

        % PM=vMag(1:ct);
        PMedges     = [vMag-half_fBin , vMag(end)+half_fBin];
        [bval, ~]   = histcounts(theseMags, PMedges);
        b3          = cumsum(bval,'reverse');    % N for M >= (counted backwards)
        result      = sum(abs(b3 - vNumber)) / sum(b3)*100; %res2
end

function Mc = maxCurvature(m, centers, halfBinwidth)
    % MAXCURVATURE First estimation of magnitude of completeness (maximum curvature)
    % Mc = MAXCURVATURE(m, min_max, binwidth)
    %
    % vectorized
    edges           = [centers - halfBinwidth , centers(end) + halfBinwidth];
    [vEvents, ~]    = histc(m, edges, 1);
    [~,idx] = max(flipud(vEvents));
    nSel = size(vEvents,1) - idx + 1;
    % nSel    = find(vEvents == max(vEvents), 1, 'last' );
    Mc      = centers(nSel);
end


function [out] = evap_18(p1,p2,p3,S,Ep)
%evap_18

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Exponentially declining evaporation from deficit store
% Constraints:  -
% @(Inputs):    p1   - linear scaling parameter [-]
%               p2   - linear scaling parameter [-]
%               p3   - storage scaling parameter [mm]
%               S    - current storage [mm]
%               Ep   - potential evapotranspiration rate [mm/d]

out = p1.*exp(-1.*p2.*S./p3).*Ep;

end


function A = nnls_spatial(Y, A, C, active_pixel, maxN)
%% run HALS by fixating all spatial components
% input:
%   Y:  d*T, fluorescence data
%   A:  d*K, spatial components
%   C:  K*T, temporal components
%   active_pixel: d*T binary matrix, indicating the nonzero elements of A
%   maxN: scalar, maximum number of neurons overlapping at one pixel
% output:
%   A: d*K, updated spatial components

% Author: Pengcheng Zhou, Carnegie Mellon University, adapted from Johannes

%% options for HALS
d = size(Y, 1);
K = size(C, 1);
if nargin<5;    maxN = 5;    end;    %maximum iteration number
if nargin<4;    active_pixel=true(d, K);
elseif isempty(active_pixel)
    active_pixel = true(d, K);
else
    active_pixel = logical(active_pixel);
end;     %determine nonzero pixels

%% initialization
Ymean = mean(Y,2);
Y = bsxfun(@minus, Y, Ymean);
C = bsxfun(@minus, C, mean(C,2));
CC = C*C';
YC = C*Y';
ind_fit = find(sum(active_pixel,2)>1e-9);
A = zeros(size(A));

%% updating
for m=1:length(ind_fit)
    ind = active_pixel(ind_fit(m), :);
    A(ind_fit(m), ind) = nnls(CC(ind, ind), YC(ind, ind_fit(m)), [], 1e-4, maxN);
end

function s = nnls(A, b, s, tol, maxIter)
%% fast algorithm for solving nonnegativity constrained least squared
% problem minize norm(y-K*s, 2), s.t. s>=0.

%% inputs:
%   A: n x p matrix, K'*K
%   b: n x 1 vector, K'*y
%   s: p x 1 vector, warm started s
%   tol: scalar, smallest nonzero values
%   maxIter: scalar, maximum nonzero values

%% outputs:
%   s: p x 1 vector, solution

%% Authors: Pengcheng Zhou, Carnegie Mellon University, 2016
% ported from the Python implementation from Johannes Friedrich

%% References
% Friedrich J et.al., NIPS 2016, Fast Active Set Method for Online Spike Inference from Calcium Imaging
% Bro R & Jong S, Journal of Chemometrics 1997, A FAST NON-NEGATIVITY-CONSTRAINED LEAST SQUARES ALGORITHM

%% input arguments
p = size(A,2);      % dimension of s
if ~exist('s', 'var') || isempty(s)
    s = zeros(p, 1);
end
if ~exist('tol', 'var') || isempty(tol)
    tol = 1e-9;
end
if ~exist('maxIter', 'var') || isempty(maxIter)
    maxIter = p;
end
if sum(s>0)>maxIter
    s = zeros(p,1);
end
for miter=1:maxIter
    l = b - A*s;            % negative gradient
    Pset = (s>0);       % passive set

    if max(l) < tol         % no more passive set
        break;
    end

    [~, temp] = max(l);         % choose the one with the largest gradient
    Pset(temp) = true;         % add it to the passive set
    if sum(Pset)>maxIter
        break;
    end
    % correct nonnegativity violations
    while any(Pset)
        % run unconstrained least squares for variables in passive sets
        try
            mu = A(Pset, Pset) \ b(Pset);
        catch
            mu = (A(Pset, Pset) + tol*eye(sum(Pset))) \ b(Pset);
        end

        if all(mu>tol)
            break;
        end

        temp = s(Pset) ./ (s(Pset)-mu);
        temp(mu>tol) = [];
        a = min(temp);
        s(Pset) = s(Pset) + a*(mu-s(Pset));
        Pset(s<tol) = false;
    end

    s(Pset) = mu;
end


% Fig. 9.17   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%script for Figure 5.59
sysG=tf([1],[1 0.2 1 0]);
sysH=tf(123*[1 .18 .81],[1 20 100]);
sysL=sysG*sysH;
rlocus(sysL)
title('Figure 9.17  Root locus for the system of Figure 9.18')
axis([-3 1 -1.5 1.5])
hold on
sysCL=feedback(sysG,sysH);
r=eig(sysCL);
plot(r,'*')
z=0:.1:.9;
wn= .5:.5:3;
sgrid(z, wn)
hold off


%ex2 simulates the example of Section 3.4.3
%	_______________________________________________________________________
%  /Program:  ex2.m								\
% / Description:  This program runs the simulation of the Rhors example     \
%|  	found in sections 3.4.3 of the text.  The program allows the user	 |
%|  	to choose between the three algorithms defined in the text and 		 |
%|	between the stable or unstable examples.  The program prompts the    |
%|	prompts the user to enter values for the variables that are adjusted |
%|	within the text.  This program calls and requires the fex2.m file.	 |
% \
%  \_______________________________________________________________________/

%Copyright

% Programmed Summer 1996 by Kenyon Thayer, RPI, Troy, NY

global am bm cm ap bp cp Af Bf Cf Df T Tbar G Qp Qf algor a b u

clear yp ym kpe kpf
temp=exist('Af');
if temp==1
	Af_old=Af;
	Bf_old=Bf;
	Cf_old=Cf;
	Df_old=Df;
	G_old=G;
	Qp_old=Qp;
	Qf_old=Qf;
else
	Af_old=0;
	Bf_old=-10;
	Cf_old=1;
	Df_old=0;
	G_old=-5.714;
	Qp_old=57.14;
	Qf_old=20;
end;

am=-3;bm=1;cm=3;

disp('   **This program simulates the text book example 3.4.3**');

algor=input('Which algorithm do you wish to model? 1,2,3: ');
ch2=input('Is this the stable example? Y/N: ','s');

if ch2=='n' | ch2=='N'
	%example 2 (unstable)
	ap=[-7 -92 100;1 0 0;0 1 0];bp=[1;0;0];cp=[0 0 200];
else
	%example 1 (rohrs)
	ap=[-31 -259 -229;1 0 0;0 1 0];bp=[1;0;0];cp=[0 0 458];
end;
% Initial Conditions
% ************************************************************
xp0=[0 0 0];xm0=0;yf0=0;ki0=[0 0 0 0];x0=[xp0,xm0,yf0,ki0]';
t0=0;tfinal=20;tol=1.e-6;
Af=input(sprintf('Enter a value for Af (default is %5.3f):  ',...
	Af_old));
if isempty(Af)
	Af=Af_old;
end
Bf=input(sprintf('Enter a value for Bf (default is %5.3f):  ',...
	Bf_old));
if isempty(Bf)
	Bf=Bf_old;
end
Cf=input(sprintf('Enter a value for Cf (default is %5.3f):  ',...
	Cf_old));
if isempty(Cf)
	Cf=Cf_old;
end
Df=input(sprintf('Enter a value for Df (default is %5.3f):  ',...
	Df_old));
if isempty(Df)
	Df=Df_old;
end
G=input(sprintf('Enter a value for G (default is %5.3f):  ',...
	G_old));
if isempty(G)
	G=G_old;
end
Qp=input(sprintf('Enter a value for Qp (default is %5.3f):  ',...
	Qp_old));
if isempty(Qp)
	Qp=Qp_old;
end
Qf=input(sprintf('Enter a value for Qf (default is %5.3f):  ',...
	Qf_old));
if isempty(Qf)
	Qf=Qf_old;
end

T=1;Tbar=1;

[tout,xout]=ode45('fex2',t0,tfinal,x0,tol,0);

for i=1:length(tout)
	yp(i)=cp*xout(i,[1:3])';
	ym(i)=cm*xout(i,4) ;
	yf=Cf*xout(i,5);
	eyp(i)=ym(i)-yp(i);
	xm=xout(i,4);
	um=0.3;
	if tout(i)>10
		um=-0.3;
	end;
	[ng,ng]=size(G);
	rvec=[eyp(i);-yf;xm;um];
	v1=Qp*eyp(i)-Qf*yf;
	up=inv(eye(ng)-G*rvec'*Tbar*rvec)*(xout(i,[6:9])+v1*rvec'*Tbar)*rvec;
	v=v1+G*up;
	kp=v*rvec'*Tbar;
	kpe(i)=kp(1,1);
	kpf(i)=kp(1,2);
end;

z=max(length(tout),length(ym(1,:)));
if z>length(tout)
	for i=length(tout):length(ym(1,:))
		tout(i,:)=tout((length(tout)),:);
	end;
end;
figure(1)
plot(tout,yp,tout,ym,'--')
axis([0 20 -.5 .5])
text(.41, 1.05, 'Ym', 'color', [1 0 1], 'FontSize', 12, 'Units', 'normal');
text(.51, 1.05, 'Yp', 'color', [1 1 0], 'FontSize', 12, 'Units', 'normal');


%sculling (essentially sculling+rotation) correction for each minor
%interval
%gyro & acc must be increment type (3,length) outputs. alg is the desired
%sculling algorithm
function [vinc, ainc, corr]=sculling_minor(gyro, acc, alg)
inlen=size(acc,2);
switch (alg)
    case (0)
        %ignagni(1998):algorithm 1 -> (1996):algo2
        outlen=floor((inlen-3)/2)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=3:2:inlen
            vinc(:,ind)=sum(acc(:,i-1:i),2);
            ainc(:,ind)=sum(gyro(:,i-1:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((-1/30)*gyro(:,i-2)+(11/15)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((-1/30)*acc(:,i-2)+(11/15)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case (1)
        %ignagni(1998):algorithm 2 -> (1996):algo3
        outlen=floor((inlen-3)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=3:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((9/20)*gyro(:,i-2)+(27/20)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((9/20)*acc(:,i-2)+(27/20)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(2)
        %ignagni(1998):algorithm 3 -> (1996):algo5
        outlen=floor((inlen-4)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=4:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((3/280)*gyro(:,i-3)+(57/140)*gyro(:,i-2)+(393/280)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((3/280)*acc(:,i-3)+(57/140)*acc(:,i-2)+(393/280)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(3)
        %ignagni(1998):algorithm 4 -> (1996):algo6
        outlen=floor((inlen-5)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=5:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((-1/420)*gyro(:,i-4)+(1/40)*gyro(:,i-3)+(157/420)*gyro(:,i-2)+(1207/840)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((-1/420)*acc(:,i-4)+(1/40)*acc(:,i-3)+(157/420)*acc(:,i-2)+(1207/840)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(4)
        %ignagni(1998):algorithm 5 -> (1996):algo7
        outlen=floor((inlen-4)/4)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=4:4:inlen
            vinc(:,ind)=sum(acc(:,i-3:i),2);
            ainc(:,ind)=sum(gyro(:,i-3:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((54/105)*gyro(:,i-3)+(92/105)*gyro(:,i-2)+(214/105)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((54/105)*acc(:,i-3)+(92/105)*acc(:,i-2)+(214/105)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(5) %%quadratic fit to both gyro and accel outputs (similar to the coning algorithm in ignagni(1990):algoD
        outlen=floor((inlen-3)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=3:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            %compute the polynomial coefficients
            b=(11/6)*gyro(:,i-2)+(-7/6)*gyro(:,i-1)+(1/3)*gyro(:,i);
            c=(-2)*gyro(:,i-2)+(3)*gyro(:,i-1)+(-1)*gyro(:,i);
            d=(1/2)*gyro(:,i-2)+(-1)*gyro(:,i-1)+(1/2)*gyro(:,i);

            e=(11/6)*acc(:,i-2)+(-7/6)*acc(:,i-1)+(1/3)*acc(:,i);
            f=(-2)*acc(:,i-2)+(3)*acc(:,i-1)+(-1)*acc(:,i);
            g=(1/2)*acc(:,i-2)+(-1)*acc(:,i-1)+(1/2)*acc(:,i);

            %Compute sculling(+rotation)
            corr(:,ind)=(3^2/2)*cross(b,e)+(3^3/3)*(cross(b,f)+cross(c,e)/2)+(3^4/4)*(cross(b,g)+cross(c,f)/2+cross(d,e)/3)+...
                (3^5/5)*(cross(c,g)/2+cross(d,f)/3)+(3^6/6)*cross(d,g)/3;

            ind=ind+1;
        end

    otherwise
        disp('undefined algorithm');
end


function indx = r8vec2_sort_heap_index_a ( n, x, y )

%*****************************************************************************80
%
%% R8VEC2_SORT_HEAP_INDEX_A does an indexed heap ascending sort of an R8VEC2.
%
%  Discussion:
%
%    An R8VEC2 is two R8VEC's.
%
%    An R8VEC is a vector of R8 values.
%
%    The sorting is not actually carried out.  Rather an index array is
%    created which defines the sorting.  This array may be used to sort
%    or index the array, or to sort or index related arrays keyed on the
%    original array.
%
%    ( X(I), Y(I) ) < ( X(J), Y(J) ) if:
%
%    * X(I) < X(J), or
%
%    * X(I) = X(J), and Y(I) < Y(J).
%
%    Once the index array is computed, the sorting can be carried out
%    "implicitly:
%
%      ( X(INDX(1:N)), Y(INDX(1:N) ), is sorted,
%
%    or explicitly, by the call
%
%      call dvec_permute ( n, x, indx )
%      call dvec_permute ( n, y, indx )
%
%    after which ( X(1:N), Y(1:N) ), is sorted.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real X(N),Y(N), pairs of X, Y coordinates of points.
%
%    Output, integer INDX(N), the sort index.  The
%    I-th element of the sorted array has coordinates ( X(INDX(I)), Y(INDX(I) ).
%
  if ( n < 1 )
    indx = [];
    return
  end

  indx(1:n) = 1 : n;

  if ( n == 1 )
    return
  end

  l = floor ( n / 2 ) + 1;
  ir = n;

  while ( 1 )

    if ( 1 < l )

      l = l - 1;
      indxt = indx(l);
      xval = x(indxt);
      yval = y(indxt);

    else

      indxt = indx(ir);
      xval = x(indxt);
      yval = y(indxt);
      indx(ir) = indx(1);
      ir = ir - 1;

      if ( ir == 1 )
        indx(1) = indxt;
        break
      end

    end

    i = l;
    j = l + l;

    while ( j <= ir )

      if ( j < ir )

        if ( x(indx(j)) < x(indx(j+1)) || ...
          ( x(indx(j)) == x(indx(j+1)) && y(indx(j)) < y(indx(j+1)) ) )
          j = j + 1;
        end

      end

      if ( xval < x(indx(j)) || ...
          ( xval == x(indx(j)) && yval < y(indx(j)) ) )
        indx(i) = indx(j);
        i = j;
        j = j + j;
      else
        j = ir + 1;
      end

    end

    indx(i) = indxt;

  end

  return
end


function normals = vertexNormal(vertices, faces)
%VERTEXNORMAL Compute normals to a mesh vertices.
%
%   N = vertexNormal(V, F)
%   Computes vertex normals of the mesh given by vertices V and F.
%   V is a vertex array with 3 columns, F is either a NF-by-3 or NF-by-4
%   index array, or a cell array with NF elements.
%
%   Example
%     % Draw the vertex normals of a sphere
%     s = [10 20 30 40];
%     [v f] = sphereMesh(s);
%     drawMesh(v, f);
%     view(3);axis equal; light; lighting gouraud;
%     normals = vertexNormal(v, f);
%     drawVector3d(v, normals);
%
%   See also
%     meshes3d, meshFaceNormals, triangulateFaces

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-12-19, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

nv = size(vertices, 1);
nf = size(faces, 1);

% unit normals to the faces
faceNormals = normalizeVector3d(meshFaceNormals(vertices, faces));

% compute normal of each vertex: sum of normals to each face
normals = zeros(nv, 3);
if isnumeric(faces)
    for i = 1:nf
        face = faces(i, :);
        for j = 1:length(face)
            v = face(j);
            normals(v, :) = normals(v,:) + faceNormals(i,:);
        end
    end
else
    for i = 1:nf
        face = faces{i};
        for j = 1:length(face)
            v = face(j);
            normals(v, :) = normals(v,:) + faceNormals(i,:);
        end
    end
end

% normalize vertex normals to unit vectors
normals = normalizeVector3d(normals);


% Constructing a random graph based on a given degree sequence.
% Idea source: Molloy M. & Reed, B. (1995) Random Structures and Algorithms 6, 161-179
%
% INPUTs: a graphic sequence of numbers, 1xn
% OUTPUTs: adjacency matrix of resulting graph, nxn
%
% Note: The simple version of this algorithm gets stuck about half
%       of the time, so in this implementation the last problematic
%       edge is rewired.
%
% Other routines used: adj2edgeL.m, rewireThisEdge.m, edgeL2adj.m
% GB: last updated, Oct 25 2012

function adj= randomGraphFromDegreeSequence(Nseq)

stubs=Nseq;                % assign degrees to stubs
adj = zeros(length(Nseq)); % initialize adjacency matrix

old_sum = 0;
cnt=0;

while sum(stubs)>0   % while no more stubs are left to connect

  if cnt>5                       % if rewiring did not work 5 times

    el = adj2edgeL(adj);
    ind = find(stubs>0);

    if length(ind) == 1; elr = rewireThisEdge([el; ind(1) ind(1) 1],ind(1),ind(1));  end
    if length(ind) == 2;  elr = rewireThisEdge([el; ind(1) ind(2) 1; ind(2) ind(1) 1],ind(1),ind(2)); end

    if length(ind)>2 || isempty(elr)           % restart algorithm
      printf('randomGraphFromDegreeSequence(): restarting ...\n')
      stubs = Nseq;
      adj = zeros(length(Nseq));
      old_sum = 0;
      cnt=0;

    else
      adj = edgeL2adj(elr);  % return matrix with last edge rewired
      return

    end

  end

  new_sum = sum(stubs);

  if old_sum==new_sum; cnt = cnt+1; end       % no new nodes have been connected, counter+1
  if old_sum~=new_sum; cnt=0; end             % new connections, restart count

  [~,n1] = max(stubs);                % pick the node with highest number of remaining stubs

  old_sum = sum(stubs);

  ind = find(stubs>0);
  n2 = ind(randi(length(ind)));

  if n1==n2; continue; end            % no self-loops

  if adj(n1,n2)>0; continue; end      % no double edges
  adj(n1,n2)=1; adj(n2,n1)=1;
  stubs(n1) = stubs(n1) - 1;
  stubs(n2) = stubs(n2) - 1;

end


function [ a, info ] = spofa ( a, lda, n )

%*****************************************************************************80
%
%% SPOFA factors a real symmetric positive definite matrix.
%
%  Discussion:
%
%    SPOFA is usually called by SPOCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  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 A(LDA,N), the symmetric matrix to be  factored.  Only the
%    diagonal and upper triangle are used.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(LDA,N), an upper triangular matrix R so that A = R'*R
%    where R' is the transpose.  The strict lower triangle is unaltered.
%    If INFO /= 0, the factorization is not complete.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
  for j = 1 : n

    s = 0.0;

    for k = 1 : j-1
      t = a(k,j) - sdot ( k-1, a(1:k-1,k), 1, a(1:k-1,j), 1 );
      t = t / a(k,k);
      a(k,j) = t;
      s = s + t * t;
    end

    s = a(j,j) - s;

    if ( s <= 0.0 )
      info = j;
      return
    end

    a(j,j) = sqrt ( s );

  end

  info = 0;

  return
end


%%**********************************************************
%% cheby0:
%%
%%    minimize || p(d) ||_infty
%%    p = polynomial of degree <= m such that p(0) = 1.
%%
%%    Here d = n-vector
%%----------------------------------------------------------
%% [blk,Avec,C,b,X0,y0,Z0,objval,p] = cheby0(d,m,solve);
%%
%% d = a vector.
%% m = degree of polynomial.
%% feas  = 1 if want feasible starting point
%%       = 0 if otherwise.
%% solve = 0 if just want initialization
%%       = 1 if want to solve the problem
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

function [blk,Avec,C,b,X0,y0,Z0,objval,p] = cheby0(d,m,solve);

  if nargin <= 2; solve = 0; end;
  if (size(d,1) < size(d,2)); d = d.'; end;
  cmp = 1-isreal(d);

  tstart=cputime;
  n = length(d);
  e = ones(n,1);
  V(1:n,1) = e/norm(e); R(1,1) = 1/norm(e);
  for i =1:m
      v = d.*V(:,i);
      for j = 1:i                 %% Arnoldi iterations:
          H(j,i) = (V(:,j))'*v;   %% constructing upper-Hessenberg matrix.
          v = v - H(j,i)*V(:,j);  %% orthonormaliztion of Krylov basis.
      end;
      H(i+1,i) = norm(v);
      V(:,i+1) = v/H(i+1,i);
      R(1:i+1,i+1) = (1/H(i+1,i))*([0; R(1:i,i)] - [R(1:i,1:i)*H(1:i,i); 0]);
  end
  if (cmp)
     blk{1,1} = 'q'; blk{1,2} = 3*ones(1,n);
     C = zeros(3*n,1); C(2:3:3*n) = ones(n,1);
     b = [zeros(2*m,1); -1];
     Atmp = [];
     II = [0:3:3*n-3]'; ee = ones(n,1);
     for k=1:m
         dVk = d.*V(:,k);
         Atmp = [Atmp; [2+II, k*ee, real(dVk)]; [3+II, k*ee, imag(dVk)]];
         Atmp = [Atmp; [2+II, (m+k)*ee, -imag(dVk)]; [3+II, (m+k)*ee, real(dVk)]];
     end
     Atmp = [Atmp;  [1+II, (2*m+1)*ee, -ones(n,1)]];
  else
     blk{1,1} = 'l'; blk{1,2} = 2*ones(1,n);
     b = [zeros(m,1); -1];
     C = [ones(n,1); -ones(n,1)];
     Atmp = [];
     II = [1:n]'; ee = ones(n,1);
     for k=1:m
         dVk = d.*V(:,k);
         Atmp = [Atmp; [II, k*ee, dVk]; [n+II, k*ee, -dVk]];
     end
     Atmp = [Atmp; [II, (m+1)*ee, -ee]; [n+II, (m+1)*ee, -ee]];
  end
  Avec = spconvert(Atmp);
  [X0,y0,Z0] = infeaspt(blk,Avec,C,b);
%%
  if (solve)
     [obj,X,y,Z] = sqlp(blk,Avec,C,b,[],X0,y0,Z0);
     if (cmp)
        y  = y(1:m) + sqrt(-1)*y(m+1:2*m);
     else
        y = y(1:m);
     end
     x1 = R(1:m,1:m)*y(1:m);
     p = [-x1(m:-1:1); 1];
     objval = -mean(obj);
  else
     objval = []; p = [];
  end
%%**********************************************************


function [vdeg] = trideg2(pp,tt)
%TRIDEG2 calc. topological degree for vertices in a 2-simpl-
%ex triangulation.
%   [VDEG] = TRIDEG2(VERT,TRIA) returns the no. of triangles
%   incident to each vertex. VDEG is a V-by-1 array of vert-
%   ex degrees, VERT is a V-by-D array of XY coordinates,
%   and TRIA is a T-by-3 array of vertex indexing, where
%   each row defines a triangle, such that
%   VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and
%   VERT(TRIA(II,3),:) are the coordinates of the II-TH tri-
%   angle.
%
%   See also TRISCR2, TRIVOL2, TRIANG2, TRIBAL2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 10/07/2018

%---------------------------------------------- basic checks
    if (~isnumeric(pp) || ~isnumeric(tt) )
        error('trideg2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ndims(tt) ~= +2 )
        error('trideg2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(pp,2) < +2 || size(tt,2) < +3 )
        error('trideg2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nvrt = size(pp,1) ;
    ntri = size(tt,1) ;

%---------------------------------------------- basic checks
    if (min(min(tt(:,1:3))) < +1 || ...
            max(max(tt(:,1:3))) > nvrt )
        error('trideg2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%------------------------------------- compute vertex degree
    vdeg = sum(sparse( ...
        tt(:,1:3),repmat( ...
            (1:ntri)',1,3),+1,nvrt,ntri),2) ;

end


function bas = bas3DP1(p,t)

%% USAGE: coefficient of Lagrange P1 basis for tetrahedral mesh
%
% INPUTS:
% p --- np-by-3 vector
% t --- nt-by-4 vector
%
% OUTPUTS:
% bas --- nt-4-4 matrix stores the coefficient of basis value(s)
%         The k-i-j th entry denotes the j-th basis function on k-th cell
%         with the i-th coefficient where c_i (i=1,2,3,4) stands for
%                 c_1 + c_2*x + c_3*y + c_4*z

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

%%
x1 = p(t(:,1),1); y1 = p(t(:,1),2); z1 = p(t(:,1),3);
x2 = p(t(:,2),1); y2 = p(t(:,2),2); z2 = p(t(:,2),3);
x3 = p(t(:,3),1); y3 = p(t(:,3),2); z3 = p(t(:,3),3);
x4 = p(t(:,4),1); y4 = p(t(:,4),2); z4 = p(t(:,4),3);

A = -x2.*y3.*z1 + x2.*y4.*z1 + x1.*y3.*z2 - x1.*y4.*z2 + x2.*y1.*z3 ...
    -x1.*y2.*z3 + x1.*y4.*z3 - x2.*y4.*z3 + x4.*(-y2.*z1 + y3.*z1 + ...
    y1.*z2 - y3.*z2 - y1.*z3 + y2.*z3) + (-x2.*y1 + x1.*y2 - x1.*y3 + ...
    x2.*y3).*z4 + x3.*(-y4.*z1 - y1.*z2 + y4.*z2 + y2.*(z1 - z4) + y1.*z4);

nt = size(t,1);  bas = zeros(nt,4,4);
bas(:,1,1) = (-x4.*y3.*z2 + x3.*y4.*z2 + x4.*y2.*z3 - x2.*y4.*z3 - x3.*y2.*z4 + x2.*y3.*z4);
bas(:,2,1) =-(-y3.*z2 + y4.*z2 + y2.*z3 - y4.*z3 - y2.*z4 + y3.*z4);
bas(:,3,1) = (-x3.*z2 + x4.*z2 + x2.*z3 - x4.*z3 - x2.*z4 + x3.*z4);
bas(:,4,1) =-(-x3.*y2 + x4.*y2 + x2.*y3 - x4.*y3 - x2.*y4 + x3.*y4);
bas(:,1,2) =-(-x4.*y3.*z1 + x3.*y4.*z1 + x4.*y1.*z3 - x1.*y4.*z3 - x3.*y1.*z4 + x1.*y3.*z4);
bas(:,2,2) = (-y3.*z1 + y4.*z1 + y1.*z3 - y4.*z3 - y1.*z4 + y3.*z4);
bas(:,3,2) =-(-x3.*z1 + x4.*z1 + x1.*z3 - x4.*z3 - x1.*z4 + x3.*z4);
bas(:,4,2) = (-x3.*y1 + x4.*y1 + x1.*y3 - x4.*y3 - x1.*y4 + x3.*y4);
bas(:,1,3) = (-x4.*y2.*z1 + x2.*y4.*z1 + x4.*y1.*z2 - x1.*y4.*z2 - x2.*y1.*z4 + x1.*y2.*z4);
bas(:,2,3) =-(-y2.*z1 + y4.*z1 + y1.*z2 - y4.*z2 - y1.*z4 + y2.*z4);
bas(:,3,3) = (-x2.*z1 + x4.*z1 + x1.*z2 - x4.*z2 - x1.*z4 + x2.*z4);
bas(:,4,3) =-(-x2.*y1 + x4.*y1 + x1.*y2 - x4.*y2 - x1.*y4 + x2.*y4);
bas(:,1,4) =-(-x3.*y2.*z1 + x2.*y3.*z1 + x3.*y1.*z2 - x1.*y3.*z2 - x2.*y1.*z3 + x1.*y2.*z3);
bas(:,2,4) = (-y2.*z1 + y3.*z1 + y1.*z2 - y3.*z2 - y1.*z3 + y2.*z3);
bas(:,3,4) =-(-x2.*z1 + x3.*z1 + x1.*z2 - x3.*z2 - x1.*z3 + x2.*z3);
bas(:,4,4) = (-x2.*y1 + x3.*y1 + x1.*y2 - x3.*y2 - x1.*y3 + x2.*y3);
bas = bas./A;


function [u,p] = StokesIUzawa(u,p,f,g,A,B,auxMat,elem,smootherOpt)
%% STOKESIUzawa Inexact Uzawa smoother for the Stokes eqns.
%
%  In matrix form, we solve the Stokes equations
%
%               |A B'| |u| = |f|
%               |B 0 | |p| = |g|
%
%  Define
%            M = |I -S_u^-1 B'|   T = |S_u    0|
%                |     I      |       |B   -S_p|
%  The matrix form DGS update can be written as
%
%    |uk+1|   |uk|            |ru|     with ru = f-Auk -B'pk,
%    |    | = |  | + M*inv(T)*|  |
%    |pk+1|   |pk|            |rp|     and  rp = 0-Buk
%
%  Created by Ming Wang (with discussion of Long Chen) at Jan, 2012.
%  Revised at Sept 2012.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Parameters
itStep = smootherOpt.itStep;
Su = auxMat.DA; % DA = 2*diag(A)
Bt = auxMat.Bt;
BinvDABt = auxMat.BinvDABt; % invDA = 1/(2*diag(A));
% mg options
mgoption.printlevel = 0;
mgoption.maxIt = 1;
mgoption.N0=50;
mgoption.solver = 'Vcycle';
mgoption.mu = 2;

%% DGS relaxation step
for k = 1: itStep
    % Step 1: relax Momentum eqns
    if isempty(u) && isempty(p)
        ru = f;
        u = zeros(size(f));
        p = zeros(size(g));
    else
        ru = f-Bt*p-A*u;
    end
    u = u + ru./Su;
    rp = g-B*u;
    rp = rp - mean(rp);
    % Step 2: relax transformed Continuity eqns
    dp = mg(BinvDABt,-rp,elem,mgoption);
    % Step 3: transform the correction back to the original variables.
    p = p + dp;
    u = u - (Bt*dp)./Su;
end
end


function rcov = cov_blockgeom(X,blocksize)
% like cov(), just robust (using the blockwise geometric median)
% The blocksize allows to reduce the memory requirements, at the cost of reduced robustness
% against outliers that occupy fewer samples than the blocksize.

% Copyright (C) Christian Kothe, SCCN, 2013, christian@sccn.ucsd.edu
%
% 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 should have received a copy of the GNU General Public License along with this program; if not,
% write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
% USA

if ~exist('blocksize','var')
    blocksize = 10; end

[n,m] = size(X);
X = bsxfun(@minus,X,median(X));
U = zeros(length(1:blocksize:n),m*m);
for k=1:blocksize
    range = min(n,k:blocksize:(n+k-1));
    U = U + reshape(bsxfun(@times,reshape(X(range,:),[],1,m),reshape(X(range,:),[],m,1)),size(U));
end
rcov = real(reshape(geometric_median(U/blocksize),m,m));


function [e, edata, eprior] = rbferr(net, x, t)
%RBFERR	Evaluate error function for RBF network.
%
%	Description
%	E = RBFERR(NET, X, T) takes a network data structure NET together
%	with a matrix X of input vectors and a matrix T of target vectors,
%	and evaluates the appropriate error function E depending on
%	NET.OUTFN.  Each row of X corresponds to one input vector and each
%	row of T contains the corresponding target vector.
%
%	[E, EDATA, EPRIOR] = RBFERR(NET, X, T) additionally returns the data
%	and prior components of the error, assuming a zero mean Gaussian
%	prior on the weights with inverse variance parameters ALPHA and BETA
%	taken from the network data structure NET.
%
%	See also
%	RBF, RBFFWD, RBFGRAD, RBFPAK, RBFTRAIN, RBFUNPAK
%

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

% Check arguments for consistency
switch net.outfn
case 'linear'
   errstring = consist(net, 'rbf', x, t);
case 'neuroscale'
   errstring = consist(net, 'rbf', x);
otherwise
   error(['Unknown output function ', net.outfn]);
end
if ~isempty(errstring);
  error(errstring);
end

switch net.outfn
case 'linear'
   y = rbffwd(net, x);
   edata = 0.5*sum(sum((y - t).^2));
case 'neuroscale'
   y = rbffwd(net, x);
   y_dist = sqrt(dist2(y, y));
   % Take t as target distance matrix
   edata = 0.5.*(sum(sum((t-y_dist).^2)));
otherwise
   error(['Unknown output function ', net.outfn]);
end

% Compute Bayesian regularised error
[e, edata, eprior] = errbayes(net, edata);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = svol_2(a, b, r, n, f, k, t)
% computes the SABR implied BS volatility due to Hagan's formula

	z = n./a.*(f*k).^((1-b)/2).*log(f./k);
	x = log((sqrt(1 - 2*r*z + z.^2) + z - r)/(1-r));
	Term1 = a ./ (f*k).^((1-b)/2) ./ (1 + (1-b)^2/24*log(f./k).^2 ...
        + (1-b)^4/1920*log(f./k).^4);
	Term2 = z ./ x;
	Term2(abs(x-z) < 1e-008) = 1;           % account for ATM
	Term3 = 1 + ((1-b)^2/24*a^2./(f*k).^(1-b) ...
        + r*b*n*a/4./(f*k).^((1-b)/2) + (2-3*r^2)/24*n.^2)*t;
	y = Term1.*Term2.*Term3;
end


% Dasslc test problem with classical pendulum
% requires files: pend.m

dae_index = 3;		% differential index of the DAE
g   = 9.8;			% gravity acceleration
L	 = 1.0;			% pendulum cord lenght
t0  = 0.0;        % initial value for independent variable
tf  = 10;         % final value for independent variable
y0  = [1 0 0 0 0]'; % initial state variables (overwritten by pend.dat)
rpar=[g L dae_index]; % optional arguments passed to residual and jacobian functions

index = [0 0 0 0 0	% index 0 formulation (with drift-off effect)
		   1 1 1 1 1	% index 1 formulation
			1 1 2 2 2	% index 2 formulation
         1 1 2 2 3];	% index 3 formulation

tspan=[t0:0.001:tf];
[t,y]=dasslc('pend',tspan,y0,rpar,[],[],index(dae_index+1,:),'pend.dat','jacpend');

plot(t,y);


function [varargout]=repop(varargin)
% Replicating arithemetical and logical operators.
%
% [Z]=repop(X,operator,Y [,options])
%
% Does element by element operations on X and Y where non-same sized
% dimensions are implicity wrapped round to match the size of the larger
% to give a result matrix Z with size max(size(X),size(Y));
%
% What this means is that if you give a [Nx1] and a [1xM] input you get
% a [NxM] output, or if you give it a [Nx1xQ] and a [NxMx1xP] you get
% [NxMxQxP] output etc..
% Note that the size of the input *completely and uniquely* determines the
% size of the output.
%
% In general this is at least 2x faster than the equivalent matlab code
% using repmats and has the advantage of requiring no additional memory.
%
% Example Usage:
%     X = randn(10000,10);                  % example signal with data in rows
%     stdX = repop(X,'-',mean(X,1));        % subtract the mean vector
%     stdX = repop(stdX,'/',std(stdX,0,1)); % divide by std-deviation
%
% Operator can be one of:
%
% Arthemetical -- returns a double matrix
%   '+','.+',plus   - Implicitly repmatted elementwise addition
%   '-','.-',minus  - Implicitly repmatted elementwise addition
%   '*','.*',times  - Implicitly repmatted elementwise multiplication
%   '^','.^',power  - Implicitly repmatted elementwise raise X to power Y
%   '\','.\',ldivide- Implicitly repmatted elementwise divide Y by X
%   '/','./',rdivide- Implicitly repmatted elementwise divide X by Y
%   'min'           - Implicitly repmatted elementwise min of X by Y
%   'max'           - Implicitly repmatted elementwise max of X by Y
%
% Relational -- returns a logical matrix
% N.B. for complex inputs the <,>,<=,>= operators are based upon abs(x)
% (not real(x) as in matlab)
%   '==',eq         - Implicitly repmatted elementwise equality
%   '~=',ne         - Implicitly repmatted elementwise dis-equality
%   '<' ,lt         - Implicitly repmatted elementwise less than
%   '>' ,gt         - Implicitly repmatted elementwise greater than
%   '<=',le         - Implicitly repmatted elementwise less than equal
%   '>=',ge         - Implicitly repmatted elementwise greater than equal
%
% N.B. the operator can go in any of the 3 argument positions, i.e.
% these are all valid: repop(X,'-',Y), repop(X,Y,'-'), repop('-',X,Y)
%
% The optional final argument is a string of single letter switches
% consisting off
%  'm'  -- allows replication of non-unit dimensions if the larger
%          dimensions size is an integer multiple of the smaller ones
%  'n'  -- allow replication of non-unit dimensions in *all* cases
%  'i'  -- perform "inplace" operation.  This means that we use a
%          *dangerous* matlab *hack* to perform the operation *without*
%          allocating new memory for the output, but by simply overwriting
%          the memory used for X in the input.  Thus the following code is
%          a memory (and time) efficient way to increment X.
%              X = repop(X,'+',1,'i');
%
%
% Class support of input P:
%     float: double, single
%
% SEE ALSO:   repop_testcases rplus rminus rtimes rpower rldivide rrdivide
%             req rne rlt rgt rle rge
%
% Copyright 2006-     by Jason D.R. Farquhar (jdrf@zepler.org)
% Permission is granted for anyone to copy, use, or modify this
% software and accompanying documents for any uncommercial
% purposes, provided this copyright notice is retained, and note is
% made of any changes that have been made. This software and
% documents are distributed without any warranty, express or
% implied
%
% Inspired by code from Douglas M. Schwarz and & Aki Vehtari.

% The rest of this code is a mex-hiding mechanism which compilies the mex if
% this runs and recursivly calls itself.
% Based upon code from: http://theoval.sys.uea.ac.uk/matlab
cwd  = pwd; % store the current working directory
name = mfilename('fullpath'); % get the directory where we are
% find out what directory it is defined in
name(name=='\')='/'; % deal with dos'isms
dir=name(1:max(find(name == '/')-1)); % dir is everything before final '/'
try % try changing to that directory
   cd(dir);
catch   % this should never happen, but just in case!
   cd(cwd);
   error(['unable to locate directory containing ''' name '.m''']);
end

try % try recompiling the MEX file
   fprintf(['Compiling ' mfilename ' for first use\n']);
   mex('ddrepop.c','dsrepop.c','sdrepop.c','ssrepop.c','repop_util.c','repop_mex.c','mxInfo.c','mxInfo_mex.c','-O','-output',mfilename);
   fprintf('done\n');
catch
   % this may well happen happen, get back to current working directory!
   cd(cwd);
   error('unable to compile MEX version of ''%s''%s\n%s%s', name, ...
         ', please make sure your', 'MEX compiler is set up correctly', ...
         ' (try ''mex -setup'').');
end

cd(cwd); % change back to the current working directory
rehash;  % refresh the function and file system caches

% recursively invoke MEX version using the same input and output arguments
[varargout{1:nargout}] = feval(mfilename, varargin{:});

% bye bye...

return;


function x = typeIV(alpha)
%4 times of Bit-wise MAP
alpha_1 = alpha(1 : end/4);
% llr_1 = 2*atanh(prod(tanh(alpha_1/2)));
llr_1 = prod(sign(alpha_1))*min(abs(alpha_1));

alpha_2 = alpha(end/4 + 1 : end/2);
% llr_2 = 2*atanh(prod(tanh(alpha_2/2)));
llr_2 = prod(sign(alpha_2))*min(abs(alpha_2));

alpha_3 = alpha(end/2 + 1 : end*3/4);
% llr_3 = 2*atanh(prod(tanh(alpha_3/2)));
llr_3 = prod(sign(alpha_3))*min(abs(alpha_3));

alpha_4 = alpha(end*3/4 + 1 : end);
% llr_4 = 2*atanh(prod(tanh(alpha_4/2)));
llr_4 = prod(sign(alpha_4))*min(abs(alpha_4));

%Even Parity check
check_bit = (llr_1 + llr_2 + llr_3 + llr_4) < 0;
%Wagner Decoder
x1 = alpha_1 < 0;
if mod(sum(x1), 2) ~= check_bit
    x1(abs(alpha_1) == min(abs(alpha_1))) = mod(x1(abs(alpha_1) == min(abs(alpha_1))) + 1, 2);
end

x2 = alpha_2 < 0;
if mod(sum(x2), 2) ~= check_bit
    x2(abs(alpha_2) == min(abs(alpha_2))) = mod(x2(abs(alpha_2) == min(abs(alpha_2))) + 1, 2);
end

x3 = alpha_3 < 0;
if mod(sum(x3), 2) ~= check_bit
    x3(abs(alpha_3) == min(abs(alpha_3))) = mod(x3(abs(alpha_3) == min(abs(alpha_3))) + 1, 2);
end

x4 = alpha_4 < 0;
if mod(sum(x4), 2) ~= check_bit
    x4(abs(alpha_4) == min(abs(alpha_4))) = mod(x4(abs(alpha_4) == min(abs(alpha_4))) + 1, 2);
end

x_tmp = [x1; x2; x3; x4];

x = x_tmp(:);

end


function [newRO, newRR] = mstep_update_rotation(P, S_bar, V, E_z, E_zz, RO, c, Tr)
%[newRO, newRR] = mstep_update_rotation(P, S_bar, V, E_z, E_zz, RO, Tr)

% Linearizes the expression in Eq 24 using exponential maps and
% solves for an improved rotation

% update step
tw_step = 0.3;

[K, T] = size(E_z);
J = size(S_bar, 2);

Pc = P - Tr(:)*ones(1,J);

newRR = zeros(2*T,3);
newRO = RO;

for iter=1:1,
   for t = 1:T,
      A = zeros(3);
      B = zeros(2,3);

      zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*K+1:t*K,:)];

      for j=1:J,
         H_j = [S_bar(:,j) reshape(V(:,j), 3, K)];

         A = A + H_j*zz_hat_t*H_j';

         B = B + ([Pc(t,j); Pc(t+T,j)] * [1 E_z(:,t)'] * H_j');
      end

      oldRO_t = RO{t};

      %%%%%%%%%%%%%%%%%%%%%%%%%%% Changed code here %%%%%%%%%%%%%%%%%%%%%
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %C = oldRO_t*A;
      %D = B - oldRO_t(1:2,:)*A;

      C = c(t,1)^2*oldRO_t*A;
      D = c(t,1)*B - c(t,1)^2*oldRO_t(1:2,:)*A;
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

      % now we solve the system: [1 0 0; 0 1 0]*twist*C = D
      CC = [0   C(3,1) -C(2,1)
         -C(3,1) 0 C(1,1)
         0   C(3,2) -C(2,2)
         -C(3,2) 0 C(1,2)
         0   C(3,3) -C(2,3)
         -C(3,3) 0 C(1,3)];

      DD = D(:);

      % twist optimization
      twist_vect = tw_step*pinv(CC)*DD;

      twh = [0      -twist_vect(3) twist_vect(2)
         twist_vect(3)  0      -twist_vect(1)
         -twist_vect(2) twist_vect(1)  0     ];
      dR = expm(twh);
      newRO_t = dR*oldRO_t;

      newRO{t} = newRO_t;
      newRR(t,:) = newRO_t(1,:);
      newRR(t+T,:) = newRO_t(2,:);
   end

   RO = newRO;
end


%  Figure 10.32      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% Fig. 10.32 Script for computation of the root locus of yaw response,
% with no compensation, direct feedback

f =[-0.0558   -0.9968    0.0802    0.0415;
    0.5980   -0.1150   -0.0318         0 ;
   -3.0500    0.3880   -0.4650         0 ;
         0    0.0805    1.0000         0] ;
g =[0.0073;
   -0.4750;
    0.1530;
         0];
h = [0     1     0     0];
j =[0];
% the equations of the actuator:

na=[0 10];
da=[1 10];
[fa,ga,ha,ja]=tf2ss(na,da);

% the equations of the aircraft with actuator:

[fp,gp,hp,jp]=series(fa,ga,ha,ja,f,g,h,j);
% the uncompensated rootlocus
 hold off; clf
 rlocus(fp,-gp,hp,jp);
 axis([-2, 2, -1.5, 1.5]);
 grid;
 title('Fig. 10.32 Root locus of the aircraft with positive feedback')


function [AR, lambdamax]=sim_genRndVARcoeffs(varargin)
% Generate random coefficients for a VAR model of given order and with a
% specified degree of interaction sparsity.
% Coefficients are drawn from a normal distribution with specified standard
% deviation (sigma).
%
% The function will repeatedly generate random models until it obtains a
% stable VAR process (all eigenvalues of the system matrix are less than or
% equal to 1 in magnitude).
%
%
% See Also: sim_genTVARcoeffs()
%
% References:
% [1] This function is adapted from Stefan Haufe's gen_ar_sech.m
%
% Author: Tim Mullen 2011-2013, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

g = arg_define(varargin,...
        arg({'nchs','NumChans'},10,[],'Number of channels (variables)'), ...
        arg({'morder','ModelOrder'},10,[],'Model order'), ...
        arg({'sigma','Sigma'},0.2,[0 Inf],'Scale (std) of random VAR parameters. This determines the variance of the determinstic component of the generated process.'), ...
        arg({'maxinter','MaxNumInter'},0.5,[],'Maximum number of interactions. If >= 1, this is the expected number of interacting variables (chosen randomly). If < 1, this represents the proportion of interacting variable (out of nchs^2-nchs max possible interactions). This can also be a vector specifying the linear (column-major) index of each nonzero (non-diagonal) element of the interaction (connectivity) matrix'), ...
        arg({'probinter','PerLagInteractProb'},1,[0 1],'Percent of nonzero AR coefficients for a given interaction. This applies to auto-interactions as well as cross-interactions. For instance, a value of 1 will generate nonzero gaussian iid AR filter coefficients. A value of 0.5 will generate a mixed sparse AR filter with 50% zero coefficients and remaining nonzero iid gaussian coefficients'), ...
        arg({'maxtrys','MaxAttempts','maxattempts'},Inf,[],'Max number of retries for stable model gen.'), ...
        arg({'maxlambda','MaxLambdaMag'},1,[0 1],'Largest allowable eigenvalue (magnitude) of system matrix. This determines the stability of the process. The smaller this value the more stable the process. All eigenvalues must be <= 1 in magnitude for a stable process.') ...
        );

% RandStream.setDefaultStream(RandStream('mt19937ar','seed',sum(100*clock)));

if g.maxinter < 1
    g.maxinter = ceil(g.maxinter*(g.nchs^2-g.nchs));
end

% generate canonical interaction indices
if length(g.maxinter) == 1
    inddiag     = linspace(1, g.nchs^2, g.nchs);
    indndiag    = setdiff(1:g.nchs^2, inddiag);
    per         = randperm(length(indndiag));
    indndiag    = indndiag(per);
    indndiag    = indndiag(1:g.maxinter);
    ind         = [inddiag indndiag];
else
    inddiag     = linspace(1, g.nchs^2, g.nchs);
    ind         = unique([inddiag g.maxinter]);
end
ind = sort(ind);
% generate interaction indices for all model orders
ind = tensorsum(0:g.nchs^2:g.nchs^2*(g.morder-1),ind);

% generate random coefficients for interacting variables
lambdamax = Inf; ntrys = 0;
while lambdamax > g.maxlambda && ntrys < g.maxtrys
    AR      = zeros(g.nchs,g.nchs*g.morder);
    AR(ind) = double(rand(length(ind), 1) <= g.probinter).*randn(length(ind), 1)*g.sigma;

%     for k=1:g.morder
%       aloc = zeros(g.nchs);
%       aloc(ind) = double(rand(length(ind), 1) < g.probinter).*randn(length(ind), 1)*g.sigma;
%       AR(:,:,(k-1)*g.nchs+1:k*g.nchs) = aloc;
%     end

    % check model stability
    E         = eye(g.nchs*g.morder);
    AA        = [AR;E(1:end-g.nchs,:)];
    lambda    = eig(AA);
    lambdamax = max(abs(lambda));

    ntrys = ntrys + 1;
end
if ntrys>=g.maxtrys
    fprintf('Unable to find a stable model. Maximum number of attempts exceeded\n');
    AR = [];
    return;
end


function [I,src] = rayMeasure(ray,Xmes,rad,rMax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayMeasure.m                                  |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Spherical measurement                         |
%|  `---'  |                                                              |
%+========================================================================+

% Initialization
I   = cell(1,length(ray.pos)-1);
src = cell(length(ray.pos)-1,1);
dst = zeros(length(ray),1);

% Loop
for i = 1:size(I,2)
    % Ray positions
    Pi   = ray.pos{i};
    Pip1 = ray.pos{i+1};

    % Ray direction and length
    U   = (Pip1 - Pi);
    lgt = sqrt(sum(U.^2,2));
    U   = U ./ (lgt * [1 1 1]);

    % Initial position -> micro
    PM = ones(size(Pi,1),1)*Xmes - Pi;

    % Measure triangle (pythagore)
    hyp2 = sum(PM.^2,2);
    adj  = sum(PM.*U,2);
    opp2 = hyp2 - adj.^2;

    % Measured ray
    I{i} = find( (hyp2 < lgt.^2) & (adj >= 0) & (opp2 <= rad^2) & (dst + sqrt(hyp2) < rMax) );

    % Sources (focusing)
    if ~isempty(I{i})
        src{i} = Pi(I{i},:) - (dst(I{i})*[1 1 1]) .* U(I{i},:);
    else
        src{i} = zeros(0,3);
    end

    % Total distance from initial position
    dst = dst + lgt;
end
end


function [S] = lukfPropagation(dt,chi,chiAnt,omega_b,a_b,S,omega,acc,Qc,g)
%Left-UKF on Lie Groups
q = length(S); % state size
S_aug = blkdiag(S,Qc);
N_aug = length(S_aug);

%scaled unsented transform
W0 = 1-N_aug/3;
Wj = (1-W0)/(2*N_aug);
gamma = sqrt(N_aug/(1-W0));

% Prediction
ichi = invSE3(chi);
omega = omega-omega_b; % unbiased inputs
acc = acc-a_b;
X = gamma*[zeros(N_aug,1) S_aug' -S_aug'];% sigma-points
X(10:15,:) = X(10:15,:)+X(q+7:q+12,:)*dt; % add bias noise
for j = 2:2*N_aug+1
    xi_j = X([1:9,16:q],j); %do not take bias
    w_j = X(q+1:N_aug,j);
    omega_bj = X(10:12,j);
    a_bj = X(13:15,j);
    chi_j = chiAnt*exp_multiSE3(xi_j);
    Rot = chi_j(1:3,1:3)*expSO3((omega+w_j(1:3)-omega_bj)*dt);
    v = chi_j(1:3,4)+(Rot*(acc+w_j(4:6)-a_bj)+g)*dt;
    x = chi_j(1:3,5)+v*dt;
    chi = state2chi(Rot,v,x,chi_j(1:3,6:end));
    Xi_j = ichi*chi; % can be more time efficient
    X([1:9,16:q],j) = log_multiSE3(Xi_j); %propagated sigma points
end
X = sqrt(Wj)*X;
[~,Rs] = qr(X(1:q,2:2*N_aug+1)');
S = Rs(1:q,1:q);
end


clear;
clc
close all;

addpath('../../library/nav_lib');

%% attidude

true_attitude = deg2rad([4 -6 30])';
roll_meas_err = deg2rad(0.1);
pitch_meas_err = deg2rad(-0.15);

%% Earth's Magnetic Field
flux = 45;
declination= deg2rad(10);
inclination = deg2rad(40);

delta_alpha_nE= deg2rad(0.3);

%% Other Souces of Magnetism
MAn = [2 -1.8 3]'; % local magnetic anomalies(N,E,D)
hard = [-1 -0.5 1]'; % hard-iron magnetism(body-frame axes)
soft = [0.012 -0.014 0.006; -0.008 0.007 0.017; 0.003 -0.019 -0.011]; % soft-iron magnetism, body-rame-axes

%%
Cn2b = ch_eul2m(true_attitude);

%% flux desity of Earth's magnetic filed
m_En = [cos(declination)*cos(inclination)  sin(declination)*cos(inclination) sin(inclination)]'*flux;

%% Total magnetic flux density
m_mb = hard + (eye(3) + soft)*Cn2b*(m_En + MAn);

%% Calculate Magnetic Heading Measurement
roll_meas = true_attitude(1) + roll_meas_err;
pitch_meas =  true_attitude(2) +pitch_meas_err;

yaw = atan2(-m_mb(2)*cos(roll_meas) + m_mb(3)*sin(roll_meas), m_mb(1)*cos(pitch_meas) + m_mb(2)*sin(pitch_meas)*sin(roll_meas) + m_mb(3)*cos(roll_meas)*sin(pitch_meas));

%% Calculate True Heading
data_base_magnetic_declination = declination + delta_alpha_nE

%% True heading
true_heading = yaw+ data_base_magnetic_declination;
%rad2deg(true_heading)

%% Heading error
error = true_heading - true_attitude(3);
rad2deg(error)


% RANSACFITLINE - fits line to 3D array of points using RANSAC
%
% Usage  [L, inliers] = ransacfitline(XYZ, t, feedback)
%
% This function uses the RANSAC algorithm to robustly fit a line
% to a set of 3D data points.
%
% Arguments:
%          XYZ - 3xNpts array of xyz coordinates to fit line to.
%          t   - The distance threshold between data point and the line
%                used to decide whether a point is an inlier or not.
%          feedback - Optional flag 0 or 1 to turn on RANSAC feedback
%                     information.
%
% Returns:.
%           V - Line obtained by a simple fitting on the points that
%               are considered inliers.  The line goes through the
%               calculated mean of the inlier points, and is parallel to
%               the principal eigenvector.  The line is scaled by the
%               square root of the largest eigenvalue.
%               This line is a n*2 matrix.  The first column is the
%               beginning point, the second column is the end point of the
%               line.
%           L - The two points in the data set that were found to
%               define a line having the most number of inliers.
%               The two columns of L defining the two points.
%           inliers - The indices of the points that were considered
%                     inliers to the fitted line.
%
% See also:  RANSAC, FITPLANE, RANSACFITPLANE

% Copyright (c) 2003-2006 Peter Kovesi and Felix Duvallet (CMU)
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% Aug  2006 - created ransacfitline from ransacfitplane
%             author: Felix Duvallet

function [V, L, inliers] = ransacfit3Dline(XYZ, t, feedback)

    if nargin == 2
	feedback = 0;
    end

    [rows, npts] = size(XYZ);

    if rows ~=3
        error('data is not 3D');
    end

    if npts < 2
        error('too few points to fit line');
    end

    s = 2;  % Minimum No of points needed to fit a line.

    fittingfn = @defineline;
    distfn    = @lineptdist;
    degenfn   = @isdegenerate;

    [L, inliers] = ransac(XYZ, fittingfn, distfn, degenfn, s, t, feedback);

    % Find the line going through the mean, parallel to the major
    % eigenvector
    V = fitline3d(XYZ(:, inliers));

%------------------------------------------------------------------------
% Function to define a line given 2 data points as required by
% RANSAC.

function L = defineline(X);
    L = X;

%------------------------------------------------------------------------
% Function to calculate distances between a line and an array of points.
% The line is defined by a 3x2 matrix, L.  The two columns of L defining
% two points that are the endpoints of the line.
%
% A line can be defined with two points as:
%        lambda*p1 + (1-lambda)*p2
% Then, the distance between the line and another point (p3) is:
%        norm( lambda*p1 + (1-lambda)*p2 - p3 )
% where
%                  (p2-p1).(p2-p3)
%        lambda =  ---------------
%                  (p1-p2).(p1-p2)
%
% lambda can be found by taking the derivative of:
%      (lambda*p1 + (1-lambda)*p2 - p3)*(lambda*p1 + (1-lambda)*p2 - p3)
% with respect to lambda and setting it equal to zero

function [inliers, L] = lineptdist(L, X, t)

    p1 = L(:,1);
    p2 = L(:,2);

    npts = length(X);
    d = zeros(npts, 1);

    for i = 1:npts
        p3 = X(:,i);

        lambda = dot((p2 - p1), (p2-p3)) / dot( (p1-p2), (p1-p2) );

        d(i) = norm(lambda*p1 + (1-lambda)*p2 - p3);
    end

    inliers = find(abs(d) < t);

%------------------------------------------------------------------------
% Function to determine whether a set of 2 points are in a degenerate
% configuration for fitting a line as required by RANSAC.
% In this case two points are degenerate if they are the same point
% or if they are exceedingly close together.

function r = isdegenerate(X)
    %find the norm of the difference of the two points
    % this will be 0 iff the two points are the same (the norm of their
    % difference is zero)
    r = norm(X(:,1) - X(:,2)) < eps;


function W = tess_smooth_sources(Vertices, Faces, VertConn, FWHM, Method)
% TESS_SMOOTH_SOURCES: Gaussian smoothing matrix over a mesh.
%
% USAGE:  W = tess_smooth_sources(Vertices, Faces, VertConn=[], FWHM=0.010, Method='average')
%
% INPUT:
%    - Vertices : Vertices positions ([X(:) Y(:) Z(:)])
%    - Faces    : Triangles matrix
%    - VertConn : Vertices connectivity, logical sparse matrix [Nvert,Nvert]
%    - FWHM     : Full width at half maximum, in meters (default=0.010)
%    - Method   : {'euclidian', 'path', 'average', 'surface'}
% OUPUT:
%    - W: smoothing matrix (sparse)
%
% DESCRIPTION:
%    - The distance between two points is an average of:
%        - the direct euclidian between the two points and
%        - the number of edges between the two points * the average length of an edge
%    - Gaussian smoothing function on the euclidian distance:
%      f(r) = 1 / sqrt(2*pi*sigma^2) * exp(-(r.^2/(2*sigma^2)))
%    - Full Width at Half Maximum (FWHM) is related to sigma by:
%      FWHM = 2 * sqrt(2*log2(2)) * sigma

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2010-2013

% ===== PARSE INPUTS =====
if (nargin < 5) || isempty(Method)
    Method = 'average';
end
if (nargin < 4) || isempty(FWHM)
    FWHM = 0.010;
end
if (nargin < 3) || isempty(VertConn)
    VertConn = tess_vertconn(Vertices, Faces);
end
if ~islogical(VertConn)
    error('Invalid vertices connectivity matrix.');
end
nv = size(Vertices,1);

% ===== ANALYZE INPUT =====
% Calculate Gaussian kernel properties
Sigma = FWHM / (2 * sqrt(2*log2(2)));
% FWTM = 2 * sqrt(2*log2(10)) * Sigma;
% Get the average edge length
[vi,vj] = find(VertConn);
meanDist = mean(sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2));
% Guess the number of iterations
nIter = min(10, ceil(FWHM / meanDist));

% ===== COMPUTE DISTANCE =====
switch lower(Method)
    % === METHOD 1: USE EUCLIDIAN DISTANCE ===
    case 'euclidian'
        % Get the neighborhood around each vertex
        VertConn = mpower(VertConn, nIter);
        [vi,vj] = find(VertConn);
        % Use Euclidean distance
        d = sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2);
        Dist = sparse(vi, vj, d, nv, nv);

    % === METHOD 2: USE NUMBER OF CONNECTIONS =====
    % === METHOD 3: AVERAGE METHOD 1+2 ===
    case {'path', 'average'}
        % Initialize loop variables
        VertConnGrow = speye(nv);
        VertIter = sparse(nv,nv);
        vall = [];

        % Grow and keep track of the layers
        for iter = 1:nIter
            disp(sprintf('SMOOTH> Iteration %d/%d', iter, nIter));
            % Grow selection of vertices
            VertConnPrev = VertConnGrow;
            VertConnGrow = double(VertConnGrow * VertConn > 0);
            % Find all the new connections
            vind = find(VertConnGrow - VertConnPrev > 0);
            [vi,vj] = ind2sub([nv,nv], vind);
            VertIter = VertIter + iter * sparse(vi, vj, ones(size(vi)), nv, nv);
        end

        % Use distance in number of connections
        Dist = VertIter * meanDist;
        Dist(1:nv+1:nv*nv) = 0;

        % == AVERAGE WITH METHOD 1 ==
        if strcmpi(Method, 'average')
            % Calculate Euclidean distance
            [vi,vj] = find(VertConnGrow);
            d = sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2);
            % Average with results of method #2
            Dist = (0.5 .* Dist + 0.5 .* sparse(vi, vj, d, nv, nv));
        end

    % ===== METHOD 4: CALCULATE SURFACE DISTANCE =====
    % WARNING: NOT FINISHED!!!!
    case 'surface'
        % Initialize loop variables
        VertConnGrow = speye(nv);
        Dist = sparse([], [], [], nv, nv, 3*nnz(VertConn));
        vall = [];
        nIter = 2;
        % Grow until we reach an accepteable distance
        for iter = 1:nIter
            disp(sprintf('Iteration %d', iter));
            % Get neighbors
            VertConnGrow = VertConnGrow * VertConn;
            % Get all the existing edges in the surface
            vind = find(VertConnGrow);
            % Remove all the previously processed connections
            vind = setdiff(vind, vall);
            [vi,vj] = ind2sub([nv,nv], vind);
            % Remove diagonal
            iDel = (vi == vj);
            vi(iDel) = [];
            vj(iDel) = [];
            % Calculate the distance to the neighbor nodes
            if (iter == 1)
                % Calculate all the distances for all the pairs of edges
                d = sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2);
            else
                % Initialize d matrix
                d = zeros(size(vi));
                % Process each new connection separately
                for i = 1:length(vi)
                    % Find nodes that are connected to both nodes
                    iMid = find((Dist(vi(i),:) & VertConn(vj(i),:)) | (VertConn(vi(i),:) & Dist(vj(i),:)));
                    % Find nodes for which we know one connection at least
                    iMid0 = (Dist(vi(i),iMid) & Dist(vj(i),iMid));
                    iMid1 = (Dist(vi(i),iMid) & ~iMid0);
                    iMid2 = (Dist(vj(i),iMid) & ~iMid0);
                    % Compute distances
                    dMid = 0*iMid;
                    dMid(iMid0) = Dist(vi(i),iMid0) + Dist(vj(i),iMid0);
                    dMid(iMid1) = Dist(vi(i),iMid1) + sqrt((Vertices(vj(i),1) - Vertices(iMid1,1)).^2 + (Vertices(vj(i),2) - Vertices(iMid1,2)).^2 + (Vertices(vj(i),3) - Vertices(iMid1,3)).^2)';
                    dMid(iMid2) = Dist(vj(i),iMid2) + sqrt((Vertices(vi(i),1) - Vertices(iMid2,1)).^2 + (Vertices(vi(i),2) - Vertices(iMid2,2)).^2 + (Vertices(vi(i),3) - Vertices(iMid2,3)).^2)';
                    dMid(dMid == 0) = Inf;
                    % Find the shortest path
                    d(i) = min(dMid);
                    if isinf(d(i))
                        error('???');
                    end
                end
            end
            % Add to processed vertices
            vall = union(vind, vall);
            % Create a sparse distance matrix
            Dist = Dist + sparse(vi, vj, d, nv, nv);
        end
end

% ===== APPLY GAUSSIAN FUNCTION =====
% Gaussian function
fun = inline('1 / sqrt(2*pi*sigma2) * exp(-(x.^2/(2*sigma2)))', 'x', 'sigma2');
% Calculate interpolation as a function of distance
[vi,vj] = find(Dist>0);
vind = sub2ind([nv,nv], vi, vj);
w = fun(Dist(vind), Sigma^2);
% Build final symmetric matrix
%W = sparse([vi;vj], [vj;vi], [w;w], nv, nv);
W = sparse(vi, vj, w, nv, nv);
% Add the diagonal
W = W + fun(0,Sigma^2) * speye(nv);
% Normalize columns
W = bst_bsxfun(@rdivide, W, sum(W,1));
% Remove insignificant values
[vi,vj] = find(W>0.005);
vind = sub2ind([nv,nv], vi, vj);
W = sparse(vi, vj, W(vind), nv, nv);

% ===== FIX BAD TRIANGLES =====
% Only for methods including neighbor distance
if ismember(lower(Method), {'path', 'average'})
    % Configurations to detect:
    %    - One face divided in 3 with a point in the middle of the face
    %    - Square divided into 4 triangles with one point in the middle
    % Calculate face-vertex connectivity
    VertFacesConn = tess_faceconn(Faces);
    % Find vertices connected to three or four faces
    sumVert  = sum(VertConn,2);
    sumFaces = sum(VertFacesConn,2);
    % Three/Four vertices: average the values of their neighbors
    iVert = find(((sumVert == 3) & (sumFaces == 3)) | ((sumVert == 4) & (sumFaces == 4)));
    AvgConn = bst_bsxfun(@rdivide, W * VertConn(:,iVert), sumVert(iVert)');
    W(iVert,:) = AvgConn';
    W(:,iVert) = AvgConn;
end


%% mri_sense_demo1.m
%|
%| Example illustrating regularized iterative reconstruction for parallel MRI
%| (sensitivity encoding imaging or SENSE), from nonuniform k-space samples.
%| (This example does not include field inhomogeneity or relaxation.)
%|
%| Copyright 2006-4-18, Jeff Fessler, University of Michigan

%% make coil sensitivity maps
if ~isvar('smap'), printm 'sense maps'
	ig = image_geom('nx', 64, 'fov', 250); % 250 mm FOV
	ig.mask = ig.circ(ig.dx * (ig.nx/2-2), ig.dy * (ig.ny/2-1)) > 0;
	f.ncoil = 4;
	smap = mri_sensemap_sim('nx', ig.nx, 'ny', ig.ny, 'dx', ig.dx, ...
		'rcoil', 120, 'ncoil', f.ncoil, 'chat', 1);
prompt
end

%% true object (discrete because of discrete sense map)
if ~isvar('xtrue'), printm 'true object'
	xtrue = ellipse_im(ig, 'shepplogan-emis', 'oversample', 2);
	clim = [0 8];
	im plc 2 2
	im(1, ig.x, ig.y, xtrue, '\x true', clim), cbar
	im(4, ig.mask, 'mask')
prompt
end

%% trajectory and system matrix
if ~isvar('Am'), printm 'system matrix A'
	f.traj = 'spiral1';
	f.dens = 'voronoi';

	N = [ig.nx ig.ny];
	[kspace, omega, wi_traj] = mri_trajectory(f.traj, {}, ...
		N, ig.fov, {f.dens});

	% create Gnufft class object
	J = [6 6];
	nufft_args = {N, J, 2*N, N/2, 'table', 2^10, 'minmax:kb'};
	Am = Gmri(kspace, ig.mask, ...
		'fov', ig.fov, 'basis', {'rect'}, 'nufft', nufft_args);

	if im
		im subplot 2
		plot(omega(1:5:end,1), omega(1:5:end,2), '.')
		axis_pipi, axis square
		titlef('%s: %d', f.traj, size(omega,1))
	end
prompt
end

wi_basis = wi_traj ./ Am.arg.basis.transform;

%% SENSE system object
if ~isvar('Ab'), printm 'Ab object with sense maps within'
	for ic=1:f.ncoil
		tmp = smap(:,:,ic);
		tmp = Gdiag(tmp(ig.mask), 'mask', ig.mask);
		Ac{ic} = Am * tmp; % cascade
	end
	Ab = block_fatrix(Ac, 'type', 'col'); % [A1; A2; ... ]
end

%% noisy data
if ~isvar('yi'), printm 'data yi'
	ytrue = Ab * xtrue(ig.mask);

	% add noise
	rng(0)
	yi = ytrue + 0 * randn(size(ytrue));
	% todo: visualize data...
end

%% CP recon
if ~isvar('xcp'), printm 'conj. phase reconstruction'
	xcp = zeros(ig.nx, ig.ny, f.ncoil, 'single');
	y4 = reshape(yi, [], f.ncoil);
	for ic=1:f.ncoil
		tmp = Am' * (wi_basis .* y4(:,ic));
		xcp(:,:,ic) = ig.embed(tmp);
	end

	im(2, abs(xcp), 'Conj. Phase Recon for each coil'), cbar
prompt
end

%% SSoS
if ~isvar('xssos'), printm 'sqrt sum-of-squares reconstruction'
	xssos = sqrt(sum(abs(xcp).^2, 3));
	xssos = ir_wls_init_scale(1, xtrue, xssos); % cheat trick for scaling
	im(3, abs(xssos), 'sum-of-squares recon'), cbar
	xlabelf('NRMSE %.1f%%', 100*nrms(xssos(:), xtrue(:)))
prompt
end

%% regularizer
if ~isvar('R'), printm 'regularizer'
	f.beta = 2^11;
	R = Reg1(ig.mask, 'beta', f.beta, 'type_penal', 'mat'); % complex
	if 1
		psf = qpwls_psf(Ab, R, 1, ig.mask, 1, 'offset', [0 0]);
		im(4, psf)
	end
prompt
end

%% PCG
if ~isvar('xpcg'), printm 'PCG with quadratic penalty'
	f.niter = 10;
	xpcg = qpwls_pcg(xssos(ig.mask), Ab, 1, yi(:), 0, R.C, 1, f.niter);
	xpcg = ig.embed(xpcg(:,end)); % convert last vector to image for display

	im(4, abs(xpcg), '|\x| PCG quad', clim), cbar
	xlabelf('NRMSE %.1f%%', 100*nrms(xpcg(:), xtrue(:)))
end


function [IX,IX1,IX2] = FindLinearIdx(x,y,lon,lat)
% given points in vectors x,y (np x 1) find their linear indices IX (np x 1)
% from a matrix of x-locations X and y-locations Y of both size nx columns and ny rows.
% lon and lat must be matrices created by ndgrid.
% kjr, 20171210 chl, und.
% wjp, 20201120 making sure dx and dy can be different
%      20200325 implementing method for irregular grid, cleaning up
ny = size(lon,1);
nx = size(lon,2);
np = numel(x);

if nx == 1 && ny == 1
   IX = 1; IX1 = 1; IX2 = 1;
   return
end

% make sure entry points are column vectors
x = x(:);
y = y(:);

% Get the grid spacing in x and y directions
% (trying both directions so could be meshgrid or ndgrid format)
dx  = diff(lon(:,1));
dy  = diff(lat(1,:));
dx_cutoff = 0.1/111e3; % approx 10 cm
if (max(dx) - min(dx)) > dx_cutoff || (max(dy) - min(dy)) > dx_cutoff
    % % IRREGULAR GRID (SLOWER)

    % convert ndgrid to vector
    lonV = reshape(lon(:,1),1,[]);
    % repeat the vector along the length of x
    LON = repmat(lonV,np,1);
    % find the closest lon to each point of x
    [~,IX1] = min(abs(x - LON),[],2);

    % convert ndgrid to vector
    latV = lat(1,:);
    % repeat the vector along the length of y
    LAT = repmat(latV,np,1);
    % find the closest lon to each point of y
    [~,IX2] = min(abs(y - LAT),[],2);

else
    % % REGULAR GRID (FASTER)
    dx = dx(1); dy = dy(1);

    IX1 = (x-lon(1,1))/dx + 1;
    IX2 = (y-lat(1,1))/dy + 1;

    IX1 = round(IX1);
    IX2 = round(IX2);

    IX1 = max([IX1,ones(np,1)],[],2);
    IX1 = min([IX1,ny*ones(np,1)],[],2);

    IX2 = max([IX2,ones(np,1)],[],2);
    IX2 = min([IX2,nx*ones(np,1)],[],2);

end

IX = sub2ind([ny,nx],IX1,IX2);

end


function x = r8row_to_r8vec ( m, n, a )

%*****************************************************************************80
%
%% R8ROW_TO_R8VEC converts an R8ROW into an R8VEC.
%
%  Example:
%
%    M = 3, N = 4
%
%    A =
%      11 12 13 14
%      21 22 23 24
%      31 32 33 34
%
%    X = ( 11, 12, 13, 14, 21, 22, 23, 24, 31, 32, 33, 34 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), the R8ROW.
%
%    Output, real X(M*N), a vector containing the M rows of A.
%
  j = 1;
  for i = 1 : m
    x(j:j+n-1) = a(i,1:n);
    j = j + n;
  end

  return
end