classdef VonMisesD
%%VONMISESD Functions to handle the von Mises distribution, which is a
%    common circular distribution. The distribution is given in Chapter
%    3.5.4 of [1].
%Implemented methods are: circVar, PDF, CDF,trigMoment, normProdDist,
%                         convDistApprox, params4TrigMoment, rand, entropy
%
%REFERENCES:
%[1] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester: John
%    Wiley and Sons, 2000.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)

function val=circVar(kappa)
%%CIRCVAR Obtain the circular variance of a particular von Mises
%         distribution.
%
%INPUTS: kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: val The circular variance of the distribution.
%
%The circular variance is defined as 1-abs(r), where r is the mean
%resultant value of the second moment. The mean resultant values for all
%moment of the von Mises distribution are given in Chapter 2.2.4 of [1].
%
%EXAMPLE:
%Here, we verify that the result agrees with the value obtained by
%sampling.
% numRuns=1e5;
% kappa=100;
% mu=0.25;
% x=VonMisesD.rand([numRuns,1],mu,kappa);
% circVarEst=findCircVar(x);
% abs(VonMisesD.circVar(kappa)-circVarEst)./abs(VonMisesD.circVar(kappa))
%One will see that the relative error is on the order of 1e-3.
%
%REFERENCES:
%[1] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

val=1-BesseliRatio(1,kappa);

end

function val=PDF(x,mu,kappa)
%%PDF Evaluate a von Mises PDF at a given point with the specified
%     parameters.
%
%INPUTS: x The point(s) at which the von Mises distribution is to be
%          evaluated.
%       mu The mean direction of the distribution. This is normally between
%          -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: val The value(s) of the von Mises PDF with the specified
%             parameters evaluated at x.
%
%The von Mises distribution is used to model errors on angular quantities.
%It is described in [1] and [2].
%
%REFERENCES:
%[1] N. Fisher, Statistical Analysis of Circular Data. Cambridge, UK:
%    Cambridge University Press, 1993.
%[2] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester,
%    England: John Wiley & Sons, 2000.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=exp(kappa*cos(x-mu))/(2*pi*besseli(0,kappa));
end

function vals=CDF(x,mu,kappa,startAng)
%%CDF Evaluate  the cumulative distribution function (CDF) of the von Mises
%     distribution. Since this is a circular distribution, the CDF has to
%     be defined in terms of a specific starting angle. Increading density
%     goes in the direction of increasing angle from the starting point
%     (around the circle).
%
%INPUTS: z The point(s) at which the CDF should be evaluated.
%       mu The mean direction of the distribution. This is normally between
%          -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf.
%  startAng The starting angle. This is where the CDF is zero. The default
%           if this parameter is omitted or an empty matrix is passed is
%           zero.
%
%OUTPUTS: vals The scalar value(s) of the von Mises CDF with the given PDF
%              evaluated at the points in z.
%
%The function Besseli0Inc gives the CDF of the von Mises distribution
%starting from -pi. Thus, by taking the difference of two evaluations of
%Besseli0Inc (and determining whether we are going the long or the short
%way around the circle) one can evaluate the CDF.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(nargin<4||isempty(startAng))
    startAng=-pi;
end

vals=Besseli0Inc(x-mu,kappa)-Besseli0Inc(startAng-mu,kappa);

sel=vals<0;
vals(sel)=vals(sel)+1;
end

function [rho,theta,R]=trigMoment(n,mu,kappa)
%%TRIGMOMENT Compute the nth trigonometric moment of a von Mises
%            distribution having the given parameters. A direction theta on
%            the unit circle can be modeled as a complex quantity having
%            unit magnitude as exp(1j*theta). The nth raw complex
%            trigonometric moment is the expected value of exp(1j*n*theta),
%            where the integral for the expected value is taken over any
%            interval of length 2*pi.
%
%INPUTS: n The order of the moment desired. This is >=1.
%       mu The mean direction of the distribution. This is normally between
%          -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf. For
%          kappa>500, the results might be inaccurate.
%
%OUTPUTS: rho The (complex) mean resultant value for the nth moment. Note
%             that rho=R*exp(1j*theta).
%       theta The (real) trigonometric mean angle in radians for the nth
%             moment. This is between -pi and pi.
%           R The (real) mean resultant length for the nth moment.
%
%The moments of a von Mises distribution are given in Chapter 2.2.4 of [1]
%and Chapter 3.5.4 of [2].
%
%EXAMPLE:
%Here, we verify the first trigonometric moment based on samples.
% numRuns=1e5;
% kappa=100;
% mu=0.25;
% x=VonMisesD.rand([numRuns,1],mu,kappa);
% [rhoS,thetaS,RS]=findTrigMomentFromSamp(1,x);
% [rho,theta,R]=VonMisesD.trigMoment(1,mu,kappa);
% abs(rhoS-rho)
% abs(thetaS-theta)
% abs(RS-R)
%Here, we will see that the errors tend to be on the order of 1e-4,
%dominated by the angular error.
%
%REFERENCES:
%[1] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%[2] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester: John
%    Wiley and Sons, 2000.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

%For n=1 or 2, there should be no need to use anything but the default
%maximum number of iterations in BesseliRatio.
if(n==1)
    rho=BesseliRatio(1,kappa)*exp(1j*n*mu);
elseif(n==2)
    rho=BesseliRatio(2,kappa)*BesseliRatio(1,kappa)*exp(1j*n*mu);
else
    rho=besseli(n,kappa,1)/besseli(0,kappa,1)*exp(1j*n*mu);
end

if(nargout>1)
    theta=angle(rho);
    R=abs(rho);
end
end

function [mu,kappa]=normProdDist(mu1,kappa1,mu2,kappa2)
%%NORMPRODDIST The product of two von Mises distributions is an
%              unnormalized von Mises distribution. This finds the
%              parameters of the product distribution.
%
%INPUTS: mu1, kappa1 The mean direction and concentration of the first
%                    distribution. Note that 0<=kappa<inf.
%        mu2, kappa2 The mean direction and concentration of the second
%                    distribution.
%
%OUTPUTS: mu,kappa The mean direction and concentration of the product
%                  distribution -pi<=mu<pi.
%
%The product of two von Mises distributions is described in [1].
%
%REFERENCES:
%[1] M. Azmani, S. Reboul, J.-B. Choquel, and M. Benkelloun, "A recursive
%    fusion filter for angular data," in Proceedings of the 2009 IEEE
%    International Conference on Robotics and Biomimetics, Guilin, China,
%    19-23 Dec. 2009, pp. 882-887.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

C=kappa1*cos(mu1)+kappa2*cos(mu2);
S=kappa1*sin(mu1)+kappa2*sin(mu2);
mu=atan2(S,C);
kappa=sqrt(C^2+S^2);
end

function [mu,kappa]=convDistApprox(mu1,kappa1,mu2,kappa2)
%%CONVDISTAPPROX The convolution of two von Mises distributions is not a
%          von Mises distribution. However, it can be approximated by a von
%          Mises distribution that matches the moments, as is done in
%          Equation 7 of [1]. This function returns the paramters of the
%          approximation.
%
%INPUTS: mu1, kappa1 The mean direction and concentration of the first
%                    distribution. Note that 0<=kappa<inf.
%        mu2, kappa2 The mean direction and concentration of the second
%                    distribution.
%
%OUTPUTS: mu, kappa The mean direction and concentration of a von Mises
%                   approximation to the product distribution. -pi<=mu<pi.
%
%REFERENCES:
%[1] I. Markovic and I. Petrovic, "Bearings-only tracking with a mixture of
%    von Mises distributions," in IEEE/RSJ International Conference on
%    Intelligent Robots and Systems, Vilamoura, Algarve, Portugal, 7-12
%    Oct. 2012, pp. 707-712.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    mu=wrapRange(mu1+mu2,-pi,pi);

    AkProd=BesseliRatio(1,kappa1)*BesseliRatio(1,kappa2);
    kappa=BesseliRatioInv(1,AkProd);
end

function [mu,kappa]=params4TrigMoment(rho)
%%PARAMS4TRIGMOMENT Obtain the parameters (mu and kappa) of the von Mises
%           distribution such that the von Mises distribution matches the
%           given complex mean resultant length for the first moment.
%
%INPUTS: rho The (complex) mean resultant value for the first moment. Note
%            that abs(rho)<=1.
%
%OUTPUTS: mu, kappa The mean direction and the concentration parameter of
%                   the von Mises distribution with the specified mean
%                   resultant value.
%
%The moments of a von Mises distribution are given in Chapter 2.2.4 of [1].
%This function just implements an inverse.
%
%REFERENCES:
%[1] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

    mu=atan2(imag(rho),real(rho));
    kappa= BesseliRatioInv(1,abs(rho));
end

function vals=rand(N,mu,kappa)
%%RAND Generate von Mises random variables with the given parameters.
%
%INPUTS: N If N is a scalar, then rand returns an NXN matrix of random
%          variables. If N=[M,N1] is a two-element row vector, then rand
%          returns an MXN1 matrix of random variables.
%       mu The mean direction of the distribution. This is normally
%          between -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: vals A matrix whose dimensions are determined by N of the
%              generated von Mises random variables.
%
%The algorithm if that of Section 4 of [1], which is a rejection sampling
%method based on using a wrapped Cauchy distribution as a proposal density.
%However, the range of the random variable u1 in [1] does not appear to be
%correct. The correct formulation is based on Chapter 7.3 of [2] using the
%errata available at http://luc.devroye.org/errors.pdf so u1 is randomly
%generated between -1 and 1 as opposed to between 0 and 1.
%
%REFERENCES:
%[1] D. J. Best and N. I. Fisher, "Efficient simulation of the von Mises
%    distribution," Journal of the Royal Statistical Society. Seriec C
%    (Applied Statistics), vol. 28, no. 2, pp. 152-157, 1979.
%[2] L. Devroye, Non-Uniform random Variate Generation. New York:
%    Springer-Verlag, 1986.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(isscalar(N))
        dims=[N N];
    else
        dims=N;
    end

    vals=zeros(dims);%Allocate space
    numSamp=prod(dims);

    %Step 0
    tau=1+sqrt(1+4*kappa^2);
    rho=(tau-sqrt(2*tau))/(2*kappa);
    r=(1+rho^2)/(2*rho);

    for curSamp=1:numSamp
        while(1)
            %Step 1
            u1=2*rand(1)-1;%In range [-1,1]
            z=cos(pi*u1);
            f=(1+r*z)/(r+z);
            c=kappa*(r-f);

            %Step 2
            u2=rand(1);%in range [0,1]
            if(c*(2-c)-u2>0)
               break;
            end

            %Step 3
            if(log(c/u2)+1-c>=0)
                break;
            end
        end
        %Step 4
        u3=2*rand(1)-1;%In range [-1,1]
        vals(curSamp)=wrapRange(sign(u3)*acos(f)+mu,-pi,pi);
    end
end

function entropyVal=entropy(kappa)
%%ENTROPY Obtain the differential entropy of the von Mises distribution
%         given in nats. The differential entropy of a continuous
%         distribution is entropy=-int_x p(x)*log(p(x)) dx where the
%         integral is over all values of x. Units of nats mean that the
%         natural logarithm is used in the definition. Unlike the Shannon
%         entropy for discrete variables, the differential entropy of
%         continuous variables can be both positive and negative.
%
%INPUTS: kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: entropyVal The value of the differential entropy in nats.
%
%Differential entropy is defined in Chapter 8 of [1].
%
%If kappa is very large, besseli(0,kappa) will overflow and a warning will
%be issued. In such an instance, an approximation based on the first term
%of the series on page 289 of [2] is used. The approximation becomes
%increasingly bad as kappa becomes larger.
%
%REFERENCES:
%[1] T. M. Cover and J. A. Thomas, Elements of Information Theory, 2nd ed.
%    Hoboken, NJ: Wiley-Interscience, 2006.
%[2] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    besseliVal=besseli(0,kappa);

    if(isfinite(besseliVal))
        entropyVal=-kappa*BesseliRatio(1,kappa)+log(2*pi*besseliVal);
    else
        %Use the approximation exp(kappa)/sqrt(2*pi*kappa) for values of
        %kappa so large that besseli overflows
        warning('besseli(0,kappa) overflowed; using an approximation.')
        entropyVal=-kappa*BesseliRatio(1,kappa)+(1/2)*(4000+log(pi/1000));
    end
end

end
end

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


function [ x, w ] = fejer1_rule_compute ( n )

%*****************************************************************************80
%
%% FEJER1_RULE_COMPUTE computes a Fejer Type 1 rule.
%
%  Modified:
%
%    06 March 2007
%
%  Author:
%
%    Joerg Waldvogel
%
%  Reference:
%
%    Joerg Waldvogel,
%    Fast Construction of the Fejer and Clenshaw-Curtis Quadrature Rules,
%    BIT Numerical Mathematics
%    Volume 43, Number 1, pages 1-18, 2003.
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), W(N), the abscissas and weights of the rule.
%
  N = [ 1 : 2 : n-1 ]';
  L = length ( N );
  m = n - L;
  K = [ 0 : m-1 ]';

  s = zeros ( L+1, 1 );
  v0 = [ 2 * exp(i*pi*K/n) ./ ( 1 - 4 * K.^2 ); s ];

  v1 = v0(1:end-1) + conj ( v0(end:-1:2) );
  w = ifft ( v1 );

  x = cos ( pi * ( (n:-1:1)' - 1 / 2 ) / n );

  return
end


function [p,s]=permutes(n)
%PERMUTES All N! permutations of 1:N + signatures [P,S]=(N)
% The output P is a matrix of size (N!,N) where each row
% contains a permutation of the numbers 1:N. The rows are in
% lexically sorted order.
%
% To permute the elements of an arbitrary vector V use
% V(PERMUTES(LENGTH(V))).

% PERMUTES(N) is the same as SORTROWS(PERMS(1:N)) but much faster.

% Thanks to Peter J Acklam for several improvements.

%      Copyright (c) 1998 Mike Brookes,  mike.brookes@ic.ac.uk
%      Version: $Id: permutes.m 713 2011-10-16 14:45:43Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p=1;
m=1;
if n>1
  for a=2:n
    q=zeros(a*m,a);
    r=2:a+1;
       ix=1:m;
    for b=1:a
       q(ix,1)=b;
      q(ix,2:a)=r(p);
      r(b)=b;
      ix=ix+m;
    end
    m=m*a;
    p=q;
  end
end
if nargout>1 s=1-2*rem(fix((1:m)'/2),2); end


function [tri, x] = delaunay_sphere(N, xi)
% DELAUNAY_SPHERE  Triangulation of a sphere or a surface topologically
% equivalent to a sphere so that it can be used as a mesh
%
% [TRI, X] = delaunay_sphere(N)
%
%   Number of samples of the sphere (in latitude and longitude).
%
%   TRI is a 3-column matrix with a triangulation of the sphere. Each
%   element in TRI is an index to a row in X. Each row represents the three
%   vertices of a triangle on the sphere.
%
%   X is a 3-column matrix with the Euclidean coordinates of the
%   triangulation vertices.
%
%   The surface can be plotted running
%
%     trisurf(tri, x(:, 1), x(:, 2), x(:, 3));
%
% ... = delaunay_sphere(N, XI)
%
%   XI is an (N+1,N+1,3)-array with the coordinates of the vertices of a
%   surface topologically equivalent to a sphere. By default, XI is the
%   concatenation of the output of sphere(N), and the surface is a unit
%   sphere.
%
%   The surface can be plotted running
%
%     surf(xi(:, :, 1), xi(:, :, 2), xi(:, :, 3))
%
% See also: scimat_tri_to_raster.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 2);
nargoutchk(0, 2);

if (nargin > 1 ...
        && (size(xi, 1) ~= N+1 || size(xi, 2) ~= N+1 || size(xi, 3) ~= 3))
    error('XI must be a (N+1, N+1, 3) array')
end

% sphere surface
[x, y, z] = sphere(N);

% remove repeated points
[x, idx] = unique([x(:) y(:) z(:)], 'rows');

% triangulate the sphere and extract the surface
tri = DelaunayTri(x);
tri = freeBoundary(tri);

% if an input point configuration was provided, we use it to replace the
% unit sphere points coordinates
if (nargin > 1)
    xi = reshape(xi, (N+1)*(N+1), 3);
    x = xi(idx, :);
end


function [CL, u_U, v_U, w_U, l_t, l_s] = LiftCoeff(gamma, panel, geo, Fu_bar, Fv_bar, Fw_bar)
%Determine the lift coefficient given the vortex strengths
%and the influence coefficients from the vortex lattice method

u_U.c = 1/(4*pi)*Fu_bar*[gamma.c]';  %Eqn 25 in NASA paper
v_U.c = 1/(4*pi)*Fv_bar*[gamma.c]';    %Eqn 23 in NASA paper
w_U.c = 1/(4*pi)*Fw_bar*[gamma.c]';

u_U.alpha = 1/(4*pi)*Fu_bar*[gamma.alpha]';
v_U.alpha = 1/(4*pi)*Fv_bar*[gamma.alpha]';
w_U.alpha = 1/(4*pi)*Fw_bar*[gamma.alpha]';

for i = 1:geo.ns
    for j = 1:geo.nc
        if i == geo.ns  %wingtip
            DeltaGamma(i,j).c = gamma(i,j).c;
            DeltaGamma(i,j).alpha = gamma(i,j).alpha;
        elseif j == 1 %Leading edge
            DeltaGamma(i,j).c = gamma(i,j).c-gamma(i+1,j).c;
            DeltaGamma(i,j).alpha = gamma(i,j).alpha-gamma(i+1,j).alpha;
        else
            DeltaGamma(i,j).c = DeltaGamma(i,j-1).c + gamma(i,j).c - gamma(i+1,j).c;
            DeltaGamma(i,j).alpha = DeltaGamma(i,j-1).alpha + gamma(i,j).alpha - gamma(i+1,j).alpha;
        end
    end
end

l_t.c = 2/geo.S*[DeltaGamma.c]'.*[panel.cc]'.*[v_U.c];  %Eqn 24 in NASA paper
l_t.alpha = 2/geo.S*[DeltaGamma.alpha]'.*[panel.cc]'.*[v_U.alpha];  %Eqn 24 in NASA paper

l_s.c = 2/geo.S*[gamma.c]'*2.*[panel.s]'.*((ones(size(u_U))-[u_U.c])+[v_U.c].*tan([panel.sweep]'))*cos(geo.dih);  %Eqn 28 in NASA paper
l_s.alpha = 2/geo.S*[gamma.alpha]'*2.*[panel.s]'.*((ones(size(u_U))-[u_U.alpha])+[v_U.alpha].*tan([panel.sweep]'))*cos(geo.dih);  %Eqn 28 in NASA paper

% reshape([l_t.c],geo.ns,geo.nc)
% reshape([l_t.alpha],geo.ns,geo.nc)
% reshape([l_s.c],geo.ns,geo.nc)
% reshape(((ones(size(u_U))-[u_U.alpha])+[v_U.alpha].*tan([panel.sweep]')),geo.ns,geo.nc)
% reshape([panel.s],geo.ns,geo.nc)
% reshape([l_s.alpha],geo.ns,geo.nc)
% reshape([u_U.alpha],geo.ns,geo.nc)
% reshape([v_U.alpha],geo.ns,geo.nc)
% reshape([gamma.alpha],geo.ns,geo.nc)
% sum([gamma.c])
% geo.S

CL.c = 2*(sum([l_t.c])+sum([l_s.c]));
CL.alpha = 2*(sum([l_t.alpha])+sum([l_s.alpha]));


classdef grassmannian < handle
    % Ways to generate Grassmannian frames.
    properties
    end

    methods(Static)

        function result = minimum_coherence(m, n)
            % Returns the minimum coherence of an m times n matrix
            num = n - m;
            den = m * (n - 1);
            result = sqrt(num / den);
        end

        function n = n_upper_bound(m)
            % for a given value of m, returns the upper bound on n.
            a = m * (m  + 1) / 2;
            n = m  * (m + 1) / 2 - 1;
            lower_limit = m;
            while true
                b = (n - m) * (n - m + 1) / 2;
                upper_limit = min (a, b);
                if n < upper_limit
                    break;
                end
                n = n- 1;
            end
        end
        function [ns, coherences] = min_coherence_max_n(ms)
            if ~isvector(ms)
                error('ms must be a vector');
            end
            mm = length(ms);
            ns = zeros(1, mm);
            coherences = zeros(1, mm);
            for i=1:mm
                m = ms(i);
                n = spx.dict.grassmannian.n_upper_bound(m);
                ns(i) = n;
                coherences(i) = spx.dict.grassmannian.minimum_coherence(m, n);
            end
        end

        function n = max_n_for_coherence(m, mu)
            % estimates a value of n for m x n matrix which achieves a
            % given coherence
            num = (mu^2 -1) * m;
            den = mu^2 * m - 1;
            n = floor(num / den);
            if n < 0
                error('A Grassmannian frame for this dimension will never have so high coherence.');
            end
        end

        function result = alternate_projections(dict, options)
            % Converts a given dictionary to Grassmannian frame via
            % alternate projections
            [n, d] = size(dict);
            % We assume that the dictionary contains unit norm atoms
            target_mu  = spx.dict.grassmannian.minimum_coherence(n, d);
            % Compute the Gram matrix
            gram_matrix = dict' * dict;
            iterations = 1000;
            entry_shrinkage_factor = 0.9;
            coherence_shrinkage_factor = 0.9;
            verbose = false;
            if nargin > 1 && isstruct(options)
                if isfield(options, 'iterations')
                    iterations = options.iterations;
                end
                if isfield(options, 'entry_shrinkage_factor')
                    entry_shrinkage_factor = options.entry_shrinkage_factor;
                end
                if isfield(options, 'coherence_shrinkage_factor')
                    coherence_shrinkage_factor = options.coherence_shrinkage_factor;
                end
                if isfield(options, 'verbose')
                    verbose = options.verbose;
                end
            end
            total_entries = d^2 - d; % excluding the main diagonal.
            % Identify the indices in the gram matrix which are off diagonal
            off_diagonal_indices = abs(gram_matrix(:) - 1)> 1e-6;
            coherence_array = zeros(iterations, 1);
            mean_coherence_array = zeros(iterations, 1);
            for k=1:iterations
                % Convert gram matrix to an array
                gram_array = gram_matrix(:);
                % Absolute Gram matrix
                abs_gram_array = abs(gram_array);
                % sort the inner products by their absolute values
                sorted_gram_array = sort(abs_gram_array);
                upper_threhold_index = round(entry_shrinkage_factor * total_entries);
                upper_threshold = sorted_gram_array(upper_threhold_index);
                % identify coherence values above the threshold
                above_indices = off_diagonal_indices & (abs_gram_array > upper_threshold);
                % map the boolean flags to positions.
                above_indices = find(above_indices);
                above_values = abs_gram_array(above_indices);
                current_mu = max(above_values);
                mean_mu = mean(above_values);
                coherence_array(k) = current_mu;
                mean_coherence_array(k) = mean_mu;
                if verbose
                    fprintf('%6d:, Coherence:: target: %12.8f, ', k, target_mu);
                    fprintf('current: %12.8f, threshold: %12.8f, above: %d, mean: %12.8f\n', ...
                        current_mu, upper_threshold, length(above_values), mean_mu);
                else
                    fprintf('.');
                    if mod(k, 100) == 0
                        fprintf('\n');
                    end
                end
                % Update off diagonal entries
                gram_matrix(above_indices)=gram_matrix(above_indices)*coherence_shrinkage_factor;

                % reduce the rank back to n
                [U,S,V]=svd(gram_matrix);
                % Ensure that all higher singular values are set to 0.
                S(n+1:end,n+1:end)=0;
                % Reconstruct the Gram matrix.
                gram_matrix=U*S*V';
                % Ensure that the diagonal elements of G continue to be 1.
                % Compute  d = diag(G)  d^{-1/2}.
                gram_diag_sqrt_inv = diag(1./sqrt(diag(gram_matrix)));
                % Compute G = d^{-1/2} G d^{-1/2}.
                gram_matrix = gram_diag_sqrt_inv*gram_matrix*gram_diag_sqrt_inv;
            end
            % final dictionary
            % perform SVD of the gram matrix.
            [U,S,V]=svd(gram_matrix);
            % Construct the dictionary
            dict=S(1:n,1:n).^0.5*U(:,1:n)';
            result.dictionary = dict;
            result.coherence_array = coherence_array;
            result.mean_coherence_array = mean_coherence_array;
            result.target_coherence = target_mu;
            result.iterations = iterations;
        end
    end
end


%% KEOLLOGG Problem
%
% KELLOGG solves a diffusion equation with jump coefficients with AFEM.
%
% KELLOGG solves the problem within maxN number of vertices. The
% input argument theta is a parameter used in the marking step.
%
% The KELLOGG command, if no input arguments, use maxN = 5e3 and theta = 0.5.
%
% EXAMPLE
%
%    Kellogg
%
% See also  crack, Lshape
%
% TODO: rewrite M-lint
%
% Created by Chen-Song Zhang. Modified by Long Chen.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

close all
%% Problem setting
% $$-\nabla\cdot(d\nabla u) = 0  \quad \Omega = (-1,1)\times (-1,1)$$
%
% $$u = g_D \quad \partial \Omega$$
%
% The diffusion constant is discontinous; see the figure below. We set a2  =
% 1; a1 = 161.4476387975881 and choose boundary condition g_D such that the
% exact solution is $z = r^{0.1}\mu(\theta)$ in the poloar coordinate, where
% the formula of mu can be found in exactu function.

[x,y] = meshgrid(-1:1:1,-1:1:1);
z = 0*x;
surf(x,y,z,'linewidth',2); view(2);
axis equal; axis tight;
text(0.5,0.5,'a1','FontSize',12,'FontWeight','bold');
text(-0.5,-0.5,'a1','FontSize',12,'FontWeight','bold');
text(-0.5,0.5,'a2','FontSize',12,'FontWeight','bold');
text(0.5,-0.5,'a2','FontSize',12,'FontWeight','bold');

%% Parameters
maxN = 3e3;     theta = 0.2;    maxIt = 1000;
N = zeros(maxIt,1);     uIuhErr = zeros(maxIt,1);
errH1 = zeros(maxIt,1);

%%  Generate an initial mesh
[node,elem] = squaremesh([-1 1 -1 1], 0.25);
bdFlag = setboundary(node,elem,'Dirichlet');

%% Set up PDE data
pde = Kelloggdata;

%%  Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*

for k = 1:maxIt
    % Step 1: SOLVE
    [u,Du,eqn] = Poisson(node,elem,pde);
    % Plot mesh and solution
    figure(1);  showresult(node,elem,u,[27,26]);
    % Step 2: ESTIMATE
   eta = estimaterecovery(node,elem,u);            % recovery type
%     eta = estimateresidual(node,elem,u,pde);    % residual type
    % Record error and number of vertices
    uI = pde.exactu(node);
    uIuhErr(k) = sqrt((uI-u)'*eqn.A*(uI-u));
    errH1(k) = getH1error(node,elem,@pde.Du,u);
    N(k) = size(node,1);
    if (N(k)>maxN), break; end
    % Step 3: MARK
    markedElem = mark(elem,eta,theta);
    % Step 4: REFINE
    [node,elem,bdFlag] = bisect(node,elem,markedElem,bdFlag);
end

%%  Plot convergent rates in energy norm
N = N(1:k);
uIuhErr = uIuhErr(1:k);
errH1 = errH1(1:k);
figure(2)
showrate2(N,uIuhErr,10,'-*','||Du_I-Du_h||',...
          N,errH1,10,'k-.','||Du-Du_h||');


function [L] = lap2DDir(N, h);
%
% [L] = lap2DDir(N, h)
%
%  Constructs the 2D Laplacian for a square mesh with zero Dirichlet
%     boundary conditions using the standard 5-point stencil.
%
%  Returns:
%     L = discrete Laplacian
%
%  Input:
%     N = number of mesh points in each direction
%     h = mesh width
%
%
%
%  License: This code is free to use for any purposes, provided
%           any publications resulting from the use of this code
%           reference the original code/author.
%
%  Author:  Samuel Isaacson (isaacson@math.utah.edu)
%  Date:    11/2007
%
%  Please notify the author of any bugs, and contribute any
%  modifications or bug fixes back to the original author.
%
%  Disclaimer:
%   This code is provided as is. The author takes no responsibility
%   for its results or effects.

M = N * N;

% 2D Laplace Operator on Square:
e              = ones(M,1);
ed             = -4*e;
eu1            = ones(M,1);
eu1((N+1):N:M) = 0;
ed1            = ones(M,1);
ed1(N:N:M)     = 0;

L = spdiags([e ed1 ed eu1 e], [-N -1:1 N], M, M);

L = L ./ (h*h);


function diff = getDiffuseness_SV(i_vecs)
%GETDIFFUSENESS_SV Compute the DoA spherical variance diffuseness
%
%   This routine estimates diffuseness based on the assumption that DoA
%   estimates should have a small variance around the source DoA, if the
%   direct-to-diffuse ratio is high, and be completely uniformly
%   distributed if the sound is fully diffuse. This variation can be
%   captured on the spherical variance of the DoA estimates. The method can
%   utilize intensity vectors, or any other vectors that indicate DoAs. The
%   statistics can be computed from multiple temporal observations or
%   across different frequency bands. This estimator has been used e.g. in
%
%       Politis, A., Delikaris-Manias, S. and Pulkki, V., 2015.
%       Direction-of-arrival and diffuseness estimation above spatial aliasing for symmetrical directional microphone arrays.
%       In 2015 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP).
%
%   Inputs:
%       i_vecs:  Kx3 matrix of DoA vectors, for K temporal or frequency
%           observations
%
%   Outputs:
%       diff:       diffuseness value
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% GETDIFFUSENESS_SV.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% unit DoA vectors
doa_vecs = i_vecs ./ (sqrt(sum(i_vecs.^2,2))*ones(1,3));
% mean DoA vector
mean_doa_vec = mean(doa_vecs,1);
% spherical variance
diff = 1 - sqrt(sum(mean_doa_vec.^2, 2));

end


% Author : FUAT COGUN
% Date   : 16.07.2009
%
%
% Find the Covariance Matrix of given region.
%
%
% Inputs  : -I                                                  (uint8 Matrix)
% ======    -x position                                         (Integer)
%           -y position                                         (Integer)
%           -size                                               (Integer)
%
% Outputs : -Covariance Matrix                                  (7x7 Matrix)
% =======

function CR = findCovarianceMatrix(I, positionX, positionY, size)

    % First and second derivatives
    d = [-1 0 1];
    dd = [-1 2 -1];
    dI = double(I);

    % Ix, Iy, Ixx, Iyy
    Ix = conv2(d, dI);
    Iy = conv2(d,1,dI);
    Ixx = conv2(dd, dI);
    Iyy = conv2(dd,1,dI);

    for j = 1:size
        for i = 1:size

            f(i+(j-1)*size,:) = [positionX+i-1 positionY+j-1 dI(positionY+j-1, positionX+i-1) ...
                         Ix(positionY+j-1, positionX+i-1) Iy(positionY+j-1, positionX+i-1) ...
                        Ixx(positionY+j-1, positionX+i-1) Iyy(positionY+j-1,positionX+i-1)];

        end
    end

    % vector of means of features in region R
    uR = mean(f);

    T = zeros(7);
    for k = 1:size^2
        temp = (f(k,:)-uR)'*(f(k,:)-uR);
        T = T + temp;
    end

    CR = (1/size^2)*T;


% DEMO  --  Chebyshev Polynomial Integral
% UPDATED  --  October 28, 2013
% Written by Matthew Kelly, Cornell University
%
% This demo is designed to show how to compute integrals of chebyshev
% polynomials.
%
% Results - Note that there are relative large errors between the chebyshev
% approximation of the integral and the analytic integral, BUT the last
% value (which is the value of the definite integral) is very accurate.
%
% The last point is so accurate because it is the solution to the definite
% integral over the domain using Clenshaw-Curtis quadrature for an order
% "order" accurate polynomial.
%

%% General Settings
clear; clc;

%What order should the approximation be?
order = 50;

%What domain should we be looking at?
d = [0,1];

%Get chebyshev values at the nodes:
[x,w] = chebyshevPoints(order+1,d);
[~,f] = testFunction(x);
If= chebyshevIntegral(f,d);

%Set the time vector
time = linspace(d(1),d(2),10000);
[Ig, g] = testFunction(time);

%Adjust the constants of integration S.T. they all start at zero:
Ig = Ig-Ig(1);

%Interpolate the chebyshev approximations:
y = chebyshevInterpolate(f,time,d);
Iy = chebyshevInterpolate(If,time,d);

%Show results
figure(407); clf;
subplot(2,2,1); hold on;
    plot(time,g,'b-','LineWidth',2)
    plot(x,f,'ko','MarkerSize',5);
    title(['function approximation - order ' num2str(order)]);
    legend('Analytic','Cheb. nodes','Location','NorthWest')
subplot(2,2,3); hold on;
    plot(time,Ig,'b-','LineWidth',2)
    plot(x,If,'ko','MarkerSize',5);
    title(['integral approximation - order ' num2str(order)]);
        legend('Analytic','Cheb. nodes','Location','NorthWest')
subplot(2,2,2);
    semilogy(time,abs(g-y))
    title('error in function');
subplot(2,2,4); hold on;
    semilogy(time,abs(Ig-Iy))
    semilogy(time(end),abs(Ig(end)-Iy(end)),...
        'bo','MarkerSize',10,'LineWidth',2)
    title(['error in definite integral: ' num2str(Ig(end)-Iy(end))]);
    set(gca,'yscale','log')


function [ pn, dist ] = circle_arc_point_near_2d ( r, center, theta1, ...
  theta2, p )

%*****************************************************************************80
%
%% CIRCLE_ARC_POINT_NEAR_2D : nearest point on a circular arc.
%
%  Discussion:
%
%    A circular arc is defined by the portion of a circle (R,C)
%    between two angles (THETA1,THETA2).
%
%    Thus, a point (X,Y) on a circular arc satisfies
%
%      ( X - C(1) ) * ( X - C(1) ) + ( Y - C(2) ) * ( Y - C(2) ) = R * R
%
%    and
%
%      Theta1 <= Theta <= Theta2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2,1), the center of the circle.
%
%    Input, real THETA1, THETA2, the angles defining the arc,
%    in radians.  Normally, THETA1 < THETA2.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, real PN(2,1), a point on the circular arc which is
%    nearest to the point.
%
%    Output, real DIST, the distance to the nearest point.
%
  dim_num = 2;
%
%  Special case, the zero circle.
%
  if ( r == 0.0 )
    pn(1:dim_num,1) = center(1:dim_num,1);
    dist = sqrt ( sum ( ( p(1:dim_num,1) - pn(1:dim_num,1) ).^2 ) );
    return
  end
%
%  Determine the angle made by the point.
%
  theta = atan4 ( p(2,1) - center(2,1), p(1,1) - center(1,1) );
%
%  If the angle is between THETA1 and THETA2, then you can
%  simply project the point onto the arc.
%
  if ( r8_modp ( theta  - theta1,  2.0 * pi ) <= ...
       r8_modp ( theta2 - theta1,  2.0 * pi ) )

    r2 = sqrt ( sum ( ( p(1:dim_num,1) - center(1:dim_num,1) ).^2 ) );

    pn(1:dim_num,1) = center(1:dim_num,1) ...
      + ( p(1:dim_num,1) - center(1:dim_num,1) ) * r / r2;
%
%  Otherwise, if the angle is less than the negative of the
%  average of THETA1 and THETA2, it's on the side of the arc
%  where the endpoint associated with THETA2 is closest.
%
  elseif ( r8_modp ( theta - 0.5 * ( theta1 + theta2 ), 2.0 * pi ) <= pi )

    pn(1:dim_num,1) = center(1:dim_num,1) + r * [ cos ( theta2 ), sin ( theta2 ) ];
%
%  Otherwise, the endpoint associated with THETA1 is closest.
  else

    pn(1:dim_num,1) = center(1:dim_num,1) + r * [ cos ( theta1 ), sin ( theta1 ) ];

  end

  dist = sqrt ( sum ( ( p(1:dim_num,1) - pn(1:dim_num,1) ).^2 ) );

  return
end


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];


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.


% 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, 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


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