function [amp,phi,yr]=HANTS(ni,nb,nf,y,ts,HiLo,low,high,fet,dod,delta)
% HANTS processing
%
% Wout Verhoef
% NLR, Remote Sensing Dept.
% June 1998
%
% Converted to MATLAB:
% Mohammad Abouali (2011)
%
% NOTE: This version is tested in MATLAB V2010b. In some older version you
% might get an error on line 117. Refer to the solution provided on that
% line.
%
% Modified:
%   Apply suppression of high amplitudes for near-singular case by
%	adding a number delta to the diagonal elements of matrix A,
%	except element (1,1), because the average should not be affected
%
%	Output of reconstructed time series in array yr June 2005
%
%   Change call and input arguments to accommodate a base period length (nb)
%   All frequencies from 1 (base period) until nf are included
%
% Parameters
%
% Inputs:
%   ni    = nr. of images (total number of actual samples of the time
%           series)
%   nb    = length of the base period, measured in virtual samples
%           (days, dekads, months, etc.)
%   nf    = number of frequencies to be considered above the zero frequency
%   y     = array of input sample values (e.g. NDVI values)
%   ts    = array of size ni of time sample indicators
%           (indicates virtual sample number relative to the base period);
%           numbers in array ts maybe greater than nb
%           If no aux file is used (no time samples), we assume ts(i)= i,
%           where i=1, ..., ni
%   HiLo  = 2-character string indicating rejection of high or low outliers
%   low   = valid range minimum
%   high  = valid range maximum (values outside the valid range are rejeced
%           right away)
%   fet   = fit error tolerance (points deviating more than fet from curve
%           fit are rejected)
%   dod   = degree of overdeterminedness (iteration stops if number of
%           points reaches the minimum required for curve fitting, plus
%           dod). This is a safety measure
%   delta = small positive number (e.g. 0.1) to suppress high amplitudes
%
% Outputs:
%
% amp   = returned array of amplitudes, first element is the average of
%         the curve
% phi   = returned array of phases, first element is zero
% yr	= array holding reconstructed time series

mat=zeros(min(2*nf+1,ni),ni,'single');
amp=zeros(nf+1,1,'single');
phi=zeros(nf+1,1,'single');
yr=zeros(ni,1,'double');

% if (Opt.FirstRun==true)
    sHiLo = 0;
    if (strcmp(HiLo,'Hi'))
        sHiLo =-1;
    end
    if (strcmp(HiLo,'Lo'))
        sHiLo = 1;
    end
    nr=min(2*nf+1,ni);
    noutmax=ni-nr-dod;
    dg=180.0/pi;
    mat(1,:)=1.0;

    ang=2.*pi*(0:nb-1)/nb;
    cs=cos(ang);
    sn=sin(ang);
%     Opt.FirstRun=false;
% end
i=1:nf;
for j=1:ni
    index=1+mod(i*(ts(j)-1),nb);
    mat(2*i  ,j)=cs(index);
    mat(2*i+1,j)=sn(index);
end

p=ones(ni,1);
p(or(y<low,y>high))=0;
nout=sum(p==0);

if (nout>noutmax)
%     disp('Not enough data points')
%     disp(['nout:' num2str(nout) ' ,noutmax:' num2str(noutmax)])
%     error('nout > noutmax')
    return
end

ready=false;
nloop=0;
nloopmax=ni;

while ((~ready)&&(nloop<nloopmax))
    nloop=nloop+1;
    za=mat*(p.*y);

    A=mat*diag(p)*mat';
    A=A+diag(ones(nr,1))*delta;
    A(1,1)=A(1,1)-delta;
    zr=A\za;

    yr=mat'*zr;
    diffVec=sHiLo*(yr-y);
    err=p.*diffVec;

	[~, rankVec]=sort(err,'ascend');
% The above line may not be recognized on some older MATLAB versions.
% Simply comment the above line and uncomment the line below.
%    [tmp, rankVec]=sort(err,'ascend');

    maxerr=diffVec(rankVec(ni));
    ready=(maxerr<=fet)||(nout==noutmax);

    if (~ready)
        i=ni;
        j=rankVec(i);
        while ( (p(j)*diffVec(j)>maxerr*0.5)&&(nout<noutmax) )
				p(j)=0;
				nout=nout+1;
				i=i-1;
				j=rank(i);
        end
    end
end

amp(1)=zr(1);
phi(1)=0.0;

zr(ni+1)=0.0;

i=2:2:nr;
ifr=(i+2)/2;
ra=zr(i);
rb=zr(i+1);
amp(ifr)=sqrt(ra.*ra+rb.*rb);
phase=atan2(rb,ra)*dg;
phase(phase<0)=phase(phase<0)+360;
phi(ifr)=phase;

end


%  Figure 10.14      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_14.m is a script to generate Fig. 10.14, the transient
%  response of the PD plus notch compensator of the
%  satellite position control, non-colocated case

% satellite system matrices
f =[0    1.0000         0         0;
   -0.9100   -0.0360    0.9100    0.0360;
         0         0         0    1.0000 ;
    0.0910    0.0036   -0.0910   -0.0036];
g =[0;
     0;
     0;
     1];

h =[1     0     0     0];

j =[0];
no3=conv(.25*[2 1],[1/.81 0 1]);
do3=conv([1/20 1],[1/625 2/25 1]);
sys3=tf(no3,do3);
sysG=ss(f,g,h,j);
[sysol]=series(sys3,sysG);
[Aol,Bol,Col,Dol]=ssdata(sysol);
Acl=Aol-Bol*Col;
hold off; clf
t=[0:0.1:40];
syscl=ss(Acl,Bol,Col,Dol)
[y,t]=step(syscl,t);
plot(t,y,'LineWidth',2);
grid on;
xlabel('Time (sec)');
ylabel('Amplitude');
title('Closed-loop step response of KD_3(s)G(s)')

%grid
nicegrid;


function tests = test_cosamp
  tests = functiontests(localfunctions);
end

function [A, x, b, k] = problem_1()
    m = 100;
    n = 1000;
    k = 4;
    A = spx.dict.simple.gaussian_dict(m, n);
    gen = spx.data.synthetic.SparseSignalGenerator(n, k);
    % create a sparse vector
    x =  gen.biGaussian();
    b = A*x;
end

function [dict, reps, signals, k] = problem_2()
    m = 200;
    n = 1000;
    k = 10;
    s = 500;
    dict = spx.dict.simple.gaussian_dict(m, n);
    gen = spx.data.synthetic.SparseSignalGenerator(n, k, s);
    % create a sparse vector
    reps =  gen.biGaussian();
    signals = dict*reps;
end

function test_cosamp_1(testCase)
    [A, x, b, k] = problem_1();
    solver = spx.pursuit.single.CoSaMP(A, k);
    result = solver.solve(b);
    cmpare = spx.commons.SparseSignalsComparison(x, result.z, k);
    %cmpare.summarize();
    verifyTrue(testCase, cmpare.all_have_matching_supports(1.0));
end

function test_cosamp_2(testCase)
    [dict, reps, signals, k] = problem_2();
    solver = spx.pursuit.single.CoSaMP(dict, k);
    ns = size(signals, 2);
    dd = size(dict, 2);
    recovered = zeros(dd, ns);
    for s=1:ns
        signal = signals(:, s);
        result = solver.solve(signal);
        recovered(:, s) = result.z;
    end
    cmpare = spx.commons.SparseSignalsComparison(reps, recovered, k);
    % cmpare.summarize();
    verifyTrue(testCase, cmpare.all_have_matching_supports(1.0));
end


function list = gnomeSort(list)

    i = 2;
    j = 3;

    while i <= numel(list)

        if list(i-1) <= list(i)
            i = j;
            j = j+1;
        else
            list([i-1 i]) = list([i i-1]);         %Swaping
            i = i-1;
            if i == 1
                i = j;
                j = j+1;
            end
        end  %if

    end  %while
end     %gnomeSort


function y = prtRvUtilMvtLogPdf(x,mu,Sigma,dof)
% y = prtRvUtilMvtLogPdf(x,mu,Sigma,dof)
% xxx Need Help xxx

d = size(x,2);

[R,err] = cholcov(Sigma,0);
if err ~= 0
    error('mvtLogPdf:BadCovariance', ...
        'SIGMA must be symmetric and positive definite.');
end

% Create array of standardized data
xRinv = bsxfun(@minus,x,mu(:)') / R;

c = gammaln((d+dof)*0.5) - (d*0.5)*log(dof*pi) - gammaln(dof*0.5) - 0.5*log(det(Sigma));

y = c - (dof+d)*0.5*log(1 +1/dof*sum(xRinv.^2, 2));


% LOGNORMAL_RAND
%
%   X = LOGNORMAL_RAND(MU, SIGMA)
%
% MU is the location parameter in (-INF, INF)
% SIGMA is the scale parameter in (0, INF)
%
% Function calls
%   X = LOGNORMAL_RAND(MU,SIGMA,M,N,...)
%   X = LOGNORMAL_RAND(MU,SIGMA,[M,N,...])
% return an M-by-N-by-... array.
%
% See also LOGNORMAL_LOGPDF, NORMRND.

% Last modified 2010-11-12
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function x = lognormal_rand(varargin)

x = exp(normrnd(varargin{:}));


function F=tstatis(X,y,P,N)

if nargin<4;N=3;end
if nargin<3;P=1000;end

[M Nvar]=size(X);
t0=tstatis1(X,y);
k1=find(y==1);
k2=find(y~=1);
T=zeros(P,Nvar);
TMC=zeros(N,Nvar);
Q=floor(M*0.9);

%++++ Reliability index
i=1;
while i<=N
  index=randperm(M);   % MCS
  cal=index(1:Q);
  tmc=tvalue(X(cal,:),y(cal));
  TMC(i,:)=tmc;
  fprintf('The %dth MCS finished.\n',i);
  i=i+1;
end
RI=abs(mean(TMC)./std(TMC));

%+++ Permutation test for p value computation.
i=1;
while i<=P
  index=randperm(M);   % permutation
  yr=y(index);
  if sum(abs(yr-y))==0;continue;end
  tr=tvalue(X,yr);
  T(i,:)=tr;
  fprintf('The %dth permutation finished.\n',i);
  i=i+1;
end

p=zeros(1,Nvar);
for i=1:Nvar
   p(i)=length(find(abs(T(:,i))>abs(t0(i))))/P;
end

%+++ Output
F.t0=abs(t0)/max(abs(t0));
F.TMC=TMC;
F.RI=RI;
F.tr=T;
F.p=p;


function h = dmaxflat(N,d)

% returns 2-D diamond maxflat filters of order 'N'
% the filters are nonseparable and 'd' is the (0,0) coefficient, being 1 or 0 depending on use
% by Arthur L. da Cunha, University of Illinois Urbana-Champaign
% Aug 2004

if (N > 7 | N < 1), error('N must be in {1,2,3,4,5,6,7}');end;

switch N
 case 1
     h = [0 1 0; 1 0 1; 0 1 0]/4;
     h(2,2) = d;
 case 2
     h = [0 -1 0; -1 0 10; 0 10 0];
     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))]/32;
     h(3,3) = d;
 case 3
     h = [0  3   0  2;
          3  0  -27 0;
          0 -27  0 174;
          2  0  174  0];

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))]/512;
     h(4,4) = d;
 case 4
     h = [0   -5     0   -3    0    ;
         -5    0    52    0    34   ;
          0    52    0   -276  0    ;
         -3    0   -276   0   1454  ;
          0    34    0   1454  0   ]/2^12;

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(5,5) = d;
 case 5
     h = [0    35    0    20    0    18;
          35   0   -425   0   -250    0;
          0   -425   0   2500   0    1610 ;
         20    0    2500  0  -10200   0;
          0   -250    0 -10200  0   47780;
         18    0    1610  0   47780   0]/2^17;

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(6,6) = d;
 case 6
     h = [0    -63    0    -35    0    -30    0   ;
         -63    0    882    0    495    0    444  ;
          0    882    0   -5910   0   -3420   0   ;
         -35    0   -5910   0   25875   0   16460 ;
          0    495    0    25875  0   -89730  0   ;
         -30    0   -3420   0  -89730   0   389112;
          0    44     0    16460  0   389112  0   ]/2^20;

     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(7,7) = d;
 case 7
     h = [0    231     0     126     0      105      0      100   ;
         231    0    -3675    0    -2009     0     -1715     0    ;
          0   -3675    0    27930    0     15435     0     13804  ;
         126    0     27930   0   -136514    0    -77910     0    ;
          0   -2009    0   -136514   0     495145    0     311780 ;
         105    0     15435   0    495145    0    -1535709   0    ;
          0   -1715    0   -77910    0    -1535709   0    6305740 ;
         100    0    13804    0    311780    0    6305740    0    ]/2^24 ;
     h = [h fliplr(h(:,1:end-1))];
     h = [h ; flipud(h(1:end-1,:))];
     h(8,8) = d;
 end


function X = cholsolve(A,B)
% Solves (A'*A)*X=B with respect to X.
% A should be an upper triangular matrix (Cholesky decomposition).

opts.UT = true;
opts.TRANSA = true;
Z = linsolve(A, B, opts);
opts.TRANSA = false;
X = linsolve(A, Z, opts);


function [im] = nonmax(im,theta)
% function [im] = nonmax(im,theta)
%
% Perform non-max suppression on im orthogonal to theta.  Theta can be
% a matrix providing a different theta for each pixel or a scalar
% proving the same theta for every pixel.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

if numel(theta)==1,
  theta = theta .* ones(size(im));
end

% Do non-max suppression orthogonal to theta.
theta = mod(theta+pi/2,pi);

% The following diagram depicts the 8 cases for non-max suppression.
% Theta is valued in [0,pi), measured clockwise from the positive x
% axis.  The 'o' marks the pixel of interest, and the eight
% neighboring pixels are marked with '.'.  The orientation is divided
% into 8 45-degree blocks.  Within each block, we interpolate the
% image value between the two neighboring pixels.
%
%        .66.77.
%        5\ | /8
%        5 \|/ 8
%        .--o--.-----> x-axis
%        4 /|\ 1
%        4/ | \1
%        .33.22.
%           |
%           |
%           v
%         y-axis
%
% In the code below, d is always the distance from A, so the distance
% to B is (1-d).  A and B are the two neighboring pixels of interest
% in each of the 8 cases.  Note that the clockwise ordering of A and B
% changes from case to case in order to make it easier to compute d.

% Determine which pixels belong to which cases.
mask15 = ( theta>=0 & theta<pi/4 );
mask26 = ( theta>=pi/4 & theta<pi/2 );
mask37 = ( theta>=pi/2 & theta<pi*3/4 );
mask48 = ( theta>=pi*3/4 & theta<pi );

mask = ones(size(im));
[h,w] = size(im);
[ix,iy] = meshgrid(1:w,1:h);

% case 1
idx = find( mask15 & ix<w & iy<h);
idxA = idx + h;
idxB = idx + h + 1;
d = tan(theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 5
idx = find( mask15 & ix>1 & iy>1);
idxA = idx - h;
idxB = idx - h - 1;
d = tan(theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 2
idx = find( mask26 & ix<w & iy<h );
idxA = idx + 1;
idxB = idx + h + 1;
d = tan(pi/2-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 6
idx = find( mask26 & ix>1 & iy>1 );
idxA = idx - 1;
idxB = idx - h - 1;
d = tan(pi/2-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 3
idx = find( mask37 & ix>1 & iy<h );
idxA = idx + 1;
idxB = idx - h + 1;
d = tan(theta(idx)-pi/2);
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 7
idx = find( mask37 & ix<w & iy>1 );
idxA = idx - 1;
idxB = idx + h - 1;
d = tan(theta(idx)-pi/2);
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 4
idx = find( mask48 & ix>1 & iy<h );
idxA = idx - h;
idxB = idx - h + 1;
d = tan(pi-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% case 8
idx = find( mask48 & ix<w & iy>1 );
idxA = idx + h;
idxB = idx + h - 1;
d = tan(pi-theta(idx));
imI = im(idxA).*(1-d) + im(idxB).*d;
mask(idx(find(im(idx)<imI))) = 0;

% apply mask
im = im .* mask;


function [B3, B2, B1, B] = construct_B_matrices(measurements)
% function [B3, B2, B1, B] = construct_B_matrices( measurements )
%
% Given a measurement struct of the same format that the SE-Sync method
% expects, this function computes and returns the (sparse) system matrices
% B1, B2, B3, and B defined in the first section of the appendix
% "Reformulating the estimation problem" in the SE-Sync paper

d = length(measurements.t{1});  % Dimension of observations
n = max(max(measurements.edges));  % Number of poses
m = size(measurements.edges, 1);  % Number of measurements

% B3 matrix:
B3_nnz = (d^3 + d^2)*m;
B3_rows = zeros(1, B3_nnz);
B3_cols = zeros(1, B3_nnz);
B3_vals = zeros(1, B3_nnz);

for e = 1:m
    tail = measurements.edges(e,1);
    head = measurements.edges(e,2);

    sqkappa = sqrt(measurements.kappa{e});
    Rt = measurements.R{e}';

    % Block entries corresponding to the tail of this edge

    for r = 1:d
        for c = 1:d

            % Block representation of the -sqrt(kappa) *Rt(i,j) * I_d that
            % appears in the Kronecker product

            idxs = [(d^3 + d^2)*(e-1) + d^2*(r-1) + d*(c - 1) + 1 : (d^3 + d^2)*(e-1) + d^2*(r-1) + d*(c - 1) + d];

            B3_rows(idxs) = [d^2 * (e-1) + d*(r-1) + 1 : d^2 * (e-1) + d*(r-1) + d];
            B3_cols(idxs) = [d^2 * (tail - 1) + d*(c-1) + 1 : d^2 * (tail - 1) + d*(c-1) + d];
            B3_vals(idxs) = -sqkappa * Rt(r,c) * ones(1, d);
        end
    end

    % Large d x d diagonal block entry corresponding to the head of this
    % edge

    idxs = [d^3 * e + d^2*(e-1) + 1 : (d^3 + d^2)*e];

    B3_rows(idxs) = [d^2 *(e-1) + 1 : d^2 * e];
    B3_cols(idxs) = [d^2 *(head -1) + 1 : d^2 * head];
    B3_vals(idxs) = sqkappa * ones(1, d^2);
end

B3 = sparse(B3_rows, B3_cols, B3_vals, d^2*m, d^2*n);

if nargout > 1

    % B2 matrix:
    B2_nnz = m*d^2;
    B2_rows = zeros(1, B2_nnz);
    B2_cols = zeros(1, B2_nnz);
    B2_vals = zeros(1, B2_nnz);

    for e = 1:m
        tail = measurements.edges(e, 1);

        sqtau = sqrt(measurements.tau{e});
        tij = measurements.t{e};

        for idx = 1:d
            % Block representation of -sqrt(tau)*t_ij(i) * I_d in the Kronecker
            % product:

            B2_rows(d^2 * (e-1) + d*(idx-1) + 1 : d^2 * (e-1) + d*idx) = [d*(e-1) + 1 : d*e];
            B2_cols(d^2 * (e-1) + d*(idx-1) + 1 : d^2 * (e-1) + d*idx) = [d^2 * (tail-1) + d*(idx - 1) + 1 : d^2 * (tail-1) + d*idx];
            B2_vals(d^2 * (e-1) + d*(idx-1) + 1 : d^2 * (e-1) + d*idx) = -sqtau * tij(idx)*ones(1,d);
        end
    end

    B2 = sparse(B2_rows, B2_cols, B2_vals, d*m, d^2*n);

    % B1 matrix:

    B1_nnz = 2*d*m;
    B1_rows = zeros(1, B1_nnz);
    B1_cols = zeros(1, B1_nnz);
    B1_vals = zeros(1, B1_nnz);

    for e = 1:m
        tail = measurements.edges(e,1);
        head = measurements.edges(e,2);

        % Block entry corresponding to the tail of this edge
        B1_rows(2*d*(e-1) + 1 : 2*d*e - d) = [d*(e-1) + 1 : d*e];
        B1_cols(2*d*(e-1) + 1 : 2*d*e - d) = [d*(tail - 1) + 1 : d*tail];
        B1_vals(2*d*(e-1) + 1 : 2*d*e - d) = -sqrt(measurements.tau{e})*ones(1, d);

        % Block entry corresponding to the head of this edge
        B1_rows(2*d*e - d + 1 : 2*d*e) = [d*(e-1) + 1 : d*e];
        B1_cols(2*d*e - d + 1 : 2*d*e) = [d*(head - 1) + 1 : d*head];
        B1_vals(2*d*e - d + 1 : 2*d*e) = sqrt(measurements.tau{e})*ones(1, d);
    end

    B1 = sparse(B1_rows, B1_cols, B1_vals, d*m, d*n);

end

if nargout >= 4
    B = [B1, B2;
        sparse(size(B3, 1), size(B1, 2)), B3];
end


function [ bb, bx ] = quadbf ( x, it, in, xc, node )

%*****************************************************************************80
%
%% QUADBF evaluates the quadratic basis functions and derivatives.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2006
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, integer IT, the element in which X lies.
%
%    Input, integer IN, the node associated with the basis function.
%
%    Input, real XC(*), the coordinates of the nodes.
%
%    Input, integer NODE(ELEMENT_NUM,3), the nodes that make up
%    each element.
%
%    Output, real BB, BX, the values of the basis function and its
%    derivative with respect to X.
%
  in1 = in;
  in2 = mod ( in, 3 ) + 1;
  in3 = mod ( in + 1, 3 ) + 1;

  i1 = node(it,in1);
  i2 = node(it,in2);
  i3 = node(it,in3);

  den = ( xc(i3) - xc(i1) ) * ( xc(i2) - xc(i1) );

  bb = ( xc(i3) - x ) * ( xc(i2) - x ) / den;

  bx = ( 2.0 * x - xc(i2) - xc(i3) ) / den;

  return
end


function [connections]=findConnectionMatrix2(mesh)
% Finds the sparse connection matrix for a mesh
%
%  connections=findConnectionMatrix2(mesh)
%
% The mesh slots must be:
%     .vertices  (nPoints,3): 3D coords of all mesh vertices
%     .triangles (nFaces,3) : triplets of indices specifies one corner of a face triangle
%
% The returned connections is a sparse matrix (nPoints*nPoints)
%
% AUTHOR: Wade
% DATE : Last modified 020701

nVerts    = meshGet(mesh,'nVertices');
triangles = meshGet(mesh,'triangles')';

% faceIndexList defines vertices that are connected to each other:
% If two vertices are in the same row, they're in the same triangle and
% therefore are connected

% The values here are going to be used to create scalars
% (connection_locations) that index into the connections matrix.
% The multiplication by nVerts makes it possible to add together
% a row and column value (see below).
%
r1=(triangles(:,1))*nVerts;
r2=(triangles(:,2));
r3=(triangles(:,3));
r4=(triangles(:,2))*nVerts;

% Calculate the connection_locations
% These are connections between (1,2), (1,3), and (2,3)
%
connect_locations=[ (r1+r2+1) ;(r1+r3+1); (r4+r3+1) ];

[cly,clx]=ind2sub([nVerts,nVerts],connect_locations);
connections = sparse(cly,clx,1,nVerts,nVerts);

%Now make it symmetric because if a is connected to b then b is connected to a.
connections = connections+(connections');
connections = (connections~=0);

return;


function [out] = rainfall_1(In,T,p1,varargin)
%rainfall_1

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

% Flux function
% ------------------
% Description:  Rainfall based on temperature threshold
% Constraints:  -
% @(Inputs):    p1   - temperature threshold above which rainfall occurs [oC]
%               T    - current temperature [oC]
%               In   - incoming precipitation flux [mm/d]
%               varargin(1) - smoothing variable r (default 0.01)

if size(varargin,2) == 0
    out = In.*(1-smoothThreshold_temperature_logistic(T,p1));
elseif size(varargin,2) == 1
    out = In.*(1-smoothThreshold_temperature_logistic(T,p1,varargin(1)));
end

end


function VisualServoTest(testCase)
      tests = functiontests(localfunctions);
end

        function testFunctions
            %BundleAdjust.testFunctions Test generated functions
            %
            % BundleAdjust.testFunctions will run a batch of unit tests on the
            % functions generated by BundleAdjust.generateFunctions.
            %
            % See also BundleAdjust.generateFunctions.

            cam = CentralCamera('default' );

            t = [0 0 0]';
            R = rpy2tr(0.3, 0.3, 0.4);
            q = UnitQuaternion(R).double

            T =  transl(t) * R;

            P = [1 2 5]';
            [p0,vis] = cam.project(P, 'Tcam', T);

            d = 0.00001;
            dd = d*eye(3);

            [UVS,JAS,JBS] = cameraModel(t(1), t(2), t(3), q(2), q(3), q(4), ...
                P(1), P(2), P(3), ...
                cam.f, cam.rho(1), cam.rho(2), cam.u0, cam.v0);

            %% projection
            fprintf('-- test projection function\n');
            fprintf(' toolbox\n');
            p0
            fprintf(' symbolic\n');
            UVS

            assert( max(abs(UVS-p0)) < 0.1, 'Error with projection equation')

            %% jacobian B
            fprintf('-- Jacobian B\n');
            fprintf(' toolbox\n');
            p1 = cam.project( bsxfun(@plus, P, dd), 'Tcam', T);

            JB = bsxfun(@minus, p1, p0) / d

            % symbolically derived result
            fprintf(' symbolic\n');
            JBS
            assert( max(max(abs(JBS-JB))) < 0.1, 'Error with Jacobian B equation')

            %% jacobian A

            R = UnitQuaternion(q).R;
            T =  rt2tr(R, t);

            JA = [];
            for i=1:3
                T =  rt2tr(R, t+dd(:,i));
                p = cam.project(P, 'Tcam', T);
                JA = [JA (p-p0)/d];
            end

            for i=1:3
                qq = q(2:4);
                qq(i) = qq(i) + d;
                qs = sqrt(1-sum(qq.^2));
                RR = UnitQuaternion([qs qq]).R;
                T =  rt2tr(RR, t);
                p = cam.project(P, 'Tcam', T);
                JA = [JA (p-p0)/d];
            end

            fprintf('-- Jacobian A\n');
            fprintf(' toolbox\n');
            JA

            % symbolically derived result
            fprintf(' symbolic\n');
            JAS
            assert( max(max(abs(JAS-JA))) < 0.1, 'Error with Jacobian A equation')

function camera_test(testCase)
    cam = CentralCamera('default');
    s = cam.char();
    cam

    P = mkgrid(3, 1.0, transl(0,0,2));

    uv = cam.project(P)
    cam.plot(P);
    uv = cam.plot(P);

    fov = cam.fov();
    K = cam.K();
    C = cam.C();

    about P
    ray = cam.ray(uv);

    cam.hold()
    cam.plot(P);
    cam.ishold();
    cam.clf();
    cam.plot([200 300 1]');
    cam.homline([200 200 1]');
    cam.plot_camera();
    %cam.plot_epiline();

    J = cam.visjac_p(P, 1);
    J = cam.visjac_p_polar(P, 1);
    %J = cam.visjac_l(P);
    %J = cam.visjac_e(P);
    cam.flowfield([1 0 0 0 0 0]');
end

function epipolar_test(testCase)
    return
    F = fmatrix(uv1, uv2)
    %[F,r] = fmatrix(uv1, uv2)

    epidist(F, uv1, uv2)

    [F,r] = fmatrix([uv1; uv2])

    clearfigs
    cam1 = camera('camera 1');

    P = mkgrid(3, 1.0, transl(0,0,2));
    P

    T = transl(0.5,-0.2,0)*trotx(-0.2)*trotz(0.3);
    T = transl(-0.3, 0.4, -0.8)*trotz(0.5)*troty(.3)*trotx(.3);
    cam2 = camera(T, 'camera 2');

    uv1 = cam1.plot(P, 'o')
    uv2 = cam2.plot(P, 'o')

    H = homography(uv1, uv2)

    homtrans(H, uv1)-uv2

    cam2.hold
    plot2(homtrans(H, uv1)', '+')
    cam2.hold(0)
    cam2.clf

    homtrans(H, uv1)
    homtrans(inv(H), uv2)

    P2 = pinv(cam1.C) * [
        312 512 712
        912 912 912
          1   1   1];

    P2
    cam1.project(P2(1:3,:))
    pause
    % columns at (X,Y,Z) and any multiple will project to the same point
    % set range of points to be 1, 3, 4
    P2 = P2(1:3,:) * diag([1 3 4])
    cam1.project(P2(1:3,:))
    P = [P P2]
    cam1.plot(P, 'o')
    cam2.plot(P, 'o')

    uv1 = cam1.project(P)
    uv2 = cam2.project(P)

    homtrans(H, uv1)-uv2

    H = ransac(@homography, [uv1 uv2], .01)
end
%function sphcamera_test(testCase)
%function panocamera_test(testCase)


function L = laplacian( F )
%LAPLACIAN Vector Laplacian of a CHEBFUN2V.
%   LAPLACIAN(F) returns a CHEBFUN2V representing the vector Laplacian of F.
%
% See also CHEBFUN2V/LAP.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check:
if ( isempty( F ) )
    L = chebfun2v;
    return
end

% laplacian = f_xx + f_yy
L = diff(F, 2, 1) + diff(F, 2, 2);

end


function [vect] = getPointingVectFromPitchHeading(rVect, pitchAngle, headingAngle)
%getPointingVectFromPitchHeading Summary of this function goes here
%   Detailed explanation goes here

    zHat = [0,0,1];

    up = rVect/norm(rVect);
    north = cross(cross(up,zHat),up);
    north = north/norm(north);
    west = cross(up,north);
    west = west/norm(west);

    hP = 2*pi - headingAngle;
    vect = cos(pitchAngle)*(cos(hP)*north + sin(hP)*west) + sin(pitchAngle)*up;
    vect = vect/norm(vect);
end


function imResult = blendMode_ColorBurn(A, B, offsetW, offsetH)
%% Color Burn blending mode: divides the inverted bottom layer by the top
%   layer, and then inverts the result. This darkens the top layer
%   increasing the contrast to reflect the color of the bottom layer.
%   The darker the bottom layer, the more its color is used. Blending with
%   white produces no difference.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_ColorBurn));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

ind = B == 0;
indCompl = abs(ind - 1);
tmp = max(0, 1 - (1 - A) ./ B);
C = ind .* B + indCompl .* tmp;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


function showmeshdensity(node,elem)
%% SHOWMESHDENSITY show the density of the mesh
%
% showmeshdensity(node,elem) display the size of each triangle using different
% color.
%
% Example
%   load lakemesh
%   showmeshdensity(node,elem)
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

ve2 = node(elem(:,1),:)-node(elem(:,3),:);
ve3 = node(elem(:,2),:)-node(elem(:,1),:);
area = 0.5*abs(-ve3(:,1).*ve2(:,2) + ve3(:,2).*ve2(:,1));
h = sqrt(area);
showsolution(node,elem,h)
colorbar
axis equal; axis tight; axis off;


% Test file for trigtech/max.m

function pass = test_max(pref)

% Get preferences.
if ( nargin < 1 )
    pref = trigtech.techPref();
end

testclass = trigtech();

%%
% Spot-check the extrema for a few functions.
pass(1) = test_spotcheck_max(testclass, @(x) exp(-cos(2*pi*x)), exp(1), pref);
pass(2) = test_spotcheck_max(testclass, @(x) sin(10*pi*x), 1, pref);

pass(3) = test_spotcheck_max(testclass, @(x) exp(sin(pi*x).^100), exp(1), pref);
pass(4) = test_spotcheck_max(testclass, @(x) exp(-sin(pi*x).^100), 1, pref);

% Approx to sign function
pass(5) = test_spotcheck_max(testclass, @(x) 4/pi*(sin(pi*x) + ...
    1/3*sin(3*pi*x) + 1/5*sin(5*pi*x) + 1/7*sin(7*pi*x) + 1/9*sin(9*pi*x)), 1.182328208857607, pref);

%%
% Check operation for array-valued inputs.
fun_op = @(x) [exp(-cos(2*pi*x)) sin(10*pi*x) exp(-sin(pi*(x-0.32)).^100)];
f = testclass.make(fun_op, [], pref);
[y, x] = max(f);
exact_max = [exp(1) 1 1];
fx = [exp(-cos(2*pi*x(1))) sin(10*pi*x(2)) exp(-sin(pi*(x(3)-0.32)).^100)];
pass(6) = (all(abs(y - exact_max) < 100*eps) && ...
           all(abs(fx - exact_max) < 10*eps));

%%
% Test for complex-valued TRIGTECH objects.
pass(7) = test_spotcheck_max(testclass, ...
    @(x) 1 + cos(pi*x) + exp(1i*pi*x), ...
    3, pref);

end

% Spot-check the results for a given function.
function result = test_spotcheck_max(testclass, fun_op, exact_max, pref)

f = testclass.make(fun_op,[], pref);
[y, x] = max(f);
fx = fun_op(x);
result = (all(abs(y - exact_max) < 100*vscale(f)*eps) && ...
          all(abs(fx - exact_max) < 100*vscale(f)*eps));

end


function [blk, At, b] = sdpt3_create_hpsd_blk(n)
%SDPT3_CREATE_HPSD_BLK Creates a Hermitian PSD matrix variable for the
%SDPT3 solver.
%Note that
%   W \succeq 0 \iff U = [ Re(W)  Im(W) ] \succeq 0
%                      [ -Im(W) Re(W) ]
%We declare a 2nx2n symmetric matrix variable U and enforce additional
%constraints according to the structure of U:
%   1. U(i+n,j+n) = U(i,j) \forall 1 <= i <= j <= n
%   2. U(i,j+n) = -U(j,i+n) \forall 1 <= i <= j <= n
%Syntax:
%   [blk, At, b] = SDPT3_CREATE_HPSD_BLK(n);
%Inputs:
%   n - Dimension of the matrix.
%Outputs:
%   blk - Block definition: {'s', 2*n}.
%   At - Transposed constraint matrix (as a submatrix of the full
%        constraint matrix).
%   b - Constraint value vector (as a subvector of the full constraint
%       value vector).
blk = {'s', 2*n};
n_c = n * (n + 1);
n_var = n * (2 * n + 1);
n_sub = n * (n + 1) / 2;
n_nz = 2 * n_c - n;
% triplets for the sparse constraint matrix
idx_row = zeros(n_nz, 1);
idx_col = zeros(n_nz, 1);
vals = zeros(n_nz, 1);
i_c = 0;
i_nz = 0;
% constraints are expressed by
%   A svec(W) = b
%   W(i,j) --> svec(W)(i + j(j-1)/2)
% Re(W) = Re(W)
for jj = 1:n
    for ii = 1:jj
        i_c = i_c + 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = ii + jj*(jj-1)/2;
        vals(i_nz) = 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = ii + n + (jj + n)*(jj + n - 1)/2;
        vals(i_nz) = -1;
    end
end
% Im(W) = -Im(W)^T
% diagonals should be zeros
for ii = 1:n
    i_c = i_c + 1;
    i_nz = i_nz + 1;
    idx_row(i_nz) = i_c;
    idx_col(i_nz) = ii + (ii + 2*n)*(ii - 1)/2 + n_sub;
    vals(i_nz) = 1;
end
% skew symmetry
for jj = 2:n
    for ii = 1:jj-1
        i_c = i_c + 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = ii + (jj + 2*n)*(jj - 1)/2 + n_sub;
        vals(i_nz) = 1;
        i_nz = i_nz + 1;
        idx_row(i_nz) = i_c;
        idx_col(i_nz) = jj + (ii + 2*n)*(ii - 1)/2 + n_sub;
        vals(i_nz) = 1;
    end
end
At = sparse(idx_row, idx_col, vals, n_c, n_var)';
b = sparse(n_c, 1);
end


function [ n_data, n, x, fx ] = lobatto_polynomial_values ( n_data )

%*****************************************************************************80
%
%% LOBATTO_POLYNOMIAL_VALUES returns values of the completed Lobatto polynomials.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      n * LegendreP [ n - 1, x ] - n * x * LegendreP [ n, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 31;

  fx_vec = [ ...
    0.9375000000000000, ...
    0.7031250000000000, ...
   -0.9667968750000000, ...
   -1.501464843750000, ...
    0.3639221191406250, ...
    2.001914978027344, ...
    0.6597948074340820, ...
   -1.934441328048706, ...
   -1.769941113889217, ...
    1.215243665501475, ...
    0.000000000000000, ...
    0.8692500000000000, ...
    1.188000000000000, ...
    1.109250000000000, ...
    0.7680000000000000, ...
    0.2812500000000000, ...
   -0.2520000000000000, ...
   -0.7507500000000000, ...
   -1.152000000000000, ...
   -1.410750000000000, ...
   -1.500000000000000, ...
   -1.410750000000000, ...
   -1.152000000000000, ...
   -0.7507500000000000, ...
   -0.2520000000000000, ...
    0.2812500000000000, ...
    0.7680000000000000, ...
    1.109250000000000, ...
    1.188000000000000, ...
    0.8692500000000000, ...
    0.000000000000000 ];

  n_vec = [ ...
     1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3,  3, ...
     3,  3 ];

  x_vec = [ ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
    0.25, ...
   -1.00, ...
   -0.90, ...
   -0.80, ...
   -0.70, ...
   -0.60, ...
   -0.50, ...
   -0.40, ...
   -0.30, ...
   -0.20, ...
   -0.10, ...
    0.00, ...
    0.10, ...
    0.20, ...
    0.30, ...
    0.40, ...
    0.50, ...
    0.60, ...
    0.70, ...
    0.80, ...
    0.90, ...
    1.00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function R = InvIllco(A,iter)
%INVILLCO     Inverse of extremely ill-conditioned matrices
%
%   R = InvIllco(A,iter)
%
%On return, R is a cell array such that sum(R{i}) is an approximate inverse
%  of A and  I-R*A  is convergent.
%The input matrix A itself may be a cell array. This is convenient to store
%  not exactly representable input data in higher precision. Then R is an
%  approximate inverse of sum(A{i}).
%
%The parameter iter is optional. If specified, an extra iteration is
%  executed to produce  I-R*A  of order eps.
%
%Example:
%
%  n = 20;              % dimension os matrix
%  C = 1e50;            % anticipated condition number
%  A = randmat(n,C);    % ill-conditioned matrix
%  R = InvIllco(A);     % approximate inverse, stored in cell array R
%  norm(AccDot(R,A,-1,eye(n)),'fro')
%
%Implements algorithm InvIllco from
%  S.M. Rump: Inversion of extremely ill-conditioned matrices in floating-point,
%    Japan J. Indust. Appl. Math. (JJIAM), 26:249-277, 2009.
%
%Reference implementation! Slow due to interpretation!
%

% written  06/23/08     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, warning
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end
  wng = warning;
  warning off

  if iscell(A)
    n = size(A{1},1);
    R = inv(A{1});
  else
    n = size(A,1);
    R = inv(A);
  end
  k = 1;
  iter = ( nargin==2 );
  while 1
    k = k+1;
    P = ProdKL(R,A,k,1);
    X = inv(P);
    while any(any(isinf(X)))
      X = inv(P.*(1+eps*randn(n)));
    end
    R = ProdKL(X,R,k,k);
    if norm(X,'fro')*norm(P,'fro')<.01/eps
      if iter, iter = 0; else break, end
    end
  end

  if rndold
    setround(rndold)
  end
  warning(wng)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Eugenio Alcala Baselga
% Date: 02/06/2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function errors_pos_ex = errors_Lyapunov(input)

x_d = input(1);
y_d = input(2);
phi_d = input(3);

x_r = input(4);
y_r = input(5);
phi_r = input(6);

% Lyapunov error computation
e_x = (x_d-x_r)*cos(phi_d)  + (y_d-y_r)*sin(phi_d);
e_y = -(x_d-x_r)*sin(phi_d) + (y_d-y_r)*cos(phi_d);
e_phi = limitare_unghi(phi_d - phi_r);

% SMC error computation
% Lh = -2.5;
% e_x = (x_d-x_r + Lh*cos(phi_r))*cos(phi_d) + (y_d-y_r + Lh*sin(phi_r))*sin(phi_d);
% e_y = -(x_d-x_r + Lh*cos(phi_r))*sin(phi_d) + (y_d-y_r + Lh*sin(phi_r))*cos(phi_d);
% e_phi = limitare_unghi(phi_d - phi_r);

errors_pos_ex = [e_x; e_y; e_phi];


function z = r2z(R)
% Transformation of a correlation matrix to an unconstrained K(K-1)/2 vector
%
% USAGE:
%  [R] = r2z(Z)
%
% INPUTS:
%   R - A K by K correlation matrix
%
% OUTPUTS:
%   Z - K(K-1)/2 vector of values in (-inf,inf)
%
% COMMENTS:
%   See z2r for information about the transformation from Z to R.
%
% See also Z2R, R2PHI, PHI2R

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 3/27/2012

k = length(R);
C = zeros(k);
C2 = chol(R)';

for i=2:k
    rem = 1;
    for j=i-1:-1:1
        C(i,j) = C2(i,j)/sqrt(rem);
        rem = rem - C2(i,j)^2;
    end
end

C =C';
z = C(~tril(true(k)));
z = log((z+1)./(1-z));


%PREX-MCPLOT   PRTools example on a multi-class classifier plot
help prex_mcplot
echo on
gridsize(100)
            % Generate twice normally distributed 2-class data in 2D
a = +gendath([20,20]);    % data only
b = +gendath([20,20]);    % data only
            % Shift the second data by a vector [5,5]
            % and combine it with the first dataset in to A
A = [a; b+5];
            % Generate 4-class labels
lab = genlab([20 20 20 20],[1 2 3 4]');
            % Construct a 4-class dataset A
A = prdataset(A,lab);
A = setname(A,'4-class dataset')
            % Plot this 4-class dataset
figure
            % Make a scatter-plot of the right size
scatterd(A,'.'); drawnow;
            % Compute normal densities based quadratic classifier
w = qdc(A);
            % Plot filled classification regions
plotc(w,'col'); drawnow;
hold on;
            % Redraw the scatter-plot
scatterd(A);
hold off
echo off


function [cij,flag] = makerandCIJdegreesfixed(in,out)
%MAKERANDCIJDEGREESFIXED        Synthetic directed random network
%
%   CIJ = makerandCIJdegreesfixed(N,K);
%
%   This function generates a directed random network with a specified
%   in-degree and out-degree sequence. The function returns a flag,
%   denoting whether the algorithm succeeded or failed.
%
%   Inputs:     in,     indegree vector
%               out,    outdegree vector
%
%   Output:     CIJ,    binary directed connectivity matrix
%               flag,   flag=1 if the algorithm succeeded; flag=0 otherwise
%
%
%   Notes:  Necessary conditions include:
%               length(in) = length(out) = n
%               sum(in) = sum(out) = k
%               in(i), out(i) < n-1
%               in(i) + out(j) < n+2
%               in(i) + out(i) < n
%
%           No connections are placed on the main diagonal
%
%
% Aviad Rubinstein, Indiana University 2005/2007

% intialize
n = length(in);
k = sum(in);
inInv = zeros(k,1);
outInv = inInv;
iIn = 1; iOut = 1;

for i = 1:n
    inInv(iIn:iIn+in(i) - 1) = i;
    outInv(iOut:iOut+out(i) - 1) = i;
    iIn = iIn+in(i);
    iOut = iOut+out(i);
end

cij = eye(n);
edges = [outInv(1:k)'; inInv(randperm(k))'];

% create cij, and check for double edges and self-connections
for i = 1:k
    if cij(edges(1,i),edges(2,i)),
        warningCounter = 1;
        while (1)
            switchTo = ceil(k*rand);
            if ~(cij(edges(1,i),edges(2,switchTo)) || cij(edges(1,switchTo),edges(2,i))),
                cij(edges(1,i),edges(2,switchTo)) = 1;
                if switchTo < i,
                    cij(edges(1,switchTo),edges(2,switchTo)) = 0;
                    cij(edges(1,switchTo),edges(2,i)) = 1;
                end
                temp = edges(2,i);
                edges(2,i) = edges(2,switchTo);
                edges(2,switchTo) = temp;
                break
            end
            warningCounter = warningCounter+1;
            % If there is a legitimate subtitution, it has a probability of 1/k of being done.
            % Thus it is highly unlikely that it will not be done after 2*k^2 attempts.
            % This is an indication that the given indegree / outdegree
            % vectors may not be possible.
            if warningCounter == 2*k^2
                flag = 0;  % no valid solution found
                return;
            end
        end
    else
        cij(edges(1,i),edges(2,i)) = 1;
    end
end

cij = cij - eye(n);

% a valid solution was found
flag = 1;


function Y = sptenrand(sz,nz)
%SPTENRAND Sparse uniformly distributed random tensor.
%
%   R = SPTENRAND(sz,density) creates a random sparse tensor of the
%   specified sz with approximately density*prod(sz) nonzero
%   entries.
%
%   R = SPTENRAND(sz,nz) creates a random sparse tensor of the
%   specified sz with approximately nz nonzero entries.
%
%   Example: R = sptenrand([5 4 2],12);
%
%   See also SPTENSOR, TENRAND, RAND.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% Error check on siz
if ndims(sz) ~= 2 || size(sz,1) ~= 1
    error('Size must be a row vector');
end

% Error check on nz
if ~exist('nz','var') || (nz < 0)
    error('2nd argument must be positive');
end

% Is nz an number or a fraction? Ultimately want a number.
if (nz < 1)
    nz = prod(sz) * nz;
end

% Make sure nz is an integer
nz = ceil(nz);

% Keep iterating until we find enough unique nonzeros or we
% give up
subs = [];
cnt = 0;
while (size(subs,1) < nz) && (cnt < 10)
    subs = ceil( rand(nz, size(sz,2)) * diag(sz) );
    subs = unique(subs, 'rows');
    cnt = cnt + 1;
end

% Extract nnz subscipts and create a corresponding list of
% values
nz = min(nz, size(subs,1));
subs = subs(1:nz,:);
vals = rand(nz,1);

Y = sptensor(subs,vals,sz);
return;


% Quadratic equation function
%  determines coefficients of ax^2 + bx + c
%  input x,y arrays
%  by: Dr. Sherif Omran
%
%
function [a,b,c]=Quadratic(x,y)

p1=x(2)-x(3);
p2=x(3)-x(1);
p3=x(1)-x(2);
p4=x(3)^2-x(2)^2;
p5=x(1)^2-x(3)^2;
p6=x(2)^2-x(1)^2;
p7=x(2)^2*x(3)-x(2)*x(3)^2;
p8=x(1)*x(3)^2-x(1)^2*x(3);
p9=x(1)^2*x(2)-x(1)*x(2)^2;

delta=x(1)^2*(x(2)-x(3))-x(1)*(x(2)^2-x(3)^2)+1*(x(2)^2*x(3)-x(2)*x(3)^2);

a=(1/delta)*((x(2)-x(3))*y(1)+(x(3)-x(1))*y(2)+(x(1)-x(2))*y(3));
b=(1/delta)*(p4*y(1)+p5*y(2)+p6*y(3));
c=(1/delta)*(p7*y(1)+p8*y(2)+p9*y(3));

return;


function W_RF  = yuweiA2(V_D,V_RF)

global Vn H Nrf Nr Nk;
W_RF = ones(Nr,Nrf);

for k = 1:Nk
    F(:,:,k) = H(:,:,k)*V_RF*V_D(:,:,k)*V_D(:,:,k)'*V_RF'*H(:,:,k)';
end
F = sum(F,3)/Nk;
g = 1/Nr;
a = g/Vn;

for Nloop = 1:10
    for j = 1:Nrf
        VRF = W_RF;
        VRF(:,j)=[];
        C = eye(Nrf-1)+a*VRF'*F*VRF;
        G = a*F-a^2*F*VRF*C^(-1)*VRF'*F;
        for i = 1:Nr
            for l = 1:Nr
                if i~=l
                    x(l)=G(i,l)*W_RF(l,j);
                end
            end
            n = sum(x);
            if n ==0
                W_RF(i,j)=1;
            else
                W_RF(i,j)=n/abs(n);
            end
        end
    end
end


function varargout = gradient(f)
%GRADIENT   Gradient of a CHEBFUN3.
%   [FX, FY, FZ] = GRAD(F) returns the gradient of the CHEBFUN3 object F,
%   where
%   FX is the partial derivative of F in the first variable,
%   FY is the partial derivative of F in the second varaiable, and
%   FZ is the partial derivative of F in the third variable.
%
%   G = GRAD(F) returns a CHEBFUN3V object which represents
%            G = [FX; FY; FZ].
%
% See also CHEBFUN3/GRAD.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

F = chebfun3v(diffx(f), diffy(f), diffz(f));

if ( nargout <= 1 )
    varargout = {F};
else
    varargout = F.components;
end

end


%Needed linguistic variables for importance weight of each criterion as generalized fuzzy
%numbers:
VL=[0 0 0.1 0.2 1];     %Very low
L=[0.1 0.2 0.2 0.3 1];  %Low
ML=[0.2 0.3 0.4 0.5 1]; %Medium low
M=[0.4 0.5 0.5 0.6 1];  %Medium
MH=[0.5 0.6 0.7 0.8 1]; %Medium high
H=[0.7 0.8 0.8 0.9 1];  %High
VH=[0.8 0.9 1.0 1.0 1]; %Very High

%Needed linguistic variables for ratings as fuzzy numbers:

VP=[0,0,1,2, 1];     % Very Poor
P=[1,2,2,3, 1];     % Poor
MP=[2,3,4,5, 1];     % Medium Poor
F=[4,5,5,6, 1];     % Fair
MG=[5,6,7,8, 1];    % Medium Good
G=[7,8,8,9, 1];     % Good
VG=[8,9,10,10,1];   % Very Good


function y = randomwalk(x0,p,nsteps)

y    = zeros(1,nsteps);
y(1) = x0;
for istep = 2:nsteps
      y(istep) = y(istep-1) + step(p);
end

function y = step(p)

y = rand;
if ( rand < 1-p )
      y = -y; % variable step
end

% %% Generate and plot a random walk
% x = randomwalk(0,0.5,10000);
% plot(x)


clc;
clear all;
close all;
A=imread('cameraman.tif');
figure,imshow(uint8(A))
title('Original Image');
A=double(A);
[s1 s2]=size(A);
% bs=input('Enter the block sizes for division of the image: '); % Block Size
bs=256;

% Slant
temp=double(zeros(size(A)));
for y=1:bs:s1-bs+1
    for x=1:bs:s2-bs+1
        croppedImage = A((y:y+bs-1),(x:x+bs-1));
        t=getSlantTransform(croppedImage,bs);
        temp((y:y+bs-1),(x:x+bs-1))=t;
    end
end
figure,imshow(uint8(temp))

% Inverse Slant
temp1=double(zeros(size(A)));
for y=1:bs:s1-bs+1
    for x=1:bs:s2-bs+1
        croppedImage = temp((y:y+bs-1),(x:x+bs-1));
        t=getInvSlantTransform(croppedImage,bs);
        temp1((y:y+bs-1),(x:x+bs-1))=t;
    end
end
figure,imshow(uint8(temp1))


function erg = rsqu(q, r)
% Description:
%   rsqu(r, q) computes the r2-value for two one-dimensional distributions
%   given by the vectors q and r
%
% Example code:
%  [erg] = rsqu(q , r)
%
% Input:
%   q: Data structrue (ex) Epoched data structure
%   r: Data structrue (ex) Epoched data structure
%
% Options:
%
% Return:
%    erg:  r2-values
%
% See also:
%
% Reference:
%           G. Schalk, D.J. McFarland, T. Hinterberger, N. Birbaumer, and
%           J. R. Wolpaw,"BCI2000: A General-Purpose Brain-Computer
%           Interface (BCI) System, IEEE Transactions on Biomedical
%           Engineering, Vol. 51, No. 6, 2004, pp.1034-1043.

%         We used BCI2000 open source toolbox code related in r-square value (rsqu.m)
%
%
% Ji Hoon, Jeong
% jh_jeong@korea.ac.kr

%%

q=double(q);
r=double(r);

sum1=sum(q);
sum2=sum(r);
n1=length(q);
n2=length(r);
sumsqu1=sum(q.*q);
sumsqu2=sum(r.*r);

G=((sum1+sum2)^2)/(n1+n2);

erg=(sum1^2/n1+sum2^2/n2-G)/(sumsqu1+sumsqu2-G);
end


function [slug] = g2slug(g)
% Convert mass from grams to slugs.
% Chad Greene 2012
slug = g*(0.002204622621849)/32.17405;


function [m,a]=lpcstable(ar)
%LPCSTABLE Test AR coefficients for stability and stabilize if necessary [MA,A]=(AR)
%
% Usage: (1) [m,ar]=lpcstable(ar); % force ar polynolials to be stable
%
%   Input:  ar(:,p+1)  Autoregressive coefficients
% Outputs:  m(:,1)    Mask identifying stable polynomials
%           a(:,p+1)  Stabilized polynomials formed by reflecting unstable
%                       poles in unit circle (with a(:,1)=1)

%      Copyright (C) Mike Brookes 2016
%      Version: $Id: lpcstable.m 8558 2016-09-22 08:22:54Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1] = size(ar);
mm=ar(:,1)~=1;
if any(mm)          % first ensure leading coefficient is always 1
    ar(mm,:)=ar(mm,:)./ar(mm,ones(1,p1));
end
if p1==1
    m=ones(nf,1); % 0'th order filter is always stable
elseif p1==2
    m=abs(ar(:,2))<1; % 1'st order filter
else
    rf = ar;
    k = rf(:,p1); % check final coefficient in range
    m=abs(k)<1;
    if any(m)
        d = (1-k(m).^2).^(-1);
        wj=ones(1,p1-2);
        rf(m,2:p1-1) = (rf(m,2:p1-1)-k(m,wj).*rf(m,p1-1:-1:2)).*d(:,wj);
        for j = p1-2:-1:2
            k(m) = rf(m,j+1);
            m(m)=abs(k)<1;
            if ~any(m), break, end
            d = (1-k(m).^2).^(-1);
            wj=ones(1,j-1);
            rf(m,2:j) = (rf(m,2:j)-k(m,wj).*rf(m,j:-1:2)).*d(:,wj);
        end
    end
end
if nargout>1
    a=ar;
    if ~all(m)
        for i=find(~m)'                 % unstable frames
            z=roots(a(i,:));
            k=abs(z)>1;                 % find any unstable roots
            z(k)=conj(z(k)).^(-1);      % invert them
            a(i,:)=real(poly(z));       % force a real polynomial
        end
    end
end


function G = grad_intrinsic(l,F)
  % GRAD_INTRINSIC Construct an intrinsic gradient operator.
  %
  % Inputs:
  %  l  #F by 3 list of edge lengths
  %  F  #F by 3 list of triangle indices into some vertex list V
  % Outputs:
  %  G  #F*2 by #V gradient matrix: G=[Gx;Gy] where x runs along the 23 edge and
  %    y runs in the counter-clockwise 90° rotation.
  %

  x = (l(:,2).^2-l(:,1).^2-l(:,3).^2)./(-2.*l(:,1));
  y = sqrt(l(:,3).^2 - x.^2);
  n = max(F(:));
  m = size(F,1);
  Z = zeros(m,1);
  V2 = [x y;Z Z;l(:,1) Z];
  F2 = reshape(1:3*m,m,3);
  G2 = grad(V2,F2);
  P = sparse(F2,F,1,m*3,n);
  G = G2*P;

end


 % Data File DTPC3
 % Dynamics of a particle, moving
 % on a horizontal plane under action
 % of a body, bound up to the particle
 % by a non elastic thread. The body can
 % move in vertical direction.
   m    =  'm';  % mass of the particle
          % Projections of forces on axis [r] and [f]
   Fr   = '-G*(1+rtt/9.81)-9.81*mu*m*rt/sqrt(rt^2+r^2*ft^2)';
   Ff   = '-9.81*mu*m*r*ft/sqrt(rt^2+r^2*ft^2)';
   r0   = 1;     % Initial radius
   f0   = 0;     % Initial polar angle
   v0   = 10;    % Initial velocity
   alfa = pi/2;  % angle between v0 and polar axis
   Tend = 5;     % upper bound of integration
   eps  = 1e-10; % desirable accuracy
   np   = 3;     % number of parameters
   P{1} = 'm';   % mass of the particle
   P{2} = 'G';   % weight of the body
   P{3} = 'mu';  % coefficient of dry friction


function [rhol,rhoul,El] = ModEuler(h,h_next,q,fr,v)
%% Global Variables
global CFL r_time theta dt dtdx nx %#ok<*NUSED>
global w k nv
global gamma etpfix

%% Compute primitive Variables @ cell center variables
% Build vector U and dU, and F
    % Define:
    L = 1:nx-1; R = 2:nx; % inner nodes / middle points
    % here U = [U1 U2 U3]' is defined for every cell center
    U = q;
        % U1 = Density, U2 = Momentum, U3 = Total Energy

   % we need to review this initial formulation!
    r = q(1,:);
    u = q(2,:)./q(1,:);
    E = q(3,:);
    p = (gamma-1).*(E-r.*u.^2/2);
    H = (E + p)./r;

%% Main Loop
% Compute Roe Averages
    % Velovity 'u'
    u_bar = (sqrt (r(L)).*u(L) + sqrt (r(R)).*u(R)) ...
        ./ (sqrt(r(L))+sqrt(r(R)));
    % Total Entalpy 'H'
    H_bar = (sqrt (r(L)).*H(L) + sqrt (r(R)).*H(R)) ...
        ./ (sqrt(r(L))+sqrt(r(R)));
    % Sound Speed 'a'
    a_bar = sqrt((gamma-1)*(H_bar-0.5*u_bar.^2));

% Compute Delta U's
% dU = U(R)-U(L) at the cell boundaries { x_{i},x_{i+1}, ... }
dU = U(:,R)-U(:,L);

% Compute Fluxes at the cell centers
% F = [F1 F2 F3]
F = [r.*u; r.*u.^2 + p; r.*u.*H];
% Fluxes to the left and right of 'F_ {i+1/2}'
FL = F(:,L); FR = F(:,R);

% Scaled Right Eigenvectors are given by:
k1_bar = [1*ones(1,nx-1); u_bar-a_bar; H_bar-u_bar.*a_bar];
k2_bar = [1*ones(1,nx-1); u_bar      ; 1/2*u_bar.^2      ];
k3_bar = [1*ones(1,nx-1); u_bar+a_bar; H_bar+u_bar.*a_bar];

% compute Roe waves strength alpha_bar
alpha2_bar = (gamma-1)./(a_bar.^2).*(dU (1,:).*(H_bar-u_bar.^2) ...
    + u_bar.*dU(2,:) - dU(3,:));
alpha1_bar = 1./(2*a_bar).*(dU (1,:).*(u_bar+a_bar) ...
    - dU(2,:)-a_bar.*alpha2_bar);
alpha3_bar = dU(1,:)-(alpha1_bar + alpha2_bar);

% Eigenvalues of A_bar are (same as the original A mat)
lambda_bar(1,:) = abs(u_bar - a_bar);
lambda_bar(2,:) = abs(u_bar);
lambda_bar(3,:) = abs(u_bar + a_bar);

% Entropy Fix
boolv1 = lambda_bar(1,:) < etpfix;
lambda_bar(1,:) = 0.5*(etpfix + lambda_bar (1,:).^2/etpfix).*boolv1 ...
    + lambda_bar(1,:).*(1-boolv1);
boolv2 = lambda_bar(3,:) < etpfix;
lambda_bar(3,:) = 0.5*(etpfix + lambda_bar (3,:).^2/etpfix).*boolv2 ...
    + lambda_bar(3,:).*(1-boolv2);

% Conditioning data
alpha1_bar  = repmat(alpha1_bar,3,1);
alpha2_bar  = repmat(alpha2_bar,3,1);
alpha3_bar  = repmat(alpha3_bar,3,1);
lambda1_bar = repmat(lambda_bar(1,:),3,1);
lambda2_bar = repmat(lambda_bar(2,:),3,1);
lambda3_bar = repmat(lambda_bar(3,:),3,1);

% Roe Fluxes
Flux = 0.5*(FL+FR)-0.5*(alpha1_bar.*lambda1_bar.*k1_bar + ...
    alpha2_bar.*lambda2_bar.*k2_bar + ...
    alpha3_bar.*lambda3_bar.*k3_bar );

%% Compute next time step
% compute difference of the integral(v*fl)
[M1,M2,M3] =integrate_vf(k,w,fr,v); M = [M1;M2;M3]; dM = M(:,R) - M(:,L);

% Remap from h(nx,nx) to h(3,nx)
hh = h(1:3,:);
hh_next = h_next(1:3,:);
dhh = hh_next - hh;

% Compute next time step
U_next = zeros(3,nx);
for i = 2 : nx-1
    U_next(:,i) = U(:,i) - dtdx.*(1-hh(:,i)).*(Flux(:,i) - Flux(:,i-1)) - ...
                           dtdx.*(1-hh(:,i)).* dM(i)- ...
                           q(:,i).*dhh(:,i);
end

% BCs
 U_next(:,1)  = U(:,2); % Neumann condition to the left
 U_next(:,nx) = U(:,nx-1); % Neumann condition to the right

% Compute variables of the new time step
rhol = U_next(1,:);     % Density
rhoul = U_next (2,:);   % Momentum
El = U_next(3,:);       % Total Energy


function [ fx, fy ] = f06_f1 ( n, x, y )

%*****************************************************************************80
%
%% F06_F1 returns first derivatives of function 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real FX(N,1), FY(N,1), the derivative values.
%
  fx = zeros ( n, 1 );
  fy = zeros ( n, 1 );

  t4(1:n,1) = 64.0 - 81.0 * ( ( x(1:n,1) - 0.5 ).^2 + ( y(1:n,1) - 0.5 ).^2 );

  i = ( 0.0 < t4(1:n,1) );

  fx(i,1) = - 9.0 * ( x(i,1) - 0.5 ) ./ sqrt ( t4(i,1) );
  fy(i,1) = - 9.0 * ( y(i,1) - 0.5 ) ./ sqrt ( t4(i,1) );

  return
end


function [C,theta] = x2majoraxis(A,B)
%X2MAJORAXIS Aligns coordinate x with the major axis of a region.
%  [C,THETA] = X2MAJORAXIS(A,B) aligns the x-coordinate axis with the
%  major axis of a region or boundary. The y-axis is perpendicular to
%  the x-axis.  The rows of 2-by-2 matrix A are the coordinates of the
%  two end points of the major axis, in the form A = [x1 y1; x2 y2].
%  Input B is either a an image of class logical containing a single
%  region, or it is an np-by-2 set of points representing a (connected)
%  boundary. In the latter case, the first column of B must represent
%  x-coordinates and the second column must represent the corresponding
%  y-coordinates. Output C contains the same data as the input, but
%  aligned with the major axis. If the input is an image, so is the
%  output; similarly the output is a sequence of coordinates if the
%  input is such a sequence. Parameter THETA is the initial angle
%  between the major axis and the x-axis. The origin of the xy-axis
%  system is at the bottom left; the x-axis is the horizontal axis and
%  the y-axis is the vertical.
%
%  Keep in mind that rotations can introduce round-off errors when the
%  data are converted to integer (pixel) coordinates, which typically is
%  a requirement.  Thus, postprocessing (e.g., with bwmorph) of the
%  output may be required to reconnect a boundary.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Preliminaries.
if islogical(B)
  type = 'region';
elseif size(B,2) == 2
  type = 'boundary';
  [M,N] = size(B);
  if M < N
    error('B is boundary. It must be of size np-by-2; np > 2.')
  end
  % Compute centroid for later use. c is a 1-by-2 vector.
  % Its 1st component is the mean of the boundary in the x-direction.
  % The second is the mean in the y-direction.
  c(1) = round((min(B(:,1)) + max(B(:,1)))/2);
  c(2) = round((min(B(:,2)) + max(B(:,2)))/2);

  % It is possible for a connected boundary to develop small breaks
  % after rotation. To prevent this, the input boundary is filled,
  % processed as a region, and then the boundary is re-extracted.
  % This guarantees that the output will be a connected boundary.
  m = max(max(B));
  % The following image is of size m-by-m to make sure that there
  % there will be no size truncation after rotation.
  B = bound2im(B,m,m);
  B = imfill(B,'holes');
else
  error('Input must be a boundary or a binary image.')
end

% Major axis in vector form.
v(1) = A(2,1) - A(1,1);
v(2) = A(2,2) - A(1,2);
v = v(:);  % v is a col vector

% Unit vector along x-axis.
u = [1; 0];

% Find angle between major axis and x-axis. The angle is
% given by acos of the inner product of u and v divided by
% the product of their norms. Because the inputs are image
% points, they are in the first quadrant.
nv = norm(v);
nu = norm(u);
theta = acos(u'*v/nv*nu);
if theta > pi/2
  theta = -(theta - pi/2);
end
theta = theta*180/pi;  % Convert angle to degrees.

% Rotate by angle theta and crop the rotated image to original size.
C = imrotate(B,theta,'bilinear','crop');

% If the input was a boundary, re-extract it.
if  strcmp(type,'boundary')
  C = bwboundaries(C);
  C = C{1};
  % Shift so that centroid of the extracted boundary is
  % approx equal to the centroid of the original boundary:
  C(:,1) = C(:,1) - (min(C(:,1)) + max(C(:,1)))/2 + c(1);
  C(:,2) = C(:,2) - (min(C(:,2)) + max(C(:,2)))/2 + c(2);
end


function b = r8sr_vxm ( n, nz, row, col, diag, off, x )

%*****************************************************************************80
%
%% R8SR_VXM multiplies a vector times a R8SR matrix.
%
%  Discussion:
%
%    The R8SR storage format stores the diagonal of a sparse matrix in DIAG.
%    The off-diagonal entries of row I are stored in entries ROW(I)
%    through ROW(I+1)-1 of OFF.  COL(J) records the column index
%    of the entry in A(J).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer NZ, the number of offdiagonal nonzero elements in A.
%
%    Input, integer ROW(N+1).  The nonzero offdiagonal elements of row I of A
%    are contained in A(ROW(I)) through A(ROW(I+1)-1).
%
%    Input, integer COL(NZ), contains the column index of the element
%    in the corresponding position in A.
%
%    Input, real DIAG(N), the diagonal elements of A.
%
%    Input, real OFF(NZ), the off-diagonal elements of A.
%
%    Input, real X(N), the vector to be multiplies by A.
%
%    Output, real B(N), the product A' * X.
%
  b(1:n) = diag(1:n) .* x(1:n);

  for i = 1 : n
    for k = row(i) : row(i+1)-1
      j = col(k);
      b(j) = b(j) + off(k) * x(i);
    end
  end

  return
end


function [ p, t ] = distmesh_2d ( fd, fh, h0, box, iteration_max, pfix, ...
  varargin )

%*****************************************************************************80
%
%% DISTMESH_2D is a 2D mesh generator using distance functions.
%
%  Example:
%
%    Uniform Mesh on Unit Circle:
%
%      fd = inline('sqrt(sum(p.^2,2))-1','p');
%      [p,t] = distmesh_2d(fd,@huniform,0.2,[-1,-1;1,1],100,[]);
%
%    Rectangle with circular hole, refined at circle boundary:
%
%      fd = inline('ddiff(drectangle(p,-1,1,-1,1),dcircle(p,0,0,0.5))','p');
%      fh = inline('min(4*sqrt(sum(p.^2,2))-1,2)','p');
%      [p,t] = distmesh_2d(fd,fh,0.05,[-1,-1;1,1],500,[-1,-1;-1,1;1,-1;1,1]);
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    09 June 2012
%
%  Author:
%
%    Per-Olof Persson
%    Modifications by John Burkardt
%
%  Reference:
%
%    Per-Olof Persson, Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, function FD, signed distance function d(x,y).
%
%    Input, function FH, scaled edge length function h(x,y).
%
%    Input, real H0, the initial edge length.
%
%    Input, real BOX(2,2), the bounding box [xmin,ymin; xmax,ymax].
%
%    Input, integer ITERATION_MAX, the maximum number of iterations.
%    The iteration might terminate sooner than this limit, if the program decides
%    that the mesh has converged.
%
%    Input, real PFIX(NFIX,2), the fixed node positions.
%
%    Input, VARARGIN, aditional parameters passed to FD and FH.
%
%    Output, real P(N,2), the node positions.
%
%    Output, integer T(NT,3), the triangle indices.
%
%  Local parameters:
%
%    Local, real GEPS, a tolerance for determining whether a point is "almost" inside
%    the region.  Setting GEPS = 0 makes this an exact test.  The program currently
%    sets it to 0.001 * H0, that is, a very small multiple of the desired side length
%    of a triangle.  GEPS is also used to determine whether a triangle falls inside
%    or outside the region.  In this case, the test is a little tighter.  The centroid
%    PMID is required to satisfy FD ( PMID ) <= -GEPS.
%
  dptol = 0.001;
  ttol = 0.1;
  Fscale = 1.2;
  deltat = 0.2;
  geps = 0.001 * h0;
  deps = sqrt ( eps ) * h0;
  iteration = 0;
  triangulation_count = 0;
%
%  1. Create the initial point distribution by generating a rectangular mesh
%  in the bounding box.
%
  [ x, y ] = meshgrid ( box(1,1) : h0           : box(2,1), ...
                        box(1,2) : h0*sqrt(3)/2 : box(2,2) );
%
%  Shift the even rows of the mesh to create a "perfect" mesh of equilateral triangles.
%  Store the X and Y coordinates together as our first estimate of "P", the mesh points
%  we want.
%
  x(2:2:end,:) = x(2:2:end,:) + h0 / 2;
  p = [ x(:), y(:) ];
%
%  Instead of a regular mesh, you can initialize P with random values here.
%
%  2. Remove mesh points that are outside the region,
%  then satisfy the density constraint.
%
%  Keep only points inside (or almost inside) the region, that is, FD(P) < GEPS.
%
  p = p( feval ( fd, p, varargin{:} ) < geps, : );
%
%  Set R0, the relative probability to keep a point, based on the mesh density function.
%
  r0 = 1 ./ feval ( fh, p, varargin{:} ).^2;
%
%  Apply the rejection method to thin out points according to the density.
%
  p = [ pfix; p(rand(size(p,1),1) < r0 ./ max ( r0 ),: ) ];
%
%  The following obscure commands are written so that:
%  * We ALWAYS keep the fixed points at the beginning of the P array.
%  * We remove any points which are duplicates of these points.
%  That way, we do not later allow the fixed points to move,
%  because we know they are at the beginning of the array,
%  and DELAUNAYN stops complaining about duplicate points.
%  JVB, 09 June 2012.
%
  [ q, i, j ] = unique ( p, 'rows', 'first' );
  k = unique ( i );
  p = p(k,:);

  N = size ( p, 1 );
%
%  If ITERATION_MAX is 0, we're almost done.
%  For just this case, do the triangulation, then exit.
%  Setting ITERATION_MAX to 0 means that we can see the initial mesh
%  before any of the improvements have been made.
%
  if ( iteration_max <= 0 )
    t = delaunayn ( p );
    triangulation_count = triangulation_count + 1;
    return
  end

  pold = inf;

  while ( iteration < iteration_max )

    iteration = iteration + 1;

    if ( mod ( iteration, 10 ) == 0 )
      fprintf ( 1, '  %d iterations, %d triangulations\n', ...
        iteration, triangulation_count );
    end
%
%  3. Retriangulation by the Delaunay algorithm.
%
%  Was there large enough movement to retriangulate?
%
%  If so, save the current positions, get the list of
%  Delaunay triangles, compute the centroids, and keep
%  the interior triangles (whose centroids are within the region).
%
    if ( ttol < max ( sqrt ( sum ( ( p - pold ).^2, 2 ) ) / h0 ) )
      N = size ( p, 1 );
      pold = p;
      t = delaunayn ( p );
      triangulation_count = triangulation_count + 1;
      pmid = ( p(t(:,1),:) + p(t(:,2),:) + p(t(:,3),:) ) / 3;
      t = t( feval ( fd, pmid, varargin{:} ) < -geps, : );
%
%  4. Describe each bar by a unique pair of nodes.
%
      bars = [ t(:,[1,2]); t(:,[1,3]); t(:,[2,3]) ];
      bars = unique ( sort ( bars, 2 ), 'rows' );
%
%  5. Graphical output of the current mesh.
%
      trimesh ( t, p(:,1), p(:,2), zeros(N,1), 'EdgeColor', 'b', 'Linewidth', 2 )
      title ( sprintf ( 'Iteration %d', iteration ) );
      view(2), axis equal, axis off, drawnow
%
%  Put a "pause" command here if you'd like to see each new mesh.
%
    end
%
%  6. Move mesh points based on bar lengths L and forces F.
%
%  Make a list of the bar vectors and lengths.
%  Set L0 to the desired lengths, F to the scalar bar forces,
%  and FVEC to the x, y components of the bar forces.
%
%  At the fixed positions, reset the force to 0.
%
    barvec = p(bars(:,1),:) - p(bars(:,2),:);
    L = sqrt ( sum ( barvec.^2, 2 ) );
    hbars = feval ( fh, (p(bars(:,1),:)+p(bars(:,2),:))/2, varargin{:} );
    L0 = hbars * Fscale * sqrt ( sum(L.^2) / sum(hbars.^2) );
    F = max ( L0 - L, 0 );
    Fvec = F ./ L * [1,1] .* barvec;
    Ftot = full ( sparse(bars(:,[1,1,2,2]),ones(size(F))*[1,2,1,2],[Fvec,-Fvec],N,2) );
    Ftot(1:size(pfix,1),:) = 0;
    p = p + deltat * Ftot;
%
%  7. Bring outside points back to the boundary.
%
%  Use the numerical gradient of FD to project points back to the boundary.
%
    d = feval ( fd, p, varargin{:} );
    ix = d > 0;
    dgradx = ( feval(fd,[p(ix,1)+deps,p(ix,2)],varargin{:}) - d(ix) ) / deps;
    dgrady = ( feval(fd,[p(ix,1),p(ix,2)+deps],varargin{:}) - d(ix) ) / deps;
    p(ix,:) = p(ix,:) - [ d(ix) .* dgradx, d(ix) .* dgrady ];
%
%  8. Termination criterion: All interior nodes move less than dptol (scaled).
%
    if ( max ( sqrt ( sum ( deltat * Ftot ( d < -geps,:).^2, 2 ) ) / h0 ) < dptol )
      break;
    end

  end

  return
end


function triangulation_order6_contour ( prefix )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER6_CONTOUR contour plots data at the nodes of a six node triangle.
%
%  Discussion:
%
%    This program can read data files defining a set of nodes, the triangulation of
%    those nodes using 6 node triangles, and the value of a scalar at the nodes.
%    It then rearranges the data to set up calls to MATLAB that display the
%    triangulation, and color contour plots of the scalar.
%
%    The MATLAB plotting routines require a 3-node triangulation.  This program
%    shows two ways to turn a 6-node triangulation into a 3-node triangulation.
%    The default MATLAB plot of data on a triangular mesh uses constant color
%    over each triangle.  After displaying these plots, the program shows how to
%    get a much nicer plot using interpolated color.
%
%  Usage:
%
%    triangulation_order6_contour ( 'prefix' )
%
%    where
%
%    * 'prefix'_nodes.txt contains the node coordinates;
%    * 'prefix'_elements.txt contains the element definitions.
%    * 'prefix'_values.txt contains the nodal values
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string PREFIX, the common file prefix.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_ORDER6_CONTOUR:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot a scalar defined on a 6-node triangle triangulation.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This program expects to find three files to read:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  * "nodes.txt",     the node file,\n' );
  fprintf ( 1, '  * "elements.txt",  the element file,\n' );
  fprintf ( 1, '  * "solution.txt",  the solution file,\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  It reads the files, and makes two plots of the solution.\n' );
%
%  The command line argument is the common filename prefix.
%
  if ( nargin < 1 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER6_CONTOUR:\n' );

    prefix = input ( ...
      'Please enter the filename prefix:' );

  end
%
%  Create the filenames.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
  value_filename = strcat ( prefix, '_values.txt' );
%
%  Read the node data.
%
  [ dim_num, node_num ] = r8mat_header_read ( node_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".', node_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  Number of points NODE_NUM = %d\n', node_num );

  if ( dim_num ~= 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER6_CONTOUR - Fatal error!\n' );
    fprintf ( 1, '  Dataset must have spatial dimension 2.\n' );
    error ( 'TRIANGULATION_ORDER6_CONTOUR - Fatal error!' );
  end

  node_xy = r8mat_data_read ( node_filename, dim_num, node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', node_filename );

  r8mat_transpose_print_some ( dim_num, node_num, node_xy, 1, 1, dim_num, 5, ...
    '  First 5 nodes:' );
%
%  Read the element data.
%
  [ triangle_order, triangle_num ] = i4mat_header_read ( ...
    element_filename );

  if ( triangle_order ~= 3 && triangle_order ~= 6 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER6_CONTOUR - Fatal error!\n' );
    fprintf ( 1, '  Data is not for a 3-node or 6-node triangulation.\n' );
    error ( 'TRIANGULATION_ORDER6_CONTOUR - Fatal error!' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', ...
    element_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Triangle order = %d\n', triangle_order );
  fprintf ( 1, '  Number of triangles TRIANGLE_NUM  = %d\n', ...
    triangle_num );

  triangle_node = i4mat_data_read ( element_filename, ...
    triangle_order, triangle_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', element_filename );

  i4mat_transpose_print_some ( triangle_order, triangle_num, ...
    triangle_node, 1, 1, triangle_order, 10, ...
    '  First 10 elements:' );
%
%  Detect and correct 0-based indexing.
%
  triangle_node = mesh_base_one ( node_num, triangle_order, triangle_num, ...
    triangle_node );
%
%  Read the values.
%
  [ value_dim, value_num ] = r8mat_header_read ( value_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".', value_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', value_dim );
  fprintf ( 1, '  Number of values  = %d\n', value_num );

  if ( value_dim ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER6_CONTOUR - Fatal error!\n' );
    fprintf ( 1, '  VALUE data must be scalar.\n' );
    error ( 'TRIANGULATION_ORDER6_CONTOUR - Fatal error!' );
  end

  value = r8mat_data_read ( value_filename, value_dim, value_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', value_filename );

  r8mat_transpose_print_some ( value_dim, value_num, value, 1, 1, value_dim, 5, ...
    '  First 5 values:' );
%
%  Make a 3-node triangulation by discarding the midside nodes.
%
  t2 = triangle_node(1:3,:)';
%
%  Display the mesh.
%
  trimesh ( t2, node_xy(1,:), node_xy(2,:), 'Color', 'blue' );

  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Dropping midside nodes', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Press return...\n' );
  pause
%
%  Display the solution on this crude mesh.
%
  trisurf ( t2, node_xy(1,:), node_xy(2,:), value )

  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  zlabel ( 'U', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Contours of U(X,Y)', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Press return...\n' );
  pause
%
%  Make a 3-node triangulation by breaking each 6 node triangle
%  into 4 3-node triangles.
%
  t3 = triangulation_order6_to_order3 ( triangle_num, triangle_node );
  t3 = t3';
%
%  Display the mesh.
%
  trimesh ( t3, node_xy(1,:), node_xy(2,:), 'Color', 'blue' );

  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Splitting 6-Node Triangles', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Press return...\n' );
  pause
%
%  Display the solution on this finer mesh.
%
  trisurf ( t3, node_xy(1,:), node_xy(2,:), value );
  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  zlabel ( 'U', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Contours of U(X,Y)', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Press return...\n' );
  pause
%
%  Make a nicer plot on the finer mesh by using color interpolation.
%
  trisurf ( t3, node_xy(1,:), node_xy(2,:), value, 'FaceColor', 'interp', ...
    'EdgeColor', 'interp' )

  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  zlabel ( 'U', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Contours of U(X,Y)', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Press return...\n' );
  pause
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_ORDER6_CONTOUR:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function table = i4mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% I4MAT_DATA_READ reads data from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns in the data.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %d' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the input file.\n' );
    error ( 'I4MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
      fprintf ( 1, '  End of input while reading data.\n' );
      error ( 'I4MAT_DATA_READ - Error!' );
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = i4mat_header_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_HEADER_READ reads the header from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 10;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function element_node = mesh_base_one ( node_num, element_order, ...
  element_num, element_node )

%*****************************************************************************80
%
%% MESH_BASE_ONE ensures that the element definition is one-based.
%
%  Discussion:
%
%    The ELEMENT_NODE array contains nodes indices that form elements.
%    The convention for node indexing might start at 0 or at 1.
%    Since a MATLAB program will naturally assume a 1-based indexing, it is
%    necessary to check a given element definition and, if it is actually
%    0-based, to convert it.
%
%    This function attempts to detect 0-based node indexing and correct it.
%
%    Thanks to Feifei Xu for pointing out that I was subtracting 1 when I
%    should have been adding 1!  29 November 2012.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input/output, integer ELEMENT_NODE(ELEMENT_ORDE,ELEMENT_NUM), the element
%    definitions.
%
  node_min = min ( min ( element_node(1:element_order,1:element_num) ) );
  node_max = max ( max ( element_node(1:element_order,1:element_num) ) );

  if ( node_min == 0 && node_max == node_num - 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 0-based!\n' );
    fprintf ( 1, '  This will be converted to 1-based.\n' );
    element_node(1:element_order,1:element_num) = ...
      element_node(1:element_order,1:element_num) + 1;
  elseif ( node_min == 1 && node_max == node_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 1-based!\n' );
    fprintf ( 1, '  No conversion is necessary.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE - Warning!\n' );
    fprintf ( 1, '  The element indexing is not of a recognized type.\n' );
    fprintf ( 1, '  NODE_MIN = %d\n', node_min );
    fprintf ( 1, '  NODE_MAX = %d\n', node_max );
    fprintf ( 1, '  NODE_NUM = %d\n', node_num );
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 5;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

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

  return
end
function triangle2 = triangulation_order6_to_order3 ( tri_num1, triangle1 )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER6_TO_ORDER3 linearizes a quadratic triangulation.
%
%  Discussion:
%
%    A quadratic triangulation is assumed to consist of 6-node triangles,
%    as in the following:
%
%    11-12-13-14-15
%     |\    |\    |
%     | \   | \   |
%     6  7  8  9 10
%     |   \ |   \ |
%     |    \|    \|
%     1--2--3--4--5
%
%   This routine rearranges information so as to define the 3-node
%   triangulation:
%
%    11-12-13-14-15
%     |\ |\ |\ |\ |
%     | \| \| \| \|
%     6--7--8--9-10
%     |\ |\ |\ |\ |
%     | \| \| \| \|
%     1--2--3--4--5
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TRI_NUM1, the number of triangles in the quadratic
%    triangulation.
%
%    Input, integer TRIANGLE1(6,TRI_NUM1), the quadratic triangulation.
%
%    Output, integer TRIANGLE2(3,4*TRI_NUM1), the linear triangulation.
%
  tri2 = 0;

  triangle2 = zeros ( 3, 4 * tri_num );

  for tri1 = 1 : tri_num1

    n1 = triangle1(1,tri1);
    n2 = triangle1(2,tri1);
    n3 = triangle1(3,tri1);
    n4 = triangle1(4,tri1);
    n5 = triangle1(5,tri1);
    n6 = triangle1(6,tri1);

    tri2 = tri2 + 1;
    triangle2(1:3,tri2) = [ n1, n4, n6 ]';
    tri2 = tri2 + 1;
    triangle2(1:3,tri2) = [ n2, n5, n4 ]';
    tri2 = tri2 + 1;
    triangle2(1:3,tri2) = [ n3, n6, n5 ]';
    tri2 = tri2 + 1;
    triangle2(1:3,tri2) = [ n4, n5, n6 ]';

  end

  return
end


function bound = compute_shape_boundary(M)

% compute_shape_boundary - extract boundary points
%
% bound = compute_shape_boundary(M);
%
%   bound is oriented counter clockwise.
%   This is the 8 connectivity boundary.
%
%   Copyright (c) 2007 Gabriel Peyre

n = size(M,1);

% enforce only 1 connected component
epsilon = 1e-10;
options.nb_iter_max = Inf;
options.Tmax = 2*n;
W = epsilon+M;
I = find(M(:)==1); x = linspace(-1,1,n);
[Y,X] = meshgrid(x,x);
[tmp,c] = min( X(I).^2 + Y(I).^2 );
[a b] = ind2sub([n n],I(c));
options.constraint_map = [];
[d,tmp] = perform_fast_marching(W, [a;b], options);
I = find(d>1e5 | d<0); M(I) = 0;

% points on the boundary
if 0
    % outside the shape
    h = [0 1 0; 1 1 1; 0 1 0];
    Mh = perform_convolution(M,h);
    A = Mh>0 & M==0;
else
    % inside the shape
    h = ones(3);
    Mh = perform_convolution(M,h);
    A = Mh>0 & Mh<9 & M==1;
end

% 8 connectivity
wx = [1 -1 0  0 -1 1 -1  1];
wy = [0  0 1 -1 -1 1  1 -1];

% order the points
a = find(A(:)==1); a = a(1);
[x,y] = ind2sub(size(A),a);

boundx = x;
boundy = y;
A(x,y) = 0;

while sum(A(:))>0
    % find closest point
    for k=1:8
        if x+wx(k)>0 && x+wx(k)<=n && y+wy(k)>0 && y+wy(k)<=n && A(x+wx(k),y+wy(k))>0
            x = x+wx(k);
            y = y+wy(k);
            break;
        end
    end
    if A(x,y)==0
        % try to find the existing point the closest to the last one
        I = find(A>0);
        [xlist,ylist] = ind2sub(size(A),I);
        d = (xlist-x).^2 + (ylist-y).^2;
        [tmp,I] = min(d(:));
        x = xlist(I(1)); y = ylist(I(1));
    end
    A(x,y) = A(x,y)-1;
    boundx(end+1) = x;
    boundy(end+1) = y;
end
bound = cat(2,boundx(:), boundy(:));

% reorient the curve so that it is clockwise oriented
p = size(bound,1);
c = mean(bound,1);
v = bound - repmat(c, [p 1]);
% cross product
z = v(1:end-1,1).*v(2:end,2) - v(1:end-1,2).*v(2:end,1);
if sum(z<0)>sum(z<0)
    % should flip the curve
    bound = bound(end:-1:1,:);
end


function [seg, SEG_l] = idpLinSegment(l,t)

% IDPLINSEGMENT  IDP line endpoints.
%   IDPLINSEGMENT(L,T) returns the 3D segment corresponding to the IDP line
%   L at abscissas T = [t1;t2].
%
%   [s,S_l] = IDPLINSEGMENT(...) returns the Jacobian wrt L.
%
%   See also IDPLINENDPOINTS, IDPLIN2SEG, IDPLIN2IDPPNTS.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% abscissas
t1 = t(1);
t2 = t(2);

% support 3d segment
[s, S_l] = idpLin2seg(l);

% support points
p1 = s(1:3);
p2 = s(4:6);

P1_l = S_l(1:3,:);
P2_l = S_l(4:6,:);

% 3d endpoints
e1 = (1-t1)*p1 + t1*p2;
e2 = (1-t2)*p1 + t2*p2;

E1_p1 = 1-t1;
E1_p2 = t1;
E2_p1 = 1-t2;
E2_p2 = t2;

E1_l = E1_p1*P1_l + E1_p2*P2_l;
E2_l = E2_p1*P1_l + E2_p2*P2_l;

% 3d segment
seg   = [e1;e2];
SEG_l = [E1_l;E2_l];

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function q = p06_q ( m, c, w )

%*****************************************************************************80
%
%% P06_Q evaluates the integral for problem p06.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    Reidel, 1987, pages 337-340,
%    ISBN: 9027725144,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer M, the dimension of the argument.
%
%    Input, real C(M), W(M), the problem parameters.
%
%    Output, real Q, the integral.
%

%
%  To simplify the calculation, force W(3:M) to be at least 1.0.
%
  w(3:m) = 1.0;

  q = 1.0;

  for i = 1 : m

    if ( w(i) <= 0.0 )

      q = q * 0.0;

    elseif ( w(i) <= 1.0 )

      if ( c(i) == 0.0 )
        q = q * w(i);
      else
        q = q * ( exp ( c(i) * w(i) ) - 1.0 ) / c(i);
      end

    else

      if ( c(i) ~= 0.0 )
        q = q * ( exp ( c(i) * w(i) ) - 1.0 ) / c(i);
      end

    end

  end

  return
end