function [xi,w]=fifthOrderTriangleCubPoints()
%%FIFTHORDERTRIANGLECUBPOINTS Obtain fifth-order cubature points for
%   integration over a triangle in 2D. The points and weights are for the
%   triangle with vertices (1,0), (0,1), (0,0), but can be transformed to
%   any triangle using transformSimplexTriPoints.
%
%INPUTS: None
%
%OUTPUTS: xi A 2XnumCubPoints set of points for the standard triangle.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the triangle (1/2).
%
%This function implements the points given in [1] (7 points).
%
%EXAMPLE:
%Given the vertices of the simplex, we compare a fifth-order moment
%computed using these cubature points to one computed using
%monomialIntSimplex. The results are the same within typical finite
%precision limits.
% [xi,w]=fifthOrderTriangleCubPoints();
% alpha=[3;2];
% theMoment=findMomentFromSamp(alpha,xi,w)
% intVal=monomialIntSimplex(alpha)
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[-0.33333333333333333333333333333333333333,   -0.33333333333333333333333333333333333333,                                        0.45;
   -0.79742698535308732239802527616975234389,    0.59485397070617464479605055233950468778,    0.25187836108965430519136789100036266732;
    0.59485397070617464479605055233950468778,   -0.79742698535308732239802527616975234389,    0.25187836108965430519136789100036266732;
   -0.79742698535308732239802527616975234389,   -0.79742698535308732239802527616975234389,    0.25187836108965430519136789100036266732;
  -0.059715871789769820459117580973104798968,   -0.88056825642046035908176483805379040206,    0.26478830557701236147529877566630399935;
   -0.88056825642046035908176483805379040206,  -0.059715871789769820459117580973104798968,    0.26478830557701236147529877566630399935;
  -0.059715871789769820459117580973104798968,  -0.059715871789769820459117580973104798968,    0.26478830557701236147529877566630399935];
w=M(:,3);
xi=M(:,1:2)';
%Transform the points to the standard triangle.
v1=[-1,-1, 1;
    -1, 1,-1];
v2=[1,0,0;
    0,1,0];
[A,d]=affineTransBetweenTriangles(v1,v2);
xi=bsxfun(@plus,A*xi,d);
w=w/4;
end

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


function point = intersectLines(line1, line2, varargin)
%INTERSECTLINES Return all intersection points of N lines in 2D.
%
%   PT = intersectLines(L1, L2);
%   returns the intersection point of lines L1 and L2. L1 and L2 are 1-by-4
%   row arrays, containing parametric representation of each line (in the
%   form [x0 y0 dx dy], see 'createLine' for details).
%
%   In case of colinear lines, returns [Inf Inf].
%   In case of parallel but not colinear lines, returns [NaN NaN].
%
%   If each input is [N*4] array, the result is a [N*2] array containing
%   intersections of each couple of lines.
%   If one of the input has N rows and the other 1 row, the result is a
%   [N*2] array.
%
%   PT = intersectLines(L1, L2, EPS);
%   Specifies the tolerance for detecting parallel lines. Default is 1e-14.
%
%   Example
%   line1 = createLine([0 0], [10 10]);
%   line2 = createLine([0 10], [10 0]);
%   point = intersectLines(line1, line2)
%   point =
%       5   5
%
%   See also
%   lines2d, edges2d, intersectEdges, intersectLineEdge
%   intersectLineCircle

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-10-31
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

%% Process input arguments

% extract tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% check size of each input
N1 = size(line1, 1);
N2 = size(line2, 1);
N = max(N1, N2);
if N1 ~= N2 && N1*N2 ~= N
    error('matGeom:IntersectLines:IllegalArgument', ...
        'The two input arguments must have same number of lines');
end

%% Check parallel and colinear lines

% coordinate differences of origin points
dx = bsxfun(@minus, line2(:,1), line1(:,1));
dy = bsxfun(@minus, line2(:,2), line1(:,2));

% indices of parallel lines
denom = line1(:,3) .* line2(:,4) - line2(:,3) .* line1(:,4);
par = abs(denom) < tol;

% indices of colinear lines
col = abs(dx .* line1(:,4) - dy .* line1(:,3)) < tol & par ;

% initialize result array
x0 = zeros(N, 1);
y0 = zeros(N, 1);

% initialize result for parallel lines
x0(col) = Inf;
y0(col) = Inf;
x0(par & ~col) = NaN;
y0(par & ~col) = NaN;

% in case all line couples are parallel, return
if all(par)
    point = [x0 y0];
    return;
end

%% Extract coordinates of itnersecting lines

% indices of intersecting lines
inds = ~par;

% extract base coordinates of first lines
if N1 > 1
    line1 = line1(inds,:);
end
x1 =  line1(:,1);
y1 =  line1(:,2);
dx1 = line1(:,3);
dy1 = line1(:,4);

% extract base coordinates of second lines
if N2 > 1
    line2 = line2(inds,:);
end
x2 =  line2(:,1);
y2 =  line2(:,2);
dx2 = line2(:,3);
dy2 = line2(:,4);

% re-compute coordinate differences of origin points
dx = bsxfun(@minus, line2(:,1), line1(:,1));
dy = bsxfun(@minus, line2(:,2), line1(:,2));

%% Compute intersection points

denom = denom(inds);
x0(inds) = (x2 .* dy2 .* dx1 - dy .* dx1 .* dx2 - x1 .* dy1 .* dx2) ./ denom ;
y0(inds) = (dx .* dy1 .* dy2 + y1 .* dx1 .* dy2 - y2 .* dx2 .* dy1) ./ denom ;

% concatenate result
point = [x0 y0];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%	SurfBox-MATLAB (c)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	Yue Lu and Minh N. Do
%%
%%	Department of Electrical and Computer Engineering
%%	Coordinated Science Laboratory
%%	University of Illinois at Urbana-Champaign
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%	NXdiamondmapping.m
%%
%%	First created: 04-20-05
%%	Last modified: 04-13-06
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function m = NXdiamondmapping(N, beta)

%% Get the mapping kernel for the fan-shaped frequency response
%
%   See also ......
%

if rem(N, 2) ~= 1
    error('N must be an odd integer so that we have a zero-phase mapping function.');
end

%% Use the kaiser window to truncate and smooth the ideal sinc sequences

%% w = window(@kaiser, N, beta);
w = calculateKaiserwindow(N, beta);

ind = [1 : N]' - (N+1)/2;
w = w .* sinc(ind / 2);
w(N+1) = 0;

%% Get the mapping kernel
m = zeros(2*N-1);
[k1, k2] = meshgrid(-N+1:N-1, -N+1:N-1);
a = k1 + k2 + (N+1)/ 2;
a(a <= 0) = N + 1;
a(a > N) = N + 1;
a = w(a);
b = k1 - k2 + (N+1)/ 2;
b(b <= 0) = N + 1;
b(b > N) = N + 1;
b = w(b);

m = a .* b;
%% If you want to check the frquency response of this mapping kernel.
% mapp = m;
% dispfreqd2(CRISPfilter(mapp, [1;1]));

%%	This software is provided "as-is", without any express or implied
%%	warranty. In no event will the authors be held liable for any
%%	damages arising from the use of this software.


function [m,v]=v_chimv(n,l,s)
%V_CHIMV approximate mean and variance of non-central chi distribution [m,v]=(n,l,s)
%
%  Inputs:	n = degrees of freedom
%           l = non-centrality parameter = sqrt(sum(mean^2)) [default 0]
%               (can be a vector or matrix to calculate many different values at once)
%           s = standard deviation of Gaussian [default 1]
%
% Outputs:  m = mean of chi distribution
%           v = variance of chi distribution
%
% If x=c+randn(n,1) is a column vector of Gaussian random numbers with mean vector c, then
% z=sqrt(x'*x) has a chi distributon with n degrees of freedom and non-centrality parameter
% l=sqrt(c'*c). The mean and variance of a chi distribution are given precisely by
%
%     m = sqrt(2)*exp(gammaln(0.5*n+0.5)-gammaln(0.5*n))*hypergeom(-0.5,0.5*n,-0.5*(l/s)^2)*s
%       = sqrt(pi/2) L(0.5,0.5*n-1,-0.5*(l/s)^2)*s
%     v = n*s^2+l^2-m^2
%
% where L(n,a,x) is the generalized Laguerre polynomial L_n^{(a)}(x) but this is very slow
% to calculate so this routine approximates these expressions.
% The expressions are from [1]  together with the following identities taken
% from [2] or [3]: 13.2.39, 13.6.19, 5.2.5, 5.4.1, 5.5.1, 5.4.6.
%
% For n=1, the accuracy is high; for n>1, accuracy improves with increasing n.
% Accuracy is worst when the non-centrality parameter, l, is close to s*sqrt(n).
% Worst case errors as a function of n are:
%                       n:    1       2      3       5      10
%   worst case error in m:  1e-15   0.007  0.004  0.0015  0.0005
%
% References:
% [1]	J. H. Park. Moments of the generalized Rayleigh distribution. Quarterly of Applied Mathematics, 19: 45–49, 1961.
% [2]	F. W. J. Olver, D. W. Lozier, R. F. Boisvert, and C. W. Clark, editors. NIST Handbook of Mathematical Functions. CUP, 2010. ISBN 978-0-521-14063-8.
% [3]	F. W. J. Olver, D. W. Lozier, R. F. Boisvert, and C. W. Clark, editors. NIST Digital Library of Mathematical Functions. 2010-2014. URL http://dlmf.nist.gov/.

%      Copyright (C) Mike Brookes 2014-2020
%      Version: $Id: v_chimv.m 11260 2020-07-18 20:07:58Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent ab pp qq nab
if isempty(ab)
    nab=200; % cache a few low values of n
    pp=[ 0.595336298258636  -1.213013700592756  -0.018016200037799   1.999986150447582 0];
    qq=[ -0.161514114798972   0.368983655790737  -0.136992134476950  -0.499681107630725 2];
    ni=1./(1:nab);
    ab=[polyval(qq,ni);polyval(pp,ni)];
end
if nargin<3
    s=1;
    if nargin<2
        l=0;
    end
end
ls=l/s;
l2=(ls).^2;
s2=s^2;
if n<=nab
    if n==1
        m=l.*(1-2*normcdf(-ls))+2*s*normpdf(-ls);
    else
        m=sqrt(l2+n-1+(ab(1,n)+ab(2,n)*l2).^(-1))*s;
    end
else
    m=sqrt(l2+n-1+(polyval(qq,1/n)+polyval(pp,1/n)*l2).^(-1))*s;
end
v=(n+l2)*s2-m.^2;


function [vol] = volume_intrinsic(l)
  % VOLUME Compute volumes of tets defined intrinsically by edge lengths l
  %
  % v = volume(l)
  %
  % Inputs:
  %   l  #T by 6 list of tetrahedra side lengths of edges opposite *face* pairs
  %     [23 31 12 41 42 43]
  % Ouputs:
  %   vol  #T list of tet volumes (always positive)
  %

  % http://en.wikipedia.org/wiki/Heron%27s_formula#Heron-type_formula_for_the_volume_of_a_tetrahedron

  % U, V, W, u, v, w are lengths of edges of the tetrahedron (first three form
  % a triangle; u opposite to U and so on)
  u = l(:,1); v = l(:,2); w = l(:,3);
  U = l(:,4); V = l(:,5); W = l(:,6);
  X = (w - U + v).*(U + v + w);
  x = (U - v + w).*(v - w + U);
  Y = (u - V + w).*(V + w + u);
  y = (V - w + u).*(w - u + V);
  Z = (v - W + u).*(W + u + v);
  z = (W - u + v).*(u - v + W);
  a = sqrt(x.*Y.*Z);
  b = sqrt(y.*Z.*X);
  c = sqrt(z.*X.*Y);
  d = sqrt(x.*y.*z);
  vol = sqrt( ...
    (-a + b + c + d).* ...
    ( a - b + c + d).* ...
    ( a + b - c + d).* ...
    ( a + b + c - d))./ ...
    (192.*u.*v.*w);

end


%%
X = rand(100,400);
k = 100;

tic; [U,S,V] = svdsecon(X,k); toc; norm(U*S*V' - X,'fro')
tic; [U,S,V] = svds(X,k); toc; norm(U*S*V' - X,'fro')

tic; [U,S,V] = svdecon(X); toc; norm(U*S*V' - X,'fro')
tic; [U,S,V] = svd(X,'econ'); toc; norm(U*S*V' - X,'fro')

%%
%X = rand(400,100);
X = X';
k = 100;

tic; [U,S,V] = svdsecon(X,k); toc; norm(U*S*V' - X,'fro')
tic; [U,S,V] = svds(X,k); toc; norm(U*S*V' - X,'fro')

tic; [U,S,V] = svdecon(X); toc; norm(U*S*V' - X,'fro')
tic; [U,S,V] = svd(X,'econ'); toc; norm(U*S*V' - X,'fro')

%%
%X = rand(400,100);
X = X';
k = 9;

Xm = bsxfun(@minus,X,mean(X,2));
tic; [U,T] = pcaecon(X,k); toc
norm(Xm-U*T,'fro')

tic; [U,T] = pcasecon(X,k); toc
norm(Xm-U*T,'fro')

tic; [U,S,V] = svdsecon(Xm,k); toc
norm(Xm-U*S*V','fro')

tic; [U,S,V] = svdecon(Xm); toc
norm(Xm-U(:,1:k)*S(1:k,1:k)*V(:,1:k)','fro')

tic; [U,S,V] = svds(Xm,k); toc
norm(Xm-U*S*V','fro')

tic; [U,S,V] = svd(Xm); toc
norm(Xm-U(:,1:k)*S(1:k,1:k)*V(:,1:k)','fro')


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% sinusoidal sequence

 n=0:20;
 x=2*cos(1/2*n+pi/4);
 stem(n,x)
 legend('x[n]')
 grid

 figure
 y=2*cos(pi/6*n+pi/4);
 stem(n,y)
 legend('y[n]')
 grid

%sampling
figure
t=0:0.01:10;
x=cos(7*t);

Ts1=pi/7;
ts1=0:Ts1:10;
xs1=cos(7*ts1);

Ts2=pi/4;
ts2=0:Ts2:10;
xs2=cos(7*ts2);

plot(t,x,ts1,xs1,':o',ts2, xs2,':+')
legend('x(t)','x[n],T_s=\pi/7','x[n],T_s=\pi/4')


function r = i4int_to_r8int ( imin, imax, i, rmin, rmax )

%*****************************************************************************80
%
%% I4INT_TO_R8INT maps an integer interval to an R8 interval.
%
%  Formula:
%
%    R := RMIN + ( RMAX - RMIN ) * ( I - IMIN ) / ( IMAX - IMIN )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer IMIN, IMAX, the range.
%
%    Input, integer I, the integer to be converted.
%
%    Input, real RMIN, RMAX, the range.
%
%    Output, real R, the corresponding value in [RMIN,RMAX].
%
  if ( imax == imin )

    r = 0.5 * ( rmin + rmax );

  else

    r = (  ( imax - i        ) * rmin   ...
        +  (        i - imin ) * rmax ) ...
        /  ( imax     - imin );

  end

  return
end


function dist = line_par_point_dist_2d ( f, g, x0, y0, p )

%*****************************************************************************80
%
%% LINE_PAR_POINT_DIST_2D: distance ( parametric line, point ) in 2D.
%
%  Discussion:
%
%    The parametric form of a line in 2D is:
%
%      X = X0 + F * T
%      Y = Y0 + G * T
%
%    We normalize by choosing F*F+G*G=1 and 0 <= F.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer and John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983.
%
%  Parameters:
%
%    Input, real F, G, X0, Y0, the parametric line parameters.
%
%    Input, real P(2,1), the point whose distance from the line is
%    to be measured.
%
%    Output, real DIST, the distance from the point to the line.
%
  dx =   g * g * ( p(1,1) - x0 ) - f * g * ( p(2,1) - y0 );
  dy = - f * g * ( p(1,1) - x0 ) + f * f * ( p(2,1) - y0 );

  dist = sqrt ( dx * dx + dy * dy ) / ( f * f + g * g );

  return
end


function grid_index = sparse_grid_cc_index ( dim_num, level_max, point_num )

%*****************************************************************************80
%
%% SPARSE_GRID_CC_INDEX indexes the points forming a sparse grid.
%
%  Discussion:
%
%    The points forming the sparse grid are guaranteed to be a subset
%    of a certain product grid.  The product grid is formed by DIM_NUM
%    copies of a 1D rule of fixed order.  The orders of the 1D rule,
%    (called ORDER_1D) and the order of the product grid, (called ORDER)
%    are determined from the value LEVEL_MAX.
%
%    Thus, any point in the product grid can be identified by its grid index,
%    a set of DIM_NUM indices, each between 1 and ORDER_1D.
%
%    This routine creates the GRID_INDEX array, listing (uniquely) the
%    points of the sparse grid.
%
%    An assumption has been made that the 1D rule is closed (includes
%    the interval endpoints) and nested (points that are part of a rule
%    of a given level will be part of every rule of higher level).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer POINT_NUM, the total number of points in the grids.
%
%    Output, integer GRID_INDEX(DIM_NUM,POINT_NUM), a list of point indices,
%    representing a subset of the product grid of level LEVEL_MAX,
%    representing (exactly once) each point that will show up in a
%    sparse grid of level LEVEL_MAX.
%

%
%  The outer loop generates LEVELs from 0 to LEVEL_MAX.
%
  point_num2 = 0;

  for level = 0 : level_max
%
%  The middle loop generates the next partition LEVEL_1D(1:DIM_NUM)
%  that adds up to LEVEL.
%
    level_1d = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );
%
%  Transform each 1D level to a corresponding 1D order.
%
      order_1d = level_to_order_closed ( dim_num, level_1d );
%
%  The product of the 1D orders gives us the number of points in this grid.
%
      order_nd = prod ( order_1d(1:dim_num) );
%
%  The inner (hidden) loop generates all points corresponding to given grid.
%
      grid_index2 = multigrid_index0 ( dim_num, order_1d, order_nd );
%
%  Adjust these grid indices to reflect LEVEL_MAX.
%
      grid_index2 = multigrid_scale_closed ( dim_num, order_nd, level_max, level_1d, ...
        grid_index2 );
%
%  Determine the first level of appearance of each of the points.
%
      grid_level = abscissa_level_closed_nd ( level_max, dim_num, order_nd, ....
        grid_index2 );
%
%  Only keep those points which first appear on this level.
%
      for point = 1 : order_nd

        if ( grid_level(point) == level )

          point_num2 = point_num2 + 1;

          grid_index(1:dim_num,point_num2) = grid_index2(1:dim_num,point);

        end

      end

      if ( ~more )
        break
      end

    end

  end

  return
end


clear all;
warning('off','all');

% parameters & settings
k = 250; % spring stiffness
dt = 0.01; % time step
frame_num = 5000; % number of total frames to run
mass_num = 10; % number of masses in the mass-spring system

% construct a 2-dimensional mass-spring system
V = [linspace(1,1+mass_num,mass_num)',zeros(mass_num,1)];
E = [linspace(1,mass_num-1,mass_num-1)',linspace(2,mass_num,mass_num-1)']; % edges
V = V-(max(V)+min(V))/2; V = V/max(V(:)); % normalize the mesh s.t. it has the right size

vec = @(X) X(:); % vectorization: [x1x2 y1y2] order

% boundary condition (feel free to play with it!)
ci = [1 mass_num]; % fixed vertex indices
ci = [ci ci+mass_num]'; % match the [x1x2 y1y2] order --> Q: why is this important?

M = eye(2*size(V,1)); % mass matrix
g = 1*vec(repmat([0 -9.8],size(V,1),1)); % gravity

% set up the viewer
clf;
hold on;
ts = line('XData',V(:,1),'YData',V(:,2),'LineWidth',2); % draw springs as lines
tp = scatter(V(:,1),V(:,2),'.b','SizeData',400); % draw masses as dots
hold off;
axis equal;
axis([-1.5 1.5 -2 0.2]);
axis manual;
drawnow;

% initialization
P = vec(V); % mass positions for the next time step
Pt = vec(V); % mass positions for the current time step
Ptt = vec(V); % mass positions for the previous time step

for iter = 1:frame_num

  % save the states of previous time steps
  Ptt = Pt; Pt = P;

  % implicit euler time stepper
  max_iter = 50;
  for i = 1 : max_iter

    % gradient and hessian for the elastic potential energy
    [G,K] = mass_spring_gradient_hessian(V,E,k,reshape(P,size(V)));

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% solution %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % TASK: Use newton's method to solve the following nonlinear optimization problem
    %   min total_energy(P)
    %       s.t. P(ci,:) = 0
    %
    % White list:
    % - "min_quad_with_fixed" to solve the constrained optimization problem
    %
    % Hint:
    % - total energy = incremental kinetic energy + gravitational
    % potential energy + elastic potential energy

    dP = zeros(size(P));
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if norm(dP) < 1e-6
        break;
    end

    alpha = 1; % fixed step size for newton
    P = P + alpha * dP; % update P

  end

  Vnew = reshape(P,size(V));
  ts.XData = Vnew(:,1); ts.YData = Vnew(:,2); % update springs
  tp.XData = Vnew(:,1); tp.YData = Vnew(:,2); % update masses
  title(sprintf('%d : %d',iter, i),'Fontsize',20);
  drawnow;
%   figgif('./mass-spring.gif'); % uncomment this line to save a .gif

end


function UNew = elasticPeriodic2D(varargin);
% elasticPeriodic2D: solve elastic registraion in 2D with periodic
%        boundary conditions
%
%
% author: Nathan D. Cahill
% email: nathan.cahill@rit.edu
% affiliation: Rochester Institute of Technology
% date: January 2014
% licence: GNU GPL v3
%
% Copyright Nathan D. Cahill
% Code available from https://github.com/tomdoel/npReg
%
%

% parse input arguments
[U,F,mu,lambda,gamma,PixSize,NumPix] = parse_inputs(varargin{:});

% construct filters that implement discretized Navier-Lame equations
d1 = [1;-2;1]/(PixSize(1)^2);
d2 = [1 -2 1]/(PixSize(2)^2);
d12 = [1 0 -1;0 0 0;-1 0 1]/(4*PixSize(1)*PixSize(2));

[A11,A22] = deal(zeros(3,3));
A11(2,2) = gamma;
A11(:,2) = A11(:,2) + (lambda+2*mu)*d1;
A11(2,:) = A11(2,:) + mu*d2;
A22(2,2) = gamma;
A22(:,2) = A22(:,2) + mu*d1;
A22(2,:) = A22(2,:) + (lambda+2*mu)*d2;

A12 = d12*(lambda+mu)/4;
A21 = A12;

% add displacement vectors to multiple of force field
F = gamma*U + F;

% multiply force field by adjoint of Navier-Lame equations
Fnew = zeros(NumPix(1),NumPix(2),2);
Fnew(:,:,1) = imfilter(F(:,:,1),A22,'replicate') - imfilter(F(:,:,2),A12,'replicate');
Fnew(:,:,2) = imfilter(F(:,:,2),A11,'replicate') - imfilter(F(:,:,1),A21,'replicate');

% compute Fourier transform of new force field
FnewF1 = fft2(Fnew(:,:,1));
FnewF2 = fft2(Fnew(:,:,2));

% construct images of coordinates scaled by pi/(N or M)
[alpha,beta] = ndgrid(2*pi*(0:(NumPix(1)-1))/NumPix(1),2*pi*(0:(NumPix(2)-1))/NumPix(2));

% construct LHS factor
T = 2*cos(alpha) + 2*cos(beta) - 4;
LHSfactor = (gamma + (lambda+2*mu).*T).*(gamma + mu.*T);

% if gamma is zero, set origin term to 1, as DC term does not matter
if isequal(gamma,0),
    LHSfactor(1,1) = 1;
end

% solve for FFT of U
UF1 = FnewF1./LHSfactor;
UF2 = FnewF2./LHSfactor;

% perform inverse fft and concatenate
UNew = cat(3,ifft2(UF1,'symmetric'),ifft2(UF2,'symmetric'));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [U,F,mu,lambda,gamma,PixSize,NumPix] = parse_inputs(varargin);

% get displacement field and check size
U = varargin{1};
F = varargin{2};
gamma = varargin{3};
PixSize = varargin{4}(1:2);
NumPix = [varargin{5} varargin{6}];
mu = varargin{8};
lambda = varargin{9};
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function K = kernel(X1, X2, param)

n1 = size(X1,1);
n2 = size(X2,1);
d = size(X1,2);
switch param.type
    case 'linear'
        K = X1*X2';

    case 'rbf'
        norm1 = sum(X1.^2,2);
        norm2 = sum(X2.^2,2);
        dist = (repmat(norm1 ,1,size(X2,1)) + ...
            repmat(norm2',size(X1,1),1) - ...
            2*X1*X2');
        K = exp(-0.5/param.sig^2 * dist);

    case 'dist'
        norm1 = sum(X1.^2,2);
        norm2 = sum(X2.^2,2);
        dist = (repmat(norm1 ,1,size(X2,1)) + ...
            repmat(norm2',size(X1,1),1) - ...
            2*X1*X2');
        K = param.maxdist - dist;

    case 'Lp'
        K = zeros(n1,n2);
        for n = 1:n1
            K(n,:) = real(sum(bsxfun(@minus, X2, X1(n,:)).^param.p,2));
        end
        K = exp(-0.5/param.sig^2 * K);

    case 'L1'
        K = zeros(n1,n2);
        for n = 1:n1
            K(n,:) = real(sum(abs(bsxfun(@minus, X2, X1(n,:))),2));
        end
        K = exp(-0.5/param.sig * K);

    case 'histintersection'
		K = zeros(n1,n2);
        for n = 1:n1
            K(n,:) = sum(bsxfun(@min, X2, X1(n,:)),2);
        end

    case 'L1_james'
        K = zeros(n1,n2);
        for n = 1:n1
            K(n,:) = real(sum(abs(bsxfun(@minus, X2, X1(n,:))),2));
        end
        K = max(max(K)) - K;

    otherwise
        error('Unknown kernel');
end;


function [yPred, PInvPred]=infoFilterDiscPred(yPrev,PInvPrev,F,Q,u)
%%INFOFILTERDISCPRED Perform the discrete-time prediction step that comes
%                    with the standard linear information filter with
%                    additive process noise.
%
%INPUTS: yPrev The xDimX1 information state at the previous time-step. The
%              information state is the inverse covariance matrix times the
%              target state.
%     PInvPrev The xDimXxDim inverse of the state covariance matrix at the
%              previous time-step.
%            F An xDim X xDim state transition matrix.
%            Q The xDimX xDim process noise covariance matrix. This can be
%              singular.
%            u An optional xDimX1 vector that is the control input. If
%              omitted, no control input is used.
%
%OUTPUTS: yPred The xDim X 1 predicted information state vector.
%      PInvPred The predicted xDim X xDim inverse state covariance matrix.
%
%The implementation of the prediction step given here is from the flow
%chart given in [1]. The matrix G in the paper is omitted, since any
%control input can be pre-multipled by the matrix. The implied discrete-
%time dynamic model is
%x(k)=F(k-1)*x(k-1)+u(k-1)+noise
%where x(k) is the state at time k. However, an information filter
%propagates
%y=inv(P)*x
%and
%PInv=inv(P)
%instead of propagating P and x, where P is the covariance matrix
%associated with the Gaussian state x. This allows the filter to be used
%even when PInv is singular.
%
%More information on information filtering is given in Chapter 7.2 of [2].
%
%REFERENCES:
%[1] D. F. Crouse, P. Willett, and Y. Bar-Shalom, "A low-complexity
%    sliding-window Kalman FIR smoother for discrete-time models," IEEE
%    Signal Processing Letters, vol. 17, no. 2, pp. 177-180, Feb. 2009.
%[2] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    DInv=F'+PInvPrev/(F)*Q;
    PInvPred=DInv\PInvPrev/(F);

    %Handle possible loss of symmetry due to order of operations and finite
    %precision limitations.
    PInvPred=(PInvPred+PInvPred')/2;

    if(nargin<5||isempty(u))
        yPred=DInv\yPrev;
    else
        yPred=DInv\yPrev+PInvPred*u;
    end
end

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


function [ po, pc, pe ] = lagrange_complete ( d, n, r, nd, xd )

%*****************************************************************************80
%
%% LAGRANGE_COMPLETE: Complete Lagrange polynomial basis from data.
%
%  Discussion:
%
%    This function represents algorithm 4.1 in the reference.
%
%    This function is given XD, a set of ND distinct data points in a
%    D dimensional space, and returns information defining a set of
%    ND Lagrange polynomials L(i)(X) with the property that:
%
%      L(i)(XD(j)) = delta(i,j)
%
%    This function does a very weak form of pivoting, by selecting
%    the very next polynomial that is "nonzero" at the current point.
%    An improved version of this function chooses instead the polynomial
%    with maximum function value at the current point.
%
%    In order for this computation to be carried out, it is necessary that
%    ND, the number of data points, is equal to R, the dimension of the
%    space of polynomials in D dimensions and total degree N or less, that is:
%
%      ND = R = Choose ( N + D, N )
%
%    There will be ND polynomials returned.  Each polynomial can have
%    as many as R coefficients.
%
%    Each polynomial is given as a vector, with each entry corresponding
%    to a nonzero coefficient.  In particular, for polynomial L(i)(X):
%
%      PO(i) is the order, that is, the number of nonzero coefficients;
%      PC(i,j), for 1 <= j <= PO(i), is the coefficient of the J-th term.
%      PE(i,j), for 1 <= j <= PO(i), encodes the exponents of the J-th term.
%
%    The exponent codes are a compact way of recording the exponent vector
%    associated with each monomial.  If PE(i,j) = k, then the corresponding
%    vector of D exponents can be determined by:
%
%      E = mono_unrank_grlex ( D, k );
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Tomas Sauer, Yuan Xu,
%    On multivariate Lagrange interpolation,
%    Mathematics of Computation,
%    Volume 64, Number 211, July 1995, pages 1147-1170.
%
%  Parameters:
%
%    Input, integer D, the spatial dimension.
%
%    Input, integer N, the maximum total degree.
%
%    Input, integer R, the number of monomials in D dimensions
%    of total degree N or less.
%
%    Input, integer ND, the number of data points.
%    This function requires that the ND is equal to R.
%
%    Input, real XD(D,ND), the data points, which must be distinct.
%
%    Output, integer PO(ND), the order (number of nonzero coefficients) for the
%    Lagrange basis polynomials.
%
%    Output, real PC(ND,R), the coefficients for the
%    Lagrange basis polynomials.
%
%    Output, integer PE(ND,R), the exponent indices for the
%    Lagrange basis polynomials.
%

%
%  Verify that R is correct.
%
  if ( r ~= mono_upto_enum ( d, n ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LAGRANGE_COMPLETE - Fatal error!\n' );
    fprintf ( 1, '  The value R is not correct.\n' );
    error ( 'LAGRANGE_COMPLETE - Fatal error!' );
  end

  if ( r ~= nd )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LAGRANGE_COMPLETE - Fatal error!\n' );
    fprintf ( 1, '  The value R = %d does not equal ND = %d.\n', r, nd );
    error ( 'LAGRANGE_COMPLETE - Fatal error!' );
  end
%
%  Verify that the points are sufficiently distinct.
%
  [ d_min, d_max ] = r8col_separation ( d, nd, xd );
  d_tol = sqrt ( eps );

  if ( d_min < d_tol )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LAGRANGE_COMPLETE - Fatal error!\n' );
    fprintf ( 1, '  Some points are too close!\n' );
    fprintf ( 1, '  Minimum data point separation is = %g.\n', d_min );
    error ( 'LAGRANGE_COMPLETE - Fatal error!' );
  end
%
%  Initialize the polynomials Q to be all monomials of degree N or less.
%
  qo = zeros ( r, 1 );
  qc = zeros ( r, r );
  qe = zeros ( r, r );

  for k = 1 : r
    qo(k,1) = 1;
    qc(k,1) = 1.0;
    qe(k,1) = k;
  end
%
%  Now set up the P polynomials.
%
  po = zeros ( r, 1 );
  pc = zeros ( r, r );
  pe = zeros ( r, r );

  for k = 1 : nd
%
%  Find the first polynomial Q(K:R)(X) which is nonzero at X(K).
%
    i = r + 1;

    for j = k : r
      o = qo(j);
      value = polynomial_value ( d, o, qc(j,1:o), qe(j,1:o), 1, xd(1:d,k) );
      if ( value ~= 0.0 )
        i = j;
        break
      end
    end

    if ( i == r + 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'LAGRANGE_COMPLETE - Fatal error!\n' );
      fprintf ( 1, '  I = R+1.\n' );
      error ( 'LAGRANGE_COMPLETE - Fatal error!' );
    end
%
%  Define P(K)(X) = Q(I)(X) / Q(I)(X(k)
%
    o = qo(i);
    po(k) = qo(i);
    pc(k,1:o) = qc(i,1:o) / value;
    pe(k,1:o) = qe(i,1:o);
%
%  Modify P(1:k-1)(X).
%
    for j = 1 : k - 1

      oj = po(j);
      ok = po(k);

      value = polynomial_value ( d, oj, pc(j,1:oj), pe(j,1:oj), 1, xd(1:d,k) );

      [ o, c, e ] = polynomial_axpy ( - value, ok, pc(k,1:ok), pe(k,1:ok), ...
        oj, pc(j,1:oj), pe(j,1:oj) );

      po(j) = o;
      pc(j,1:o) = c(1:o)';
      pe(j,1:o) = e(1:o)';

    end
%
%  Modify Q(I:downto:K+1)
%
    for j = i : -1 : k + 1

      oj = qo(j-1);
      ok = po(k);

      value = polynomial_value ( d, oj, qc(j-1,1:oj), qe(j-1,1:oj), ...
        1, xd(1:d,k) );

      [ o, c, e ] = polynomial_axpy ( - value, ok, pc(k,1:ok), pe(k,1:ok), ...
        oj, qc(j-1,1:oj), qe(j-1,1:oj) );

      qo(j) = o;
      qc(j,1:o) = c(1:o)';
      qe(j,1:o) = e(1:o)';

    end
%
%  Modify Q(I+1:R)
%
    for j = i + 1 : r

      oj = qo(j);
      ok = po(k);

      value = polynomial_value ( d, oj, qc(j,1:oj), qe(j,1:oj), ...
        1, xd(1:d,k) );

      [ o, c, e ] = polynomial_axpy ( - value, ok, pc(k,1:ok), pe(k,1:ok), ...
        oj, qc(j,1:oj), qe(j,1:oj) );

      qo(j) = o;
      qc(j,1:o) = c(1:o)';
      qe(j,1:o) = e(1:o)';

    end

  end
%
%  Get rid of tiny coefficients.
%
  for i = 1 : nd
    oi = po(i);
    [ o, c, e ] = polynomial_compress ( ...
      po(i), pc(i,1:oi), pe(i,1:oi) );
    po(i) = o;
    pc(i,1:o) = c(1:o)';
    pe(i,1:o) = e(1:o)';
  end

  return
end


function value = q_measure ( n, z, triangle_order, triangle_num, triangle_node )

%*****************************************************************************80
%
%% Q_MEASURE determines the triangulated pointset quality measure Q.
%
%  Discussion:
%
%    The Q measure evaluates the uniformity of the shapes of the triangles
%    defined by a triangulated pointset.
%
%    For a single triangle T, the value of Q(T) is defined as follows:
%
%      TAU_IN = radius of the inscribed circle,
%      TAU_OUT = radius of the circumscribed circle,
%
%      Q(T) = 2 * TAU_IN / TAU_OUT
%        = ( B + C - A ) * ( C + A - B ) * ( A + B - C ) / ( A * B * C )
%
%    where A, B and C are the lengths of the sides of the triangle T.
%
%    The Q measure computes the value of Q(T) for every triangle T in the
%    triangulation, and then computes the standard deviation of this
%    set of values:
%
%      Q_MEASURE = min ( all T in triangulation ) Q(T)
%
%    In an ideally regular mesh, all triangles would have the same
%    equilateral shape, for which Q = 1.  In a good mesh, 0.5 < Q.
%
%    Given the 2D coordinates of a set of N nodes, stored as Z(1:2,1:N),
%    a triangulation is a list of NT triples of node indices that form
%    triangles.  Generally, a maximal triangulation is expected, namely,
%    a triangulation whose image is a planar graph, but for which the
%    addition of any new triangle would mean the graph was no longer planar.
%    A Delaunay triangulation is a maximal triangulation which maximizes
%    the minimum angle that occurs in any triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Max Gunzburger and John Burkardt,
%    Uniformity Measures for Point Samples in Hypercubes.
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, pages 329-345, June 2004.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real Z(DIM_NUM,N), the points.
%
%    Input, integer TRIANGLE_ORDER, the order of the triangles.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(TRIANGLE_ORDER,TRIANGLE_NUM), the triangulation.
%
%    Output, real VALUE, the Q quality measure.
%
  if ( triangle_num < 1 )
    value = -1.0;
    return
  end
%
%  Compute the mean value of Q.
%
  q_min = r8_huge ( );

  for triangle = 1 : triangle_num

    a_index = triangle_node(1,triangle);
    b_index = triangle_node(2,triangle);
    c_index = triangle_node(3,triangle);

    ab_length = sqrt ( ...
        ( z(1,a_index) - z(1,b_index) )^2 ...
      + ( z(2,a_index) - z(2,b_index) )^2 );

    bc_length = sqrt ( ...
        ( z(1,b_index) - z(1,c_index) )^2 ...
      + ( z(2,b_index) - z(2,c_index) )^2 );

    ca_length = sqrt ( ...
        ( z(1,c_index) - z(1,a_index) )^2 ...
      + ( z(2,c_index) - z(2,a_index) )^2 );

    q = ( bc_length + ca_length - ab_length ) ...
      * ( ca_length + ab_length - bc_length ) ...
      * ( ab_length + bc_length - ca_length ) ...
      / ( ab_length * bc_length * ca_length );

    q_min = min ( q_min, q );

  end

  value = q_min;

  return
end


function [fm,am,iflaw]=doppler(N,Fs,f0,d,v,t0,c);
%DOPPLER Generate complex Doppler signal.
% 	 [FM,AM,IFLAW]=DOPPLER(N,FS,F0,D,V,T0,C)
%	 Returns the frequency modulation (FM), the amplitude
%	 modulation (AM) and the instantaneous frequency law (IFLAW)
%	 of the signal received by a fixed observer from a moving target
%	 emitting a pure frequency f0.
%
%	 N  : number of points.
%	 FS : sampling frequency (in Hertz).
%	 F0 : target   frequency (in Hertz).
%	 D  : distance from the line to the observer (in meters).
%	 V  : target velocity    (in m/s)
%	 T0 : time center                  (default : N/2).
%	 C  : wave velocity      (in m/s)  (default : 340).
%	 FM : Output frequency modulation.
%	 AM : Output amplitude modulation.
%	 IFLAW : Output instantaneous frequency law.
%
%	Example:
%	 N=512; [fm,am,iflaw]=doppler(N,200,65,10,50);
%	 subplot(211); plot(real(am.*fm));
%	 subplot(212); plot(iflaw);
%        [ifhat,t]=instfreq(sigmerge(am.*fm,noisecg(N),15),11:502,10);
%        hold on; plot(t,ifhat,'g'); hold off;
%
%	See also DOPNOISE

%	F. Auger, July 94, August 95 - O. Lemoine, October 95.
%	Copyright (c) 1996 by CNRS (France).
%
%  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin <= 4),
 error ( 'At least 5 parameters are required' );
elseif (nargin == 5),
 t0=N/2; c=340.0;
elseif (nargin == 6),
 c=340.0;
end;

if (N <= 0),
 error ('The signal length N must be strictly positive' );
elseif (d <= 0.0),
 error ('The distance D must be positive' );
elseif (Fs < 0.0),
 error ('The sampling frequency FS must be positive' );
elseif (t0<1) | (t0>N),
 error ('T0 must be between 1 and N');
elseif (f0<0)|(f0>Fs/2),
 error ('F0 must be between 0 and FS/2');
elseif (v<0),
 error ('V must be positive');
else
 tmt0=((1:N)'-t0)/Fs;
 dist=sqrt(d^2+(v*tmt0).^2);
 fm = exp(j*2.0*pi*f0*(tmt0-dist/c));
 if (nargout>=2),
  if abs(f0)<eps,
   am=0;
  else
   am= 1.0 ./ sqrt(dist);
  end
 end;
 if (nargout==3), iflaw=(1-v^2*tmt0./dist/c)*f0/Fs; end;
end ;


function lambda = givens_eigenvalues ( n )

%*****************************************************************************80
%
%% GIVENS_EIGENVALUES returns the eigenvalues of the GIVENS matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  for i = 1 : n
    angle = ( 2 * i - 1 ) * pi / ( 4 * n );
    lambda(i,1) = 0.5 / ( cos ( angle ) )^2;
  end

  return
end


function [alpha, c] = dlsqrat(t,y,p,q,alpha)

%  [alpha] = dlsqrat(t,y,p,q,alpha)
%
%  A Full Newton non-linear least-squares code for discrete
%  least-squares rational approximation. This code implements the algorithm
%  described in the paper:
%
% Carlos F. Borges, A Full-Newton Approach to Separable Nonlinear Least
% Squares Problems and its Application to Discrete Least Squares Rational
% Approximation, Electronic Transactions on Numerical Analysis, Volume 35,
% pp.57-68, 2009.
%
% All are welcome to use this code as they wish. I only ask that you cite
% the paper above if you do.
%
%Inputs:
% - t,y are the data points.
% - p,q are the degrees of the numerator and denominator.
% - alpha (optional) is the starting guess
%
%Outputs:
% - alpha contains the denominator coefficients starting with alpha_1
% - c contains the numerator coefficients starting with c_0
%
%  Please note that the polynomial coefficients are generated in ascending
%  order so if you want to use Matlab's polyval routine to evaluate things
%  you need to flip the c vector, and you need to flip the alpha vector and
%  then append a 1. Here is a code fragment you can use to view the results
%  of the fit:
%
%    cla;
%    plot(t,y,'b.'); hold on
%    tt = linspace(min(t),max(t),1000)';
%    yy = polyval(flipud(c),tt)./polyval([flipud(alpha); 1],tt);
%    plot(tt,yy); hold off;
%
%
% Copyright (c) 2008 by Carlos F. Borges.
% All rights reserved.
% begin dlsqrat

% Set the convergence tolerance.
TOLERANCE = 10^(-12);

% N is the Vandermonde that will be used to evaluate the numerator.
N = zeros(length(t),p+1);
N(:,1) = ones(length(t),1);
for k=2:p+1
    N(:,k) = N(:,k-1).*t;
end
% M is the Vandermonde that will be used to evaluate the denominator.
M = zeros(length(t),q);
M(:,1) = t;
for k=2:q
    M(:,k) = M(:,k-1).*t;
end

% If we are not given an initial guess then generate one.
if nargin < 5
    tmp_pade = [N -diag(y)*M]\y;
    alpha = tmp_pade(p+2:end);
end

% Construct the model matrix and compute ancillary quantities.
update(alpha);

for iter=1:100

    % Update the error.
    old_err = err;

    % Compute the Jacobian and the Hessian.
    Tmp1 = diag(Py.*D)*M;
    Tmp2 = Q'*diag((Py-r).*D)*M;
    J = Tmp1 - Q*Tmp2;
    H = M'*diag((Py-2*r).*D)*Tmp1 - Tmp2'*Tmp2;

    % Compute the gradient.
    gradient = J'*r;

    % Compute the Cholesky factorization of H.
    [R, not_PD] = chol(H);
    % If H is not positive definite then regularize and factor
    if not_PD
        R = chol(H - 1.2*min(eig(H))*eye(q));
    end

    %Compute the Newton step.
    delta = -R\(R'\gradient);

    % Use stepsize control to take a step.
    step_control;

    % Convergence testing
    if err > old_err
        disp('Failed to find descending step length.');
        break;
    else
        alpha = new_alpha;
        rel_err = abs(old_err - err)/old_err;
        if rel_err <= TOLERANCE
            break;
        end
    end
    % End convergence testing.

end  %End of main loop.

% Compute the coefficients of the numerator.
c = (diag(D)*N)\y;

% Generate an error message if the algorithm failed to converge.
if rel_err > TOLERANCE
    disp('Algorithm did not converge.');
end

%XXXXXXXXXXXXXXXXX Subroutines XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
    function update(alpha)
        % Updates the model matrix and computes ancillary quantities.
        D = 1./(1+M*alpha);         % Compute the denominator.
        [Q R] = qr(diag(D)*N,0);    % Compute the QR factorization of A = D*N
        Py = Q*(Q'*y);              % Compute the projection of y onto the range of A.
        r = y - Py;                 % Compute the residual.
        err = r'*r;                 % Compute the current squared error.
    end

    function step_control
        % This function implements stepsize control using a simple
        % backtracking scheme from Dennis & Schnabel.

        % Try taking a full step.
        new_alpha = alpha + delta;

        % Update the model.
        update(new_alpha);

        % If a full step does not sufficiently reduce the error then we
        % use a backtracking line-search method for step-size control.
        % This involves minimizing a function f(lambda) that interpolates the
        % computed error (and its derivatives) at different values of lambda.
        f0 = old_err;
        fprime = gradient'*delta;
        steptol = f0 + .0001*fprime;
        if err > steptol

            errs(1) = err; lams(1) = 1;   % We'll need this if further refinement is necessary.

            % We start with a quadratic model at f(0), f'(0), and f(1)
            % and will take the larger of the computed step or 1/10.
            lambda = max([-fprime/(2*(err - f0 - fprime)) .1]);

            new_alpha = alpha + lambda*delta;

            % Update the model matrix and compute ancillary quantities.
            update(new_alpha);

            % If this doesn't work then we loop with a cubic model at f(0),
            % f'(0), f(lambda), and f(lam2) where the last two are errors at
            % the last two lambda that were tried.
            steptol = f0 + .0001*fprime*lambda;
            while err > steptol

                % Push the current lambda and error to the top of the lams and errs
                % stacks.
                lams = [lambda; lams(1)]; errs = [err; errs(1)];
                rhs = (errs - fprime*lams - [f0 ; f0])./(lams.*lams);
                ab = [lams [1 ; 1]]\rhs;

                lambda = (-ab(2)+sqrt(ab(2)*ab(2) - 3*ab(1)*fprime))/(3*ab(1));

                % It is still important to make certain that the new lambda
                % progresses quickly but not too quickly. So if lambda is less
                % than lam2/10 we just use lam2/10, and if it is larger than
                % lam2/2 then we use lam2/2.
                if lambda < lams(1)/10
                    lambda = lams(1)/10;
                end
                if lambda > lams(1)/2
                    lambda = lams(1)/2;
                end

                new_alpha = alpha + lambda*delta;

                % Update the model matrix and compute ancillary quantities.
                update(new_alpha);

                steptol = f0 + .0001*fprime*lambda;

            end
        end
    end

%XXXXXXXXXXXXXXXXX Subroutines End XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

end
% End of function.


% Reed-Solomon Errors and Erasures Decoding
% Decoding is based on the Massey-Berlekamp decoder.  Based on the notes of Adina Matache: http://www.ee.ucla.edu/~matache/rsc/slide.html
% This program was written by Jaco Versfeld. For any questions or remarks email Jaco at jaco.versfeld@gmail.com.
% This code is provided as is and Jaco Versfeld accepts no liability what so ever.
% Feel free to use and modify this code for whatever purpose.

clear
clc

%**************************
%*** RS code Parameters ***
%**************************

% Here we specify the parameters of the (n,k) Reed-Solomon code

m = 4          %Determine the Galois Field, GF(2^m)
n = 2^m - 1    %This is fixed for a Reed-Solomon, the length of the codeword
k = 3          %The number of data symbols, can be anything between 1 to n - 1
h = n-k
t = h/2

%**************************

%*** Generate the Galois Field and Generator polynomial ***

% This step is neccessary for Matlab.  Here we create the Galois Field which is used for
% computations of the Reed-Solomon code

field = gftuple([-1:2^m-2]', m, 2);

%Generator Polynomial:
%Lin + Costello, p.171
%The Generator polynomial is one way of encoding Reed-Solomon codes

%Construct the generator polynomial
c = [1 0];
p(1) = c(1);

for i = 1:h-1
    p(1) = gfmul(p(1),1,field);
    p(2) = 0;
    c = gfconv(c,p,field);
end
g = c;

%**************************

%*** RS Encode ***

%Generate Random Data
DATA_IN = randint(1,k,[-1 n-1]);

%RS encoding
parity = RS_ENC4(DATA_IN,n,k,g,field);
RS_CODE = [parity DATA_IN];

%********************************

%*** Channel ***
RECEIVED = RS_CODE

%I introduce the errors manually here, but any channel can be used here, like an AWGN.
% A maximum of 2*t + e <= (n-k) errors and erasures can be corrected, where t is the number of
% errors and e the number of erasures

%Introduce some errors
RECEIVED(3) = gfadd(RECEIVED(3),randint(1,1,[-1 n-1]),field);
RECEIVED(5) = gfadd(RECEIVED(3),randint(1,1,[-1 n-1]),field);

%Introduce some erasures
erasures = [ 2 3 4 5 6 7 8 9 11 14 15];  %This polynomial contains the positions of the erasures

%RECEIVED(1) = -2
RECEIVED(2) = -2
RECEIVED(3) = -2
RECEIVED(4) = -2
RECEIVED(5) = -2
RECEIVED(6) = -2
RECEIVED(7) = -2
RECEIVED(8) = -2
RECEIVED(9) = -2
RECEIVED(11) = -2
RECEIVED(14) = -2
RECEIVED(15) = -2

%****************

%*** Decoding ***

DECODED = RS_E_E_DEC(RECEIVED, erasures,n,k,t,h,g,field);

%****************

DECODED
RS_CODE

if all(DECODED == RS_CODE)
    disp('Decoding Success')
else
    disp('Decoding Failure')
end


%% Edge Element Discretization of Maxwell Equations
% We test Maxwell solvers in iFEM.

clear all; close all
rowNames ={'h=1/2';'h=1/4';'h=1/8'};
colHeaders = {'H^curl Error','L^2 Error'};

%% The data of the pde
%
% * pde = Maxwelldata1; % zero Neumann boundary condition and curl u = 0
% * pde = Maxwelldata2; % non-homogenous Neumann boundary condition
% * pde = Maxwelldata3; % polynomial data and curl u = 0
% * pde = Maxwelldata4; % zero Dirichlet boundary condition
% * pde = Maxwelldata5; % linear polynomial data
% * pde = planewavedataC; % plane wave with complex coefficients
% * pde = planewavedata1; % plane wave with real coefficients

%% Positive Definite Case
% curl curl E + E = f.

help Maxwelldata2
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
pde = Maxwelldata2;
bdFlag = setboundary3(node,elem,'Dirichlet');
option.solver = 'cg';
cubeMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
pde = Maxwelldata2;
bdFlag = setboundary3(node,elem,'Neumann');
option.solver = 'cg';
cubeMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Optimal first order of convergence is achieved. HX preconditioned CG
% converges around 20 steps.

%% Indefinite with real coefficients
% curl curl E - E = f.

help planewavedata1
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
pde = planewavedata1;
bdFlag = setboundary3(node,elem,'Dirichlet');
option.solver = 'cg';
cubeMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
pde = planewavedata1;
bdFlag = setboundary3(node,elem,'Neumann');
option.solver = 'cg';
cubeMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Optimal first order of convergence is achieved. HX preconditioned CG
% converges around 40 steps although the system is indefinite.

%% Indefinite: complex coefficients, real solution
% curl curl E - (1-i)E = f.
option.solver = 'gmres';

help planewavedataC
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
pde = planewavedataC;
bdFlag = setboundary3(node,elem,'Dirichlet');
cubeMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
pde = planewavedataC;
bdFlag = setboundary3(node,elem,'Neumann');
cubeMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Optimal first order of convergence is achieved. HX preconditioned GMRES
% converges around 90 steps although the system is indefinite.
%
% Bug: For Neumann boundary condition, the rate of L2 error is not quite right.

%% Indefinite: real coefficents, complex solution
option.solver = 'cg';

help planewavedata
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
bdFlag = setboundary3(node,elem,'Dirichlet');
planewaveMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.5);
bdFlag = setboundary3(node,elem,'Neumann');
planewaveMaxwell;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% The computation of error can't handle complex functions. So in
% planewaveMaxwell the error between uI and uh is computed. Therefore
% slightly better rate of convergence is observed. The rate of L2 error is
% almost second order.


clearvars;
close all;
clc;
rng default;

mf = spx.graphics.Figures;

mf.new_figure();
[signal, fs] = spx.dsp.dtmf({'4', '5', '0', '7'});
time = (0:(numel(signal) - 1)) / fs;
plot(1e3*time, signal);
xlabel('Time (ms)');
ylabel('Amplitude');
grid on;
saveas(gcf, 'images/dtmf_4507.png');

mf.new_figure();
envelope_signal = envelope(signal, 80,'rms');
plot(1e3*time, envelope_signal);
xlabel('Time (ms)');
ylabel('Amplitude');
grid on;
saveas(gcf, 'images/dtmf_4507_envelope.png');

mf.new_figure();
pulsewidth(envelope_signal,fs)
saveas(gcf, 'images/dtmf_4507_pulses.png');

mf.new_figure();
periodogram(signal, [], [], fs);
saveas(gcf, 'images/dtmf_4507_periodogram.png');

% find the power spectrum and corresponding frequencies
[pxx,f]=periodogram(signal,[],[],fs);

n_freqs = 5;
% Let's find the most important frequencies
% 'NPeaks', n_freqs
[peak_values, peak_freqs] = findpeaks(pxx, f, 'SortStr','descend', 'MinPeakHeight', max(pxx) / 10);
peak_freqs = round(peak_freqs');
% Let's also mark them on a plot
mf.new_figure();
findpeaks(pxx, f, 'SortStr','descend','MinPeakHeight', max(pxx) / 10);

mf.new_figure();
spectrogram(signal, [], [], [], fs, 'yaxis');
% restrict the y-axis between 500Hz to 1500 Hz.
ylim([0.5 1.5]);
saveas(gcf, 'images/dtmf_4507_spectrogram.png');

mf.new_figure();
window_length = floor(fs * 50 / 1000);
overlap_length = floor(window_length / 2);
n_fft = 2^nextpow2(window_length);
spectrogram(signal,hamming(window_length),overlap_length,n_fft, fs, 'yaxis');
ylim([0.5 1.5]);
saveas(gcf, 'images/dtmf_4507_spectrogram_50ms.png');

mf.new_figure();
overlap_length = floor(0.8 * window_length );
spectrogram(signal,hamming(window_length),overlap_length,n_fft, fs, 'yaxis', 'MinThreshold', -50);
ylim([0.5 1.5]);
saveas(gcf, 'images/dtmf_4507_spectrogram_50ms_40ms_50db.png');

mf.new_figure();
spectrogram(signal,hamming(window_length),overlap_length,n_fft, fs, 'yaxis', 'MinThreshold', -50, 'reassigned');
ylim([0.5 1.5]);
saveas(gcf, 'images/dtmf_4507_spectrogram_50ms_40ms_50db_reassigned.png');


% IM = mkGaussian(SIZE, COVARIANCE, MEAN, AMPLITUDE)
%
% Compute a matrix with dimensions SIZE (a [Y X] 2-vector, or a
% scalar) containing a Gaussian function, centered at pixel position
% specified by MEAN (default = (size+1)/2), with given COVARIANCE (can
% be a scalar, 2-vector, or 2x2 matrix.  Default = (min(size)/6)^2),
% and AMPLITUDE.  AMPLITUDE='norm' (default) will produce a
% probability-normalized function.  All but the first argument are
% optional.

% Eero Simoncelli, 6/96.

function [res] = mkGaussian(sz, cov, mn, ampl)

sz = sz(:);
if (size(sz,1) == 1)
  sz = [sz,sz];
end

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('cov') ~= 1)
  cov = (min(sz(1),sz(2))/6)^2;
end

if (exist('mn') ~= 1)
  mn = (sz+1)/2;
end

if (exist('ampl') ~= 1)
  ampl = 'norm';
end

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

[xramp,yramp] = meshgrid([1:sz(2)]-mn(2),[1:sz(1)]-mn(1));

if (sum(size(cov)) == 2)  % scalar
  if (strcmp(ampl,'norm'))
    ampl = 1/(2*pi*cov(1));
  end
  e = (xramp.^2 + yramp.^2)/(-2 * cov);
elseif (sum(size(cov)) == 3) % a 2-vector
  if (strcmp(ampl,'norm'))
    ampl = 1/(2*pi*sqrt(cov(1)*cov(2)));
  end
  e = xramp.^2/(-2 * cov(2)) + yramp.^2/(-2 * cov(1));
else
  if (strcmp(ampl,'norm'))
    ampl = 1/(2*pi*sqrt(det(cov)));
  end
  cov = -inv(cov)/2;
  e = cov(2,2)*xramp.^2 + (cov(1,2)+cov(2,1))*(xramp.*yramp) ...
      + cov(1,1)*yramp.^2;
end

res = ampl .* exp(e);


function X_rec = recoverData(Z, U, K)
%RECOVERDATA Recovers an approximation of the original data when using the
%projected data
%   X_rec = RECOVERDATA(Z, U, K) recovers an approximation the
%   original data that has been reduced to K dimensions. It returns the
%   approximate reconstruction in X_rec.
%

% You need to return the following variables correctly.
X_rec = zeros(size(Z, 1), size(U, 1));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the approximation of the data by projecting back
%               onto the original space using the top K eigenvectors in U.
%
%               For the i-th example Z(i,:), the (approximate)
%               recovered data for dimension j is given as follows:
%                    v = Z(i, :)';
%                    recovered_j = v' * U(j, 1:K)';
%
%               Notice that U(j, 1:K) is a row vector.
%

for i = 1:size(Z,1)

	v = Z(i, :)';
	X_rec(i,:) = v' * U(:, 1:K)';

end

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

end


function [ x, seed ] = r8vec_normal_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_NORMAL_01 returns a unit pseudonormal R8VEC.
%
%  Discussion:
%
%    The standard normal probability distribution function (PDF) has
%    mean 0 and standard deviation 1.
%
%    This routine can generate a vector of values on one call.  It
%    has the feature that it should provide the same results
%    in the same order no matter how we break up the task.
%
%    Before calling this routine, the user may call RANDOM_SEED
%    in order to set the seed of the random number generator.
%
%    The Box-Muller method is used, which is efficient, but
%    generates an even number of values each time.  On any call
%    to this routine, an even number of new values are generated.
%    Depending on the situation, one value may be left over.
%    In that case, it is saved for the next call.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of values desired.  If N is negative,
%    then the code will flush its internal memory; in particular,
%    if there is a saved value to be used on the next call, it is
%    instead discarded.  This is useful if the user has reset the
%    random number seed, for instance.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N), a sample of the standard normal PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
%  Local parameters:
%
%    Local, integer MADE, records the number of values that have
%    been computed.  On input with negative N, this value overwrites
%    the return value of N, so the user can get an accounting of
%    how much work has been done.
%
%    Local, real R(N+1), is used to store some uniform random values.
%    Its dimension is N+1, but really it is only needed to be the
%    smallest even number greater than or equal to N.
%
%    Local, integer SAVED, is 0 or 1 depending on whether there is a
%    single saved value left over from the previous call.
%
%    Local, integer X_LO_INDEX, X_HI_INDEX, records the range of entries of
%    X that we need to compute.  This starts off as 1:N, but is adjusted
%    if we have a saved value that can be immediately stored in X(1),
%    and so on.
%
%    Local, real Y, the value saved from the previous call, if
%    SAVED is 1.
%
  persistent made;
  persistent saved;
  persistent y;
%
%  I'd like to allow the user to reset the internal data.
%  But this won't work properly if we have a saved value Y.
%  I'm making a crock option that allows the user to signal
%  explicitly that any internal memory should be flushed,
%  by passing in a negative value for N.
%
  if ( n < 0 )
    made = 0;
    saved = 0;
    y = 0.0;
    x = [];
    return
  elseif ( n == 0 )
    x = [];
    return
  end
%
%  Record the range of X we need to fill in.
%
  x_lo_index = 1;
  x_hi_index = n;
%
%  Use up the old value, if we have it.
%
  if ( saved == 1 )
    x(1) = y;
    saved = 0;
    x_lo_index = 2;
  end
%
%  Maybe we don't need any more values.
%
  if ( x_hi_index - x_lo_index + 1 == 0 )
%
%  If we need just one new value, do that here to avoid null arrays.
%
  elseif ( x_hi_index - x_lo_index + 1 == 1 )

    [ r(1), seed ] = r8_uniform_01 ( seed );

    if ( r(1) == 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8VEC_NORMAL_01 - Fatal error!\n' );
      fprintf ( 1, '  R8_UNIFORM_01 returned a value of 0.\n' );
      error ( 'R8VEC_NORMAL_01 - Fatal error!' );
    end

    [ r(2), seed ] = r8_uniform_01 ( seed );

    x(x_hi_index) = ...
             sqrt ( - 2.0 * log ( r(1) ) ) * cos ( 2.0 * pi * r(2) );
    y =      sqrt ( - 2.0 * log ( r(1) ) ) * sin ( 2.0 * pi * r(2) );

    saved = 1;

    made = made + 2;
%
%  If we require an even number of values, that's easy.
%
  elseif ( mod ( x_hi_index - x_lo_index + 1, 2 ) == 0 )

    m = floor ( ( x_hi_index - x_lo_index + 1 ) / 2 );

    [ r, seed ] = r8vec_uniform_01 ( 2*m, seed );

    x(x_lo_index:2:x_hi_index-1) = ...
      sqrt ( -2.0 * log ( r(1:2:2*m-1) ) ) ...
      .* cos ( 2.0 * pi * r(2:2:2*m) );

    x(x_lo_index+1:2:x_hi_index) = ...
      sqrt ( -2.0 * log ( r(1:2:2*m-1) ) ) ...
      .* sin ( 2.0 * pi * r(2:2:2*m) );

    made = made + x_hi_index - x_lo_index + 1;
%
%  If we require an odd number of values, we generate an even number,
%  and handle the last pair specially, storing one in X(N), and
%  saving the other for later.
%
  else

    x_hi_index = x_hi_index - 1;

    m = floor ( ( x_hi_index - x_lo_index + 1 ) / 2 ) + 1;

    [ r, seed ] = r8vec_uniform_01 ( 2*m, seed );

    x(x_lo_index:2:x_hi_index-1) = ...
      sqrt ( -2.0 * log ( r(1:2:2*m-3) ) ) ...
      .* cos ( 2.0 * pi * r(2:2:2*m-2) );

    x(x_lo_index+1:2:x_hi_index) = ...
      sqrt ( -2.0 * log ( r(1:2:2*m-3) ) ) ...
      .* sin ( 2.0 * pi * r(2:2:2*m-2) );

    x(n) = sqrt ( -2.0 * log ( r(2*m-1) ) ) ...
      * cos ( 2.0 * pi * r(2*m) );

    y = sqrt ( -2.0 * log ( r(2*m-1) ) ) ...
      * sin ( 2.0 * pi * r(2*m) );

    saved = 1;

    made = made + x_hi_index - x_lo_index + 2;

  end

  return
end


% Calculate the number of connected components using the eigenvalues
%                   of the Laplacian - counting the number of zeros
%
% INPUTS: adjacency matrix, nxn
% OUTPUTs: positive integer - number of connected components
%
% Other routines used: graphSpectrum.m
% GB: last updated: September 22, 2012

function nc=numConnComp(adj)

s=graphSpectrum(adj);
nc=numel(find(s<10^(-5)));   % zero eigenvalues are sometimes close to zeros numerically


function C = spm_dctmtx(N,K,n,f)
% Creates basis functions for Discrete Cosine Transform.
% FORMAT C = spm_dctmtx(N,K,n)
%     OR C = spm_dctmtx(N,K)
%     OR D = spm_dctmtx(N,K,n,'diff')
%     OR D = spm_dctmtx(N,K,'diff')
% N - dimension
% K - order
% n - optional points to sample
%____________________________________________________________________________
% spm_dctmtx creates a matrix for the first few basis functions of a one
% dimensional discrete cosine transform.
% With the 'diff' argument, spm_dctmtx produces the derivatives of the
% DCT.
%
% See:    Fundamentals of Digital Image Processing (p 150-154).
%         Anil K. Jain 1989.
%____________________________________________________________________________
% @(#)spm_dctmtx.m	2.1 John Ashburner MRCCU/FIL 01/08/28

d = 0;

if nargin == 1, K = N; end;

if any(nargin == [1 2]),
	n = (0:(N-1))';
elseif nargin == 3,
	if strcmp(n,'diff'),
		d = 1;
		n = (0:(N-1))';
	elseif strcmp(n,'diff2'),
		d = 2;
		n = (0:(N-1))';
	else
		n = n(:);
	end
elseif nargin == 4,
	n = n(:);
	if strcmp(f,'diff'),
		d = 1;
	elseif strcmp(n,'diff2'),
		d = 2;
	else
		error('Incorrect Usage');
	end
else
	error('Incorrect Usage');
end

C = zeros(size(n,1),K);

if d == 0,
	C(:,1)=ones(size(n,1),1)/sqrt(N);
	for k=2:K
		C(:,k) = sqrt(2/N)*cos(pi*(2*n+1)*(k-1)/(2*N));
	end
elseif d == 1,
	for k=2:K
		C(:,k) = -2^(1/2)*(1/N)^(1/2)*sin(1/2*pi*(2*n*k-2*n+k-1)/N)*pi*(k-1)/N;
	end
elseif d == 2,
	for k=2:K,
		C(:,k) = -2^(1/2)*(1/N)^(1/2)*cos(1/2*pi*(2*n+1)*(k-1)/N)*pi^2*(k-1)^2/N^2;
	end;
else,
	error('Can''t do this');
end


function c = correlation_cubic ( n, rho, rho0 )

%*****************************************************************************80
%
%% CORRELATION_CUBIC evaluates the cubic correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Petter Abrahamsen,
%    A Review of Gaussian Random Fields and Correlation Functions,
%    Norwegian Computing Center, 1997.
%
%  Parameters:
%
%    Input, integer N, the number of arguments.
%
%    Input, real RHO(N,1), the arguments.
%
%    Input, real RHO0, the correlation length.
%
%    Output, real C(N,1), the correlations.
%
  rho = rho ( : );

  rhohat = min ( abs ( rho ) / rho0, 1.0 );

  c = 1.0 - 7.0 * rhohat.^2 + 8.75 * rhohat.^3 - 3.5 * rhohat.^5 + 0.75 * rhohat.^7;

  return
end


function X = MvnRndMatchCrossCov(S, varargin)
% X = MvnRndMatchCrossCov(S, J) simulates a panel of J scenarios of a
% multivariate normal random vector with zero sample mean and sample
% covariance exactly equal to S. Each row of X is a scenario and each
% column is a variable.
%
% X = MvnRndMatchCrossCov(S, P, F, CovF) generates a multivariate normal
% panel X with zero sample mean, sample covariance S and sample cross
% covariance P with another zero-mean panel F. CovF, the sample covariance
% of F, is computed if not provided as input.

% IMPORTANT: The order of computations in the following expressions is
% important for efficiency. Please use the same order, or have a good reason
% to change it!

% Code by S. Gollamudi. This version December 2009.

% check input
N = size(S,1);
if nargin==2,
    J = varargin{1};
    K = 0;
else
    P = varargin{1};
    F = varargin{2};
    [J,K] = size(F);
    assert(all(size(P)==[K,N]), 'Incorrect dimensions of cross-covariance P.');
    if length(varargin)<=2,
        CovF = cov(F,1);
    else
        CovF = varargin{3};
    end
    sqrtJ = sqrt(J);
end

% tolerance level
eps = 1e-9;

% Algorithm: X = F*B + V*R
% where F'*V=0, V'V=J*I and R is upper triangular

if K>0,

    % Compute B
    B = CovF \ P;

    % Compute R
    Sigma_orth = S - P'*B;
    var_orth = diag(Sigma_orth);
    var_x = diag(S);
    if any(var_orth < -eps*var_x),
        error('MvnRndMatchCrossCov: S and P are incompatible.')
    end
    i_positive = (var_orth > eps*var_x);
    num_positive = sum(i_positive);
    R = zeros(num_positive,N);
    R(:,i_positive) = chol(S(i_positive,i_positive) - P(:,i_positive)'*B(:,i_positive));

    % Generate V
    Z = randn(J/2,N);
    Z=[Z
        -Z];

    % using QR decomposition
    %[V,dummy] = qr([F Z],0);
    %V = V(:,K+1:K+num_positive)*sqrtJ;

    % using modified Gram-Schmidt
    Z = Z - (F/CovF)*((F'*Z)/J);
    V = zeros(J,0);
    for n = 1:num_positive,
        u = Z(:,n) - V*((V'*Z(:,n))/J);
        V = [V u*(sqrtJ/norm(u))];
    end

    % generate panel X
    X = F*B + V*R;

else

    % generate X to have zero sample mean and sample covariance S
    Z = randn(J/2,N);
    Z = [Z
        -Z];
    X = Z * (chol(cov(Z,1)) \ chol(S));

end


% Steady state transport solutions for one fixed value of Pe and various values of Da1
x = [0:0.01:1];
Pe = 1; Da1 = 16;
s = sqrt(0.25*Pe*Pe+Pe/Da1);
mu1 = 0.5*Pe+s; mu2 = 0.5*Pe-s;
s = mu2*exp(mu2)-mu1*exp(mu1);
c = (mu2*exp(mu2)*exp(mu1*x)-mu1*exp(mu1)*exp(mu2*x))./s;
for Da1 = [8 4 2 1 0.5 0.25 0.125];
    s = sqrt(0.25*Pe*Pe+Pe/Da1);
    mu1 = 0.5*Pe+s; mu2 = 0.5*Pe-s;
    s = mu2*exp(mu2)-mu1*exp(mu1);
    c = [c;(mu2*exp(mu2)*exp(mu1*x)-mu1*exp(mu1)*exp(mu2*x))./s];
end
plot (x,c);
legend('Da_1=16','Da_1=8','Da_1=4','Da_1=2','Da_1=1','Da_1=0.5','Da_1=0.25','Da_1=0.125');
xlabel ('x/L [-]'); ylabel ('c/c_{in} [-]');


function pdf = f_pdf ( x, m, n )

%*****************************************************************************80
%
%% F_PDF evaluates the F central PDF.
%
%  Discussion:
%
%    PDF(X)(M,N) = M**(M/2) * X**((M-2)/2)
%      / ( Beta(M/2,N/2) * N**(M/2) * ( 1 + (M/N) * X )**((M+N)/2)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    0.0 <= X
%
%    Input, integer M, N, the parameters of the PDF.
%    1 <= M,
%    1 <= N.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x < 0.0 )

    pdf = 0.0;

  else

    a = m;
    b = n;

    top = sqrt ( m^m * n^n * x^( m - 2 ) );
    bot1 = beta ( m / 2.0, n / 2.0 );
    bot2 =  sqrt ( ( n + m * x )^( m + n ) );

    pdf = top / ( bot1 * bot2 );

  end

  return
end


function X = newtonraphson(Eqn_Str,Start_Point,Max_Iter)

% Function :
%        X = newtonraphson(fun_str,Start_Point)
%              Finds the root of an equation by NEWTON-RAPHSON METHOD.
%
%         x(n+1) = x(n) - (F(x(n))/diff(F(x(n)));
%
% INPUTS :
%        Eqn_Str     : The equation whose root has to be find.
%                      Eqn_Str Should be an string format.
%                      Example :
%                       if  F(x) = x^4 - x - 10
%                       the Eqn_Str = 'x^4 - x - 10';
%
%        Start_Point : Initial value of root X ==> x0
%
%        Max_Iter    : Maximum number of iterations
%
% OUTPUT :
%        X           : Estimeted root of Equation.
%--------------------------------------------------------------------------
% By :-
%      SANDEEP SOLANKI
%      rtm_sandeep@rediffmail.com
%--------------------------------------------------------------------------
if nargin < 2
    error('Not Enough Input Arguments');
end

if nargin < 3
    Max_Iter = 1000;
end

if ~ischar(Eqn_Str)
    error('Funtion Should Be an String');
end

fx = inline(Eqn_Str);
fxd = inline(diff(Eqn_Str));

x0 = Start_Point;
disp(['F(X) = ' Eqn_Str]);
disp(['X0 = ' num2str(x0)]);
% Iterating
for i = 1 : Max_Iter
    s1=sprintf('    Iteration : %1.0f',i);
    disp(s1);
    x1 = x0 - (fx(x0)/fxd(x0));

    s2=sprintf('              X(%0.0f) = %0.15f',i,x1);
    disp(s2);
    if x1 == x0
        disp('Terminating Process : Value of Root Repeated');
        break;
    else
        x0 = x1;
    end
end

if x1 == NaN
    error('Start Point is Not Correct');
end
X = x1;


% Generate various time and frequency-varying test signals
%
% Usage 1:
%
%   output = gsig( data_type, f1, f2, num_samples, sig_type);
%
%
%   Inputs
%
%	data_type
%
%          'lin' linear FM
%          'quad' quadratic FM
%          'cubic' cubic FM
%          'hyp' hyperbolic FM
%
%	f1, f2
%
%          normalised frequency bounds for the signal (i.e. taking
%   	   sampling frequency as 1 Hz) and sig_type=1 for real data
%          otherwise  the result is complex.
%
%	num_samples
%
%          is the number of generated data samples.
%
%
%      sig_type
%
%          Must be 0 (analytic) or 1 (real).
%
% Usage 2:
%
%   output = gsig( 'sin', cf, mf, num_samples, sig_type, fdev );
%
%
%   Inputs
%
%	cf and mf
%
%          central and modulation frequencies.
%
%       fdev
%
%          frequency deviation.
%
% Usage 3:
%
%   output1 = gsig( 'step', f1, f2, num_samples, sig_type, ns );
%
%   Inputs
%
%	f1, f2
%
%          The normalised frequency bounds for the signal (i.e. taking
%	   sampling frequency as 1 Hz).
%
%       ns
%
%          The number of steps.
%
%
%
%  See Also: analyt
%
% TFSAP 7.0
% Copyright Prof. B. Boashash
% Qatar University, Doha
% email: tfsap.research@gmail.com


function net = net_init(opts)
% CNN_MNIST_LENET Initialize a CNN similar for MNIST

rng('default');
rng(0) ;

f=1/100 ;
net.layers = {} ;
net.layers{end+1} = struct('type', 'linear', ...
                           'weights', {{f*randn(128,28*28*1, 'single'), zeros(128,1,'single')}}) ;
net.layers{end+1} = struct('type', 'relu') ;
%
net.layers{end+1} = struct('type', 'linear', ...
                           'weights', {{f*randn(128,128, 'single'),  zeros(128,1,'single')}}) ;
net.layers{end+1} = struct('type', 'relu') ;

%
%}
net.layers{end+1} = struct('type', 'linear', ...
                           'weights', {{f*randn(10,128, 'single'), zeros(10,1,'single')}}) ;

net.layers{end+1} = struct('type', 'softmaxloss') ;

for i=1:numel(net.layers)
    if strcmp(net.layers{i}.type,'linear')
        net.layers{1,i}.momentum{1}=zeros(size(net.layers{1,i}.weights{1}));
        net.layers{1,i}.momentum{2}=zeros(size(net.layers{1,i}.weights{2}));
    end
end

%}


function membership=paretoset(X)

% PARETOSET  To get the Pareto set from a given set of points.
% synopsis:           membership =paretoset (objectiveMatrix)
% where:
%   objectiveMatrix: [number of points X number of objectives] array
%   membership:      [number of points X 1] logical vector to indicate if ith
%                    point belongs to the Pareto set (true) or not (false).
%
% by Yi Cao, Cranfield University, 02 June 2007
% Revised by Yi Cao on 17 October 2007
% Version 3, 21 October 2007, new sorting scheme to improve speed.
% Bugfix, 25 July 2008, devided by zero error is fixed.
%
% Examples: see paretoset_examples
%

m=size(X,1);
Xmin=min(X);
X1=X-Xmin(ones(m,1),:);     %make sure X1>=0;
Xmean=mean(X1);
%sort X1 so that dominated points can be removed quickly
[x,checklist]=sort(max(X1./(Xmean(ones(m,1),:)+1)));
Y=X(checklist,:);
membership=false(m,1);
while numel(checklist)>1
    k=checklist(1);
    [membership(k),checklist,Y]=paretosub(Y,checklist);
end
membership(checklist)=true;

function [ispareto,nondominated,X]=paretosub(X,checklist)

Z=X-X(ones(size(X,1),1),:);
nondominated=any(Z<0,2);                    %retain nondominated points from the check list
ispareto=all(any(Z(nondominated,:)>0,2));   %check if current point belongs to pareto set
X=X(nondominated,:);
nondominated=checklist(nondominated);


%%%%%%%VERSION 2
%%ANOTHER DESCRIBTION OF GABOR FILTER

%The Gabor filter is basically a Gaussian (with variances sx and sy along x and y-axes respectively)
%modulated by a complex sinusoid (with centre frequencies U and V along x and y-axes respectively)
%described by the following equation
%%
%                            -1     x' ^     y'  ^
%%% G(x,y,theta,f) =  exp ([----{(----) 2+(----) 2}])*cos(2*pi*f*x');
%                             2    sx'       sy'
%%% x' = x*cos(theta)+y*sin(theta);
%%% y' = y*cos(theta)-x*sin(theta);

%% Describtion :

%% I : Input image
%% Sx & Sy : Variances along x and y-axes respectively
%% f : The frequency of the sinusoidal function
%% theta : The orientation of Gabor filter

%% G : The output filter as described above
%% gabout : The output filtered image

%%  Author : Ahmad poursaberi  e-mail : a.poursaberi@ece.ut.ac.ir
%%          Faulty of Engineering, Electrical&Computer Department,Tehran
%%          University,Iran,June 2004

function [G,gabout] = gaborfilter(I,Sx,Sy,f,theta);

if isa(I,'double')~=1
    I = double(I);
end

for x = -fix(Sx):fix(Sx)
    for y = -fix(Sy):fix(Sy)
        xPrime = x * cos(theta) + y * sin(theta);
        yPrime = y * cos(theta) - x * sin(theta);
        G(fix(Sx)+x+1,fix(Sy)+y+1) = exp(-.5*((xPrime/Sx)^2+(yPrime/Sy)^2))*cos(2*pi*f*xPrime);
    end
end

Imgabout = conv2(I,double(imag(G)),'same');
Regabout = conv2(I,double(real(G)),'same');

gabout = sqrt(Imgabout.*Imgabout + Regabout.*Regabout);


function [var, U, lambda] = ppca(x, ppca_dim)
%PPCA	Probabilistic Principal Components Analysis
%
%	Description
%	 [VAR, U, LAMBDA] = PPCA(X, PPCA_DIM) computes the principal
%	component subspace U of dimension PPCA_DIM using a centred covariance
%	matrix X. The variable VAR contains the off-subspace variance (which
%	is assumed to be spherical), while the vector LAMBDA contains the
%	variances of each of the principal components.  This is computed
%	using the eigenvalue and eigenvector  decomposition of X.
%
%	See also
%	EIGDEC, PCA
%

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

if ppca_dim ~= round(ppca_dim) | ppca_dim < 1 | ppca_dim > size(x, 2)
   error('Number of PCs must be integer, >0, < dim');
end

[ndata, data_dim] = size(x);
% Assumes that x is centred and responsibility weighted
% covariance matrix
[l Utemp] = eigdec(x, data_dim);
% Zero any negative eigenvalues (caused by rounding)
l(l<0) = 0;
% Now compute the sigma squared values for all possible values
% of q
s2_temp = cumsum(l(end:-1:1))./[1:data_dim]';
% If necessary, reduce the value of q so that var is at least
% eps * largest eigenvalue
q_temp = min([ppca_dim; data_dim-min(find(s2_temp/l(1) > eps))]);
if q_temp ~= ppca_dim
  wstringpart = 'Covariance matrix ill-conditioned: extracted';
  wstring = sprintf('%s %d/%d PCs', ...
      wstringpart, q_temp, ppca_dim);
  warning(wstring);
end
if q_temp == 0
  % All the latent dimensions have disappeared, so we are
  % just left with the noise model
  var = l(1)/data_dim;
  lambda = var*ones(1, ppca_dim);
else
  var = mean(l(q_temp+1:end));
end
U = Utemp(:, 1:q_temp);
lambda(1:q_temp) = l(1:q_temp);


% modulator
function [data_out]=mod_d(data_in,rate_id)
switch (rate_id)
    case 0
       M=2;  %Size of signal constellation for BFSK
    case {1,2}
       M=4;  %Size of signal constellation QPSK
    case {3,4}
       M=16;  %Size of signal constellation 16QAM
    case {5,6}
       M=64;  %Size of signal constellation 64QAM
    otherwise
       display('error in constellation modulator give proper rate_id')
end

k=log2(M); % no. of bits per symbol
x=data_in;
% bits to symbol mapping
xnew1=reshape(x,k,length(x)/k);
xsym = bi2de(xnew1.','left-msb');
% modulating % the symbole signal xsym should be
%  a column vector containing integers between 0 to M-1
switch (rate_id)
    case {0,1,2}
       y = pskmod(xsym,M);
    case {3,4,5,6}
       y = qammod(xsym,M);
    otherwise
       display('error in modulation give proper rate_id')
end
% scatterplot(y)
%% Transmitted Signal
data_out = y;


%==============================================================================
% Copyright (C) 2005, Jan Modersitzki and Nils Papenberg, see copyright.m,
% this file is part of the FLIRT Package, all rights reserved;
% http://www.math.uni-luebeck.de/SAFIR/FLIRT-MATLAB.html
%==============================================================================
% function [B,Bstr] = getElasticMatrixStg(Omega,m,mu,lambda);
% generates the elastic matrix B for the domain Omega with resolution m,
% by default, mu = 1, lambda = 0;
%==============================================================================
function [B,Bstr] = getElasticMatrixStg(Omega,m,mu,lambda)

Bstr = 'elastic-stg';
if ~exist('mu','var'),     mu     = 1;  end;
if ~exist('lambda','var'), lambda = 0;  end;
h = Omega./m;

dim = length(Omega);

switch dim
  case 2
    d11 = spdiags(ones(m(1),1)*[-1,1],[0,1],m(1),m(1)+1)/h(1);
    d12 = spdiags(ones(m(2)-1,1)*[-1,1],[0,1],m(2)-1,m(2))/h(2);
    d21 = spdiags(ones(m(1)-1,1)*[-1,1],[0,1],m(1)-1,m(1))/h(1);
    d22 = spdiags(ones(m(2),1)*[-1,1],[0,1],m(2),m(2)+1)/h(2);

    D11 = sparse(kron(speye(m(2)),d11));
    D12 = sparse(kron(d12,speye(m(1)+1)));
    D21 = sparse(kron(speye(m(2)+1),d21));
    D22 = sparse(kron(d22,speye(m(1))));

    % build the elastic operator
    %
    %      | \nabla 0      |
    %  B = |               |
    %      | 0      \nabla |
    %      |               |
    %      | Div1   Div2   |
    % B[U] = [\partial_1 U1,\partial_2 U1, \partial_1 U2,\partial_2 U2,div(U)]

    a  = sqrt(mu);
    b  = sqrt(mu+lambda);
    n1 = (m(1)+1)*m(2);
    n2 = m(1)*(m(2)+1);
    j1 = size(D11,1);
    j2 = size(D12,1);
    j3 = size(D21,1);
    j4 = size(D22,1);

    B = [  a*D11,sparse(j1,n2);
      a*D12,sparse(j2,n2);
      sparse(j3,n1),a*D21;
      sparse(j4,n1),a*D22;
      b*D11,b*D22];
    return;

  otherwise
    error('nyi');
end;
%==============================================================================


function eigg_segmentation
% This code was written by Muhammet Balcilar , France,
% muhammetbalcilar@gmail.com, inspired from following reference
%
%  This Matlab code implements an edge-based active contour model as an
%  application of the Distance Regularized Level Set Evolution (DRLSE) formulation in Li et al's paper:
%
%      C. Li, C. Xu, C. Gui, M. D. Fox, "Distance Regularized Level Set Evolution and Its Application to Image Segmentation",
%        IEEE Trans. Image Processing, vol. 19 (12), pp.3243-3254, 2010.

close all

%% step1, read grayscale image
Img=imread('Inputs/eigg_scotland.jpg');
Img=rgb2gray(Img);
frm=0;
%% step2, set params
timestep=5;  % time step
mu=0.2;  % coefficient of the distance regularization term R(phi)
lambda=5; %coefficient of the weighted length term L(phi)
alfa= -3;  %  coefficient of the weighted area term A(phi)
epsilon=1.5; % papramater that specifies the width of the DiracDelta function
c0=2;
maxiter=602;
sigma=2.0;    % scale parameter in Gaussian kernel

%% step3 smooth image with gaussian filter
G=fspecial('gaussian',30,sigma); % 15 Caussian kernel
Img_smooth=conv2(Img,G,'same');  % smooth image by Gaussiin convolution
figure(1);
imagesc(Img_smooth,[0, 255]); axis off; axis equal; colormap(gray);
title('Smoothed image');

%% step4 calculate edge indicator according to Eq23
[Ix,Iy]=gradient(Img_smooth);
f=Ix.^2+Iy.^2;
g=1./(1+f);  % edge indicator function.
g=exp(-f);
figure(2);
imagesc(g); axis off; axis equal;
title('g,  edge indicator');

%% step5, set initial phi
phi = -c0*ones(size(Img));
phi(50:650,50:500)=c0;
%phi(350:370,200:220)=-c0;
figure(3);
imagesc(phi);
axis off; axis equal;colormap(jet);
title('initial phi matrix');

[vx, vy]=gradient(g);
figure(4);
subplot(1,2,1);imagesc(vx); title('x directioned gradient of g');
subplot(1,2,2);imagesc(vy); title('y directioned gradient of g');

for k=1:maxiter
    %% step6, check boundary conditions
    phi=NeumannBoundCond(phi);

    %% step 7 calculate differential of regularized term in Eq.30
    distRegTerm=distReg_p2(phi);

    %% step8 calculate differential of area term in Eq.30
    diracPhi=Dirac(phi,epsilon);
    areaTerm=diracPhi.*g;

    %% step9 calculate differential of length term in Eq.30
    [phi_x,phi_y]=gradient(phi);
    s=sqrt(phi_x.^2 + phi_y.^2);
    Nx=phi_x./(s+1e-10); % add a small positive number to avoid division by zero
    Ny=phi_y./(s+1e-10);
    edgeTerm=diracPhi.*(vx.*Nx+vy.*Ny) + diracPhi.*g.*div(Nx,Ny);

    %% step 10 update phi according to Eq.20
    phi=phi + timestep*(mu/timestep*distRegTerm + lambda*edgeTerm + alfa*areaTerm);

     %% show result in every 50 iteration
    if mod(k,50)==1
        frm=frm+1;
        close all
        h=figure(5);
        set(gcf,'color','w');
        subplot(1,2,1);
        II=Img;
        II(:,:,2)=Img;II(:,:,3)=Img;
        imshow(II); axis off; axis equal; hold on;
        q=contour(phi, [0,0], 'r');
        msg=['contour result , iteration number=' num2str(k)];
        title(msg);
        subplot(1,2,2);
        mesh(-phi,[-2 2]);
        hold on;  contour(phi, [0,0], 'r','LineWidth',2);

        view([180-30 -65-180]);
        msg=['phi result , iteration number=' num2str(k)];
        title(msg);
        pause(0.1)

        % gif video

        frame = getframe(h);
        im = frame2im(frame);
        [imind,cm] = rgb2ind(im,256);
        %Write to the GIF File
        if frm == 1
            imwrite(imind,cm,'Outputs/eigg.gif','gif', 'Loopcount',inf);
        else
            imwrite(imind,cm,'Outputs/eigg.gif','gif','WriteMode','append');
        end
    end

    %% step 11 if maxiter done then finish, else return step6
end
%% Step 12. show last iteration results
figure(6);
imagesc(Img,[0, 255]); axis off; axis equal; colormap(gray); hold on;  contour(phi, [0,0], 'r');
msg=['phi result , iteration number=' num2str(k)];
title(msg);

function f = distReg_p2(phi)
% compute the distance regularization term with the double-well potential p2 in eqaution (16)
[phi_x,phi_y]=gradient(phi);
s=sqrt(phi_x.^2 + phi_y.^2);
a=(s>=0) & (s<=1);
b=(s>1);
ps=a.*sin(2*pi*s)/(2*pi)+b.*(s-1);  % compute first order derivative of the double-well potential p2 in eqaution (16)
dps=((ps~=0).*ps+(ps==0))./((s~=0).*s+(s==0));  % compute d_p(s)=p'(s)/s in equation (10). As s-->0, we have d_p(s)-->1 according to equation (18)
f = div(dps.*phi_x - phi_x, dps.*phi_y - phi_y) + 4*del2(phi);

function f = div(nx,ny)
[nxx,junk]=gradient(nx);
[junk,nyy]=gradient(ny);
f=nxx+nyy;

function f = Dirac(x, sigma)
f=(1/2/sigma)*(1+cos(pi*x/sigma));
b = (x<=sigma) & (x>=-sigma);
f = f.*b;

function g = NeumannBoundCond(f)
% Make a function satisfy Neumann boundary condition
[nrow,ncol] = size(f);
g = f;
g([1 nrow],[1 ncol]) = g([3 nrow-2],[3 ncol-2]);
g([1 nrow],2:end-1) = g([3 nrow-2],2:end-1);
g(2:end-1,[1 ncol]) = g(2:end-1,[3 ncol-2]);


function x=modcent(x,r);
%MODCENT  Centered modulo
%   Usage:  y=modcent(x,r);
%
%   `modcent(x,r)` computes the modulo of *x* in the range $[-r/2,r/2[$.
%
%   As an example, to compute the modulo of *x* in the range $[-\pi,\pi[$ use
%   the call::
%
%     y = modcent(x,2*pi);

x=mod(x,r);
idx=x>r/2;
x(idx)=x(idx)-r;


function sin_power_int_test ( )

%*****************************************************************************80
%
%% SIN_POWER_INT_TEST demonstrates the use of SIN_POWER_INT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIN_POWER_INT_TEST\n' );
  fprintf ( 1, '  SIN_POWER_INT returns values of\n' );
  fprintf ( 1, '  the integral of SIN(X)^N from A to B.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A         B          N      Exact           Computed\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, b, n, fx ] = sin_power_int_values ( n_data );

    if ( n_data == 0 )
      break;
    end

    fx2 = sin_power_int ( a, b, n );

    fprintf ( 1, '  %8f  %8f  %6d  %14e  %14e\n', a, b, n, fx, fx2 );

  end

  return
end


function [ n_data_new, n, m, x, fx ] = legendre_associated_values ( n_data )

%*****************************************************************************80
%
%% LEGENDRE_ASSOCIATED_VALUES returns values of associated Legendre functions.
%
%  Discussion:
%
%    The function considered is the associated Legendre function P^M_N(X).
%
%    In Mathematica, the function
%
%      LegendreP [ n, m, x ]
%
%    returns the value of the associated Legendre function P^M_N(X).
%
%  Differential equation:
%
%    (1-X*X) * Y'' - 2 * X * Y + ( N (N+1) - (M*M/(1-X*X)) * Y = 0
%
%  First terms:
%
%    M = 0  ( = Legendre polynomials of first kind P(N)(X) )
%
%    P00 =    1
%    P10 =    1 X
%    P20 = (  3 X^2 -   1)/2
%    P30 = (  5 X^3 -   3 X)/2
%    P40 = ( 35 X^4 -  30 X^2 +   3)/8
%    P50 = ( 63 X^5 -  70 X^3 +  15 X)/8
%    P60 = (231 X^6 - 315 X^4 + 105 X^2 -  5)/16
%    P70 = (429 X^7 - 693 X^5 + 315 X^3 - 35 X)/16
%
%    M = 1
%
%    P01 =   0
%    P11 =   1 * SQRT(1-X*X)
%    P21 =   3 * SQRT(1-X*X) * X
%    P31 = 1.5 * SQRT(1-X*X) * (5*X*X-1)
%    P41 = 2.5 * SQRT(1-X*X) * (7*X*X*X-3*X)
%
%    M = 2
%
%    P02 =   0
%    P12 =   0
%    P22 =   3 * (1-X*X)
%    P32 =  15 * (1-X*X) * X
%    P42 = 7.5 * (1-X*X) * (7*X*X-1)
%
%    M = 3
%
%    P03 =   0
%    P13 =   0
%    P23 =   0
%    P33 =  15 * (1-X*X)^1.5
%    P43 = 105 * (1-X*X)^1.5 * X
%
%    M = 4
%
%    P04 =   0
%    P14 =   0
%    P24 =   0
%    P34 =   0
%    P44 = 105 * (1-X*X)^2
%
%  Recursion:
%
%    if N < M:
%      P(N,M) = 0
%    if N = M:
%      P(N,M) = (2*M-1)!! * (1-X*X)^(M/2) where N!! means the product of
%      all the odd integers less than or equal to N.
%    if N = M+1:
%      P(N,M) = X*(2*M+1)*P(M,M)
%    if M+1 < N:
%      P(N,M) = ( X*(2*N-1)*P(N-1,M) - (N+M-1)*P(N-2,M) )/(N-M)
%
%  Restrictions:
%
%    -1 <= X <= 1
%     0 <= M <= N
%
%  Special values:
%
%    P(N,0)(X) = P(N)(X), that is, for M=0, the associated Legendre
%    function of the first kind equals the Legendre polynomial of the
%    first kind.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%  Parameters:
%
%    Input, integer N_DATA, indicates the index of the previous test data
%    returned, or is 0 if this is the first call.  For repeated calls,
%    set the input value of N_DATA to the output value of N_DATA_NEW
%    from the previous call.
%
%    Output, integer N_DATA_NEW, the index of the test data.
%
%    Output, integer N, integer M, real X, the arguments of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 19;
  n_vec = [ ...
     1,  1,  1,  1, ...
     1,  2,  2,  2, ...
     3,  3,  3,  3, ...
     4,  5,  6,  7, ...
     8,  9, 10 ];
  m_vec = [ ...
     0,  0,  0,  0, ...
     1,  0,  1,  2, ...
     0,  1,  2,  3, ...
     2,  2,  3,  3, ...
     4,  4,  5 ];
  fx_vec = [ ...
     0.000000E+00,  0.500000E+00,  0.707107E+00,  1.000000E+00, ...
    -0.866025E+00, -0.125000E+00, -1.29904E+00,   2.25000E+00, ...
    -0.437500E+00, -0.324759E+00,  5.62500E+00,  -9.74278E+00, ...
     4.21875E+00,  -4.92187E+00,   12.7874E+00,   116.685E+00, ...
    -1050.67E+00,  -2078.49E+00,   30086.2E+00 ];
  x_vec = [ ...
    0.0E+00,       0.5E+00,       0.7071067E+00, 1.0E+00, ...
    0.5E+00,       0.5E+00,       0.5E+00,       0.5E+00, ...
    0.5E+00,       0.5E+00,       0.5E+00,       0.5E+00, ...
    0.5E+00,       0.5E+00,       0.5E+00,       0.5E+00, ...
    0.5E+00,       0.5E+00,       0.5E+00 ];

  n_data_new = n_data;

  if ( n_data_new < 0 )
    n_data_new = 0;
  end

  n_data_new = n_data_new + 1;

  if ( n_max < n_data_new )
    n_data_new = 0;
    n = 0;
    m = 0;
    x = 0.0E+00;
    fx = 0.0E+00;
  else
    n = n_vec(n_data_new);
    m = m_vec(n_data_new);
    x = x_vec(n_data_new);
    fx = fx_vec(n_data_new);
  end

  return
end


function [xi,w]=secondOrderTriangleCubPoints()
%%SECONDORDERTRIANGLECUBPOINTS Obtain second-order cubature points for
%   integration over a triangle in 2D. The points and weights are for the
%   triangle with vertices (1,0), (0,1), (0,0), but can be transformed to
%   any triangle using transformSimplexTriPoints.
%
%INPUTS: None
%
%OUTPUTS: xi A 2XnumCubPoints set of points for the standard triangle.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the triangle (1/2).
%
%This function implements the points given in [1] (3 points).
%
%EXAMPLE:
%Given the vertices of the simplex, we compare a second-order moment
%computed using these cubature points to one computed using
%monomialIntSimplex. The results are the same within typical finite
%precision limits.
% [xi,w]=secondOrderTriangleCubPoints();
% alpha=[1;1];
% theMoment=findMomentFromSamp(alpha,xi,w)
% intVal=monomialIntSimplex(alpha)
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[-0.66666666666666666666666666666666666667,   0.33333333333333333333333333333333333333,   0.66666666666666666666666666666666666667;
    0.33333333333333333333333333333333333333,  -0.66666666666666666666666666666666666667,   0.66666666666666666666666666666666666667;
   -0.66666666666666666666666666666666666667,  -0.66666666666666666666666666666666666667,   0.66666666666666666666666666666666666667];

w=M(:,3);
xi=M(:,1:2)';
%Transform the points to the standard triangle.
v1=[-1,-1, 1;
    -1, 1,-1];
v2=[1,0,0;
    0,1,0];
[A,d]=affineTransBetweenTriangles(v1,v2);
xi=bsxfun(@plus,A*xi,d);
w=w/4;
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverse kinematics for the 3dof planar robot
% T: homogeneous matrix
% robot: structure with arm parameters
% returns: all possible solutions or q = [q1 q2] that place the end effectors at the
% position specified by T. Two possible solutions q1 and q2 q3 and q4 are returned,
% generally called elbow up and elbow down, combined with
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 08/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_3dofplanar(robot, T)

fprintf('\nComputing inverse kinematics for the %s robot', robot.name);

%Initialize q
% q = [q1 q2 q3 q4], atends for two possible solutions
q=zeros(3,4);

a = eval(robot.DH.a);

%Link lengths
L1=abs(a(1));
L2=abs(a(2));
L3=abs(a(2));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Q=T(1:3,4);

%find angle Phi
x3 = T(1:3,1);

cphi = x3'*[1 0 0]';
sphi = x3'*[0 1 0]';
phi = atan2(sphi, cphi);

%Find point P from P and vector (nx, ny, nz)
P = Q - a(3)*T(1:3,1);

%Distance of the point to the origin.
R= sqrt(P(1)^2+P(2)^2);

if R > (L1+L2)
   disp('\ninversekinematic_3dofplanar: unfeasible solution. The point cannot be reached');
end

%compute geometric solution
beta = atan2(P(2),P(1));
gamma = real(acos((L1^2+R^2-L2^2)/(2*R*L1)));
delta = real(acos((L1^2+L2^2-R^2)/(2*L1*L2)));

%arrange possible combinations for q(1) and q(2)
%elbow down     elbow up solutions
q =[beta+gamma beta-gamma;
    delta-pi   pi-delta];

%in this case, phi = q(1) + q(2) + q(3) and
%q(3) can be computed as q(3) = phi - q(1) - q(2)
%corresponding to each of the previous solutions, a unique q(3) can be
%computed for each case
for i=1:2 %iterate through columns
    q(3,i) = phi - q(1,i) - q(2,i);
end