function [bandwidth pm] = pll_simulation(cap, res, ipump, vco_sensitivity, fout, fcomp )

% Simulates 2nd, 3rd and 4th order PLL loops for the topologies shown
% below using basic control systems theory.  This is useful for design
% verification.
%
% Input
%       - cap is the capacitors of the loop in Farads [C1, C2, C3, C4].  If
%           these components are not present in the loop, set their value
%           to zero.
%       - res is the resistors of the  loop in Ohms [ R2, R3, R4].  If
%           these components are not present in the loop, set their value to zero.
%       - order of the loop, either 2,3, or 4.
%       - ipump is the charge pump current in Amperes
%       - vco_sensitivity is the VCO sensitivity in Hertz/Volt
%       - fout is the output frequency in Hertz
%       - fcomp is the comparison frequency in Hertz
%
% Output
%       - bandwidth is the open loop bandwidth in Hertz
%       - pm is the phase margin in degrees
%
% The methods used here are derived from those presented in Dean Banerjee's
% Book "PLL Performance, Simulation, and Design" 4th Ed available at
% National Semiconductors site www.national.com.  Most of the work is
% derived from Chapter 8 pp.43-47 and Chapter 9 pp. 48-53.
%
%
% Loop Topologies
% 2nd Order
%        +  _____                                   ______
% fcomp -->|Phase|----------------------------------| VCO |---->fout
%          |Det. |     |      |                     |     |  |
%           -----      |      |                      -----   |
%            ^ -      C1     R2                              |
%            |         |      |                              |
%            |        GND    C2                              |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
% 3rd Order
%        +  _____                                   ______
% fcomp -->|Phase|-----------------R3---------------| VCO |---->fout
%          |Det. |     |      |         |           |     |  |
%           -----      |      |         |            -----   |
%            ^ -      C1     R2        C3                    |
%            |         |      |         |                    |
%            |        GND    C2        GND                   |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
%
% 4th Order
%        +  _____                                   ______
% fcomp -->|Phase|-----------------R3------R4-------| VCO |---->fout
%          |Det. |     |      |         |     |     |     |  |
%           -----      |      |         |     |      -----   |
%            ^ -      C1     R2        C3     C4             |
%            |         |      |         |     |              |
%            |        GND    C2        GND   GND             |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
%
%   Author: Ben Gilbert
%   Homepage: http://nicta.com.au/people/gilbertb
%   Email: ben.gilbert (wibble) nicta.com.au
%   (c) 2009 by National ICT Australia (NICTA)
%

%% Setup Parameters
C1 = cap(1);
C2 = cap(2);
C3 = cap(3);
C4 = cap(4);

R2 = res(1);
R3 = res(2);
R4 = res(3);

% Conversion of parameters to more convenient units
Kpd = ipump/2/pi; % phase detector gain
Kvco = vco_sensitivity*2*pi; % vco gain

%% Plot Setup
% Generates logarithmic spaced points for the calculations
fplotstart = 10; % Hz
fplotstop = 10E6; % Hz
plotpoints = 100;
pltfreqs = [];
for ppts=0:plotpoints
    pltfreqs = [ pltfreqs fplotstart * 10 ^ (ppts/plotpoints* log10(fplotstop/fplotstart)) ];
end

%% Loop Poll's Polynomial Coefficients

A0 = C1 + C2 + C3 + C4;
A1 = C2*R2*(C1+C3+C4) + R3*(C1+C2)*(C3+C4) + C4*R4*(C1+C2+C3);
A2 = C1*C2*R2*R3*(C3+C4) + C4*R4*(C2*C3*R3+C1*C3*R3+C1*C2*R2+C2*C3*R2);
A3 = C1*C2*C3*C4*R2*R3*R4;

T2 = R2*C2;

%% Filter Transfer Function
Zfilt = @(s)  (1 + s.*T2)./s./(A3.*s.^3 + A2.*s.^2 + A1.*s + A0); % filter transfer function
zfilt = @(f) Zfilt(2*pi*1i*f); % filter transfer function as a function of frequency

%% VCO Transfer Function
Gvco = @(s) Kvco./s; % vco transfer function
gvco = @(f) Gvco(2*pi*1i*f); % vco transfer function expressed as a function of frequency

%% Forward Path Transfer Function
G = @(s)  Kpd * Gvco(s) .* Zfilt(s); % forward path transfer function
%% Reverse (Feedback) Transfer Function
H = fcomp/fout; % feedback path transfer function
%% Open Loop Transfer Function
GH = @(s) G(s)*H; % open loop transfer function
gh = @(f) GH(2*pi*1i*f); % open loop transfer function expressed as a function of frequency

%% Gain Plots
% Plots of transfer function magnitudes
% figure;
% loglog( ...
%     pltfreqs, (abs(gh(pltfreqs))), ...
%     pltfreqs, (abs(zfilt(pltfreqs))), ...
%     pltfreqs, (abs(gvco(pltfreqs))) ...
% );
% grid on; title('Open Loop Gain and Contributing Factors');
% xlabel('Frequency [Hz]');
% legend('Open Loop Gain','Loop Filter','VCO', 'Location', 'SouthWest');

%% Bode Plot
figure;
    subplot(2,1,1);
        semilogx(pltfreqs, 20*log10(abs(gh(pltfreqs))));
        grid on;
        title('Open Loop Magnitude');
    subplot(2,1,2);
        semilogx(pltfreqs, angle(gh(pltfreqs)).*180/pi);
        grid on;
        title('Open Loop Phase');
        ylim([-180 180]);

%% Find open loop bandwidth and phase margin
ghdB = @(f) 20*log10(abs(gh(f)));
bandwidth = fzero(ghdB, [fplotstart fplotstop]); % find bandwidth numerically
pm = 180 + angle(gh(bandwidth)).*180/pi;

%% Closed Loop Transfer Function
Gcl = @(s) GH(s)./(1 + GH(s)); % closed loop transfer function
gcl = @(f) Gcl(2*pi*1i*f);
%% Closed Loop Plot (Magnitude)
% figure;
%     subplot(2,1,1);
%         semilogx(pltfreqs, 20*log10(abs(gcl(pltfreqs))));
%         grid on;
%         title('Closed Loop Magnitude');
%     subplot(2,1,2);
%         semilogx(pltfreqs, angle(gcl(pltfreqs)).*180/pi);
%         grid on;
%         title('Closed Loop Phase');
%         ylim([-180 180]);


function jed = ymdf_to_jed_saka ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_SAKA converts a Saka YMDF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm E,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 323-324.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding JED.
%

%
%  Convert the calendar date to a computational date.
%
  y_prime = y + 4794 - floor ( ( 13 - m ) / 12 );
  m_prime = mod ( m + 10, 12 );
  d_prime = d - 1;
%
%  Convert the computational date to a JED.
%
  j1 = floor ( ( 1461 * y_prime ) / 4 );

  z = floor ( m_prime / 6 );

  j2 = ( 31 - z ) * m_prime + 5 * z;

  g = floor ( ( y_prime + 184 ) / 100 );
  g = floor ( ( 3 * g ) / 4 ) - 36;

  jed = j1 + j2 + d_prime - 1348 - g - 0.5 + f;

  return
end


meas_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',2,'Mapping', [1,2], 'MeasurementErrVariance', 1);
H = meas_model.matrix();
R = meas_model.covar();
measurements = [10 15;10 15];

dist = GaussianDistributionX([1;1], 10*eye(2));

numParticles = 1000;
particles = dist.random(numParticles);
weights = repmat(1/numParticles,1,numParticles);

xPred = dist.Mean;
PPred=dist.Covar;
[yPred, S, K] = KalmanFilterX.predictMeasurement_(xPred,PPred,H,R);

lik = meas_model.pdf(measurements,particles);
l = meas_model.pdf(measurements,xPred,PPred);

% Data assoc
lambda = 1/10000;
Pd = 0.9;
hypothesiser = EfficientHypothesisManagementX();

W = hypothesiser.hypothesise([lambda*(1-Pd); sum(Pd*lik,2)]');
% W = lambda*(1-Pd);
% W = [W Pd*sum(l)];
% W = W./sum(W);
%
% Wp = lambda*(1-Pd);
% Wp = [Wp Pd*sum(lik)];
% Wp = Wp./sum(Wp);
%W = [0.009 1-0.009];

%[xPost, PPost] = KalmanFilterX.update_(xPred,dist.Covar,measurements,yPred, S, K);
[xPost, PPost] = KalmanFilterX.updatePDA_(xPred,PPred,measurements,W,yPred, S, K);

[newWeights] = ParticleFilterX_UpdatePDA(@(y,x)meas_model.pdf(y,x),measurements,particles,weights,W,lik);
XPost = particles*newWeights';
resampler = SystematicResamplerX();
new_weights = weights.*lik;
%new_weights =  W(1)*dist.Weights + sum(W(2:end).*lik.*dist.Weights,1);
new_weights = new_weights./sum(new_weights);
[new_particles, new_weights] = resampler.resample(particles,new_weights);

new_dist = ParticleDistributionX(new_particles, new_weights);

% [X,Y] = meshgrid(-3:0.1:5,0:0.1:8);
% Z = dist.pdf([X;Y]);
% for i=1:size(X,1)
%     Z(i,:) = dist.pdf([X;Y]);
% end

figure;
hold on;
[bandwidth,Z,X,Y]=kde2d([particles, measurements+meas_model.random(numParticles)]');
%contour3(X,Y,density,50);
h = surf(X,Y,Z);
shading interp
colormap(jet(3000))
[bandwidth,Z,X,Y]=kde2d([new_dist.Particles, measurements+meas_model.random(numParticles)]');
%contour3(X,Y,density,50);
h = surf(X,Y,Z);
shading interp
colormap(jet(3000))
plot(measurements(1,:),measurements(2,:),'r*');
plot_gaussian_ellipsoid(dist.Particles*dist.Weights',weightedcov(dist.Particles,dist.Weights));
plot_gaussian_ellipsoid(new_dist.Particles*new_dist.Weights',weightedcov(new_dist.Particles,new_dist.Weights));


function asa047_test01 ( )

%*****************************************************************************80
%
%% ASA047_TEST01 demonstrates the use of NELMIN on ROSENBROCK.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2008
%
%  Author:
%
%    John Burkardt
%
  n = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA047_TEST01\n' );
  fprintf ( 1, '  Apply NELMIN to ROSENBROCK function.\n' );

  start(1:n) = [ -1.2; 1.0 ];
  reqmin = 1.0E-08;
  step(1:n) = 1.0;
  konvge = 10;
  kcount = 500;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Starting point X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', start(i) );
  end

  ynewlo = rosenbrock ( start );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X) = %f\n', ynewlo );

  [ xmin, ynewlo, icount, numres, ifault ] = nelmin ( @rosenbrock, n, start, ...
  reqmin, step, konvge, kcount );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Return code IFAULT = %d\n', ifault );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate of minimizing value X*:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %f\n', xmin(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %f\n', ynewlo );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', icount );
  fprintf ( 1, '  Number of restarts =   %d\n', numres );

  return
end


function varargout=sde_decorrelate(d,r2)
%SDE_DECORRELATE  Decorrelate correlated values.
%   R1 = SDE_DECORRELATE(D, R2) returns the matrix R1 of M decorrelated values
%   given the N-by-N diffusion matrix D and the M-by-N matrix of (correlated)
%   values R2. The first column of R1 is always SQRT(D(1,1)) divided by the
%   first column of R2.
%
%   [R1, C] = SDE_DECORRELATE(D, R2) also returns the N-by-N correlation matrix,
%   C, decorrelated from the diffusion matrix, D.
%
%   C = SDE_DECORRELATE(D) without a second input argument returns the N-by-N
%   correlation matrix, C, decorrelated from the diffusion matrix, D.
%
%   Note:
%       R1 = SDE_DECORRELATE(D, SDE_CORRELATE(C, R1)) is only accurate to within
%       the floating point machine precision, EPS.
%
%   Example:
%       % Correlate and then decorrelate normally-distributed samples
%       r1 = randn(5,3)
%       c = [1 0.8 0.2;0.8 1 0.5;0.2 0.5 1];
%       [r2, d] = sde_correlate(c,r1)
%       r3 = sde_decorrelate(d,r2)
%
%   See also: SDE_CORRELATE, RAND, RANDN, RANDSTREAM, RANDSTREAM/RANDN, EPS

%   Andrew D. Horchler, horchler @ gmail . com, Created 5-20-13
%   Revision: 1.2, 7-12-13

if nargout > min(nargin,2)
    error('SDETools:sde_decorrelate:TooManyOutputs',...
          'Too many output arguments for number of supplied inputs.');
end

if isempty(d) || isempty(r2)
    if nargin == 1
        varargout{1} = [];
    else
        varargout{2} = [];
    end
else
    [m,n] = size(d);
    if ndims(d) ~= 2 || m ~= n              %#ok<ISMAT>
        error('SDETools:sde_decorrelate:NonSquareMatrix',...
              'The diffusion matrix must be square.');
    end

    if isscalar(d)
        c = d^2;
        isDiag = false;
	elseif sde_isdiag(d)
        d = diag(d);
        c = diag(d.^2);
        isDiag = true;
    else
        c = d^2;
        isDiag = false;
    end

    if nargin == 1
        varargout{1} = c;
    else
        if ndims(r2) ~= 2 || size(r2,2) ~= m	%#ok<ISMAT>
            error('SDETools:sde_decorrelate:DimensionMismatch',...
                 ['The number of columns in the matrix of normally '...
                  'distributed values must equal the dimension of the '...
                  'correlation matrix.']);
        end

        if isDiag
            varargout{1} = bsxfun(@mrdivide,r2,d);
        else
            varargout{1} = r2/d;
        end
        if nargout == 2
            varargout{2} = c;
        end
    end
end


function [E]=gramSchmidtOrtho(E)

k=size(E,1);
for i=1:1:k
    E(i,:)=E(i,:)./norm(E(i,:));
    for j=i+1:1:k
        Ei=E(i,:);
        Ej=E(j,:);
        proj_ei_ej=dot(Ei,Ej).*Ei./norm(Ei);
        E(j,:)=E(j,:)-proj_ei_ej;
    end
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function f = oka1( x )

	x1p = cos(pi./12.0).*x(:,1) - sin(pi./12.0).*x(:,2);
	x2p = sin(pi./12.0).*x(:,1) + cos(pi./12.0).*x(:,2);

	f(:,1) = x1p;
	f(:,2) = sqrt(2*pi) - sqrt(abs(x1p)) + 2 .* (abs(x2p-3.*cos(x1p)-3).^0.33333333);
end


function [spec,freqs,A]=parafrep(Rx,N,method,q);
% [spec,freqs]=parafrep(Rx,N,method,q) parametric frequency representation
% of a signal.
%
% Rx     : correlation matrix of size (p+1)x(p+1)
% N      : number of frequency bins between 0 and 0.5
% method : can be either 'ar', 'periodogram', 'capon', 'capnorm', 'lagunas',
%          or 'genlag'.
% q      : parameter for the generalized Lagunas method.
%
% noise=rand(1000,1); signal=filter([1 0 0],[1 1 1],noise);
% figure(1);parafrep(correlmx(signal,2,'hermitian'),128,'AR');title('AR (2)');
% figure(2);parafrep(correlmx(signal,4,'hermitian'),128,'Capon');title('Capon (4)');
% figure(3);parafrep(correlmx(signal,2,'hermitian'),128,'lagunas');title('Lagunas (2)');
% figure(4);parafrep(correlmx(signal,40,'hermitian'),128,'periodogram');title('periodogram (40)');

% F. Auger, july 1998, april 99.

if nargin<1,
 error('At least one parameter required');
elseif nargin==1,
 N=128; method='capon';
elseif nargin==2,
 method='capon';
end;

[Rxrow,Rxcol]=size(Rx);
if (Rxrow ~= Rxcol),
 error('Rx must be a square matrix');
end;

p=Rxrow-1;
freqs=linspace(0,0.5,N);
spec=zeros(N,1);

method=upper(method);
if strcmp(method,'AR'),
 Un=ones(Rxrow,1); Rxm1Un= (Rx\Un);
 P1=real(Un'*Rxm1Un); A=Rxm1Un/P1;
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=P1 ./ abs(Z' * A)^2 ;
 end;
elseif strcmp(method,'PERIODOGRAM'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=real(Z' * Rx *Z)/(p+1)^2;
 end;
elseif strcmp(method,'CAPON'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=1.0 / real(Z' * (Rx\Z));
 end;
elseif strcmp(method,'CAPNORM'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  spec(ifreq)=(p+1) / real(Z' * (Rx\Z));
 end;
elseif strcmp(method,'LAGUNAS'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  Rxm1Z=Rx\Z; spec(ifreq)=real(Z' * Rxm1Z)/real(Z' * (Rx\Rxm1Z));
 end;
elseif strcmp(method,'GENLAG'),
 for ifreq=1:N,
  Z=exp(2j*pi*freqs(ifreq)*(0:Rxrow-1)');
  Rxqm1Z=(Rx)^q \Z; spec(ifreq)=real(Z' * Rx * Rxqm1Z)/real(Z' * (Rx\Rxqm1Z));
 end;
else
 error('unknown frequency representation');
end;

if (nargout==0),
 figure(gcf); plot(freqs,10.0*log10(spec)); grid;
 xlabel('normalized frequency');
 ylabel('DSP  (dB)');
end;

%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA


%function [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(data,n_perm,tail,alpha_level,mu,reports,seed_state)
%
% mult_comp_perm_t1-One sample/paired sample permutation test based on a
% t-statistic.  This function can perform the test on one variable or
% simultaneously on multiple variables.  When applying the test to multiple
% variables, the "tmax" method is used for adjusting the p-values of each
% variable for multiple comparisons (Blair & Karnisky, 1993).  Like
% Bonferroni correction, this method adjusts p-values in a way that controls
% the family-wise error rate.  However, the permutation method will be more
% powerful than Bonferroni correction when different variables in the test
% are correlated.
%
% Required Input:
%  data   - 2D matrix of data (Observation x Variable)
%
% Optional Inputs:
%  n_perm      - Number of permutations used to estimate the distribution of
%                the null hypothesis. If the number of observations is less
%                than or equal to 12, all possible permutations are used and
%                this optional input has no effect.  If the number of
%                observations is greater than 12, n_perm specifies the
%                number of random permutations computed. Manly (1997)
%                suggests using at least 1000 permutations for an alpha level
%                of 0.05 and at least 5000 permutations for an alpha level of
%                0.01. {default=5000}
%  alpha_level - Desired family-wise alpha level. Note, because of the finite
%                number of possible permutations, the exact desired family-wise
%                alpha may not be possible. Thus, the closest approximation
%                is used and output as est_alpha. {default=.05}
%  tail        - [1, 0, or -1] If tail=1, the alternative hypothesis is that the
%                mean of the data is greater than 0 (upper tailed test).  If tail=0,
%                the alternative hypothesis is that the mean of the data is different
%                than 0 (two tailed test).  If tail=-1, the alternative hypothesis
%                is that the mean of the data is less than 0 (lower tailed test).
%                {default: 0}
%  mu          - The mean of the null hypothesis.  Must be a scalar or
%                1 x n vector (where n=the number of variables). {default: 0}
%  reports     - [0 or 1] If 0, function proceeds with no command line
%                reports. Otherwise, function reports what it is doing to
%                the command line. {default: 1}
%  seed_state  - The initial state of the random number generating stream
%                (see MATLAB documentation for "randstream"). If you pass
%                a value from a previous run of this function, it should
%                reproduce the exact same values. Note, this input only has
%                an effect if you have more than 12 observations.
%
% Outputs:
%  pval       - p-value (adjusted for multiple comparisons) of each
%               variable
%  t_orig     - t-score for each variable
%  crit_t     - Lower and upper critical t-scores for given alpha level.
%               t-scores that exceed these values significantly deviate from
%               the null hypothesis.  For upper tailed tests, the lower
%               critical t-score is NaN. The opposite is true of lower
%               tailed tests.
%  est_alpha  - The estimated family-wise alpha level of the test.  With
%               permutation tests, a finite number of p-values are possible.
%               This function tries to use an alpha level that is as close
%               as possible to the desired alpha level.  However, if the
%               sample size is small, a very limited number of p-values are
%               possible and the desired family-wise alpha level may be
%               impossible to approximately achieve.
%  seed_state - The initial state of the random number generating stream
%               (see MATLAB documentation for "randstream") used to
%               generate the permutations. You can use this to reproduce
%               the output of this function.  If the number of observations
%               is less than or equal to 12, seed_state='exact' since
%               all possible permutations are used in lieu of random
%               permutations.
%
% Note:
% -Unlike a parametric test (e.g., an ANOVA), a discrete set of p-values
% are possible (at most the number of possible permutations).  Since the
% number of possible permutations grows exponentially with the number of
% participants, this is only an issue for small sample sizes (e.g., 6
% participants).  When you have such a small sample size, the
% limited number of p-values may make the test overly conservative (e.g.,
% you might be forced to use an alpha level of .0286 since it is the biggest
% possible alpha level less than .05).
%
% -The null hypothesis of the permutation test is that the data come from a
% distribution that is symmetric around the mean of the null hypothesis.
% This is probably a generally reasonable assumption for
% paired-sample/repeated measures tests, but it might not be appropriate for
% one-sample tests.
%
%
% One Sample Example:
% >> data=randn(16,5);  %5 variables, 16 observations
% >> data(:,1:2)=data(:,1:2)+1; %mean of first two variables is 1
% >> [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(data,50000);
% >> disp(pval); %adjusted p-values
%
% Paired-Sample/Repated Measures Example:
% >> dataA=randn(16,5);  %data from Condition A (5 variables, 16 observations)
% >> dataA(:,1:2)=dataA(:,1:2)+1; %mean of first two variables is 1
% >> dataB=randn(16,5); %data from Condition B (all variables have mean of 0)
% >> dif=dataA-dataB; %difference between conditions
% >> [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(dif,50000);
% >> disp(pval); %adjusted p-values
%
% References:
% Blair, R.C. & Karniski, W. (1993) An alternative method for significance
% testing of waveform difference potentials. Psychophysiology.
%
% Manly, B.F.J. (1997) Randomization, Bootstrap, and Monte Carlo Methods in
% Biology. 2nd ed. Chapman and Hall, London.
%
%
%
% For a review on permutation tests and other contemporary techniques for
% correcting for multiple comparisons see:
%
%   Groppe, D.M., Urbach, T.P., & Kutas, M. (2011) Mass univariate analysis
% of event-related brain potentials/fields I: A critical tutorial review.
% Psychophysiology, 48(12) pp. 1711-1725, DOI: 10.1111/j.1469-8986.2011.01273.x
% http://www.cogsci.ucsd.edu/~dgroppe/PUBLICATIONS/mass_uni_preprint1.pdf
%
%
% Author:
% David Groppe
% Dec, 2010
% Kutaslab, San Diego
%

function [pval, t_orig, crit_t, est_alpha, seed_state]=mult_comp_perm_t1(data,n_perm,tail,alpha_level,mu,reports,seed_state)

if nargin<1,
    error('You need to provide data.');
end

if nargin<2,
    n_perm=5000;
end

if nargin<3,
    tail=0;
elseif (tail~=0) && (tail~=1) && (tail~=-1),
    error('Argument ''tail'' needs to be 0,1, or -1.');
end

if nargin<4,
    alpha_level=0.05;
elseif (alpha_level>=1) || (alpha_level<=0),
   error('Argument ''alpha_level'' needs to be a number between 0 and 1.');
end

if nargin<5,
    mu=0;
end

if nargin<6,
    reports=1;
end

defaultStream=RandStream.getDefaultStream; %random # generator state
if (nargin<7) || isempty(seed_state),
    %Store state of random number generator
    seed_state=defaultStream.State;
else
    defaultStream.State=seed_state; %reset random number generator to saved state
end

[n_obs n_var]=size(data);
if n_obs<2,
    error('You need data from at least two observations to perform a hypothesis test.')
end

if n_obs<7,
    n_psbl_prms=2^n_obs;
    if reports,
        watchit(sprintf(['Due to the very limited number of observations,' ...
            ' the total number of possible permutations is small.\nThus only a limited number of p-values (at most %d) are possible and the test might be overly conservative.'], ...
            n_psbl_prms));
    end
end

%% Remove null hypothesis mean from data
if isscalar(mu),
    data=data-mu;
elseif isvector(mu)
    s_mu=size(mu);
    if s_mu(1)>1,
        mu=mu';
        s_mu=size(mu);
    end
    if s_mu(2)~=n_var,
        error('mu needs to be a scalar or a 1 x %d vector (%d is the number of variables).',n_var,n_var);
    end
    data=data-repmat(mu,n_obs,1);
else
    error('mu needs to be a scalar or a 1 x %d vector (%d is the number of variables).',n_var,n_var);
end

if reports,
    fprintf('mult_comp_perm_t1: Number of variables: %d\n',n_var);
    fprintf('mult_comp_perm_t1: Number of observations: %d\n',n_obs);
    fprintf('t-score degrees of freedom: %d\n',n_obs-1);
end

%% Set up permutation test
if n_obs<=12,
    n_perm=2^n_obs; %total number of possible permutations
    exact=1;
    seed_state='exact';
    if reports,
        fprintf('Due to the limited number of observations, all possible permutations of the data will be computed instead of random permutations.\n');
    end
else
    exact=0;
end

if reports,
    fprintf('Executing permutation test with %d permutations...\n',n_perm);
    fprintf('Permutations completed: ');
end

%% Compute permutations

%Constant factor for computing t, speeds up computing t to precalculate
%now
sqrt_nXnM1=sqrt(n_obs*(n_obs-1));
if exact,
    %Use all possible permutations
    mxt=zeros(1,n_perm);
    for perm=1:n_perm
        if ~rem(perm,100)
            if reports,
                if ~rem(perm-100,1000)
                    fprintf('%d',perm);
                else
                    fprintf(', %d',perm);
                end
                if ~rem(perm,1000)
                    fprintf('\n');
                end
            end
        end
        %set sign of each participant's data
        if exist('de2bi.m','file') %% ?? check
            temp=de2bi(perm-1);
        else %% check ??
            temp=perm-1; %% check ??
        end %% check ??
        n_temp=length(temp);
        sn=-ones(n_obs,1);
        sn(1:n_temp,1)=2*temp-1;
        sn_mtrx=repmat(sn,1,n_var);
        d_perm=data.*sn_mtrx;

        %computes t-score of permuted data across all channels and time points
        sm=sum(d_perm,1);
        mn=sm/n_obs;
        sm_sqrs=sum(d_perm.^2,1)-(sm.^2)/n_obs;
        stder=sqrt(sm_sqrs)/sqrt_nXnM1;
        t=mn./stder;

        %get most extreme t-score
        [dummy mxt_id]=max(abs(t));
        mxt(perm)=t(mxt_id); %get the most extreme t-value with its sign (+ or -)
    end
else
    %Use random permutations
    mxt=zeros(1,n_perm*2);
    for perm=1:n_perm
        if ~rem(perm,100)
            if reports,
                if ~rem(perm-100,1000)
                    fprintf('%d',perm);
                else
                    fprintf(', %d',perm);
                end
                if ~rem(perm,1000)
                    fprintf('\n');
                end
            end
        end
        %randomly set sign of each participant's data
        sn=(rand(n_obs,1)>.5)*2-1;
        sn_mtrx=repmat(sn,1,n_var);

        d_perm=data.*sn_mtrx;

        %computes t-score of permuted data across all channels and time points
        sm=sum(d_perm,1);
        mn=sm/n_obs;
        sm_sqrs=sum(d_perm.^2,1)-(sm.^2)/n_obs;
        stder=sqrt(sm_sqrs)/sqrt_nXnM1;
        t=mn./stder;

        %get most extreme t-score (sign isn't immportant since we asumme
        %symmetric distribution of null hypothesis for one sample test)
        mxt(perm)=max(abs(t));
    end
    mxt(n_perm+1:2*n_perm)=-mxt(1:n_perm); %add the negative of all values since we assumme
    %null hypothesis distribution is symmetric
end

%End permutations completed line
if reports && rem(perm,1000)
    fprintf('\n');
end

%% Computes t-scores of observations at all variables and time points
sm=sum(data,1);
mn=sm/n_obs;
sm_sqrs=sum(data.^2,1)-(sm.^2)/n_obs;
stder=sqrt(sm_sqrs)/sqrt_nXnM1;
t_orig=mn./stder;

%% Compute p-values
pval=zeros(1,n_var);
for t=1:n_var,
    if tail==0,
        pval(t)=mean(mxt>=abs(t_orig(t)))*2;
    elseif tail==1,
        pval(t)=mean(mxt>=t_orig(t));
    elseif tail==-1,
        pval(t)=mean(mxt<=t_orig(t));
    end
end

%% Compute critical t-scores for specified alpha level,
if tail==0,
    %two-tailed
    crit_t(1)=prctile(mxt,100*alpha_level/2);
    crit_t(2)=-crit_t(1);
    est_alpha=mean(mxt>=crit_t(2))*2;
elseif tail==1,
    %upper tailed
    crit_t(1)=NaN;
    crit_t(2)=prctile(mxt,100-100*alpha_level);
    est_alpha=mean(mxt>=crit_t(2));
else
    %tail=-1, lower tailed
    crit_t(1)=prctile(mxt,alpha_level*100);
    est_alpha=mean(mxt<=crit_t(1));
    crit_t(2)=NaN;
end
if reports,
    fprintf('Desired family-wise alpha level: %f\n',alpha_level);
    fprintf('Estimated actual family-wise alpha level for returned values of crit_t: %f\n',est_alpha);
end

function watchit(msg)
%function watchit(msg)
%
% Displays a warning message on the Matlab command line.  Used by
% several Mass Univariate ERP Toolbox functions.
%

disp(' ');
disp('****************** Warning ******************');
disp(msg);
disp(' ');


function x = r8ci_sl ( n, a, b, job )

%*****************************************************************************80
%
%% R8CI_SL solves a R8CI system.
%
%  Discussion:
%
%    The R8CI storage format is used for an N by N circulant matrix.
%    An N by N circulant matrix A has the property that the entries on
%    row I appear again on row I+1, shifted one position to the right,
%    with the final entry of row I appearing as the first of row I+1.
%    The R8CI format simply records the first row of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 February 2004
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N), the R8CI matrix.
%
%    Input, real B(N), the right hand side.
%
%    Input, integer JOB, specifies the system to solve.
%    0, solve A * x = b.
%    nonzero, solve A' * x = b.
%
%    Output, real X(N), the solution of the linear system.
%
  if ( job == 0 )
%
%  Solve the system with the principal minor of order 1.
%
    r1 = a(1);
    x(1) = b(1) / r1;

    r2 = 0.0;
%
%  Recurrent process for solving the system.
%
    for nsub = 2 : n
%
%  Compute multiples of the first and last columns of
%  the inverse of the principal minor of order N.
%
      r5 = a(n+2-nsub);
      r6 = a(nsub);

      if ( 2 < nsub )

        work(nsub-1) = r2;

        for i = 1 : nsub-2
          r5 = r5 + a(n+1-i) * work(nsub-i);
          r6 = r6 + a(i+1) * work(n-1+i);
        end

      end

      r2 = -r5 / r1;
      r3 = -r6 / r1;
      r1 = r1 + r5 * r3;

      if ( 2 < nsub )

        r6 = work(n);
        work(n-1+nsub-1) = 0.0;
        for i = 2 : nsub-1
          r5 = work(n-1+i);
          work(n-1+i) = work(i) * r3 + r6;
          work(i) = work(i) + r6 * r2;
          r6 = r5;
        end

      end

      work(n) = r3;
%
%  Compute the solution of the system with the principal minor of order NSUB.
%
      r5 = 0.0;
      for i = 1 : nsub-1
        r5 = r5 + a(n+1-i) * x(nsub-i);
      end

      r6 = ( b(nsub) - r5 ) / r1;
      x(1:nsub-1) = x(1:nsub-1) + work(n:n+nsub-2) * r6;
      x(nsub) = r6;

    end

  else
%
%  Solve the system with the principal minor of order 1.
%
    r1 = a(1);
    x(1) = b(1) / r1;

    r2 = 0.0;
%
%  Recurrent process for solving the system.
%
    for nsub = 2 : n
%
%  Compute multiples of the first and last columns of
%  the inverse of the principal minor of order N.
%
      r5 = a(nsub);
      r6 = a(n+2-nsub);

      if ( 2 < nsub )

        work(nsub-1) = r2;

        for i = 1 : nsub-2
          r5 = r5 + a(i+1) * work(nsub-i);
          r6 = r6 + a(n+1-i) * work(n-1+i);
        end

      end

      r2 = -r5 / r1;
      r3 = -r6 / r1;
      r1 = r1 + r5 * r3;

      if ( 2 < nsub )

        r6 = work(n);
        work(n-1+nsub-1) = 0.0;
        for i = 2 : nsub-1
          r5 = work(n-1+i);
          work(n-1+i) = work(i) * r3 + r6;
          work(i) = work(i) + r6 * r2;
          r6 = r5;
        end

      end

      work(n) = r3;
%
%  Compute the solution of the system with the principal minor of order NSUB.
%
      r5 = 0.0E+00;
      for i = 1 : nsub-1
        r5 = r5 + a(i+1) * x(nsub-i);
      end

      r6 = ( b(nsub) - r5 ) / r1;
      for i = 1 : nsub-1
        x(i) = x(i) + work(n-1+i) * r6;
      end

      x(nsub) = r6;

    end

  end

  return
end


function NsVals=reduceStdRefrac2Spher(NMeas,height,expConst,multConst,varargin)
%%REDUCESTDREFRAC2SPHER Given a measurement of the atmospheric refractivity
%                       at a particular height above sea level, determine
%                       the equivalent refractivity at sea level using  a
%                       standard exponential atmospheric model. The
%                       algorithm can handle sea surface refractivities
%                       from 200 to 450.
%
%INPUTS: NMeas The measured atmospheric refractivity. Note that the
%              refractivity is (n-1)*1e6, where n is the index of
%              refraction.
%       height The height in meters at which the measurement of NMeas was
%              taken. One would expect this to be an orthometic height
%              (with respect to mean sea level). However, the model is low
%              fidelity, so using a height with respect to the Earth's
%              reference ellipsoid would presumably not introduce a
%              significant amount of error.
%  expConst, multConst The two optional, positive  parameters
%              parameterizing the decay constant in the model. Assume that
%              at a point, the refractivity is N. Increasing the height by
%              1km, the model is that the refractivity changes by
%              deltaN=-multConst*exp(expConst*N). deltaN cannot be
%              negative. If these parameters are omitted or empty matrices
%              are passed, then the values fitting the data in [1] are
%              used: expConst=0.005577 and multConst=7.32. Note that the
%              value ce=log(Ns/(Ns+DeltaN))/1000 is the decay constant in
%              inverse meters.
%     varargin Parameters to pass to the fminbnd function. These are
%              standard comma-separated keys and values, such as
%              'TolX',1e-8.
%
%OUTPUTS: Ns The indices of refraction reduced to sea level under a
%            standard exponential atmospheric model. There can be 1-2
%            solutions.
%
%The standard exponential atmospheric model is derived in [1]. This
%function determines the refractivity at the given altitudes for sea
%surface refractivities from 200 to 450 and subtracts the observed
%refractivity. Any zero crossings indicate potential solutions. The
%function fminbnd is then used to find each of the zeros given bounded
%regions for the zero crossings.
%
%The use of this type of very basic refraction model is discussed in [2].
%
%REFERENCES:
%[1] B. R. Bean and G. D. Thayer, CRPL Exponential Reference Atmosphere.
%     Washington, D.C.: U. S. Department of Commerce, National Bureau of
%     Standards, Oct. 1959. [Online]. Available:
%     http://digicoll.manoa.hawaii.edu/techreports/PDF/NBS4.pdf
%[2] D. F. Crouse, "Basic tracking using 3D monostatic and bistatic
%    measurements in refractive environments," IEEE Aerospace and
%    Electronic Systems Magazine, vol. 29, no. 8, Part II, pp. 54-75, Aug.
%    2014.
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(expConst))
    expConst=0.005577;
end

if(nargin<4||isempty(multConst))
	multConst=7.32;
end

%Evaluate the refractivity on a grid.
numPoints=100;
Ns=linspace(200,450,numPoints);
theVals=Ns.*(Ns./(Ns-multConst*exp(expConst*Ns))).^(-height/1000)-NMeas;

%Find where the zero crossings are
diffIdx=find(diff(theVals>0));

%Each crossing gives a region to search to find the zero. Because it is
%bounded, we will use fminbnd to find the minimum of the squared value in
%the bounded region.
numNs=length(diffIdx);
NsVals=zeros(numNs,1);

costFun=@(Ns)(Ns.*(Ns./(Ns-multConst*exp(expConst*Ns))).^(-height/1000)-NMeas)^2;
for curNs=1:numNs
    NsCur=fminbnd(costFun,Ns(diffIdx(curNs)),Ns(diffIdx(curNs)+1),varargin{:});
    NsVals(curNs)=NsCur;
end

end

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


%DEMEV3	Demonstrate Bayesian regression for the RBF.
%
%	Description
%	The problem consists an input variable X which sampled from a
%	Gaussian distribution, and a target variable T generated by computing
%	SIN(2*PI*X) and adding Gaussian noise. An RBF network with linear
%	outputs is trained by minimizing a sum-of-squares error function with
%	isotropic Gaussian regularizer, using the scaled conjugate gradient
%	optimizer. The hyperparameters ALPHA and BETA are re-estimated using
%	the function EVIDENCE. A graph  is plotted of the original function,
%	the training data, the trained network function, and the error bars.
%
%	See also
%	DEMEV1, EVIDENCE, RBF, SCG, NETEVFWD
%

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

clc;
disp('This demonstration illustrates the application of Bayesian')
disp('re-estimation to determine the hyperparameters in a simple regression')
disp('problem using an RBF netowk. It is based on a the fact that the')
disp('posterior distribution for the output weights of an RBF is Gaussian')
disp('and uses the evidence maximization framework of MacKay.')
disp(' ')
disp('First, we generate a synthetic data set consisting of a single input')
disp('variable x sampled from a Gaussian distribution, and a target variable')
disp('t obtained by evaluating sin(2*pi*x) and adding Gaussian noise.')
disp(' ')
disp('Press any key to see a plot of the data together with the sine function.')
pause;

% Generate the matrix of inputs x and targets t.

ndata = 16;			% Number of data points.
noise = 0.1;			% Standard deviation of noise distribution.
randn('state', 0);
rand('state', 0);
x = 0.25 + 0.07*randn(ndata, 1);
t = sin(2*pi*x) + noise*randn(size(x));

% Plot the data and the original sine function.
h = figure;
nplot = 200;
plotvals = linspace(0, 1, nplot)';
plot(x, t, 'ok')
xlabel('Input')
ylabel('Target')
hold on
axis([0 1 -1.5 1.5])
fplot('sin(2*pi*x)', [0 1], '-g')
legend('data', 'function');

disp(' ')
disp('Press any key to continue')
pause; clc;

disp('Next we create a two-layer MLP network having 3 hidden units and one')
disp('linear output. The model assumes Gaussian target noise governed by an')
disp('inverse variance hyperparmeter beta, and uses a simple Gaussian prior')
disp('distribution governed by an inverse variance hyperparameter alpha.')
disp(' ');
disp('The network weights and the hyperparameters are initialised and then')
disp('the output layer weights are optimized with the scaled conjugate gradient')
disp('algorithm using the SCG function, with the hyperparameters kept')
disp('fixed. After a maximum of 50 iterations, the hyperparameters are')
disp('re-estimated using the EVIDENCE function. The process of optimizing')
disp('the weights with fixed hyperparameters and then re-estimating the')
disp('hyperparameters is repeated for a total of 3 cycles.')
disp(' ')
disp('Press any key to train the network and determine the hyperparameters.')
pause;

% Set up network parameters.
nin = 1;		% Number of inputs.
nhidden = 3;		% Number of hidden units.
nout = 1;		% Number of outputs.
alpha = 0.01;		% Initial prior hyperparameter.
beta_init = 50.0;	% Initial noise hyperparameter.

% Create and initialize network weight vector.
net = rbf(nin, nhidden, nout, 'tps', 'linear', alpha, beta_init);
[net.mask, prior] = rbfprior('tps', nin, nhidden, nout, alpha, alpha);
net = netinit(net, prior);

options = foptions;
options(14) = 5;  % At most 5 EM iterations for basis functions
options(1) = -1;  % Turn off all messages
net = rbfsetbf(net, options, x);  % Initialise the basis functions

% Now train the network
nouter = 5;
ninner = 2;
options = foptions;
options(1) = 1;
options(2) = 1.0e-5;		% Absolute precision for weights.
options(3) = 1.0e-5;		% Precision for objective function.
options(14) = 50;		% Number of training cycles in inner loop.

% Train using scaled conjugate gradients, re-estimating alpha and beta.
for k = 1:nouter
  net = netopt(net, options, x, t, 'scg');
  [net, gamma] = evidence(net, x, t, ninner);
  fprintf(1, '\nRe-estimation cycle %d:\n', k);
  fprintf(1, '  alpha =  %8.5f\n', net.alpha);
  fprintf(1, '  beta  =  %8.5f\n', net.beta);
  fprintf(1, '  gamma =  %8.5f\n\n', gamma);
  disp(' ')
  disp('Press any key to continue.')
  pause;
end

fprintf(1, 'true beta: %f\n', 1/(noise*noise));

disp(' ')
disp('Network training and hyperparameter re-estimation are now complete.')
disp('Compare the final value for the hyperparameter beta with the true')
disp('value.')
disp(' ')
disp('Notice that the final error value is close to the number of data')
disp(['points (', num2str(ndata),') divided by two.'])
disp(' ')
disp('Press any key to continue.')
pause; clc;
disp('We can now plot the function represented by the trained network. This')
disp('corresponds to the mean of the predictive distribution. We can also')
disp('plot ''error bars'' representing one standard deviation of the')
disp('predictive distribution around the mean.')
disp(' ')
disp('Press any key to add the network function and error bars to the plot.')
pause;

% Evaluate error bars.
[y, sig2] = netevfwd(netpak(net), net, x, t, plotvals);
sig = sqrt(sig2);

% Plot the data, the original function, and the trained network function.
[y, z] = rbffwd(net, plotvals);
figure(h); hold on;
plot(plotvals, y, '-r')
xlabel('Input')
ylabel('Target')
plot(plotvals, y + sig, '-b');
plot(plotvals, y - sig, '-b');
legend('data', 'function', 'network', 'error bars');

disp(' ')
disp('Notice how the confidence interval spanned by the ''error bars'' is')
disp('smaller in the region of input space where the data density is high,')
disp('and becomes larger in regions away from the data.')
disp(' ')
disp('Press any key to end.')
pause; clc; close(h);


function value = r4_csevl ( x, cs, n )

%*****************************************************************************80
%
%% R4_CSEVL evaluates a Chebyshev series.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Broucke,
%    Algorithm 446:
%    Ten Subroutines for the Manipulation of Chebyshev Series,
%    Communications of the ACM,
%    Volume 16, Number 4, April 1973, pages 254-256.
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, real CS(N), the Chebyshev coefficients.
%
%    Input, integer N, the number of Chebyshev coefficients.
%
%    Output, real VALUE, the Chebyshev series evaluated at X.
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CSEVL - Fatal error!\n' );
    fprintf ( 1, '  Number of terms <= 0.\n' );
    error ( 'R4_CSEVL - Fatal error!' )
  end

  if ( 1000 < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CSEVL - Fatal error!\n' );
    fprintf ( 1, '  Number of terms > 1000.\n' );
    error ( 'R4_CSEVL - Fatal error!' )
  end

  if ( x < -1.1 || 1.0 < x )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CSEVL - Fatal error!\n' );
    fprintf ( 1, '  X outside (-1,+1).\n' );
    fprintf ( 1, '  X = %f\n', x );
    error ( 'R4_CSEVL - Fatal error!' )
  end

  b1 = 0.0;
  b0 = 0.0;
  twox = 2.0 * x;

  for i = n : -1 : 1
    b2 = b1;
    b1 = b0;
    b0 = twox * b1 - b2 + cs(i);
  end

  value = 0.5 * ( b0 - b2 );

  return
end


function differ_test01 ( )

%*****************************************************************************80
%
%% DIFFER_TEST01 tests DIFFER_MATRIX.
%
%  Discussion:
%
%    DIFFER_MATRIX computes a modified Vandermonde matrix A1.
%
%    The solution of a system A1 * X1 = B is related to the solution
%    of the system A2 * X2 = B, where A2 is the standard Vandermonde
%    matrix, simply by X2(I) = X1(I) * A(I,1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2013
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIFFER_TEST01\n' );
  fprintf ( 1, '  Demonstrate that the DIFFER matrix is "really"\n' );
  fprintf ( 1, '  a Vandermonde matrix.\n' );

  stencil(1:n,1) = [ 2.5; 3.3; -1.3; 0.5 ];
  x1(1:n,1) = [ 1.0; 2.0; 3.0; 4.0 ];
  a = differ_matrix ( n, stencil );
  r8mat_print ( n, n, a, '  Stencil matrix:' );
  b(1:n,1) = a(1:n,1:n) * x1(1:n,1);
%
%  Set up and solve the DIFFER system.
%
  x1 = a \ b;

  r8vec_print ( n, x1, '  Solution of DIFFER system:' );
%
%  R8VM_SL solves the related Vandermonde system.
%  A simple transformation gives us the solution to the DIFFER system.
%
  job = 0;
  [ x2, info ] = r8vm_sl ( n, stencil, b, job );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST01 - Warning!\n' );
    fprintf ( 1, '  VANDERMONDE system is singular.\n' );
    error ( 'TEST01 - Vandermonde system is singular.' );
    return
  end

  r8vec_print ( n, x2, '  Solution of VANDERMONDE system:' );

  x2(1:n,1) = x2(1:n,1) ./ stencil(1:n,1);
  r8vec_print ( n, x2, '  Transformed solution of VANDERMONDE system:' );

  return
end


function mckinnon_test ( )

%*****************************************************************************80
%
%% MCKINNON_TEST works with the McKinnon function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Michael McKinnon,
%    An Iterative Method for Finding Stationary Values of a Function
%    of Several Variables,
%    Computer Journal,
%    Volume 5, 1962, pages 147-151.
%
  global phi
  global tau
  global theta

  fprintf ( 1, '\n' );
  fprintf ( 1, 'MCKINNON_TEST:\n' );
  fprintf ( 1, '  Test COORDINATE_SEARCH with the McKinnon function.\n' );
  n = 2;
%
%  Test 1
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 10.0;
  tau = 1.0;
  theta = 15.0;

  x = [ a, b ];
  r8vec_print ( n, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( x ) );

  flag = 0;
  x = coordinate_search ( x, @mckinnon, flag );
  r8vec_print ( n, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g\n', mckinnon ( x ) );

  x = [ 0.0, -0.5 ];
  r8vec_print ( n, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( x ) );
%
%  Test 2
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 60.0;
  tau = 2.0;
  theta = 6.0;

  x = [ a, b ];
  r8vec_print ( n, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( x ) );

  flag = 0;
  x = coordinate_search ( x, @mckinnon, flag );
  r8vec_print ( n, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g\n', mckinnon ( x ) );

  x = [ 0.0, -0.5 ];
  r8vec_print ( n, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( x ) );
%
%  Test 3
%
  a = ( 1.0 + sqrt ( 33.0 ) ) / 8.0;
  b = ( 1.0 - sqrt ( 33.0 ) ) / 8.0;

  phi = 4000.0;
  tau = 3.0;
  theta = 6.0;

  x = [ a, b ];
  r8vec_print ( n, x, '  Initial point X0:' );
  fprintf ( 1, '  PHI = %f, TAU = %f, THETA = %f\n', phi, tau, theta );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', mckinnon ( x ) );

  flag = 0;
  x = coordinate_search ( x, @mckinnon, flag );
  r8vec_print ( n, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g\n', mckinnon ( x ) );

  x = [ 0.0, -0.5 ];
  r8vec_print ( n, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', mckinnon ( x ) );
  return
end


function bessel_jx_values_test ( )

%*****************************************************************************80
%
%% BESSEL_JX_VALUES_TEST demonstrates the use of BESSEL_JX_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_JX_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_JX_VALUES stores values of \n' );
  fprintf ( 1, '  the Bessel Jn function for NONINTEGER order.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N      X            JN(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx ] = bessel_jx_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %12f  %24.16e\n', n, x, fx );

  end

  return
end


function yval = r8poly2_val2 ( dim_num, ndata, tdata, ydata, left, tval )

%*****************************************************************************80
%
%% R8POLY2_VAL2 evaluates a parabolic interpolant through tabular data.
%
%  Discussion:
%
%    This routine is a utility routine used by OVERHAUSER_SPLINE_VAL.
%    It constructs the parabolic interpolant through the data in
%    3 consecutive entries of a table and evaluates this interpolant
%    at a given abscissa value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of a single data point.
%    DIM_NUM must be at least 1.
%
%    Input, integer NDATA, the number of data points.
%    NDATA must be at least 3.
%
%    Input, real TDATA(NDATA), the abscissas of the data points.
%    The values in TDATA must be in strictly ascending order.
%
%    Input, real YDATA(DIM_NUM,NDATA), the data points
%    corresponding to the abscissas.
%
%    Input, integer LEFT, the location of the first of the three
%    consecutive data points through which the parabolic interpolant
%    must pass.  1 <= LEFT <= NDATA - 2.
%
%    Input, real TVAL, the value of T at which the parabolic
%    interpolant is to be evaluated.  Normally, TDATA(1) <= TVAL <= T(NDATA),
%    and the data will be interpolated.  For TVAL outside this range,
%    extrapolation will be used.
%
%    Output, real YVAL(DIM_NUM), the value of the parabolic
%    interpolant at TVAL.
%

%
%  Check.
%
  if ( left < 1 || ndata-2 < left )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8POLY2_VAL2 - Fatal error!\n' );
    fprintf ( 1, '  LEFT < 1 or NDATA-2 < LEFT.\n' );
    fprintf ( 1, '  LEFT = %d\n', left );
    error ( 'R8POLY2_VAL2 - Fatal error!' );
  end

  if ( dim_num < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8POLY2_VAL2 - Fatal error!\n' );
    fprintf ( 1, '  DIM_NUM < 1.\n' );
    fprintf ( 1, '  DIM_NUM = %d\n', dim_num );
    error ( 'R8POLY2_VAL2 - Fatal error!' );
  end
%
%  Copy out the three abscissas.
%
  t1 = tdata(left);
  t2 = tdata(left+1);
  t3 = tdata(left+2);

  if ( t2 <= t1 || t3 <= t2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8POLY2_VAL2 - Fatal error!\n' );
    fprintf ( 1, '  T2 <= T1 or T3 <= T2.\n' );
    fprintf ( 1, '  T1 = %f\n', t1 );
    fprintf ( 1, '  T2 = %f\n', t2 );
    fprintf ( 1, '  T3 = %f\n', t3 );
    error ( 'R8POLY2_VAL2 - Fatal error!' );
  end
%
%  Construct and evaluate a parabolic interpolant for the data
%  in each dimension.
%
  for i = 1 : dim_num

    y1 = ydata(i,left);
    y2 = ydata(i,left+1);
    y3 = ydata(i,left+2);

    dif1 = ( y2 - y1 ) / ( t2 - t1 );
    dif2 = ( ( y3 - y1 ) / ( t3 - t1 ) ...
           - ( y2 - y1 ) / ( t2 - t1 ) ) / ( t3 - t2 );

    yval(i) = y1 + ( tval - t1 ) * ( dif1 + ( tval - t2 ) * dif2 );

  end

  return
end


function [ xy, elem ] = triangulate_rectangle ( xl, xr, xn, yb, yt, yn, base )

%*****************************************************************************80
%
%% TRIANGULATE_RECTANGLE makes a triangular grid of a rectangle.
%
%  Discussion:
%
%    The rectangle is presumed to lie between (XL,YB) and (XR,YT).
%
%    We subdivide the rectangle into XN regions in the X direction,
%    and YN regions in the Y direction, and then split each quadrilateral,
%    creating 2 * XN * YN triangular elements.
%
%    The locations of the NN=(XN+1)*(YN+1) nodes are stored in the 2 by NN
%    real array XY.
%
%    The triples of node indices that make up each triangle are stored int
%    the 3 by NE integer array ELEM.  Here, NE = 2 * XN * YN, and the
%    nodes are stored in counterclockwise order.  The node indices will
%    begin at 0 if the input quantity BASE is set to 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real XL, XR, the left and right X limits.
%
%    Input, integer XN, the number of elements along the X direction.
%
%    Input, real YB, YT, the bottom and top Y limits.
%
%    Input, integer YN, the number of elements along the Y direction.
%
%    Input, integer BASE, the indexing base:
%    0, the first node has index 0.
%    1, the first node has index 1.
%
  if ( nargin < 7 )
    base = 1;
  end
%
%  Generate 1D data.
%
  x1d = linspace ( xl, xr, xn + 1 );
  y1d = linspace ( yb, yt, yn + 1 );
%
%  Create (X,Y) table.
%  This can be done faster, using meshgrid, but then you lose control
%  of ordering and arranging.
%
  xyn = ( xn + 1 ) * ( yn + 1 );
  xy = zeros ( 2, xyn );

  k = 0;
  for j = 1 : yn + 1
    for i = 1 : xn + 1
      k = k + 1;
      xy(1,k) = x1d(i);
      xy(2,k) = y1d(j);
    end
  end
%
%  Create connectivity.
%
  en = 2 * xn * yn;
  elem = zeros ( 3, en );
  e = 0;

  for j = 1 : yn
    for i = 1 : xn
      sw = ( j - 1 ) * ( xn + 1 ) + i;
      se = ( j - 1 ) * ( xn + 1 ) + i + 1;
      ne = ( j     ) * ( xn + 1 ) + i + 1;
      nw = ( j     ) * ( xn + 1 ) + i;
      e = e + 1;
      elem(1,e) = sw;
      elem(2,e) = se;
      elem(3,e) = nw;
      e = e + 1;
      elem(1,e) = ne;
      elem(2,e) = nw;
      elem(3,e) = se;
    end
  end
%
%  Shift base if requested.
%
  if ( base == 0 )
    elem = elem - 1;
  end

  return
end


function [ grid_order, grid_point ] = cc_grids_minmax ( dim_num, q_min, ...
  q_max, grid_num, point_num )

%*****************************************************************************80
%
%% CC_GRIDS_MINMAX computes CC orders and grids with Q_MIN <= Q <= Q_MAX.
%
%  Discussion:
%
%    The necessary dimensions of GRID_ORDER and GRID_POINT can be
%    determined by calling CC_GRIDS_MINMAX first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer Q_MIN, Q_MAX, the minimum and maximum values of
%    Q, the sum of the orders in each spatial coordinate.
%
%    Input, integer GRID_NUM, the number of Clenshaw Curtis
%    grids whose Q value is between Q_MIN and Q_MAX.
%
%    Input, integer POINT_NUM, the total number of points in the grids.
%
%    Output, integer GRID_ORDER(DIM_NUM,GRID_NUM), contains, for each
%    grid, the order of the Clenshaw-Curtis rule in each dimension.
%
%    Output, real GRID_POINT(DIM_NUM,POINT_NUM), contains
%    a list of all the abscissas of all the rules, listed one grid at
%    a time.  If a point occurs in several grids, it will be listed
%    several times.
%

%
%  Outer loop generates Q's from Q_MIN to Q_MAX.
%
  point_num = 0;
  grid_num = 0;

  for q = q_min : q_max
%
%  Middle loop generates next partition that adds up to Q.
%
    more = 0;
    order_1d = [];

    while ( 1 )

      [ order_1d, more ] = compnz_next ( q, dim_num, order_1d, more );
%
%  Inner (hidden) loop generates all CC points corresponding to given grid.
%
      order_nd = prod ( order_1d(1:dim_num) );

      grid_point(1:dim_num,point_num+1:point_num+order_nd) = cc_grid ( ...
        dim_num, order_1d, order_nd );

      point_num = point_num + order_nd;

      grid_num = grid_num + 1;
      grid_order(1:dim_num,grid_num) = order_1d(1:dim_num);

      if ( ~more )
        break
      end

    end

  end

  return
end


function xy = padua_points ( l )

%*****************************************************************************80
%
%% PADUA_POINTS returns the Padua points of level L.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Marco Caliari, Stefano de Marchi, Marco Vianello,
%    Bivariate interpolation on the square at new nodal sets,
%    Applied Mathematics and Computation,
%    Volume 165, Number 2, 2005, pages 261-274.
%
%  Parameters:
%
%    Input, integer L, the level of the set.
%    0 <= L
%
%    Output, real XY(2,N), the Padua points of level L.
%
  n = ( ( l + 1 ) * ( l + 2 ) ) / 2;

  xy = zeros ( 2, n );

  if ( l == 0 )
    xy(1,1) = 0.0;
    xy(2,1) = 0.0;
    return
  end

  k = 0;

  for i = 0 : l

    j_hi = floor ( l / 2 ) + 1;
    if ( mod ( l, 2 ) == 1 && mod ( i, 2 ) == 1 )
      j_hi = j_hi + 1;
    end;

   for j = 1 : j_hi

     k = k + 1;

      if ( i * 2 == l )
        xy(1,k) = 0.0;
      else
        angle1 = i * pi / l;
        xy(1,k) = cos ( angle1 );
      end

      if ( mod ( i, 2 ) == 0 )
        if ( 2 * ( 2 * j - 1 ) == l + 1 )
          xy(2,k) = 0.0;
        else
          angle2 = ( 2 * j - 1 ) * pi / ( l + 1 );
          xy(2,k) = cos ( angle2 );
        end
      else
        if ( 2 * ( 2 * j - 2 ) == l + 1 )
          xy(2,k) = 0.0;
        else
          angle2 = ( 2 * j - 2 ) * pi / ( l + 1 );
          xy(2,k) = cos ( angle2 );
        end
      end

    end

  end

  return
end


function truncated_normal_ab_pdf_test ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_AB_PDF_TEST tests TRUNCATED_NORMAL_AB_PDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_AB_PDF_TEST\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_AB_PDF evaluates the Truncated Normal PDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The "parent" normal distribution has\n' );
  fprintf ( 1, '    mean = mu\n' );
  fprintf ( 1, '    standard deviation = sigma\n' );
  fprintf ( 1, '  The parent distribution is truncated to\n' );
  fprintf ( 1, '  the interval [a,b]\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                                                           Stored         Computed\n' );
  fprintf ( 1, '       X        Mu         S         A         B             PDF             PDF\n' );
  fprintf ( 1, '\n');

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, a, b, x, pdf1 ] ...
      = truncated_normal_ab_pdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    pdf2 = truncated_normal_ab_pdf ( x, mu, sigma, a, b );

    fprintf( 1, '  %8.1f  %8.1f  %8.1f  %8.1f  %8.1f  %14g  %14g\n', x, mu, sigma, a, b, pdf1, pdf2 );

  end

  return
end


function [xlat, alt] = geodet2(decl, rmag)

% geocentric to geodetic coordinates
% exact solution (Borkowski, 1989)

% input

%  decl = geocentric declination (radians)
%  rmag = geocentric distance (kilometers)

% output

%  xlat = geodetic latitude (radians)
%  alt  = geodetic altitude (kilometers)

% Orbital Mechanics with Matlab

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

global req flat

fr = 1 / flat;

% determine x and z components of the geocentric distance

rx = rmag * cos(decl);

rz = rmag * sin(decl);

% compute geodetic latitude and altitude

if (rz == 0)
   % special case - equatorial

   xlat = 0;
   alt = rmag - req;

elseif (abs(decl) == 0.5 * pi)
   % special case - polar

   xlat = decl;
   alt = rmag - (req - req/fr);

else
   % general case

   b = sign(rz) * (req - req/fr);

   e = ((rz + b) * b/req - req)/rx;

   f = ((rz - b) * b/req + req)/rx;

   p = (e * f + 1) * 4/3;

   q = (e * e - f * f) * 2;

   d = p * p * p + q * q;

   if (d >= 0.0)
      s = sqrt(d) + q;
      s = sign(s) * (exp(log(abs(s))/3));
      v = p/s - s;
      v = -(q + q + v * v * v)/(3 * p);
   else
      v = 2 * sqrt(-p) * cos(acos(q/p/sqrt(-p))/3);
   end

   g = 0.5 * (e + sqrt(e * e + v));

   t = sqrt(g * g + (f - v * g)/(g + g - e)) - g;

   xlat = atan((1 - t * t) * req/(2 * b * t));

   alt = (rx - req * t) * cos(xlat) + (rz - b) * sin(xlat);
end


function [ point_num, edge_num, face_num, face_order_max ] = ...
  soccer_size_3d ( )

%*****************************************************************************80
%
%% SOCCER_SIZE_3D gives "sizes" for a truncated icosahedron in 3D.
%
%  Discussion:
%
%    The shape is a truncated icosahedron, which is the design used
%    on a soccer ball.  There are 12 pentagons and 20 hexagons.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    http://polyhedra.wolfram.com/uniform/u25.html
%
%  Parameters:
%
%    Output, integer POINT_NUM, the number of points.
%
%    Output, integer EDGE_NUM, the number of edges.
%
%    Output, integer FACE_NUM, the number of faces.
%
%    Output, integer FACE_ORDER_MAX, the maximum order of any face.
%
  point_num = 60;
  edge_num = 90;
  face_num = 32;
  face_order_max = 6;

  return
end


function [panel, Vol] = DeterminePanelGeometry(inputgeo, figs)
% find coordinates for horseshoe vortices and control points and plot
% Aircraft-Fixed coordinate system:
%       x: forward along fuselage axis
%       y: out starboard wing
%       z: down
% Local Profile coordinate system:
%       x: along chord from leading edge toward trailing edge
%       z: up

plot_on = 1;

%% constants
halfSpan=inputgeo.b/2;     % half wingspan
sweep=inputgeo.sweep;       % wing sweep angle in radians
dihedral=inputgeo.dih;    %dihedral angle in radians
nc=inputgeo.nc;  %number of panels on camber line, upper and lower surfaces
ns = inputgeo.ns; % number of span segments per side
M = inputgeo.M;    %Freestream mach number
beta = sqrt(1-M^2); %Prandtl-Glauert correction

%% Root Airfoil data
y=0;            % span station
chord=inputgeo.c_r;        % chord length
alphaRoot=inputgeo.i_r;    % geometric angle of incidence at root in radians

% call function
[Croot,Troot,Aroot]=NacaCoord(inputgeo.root,y,nc); % Croot = nondimensional camber line, Troot = nondimensional surface;
%Croot coordinates = [x location in fraction chord, y location in fraction
%                       chord, z location in fraction chord]
%Troot coordinates = [x location in fraction chord, y location in fraction
%                       chord, z location in fraction chord]

%% Scale to chord length
Croot(:,1)=chord*Croot(:,1);Croot(:,3)=chord*Croot(:,3);
Troot(:,1)=chord*Troot(:,1);Troot(:,3)=chord*Troot(:,3);
Aroot=Aroot*chord^2;

%Calculate slope of camber line for each panel at the root
%Calculate the chord along each elemental panel
dzdxRoot = zeros(1,nc);
ccRoot = zeros(1,nc);
if nc == 1 %If there is only one chordwise element
    i=1;
    dzdxRoot(i)=-(Croot(i+1,3)-Croot(i,3))/(Croot(i+1,1)-Croot(i,1));
    ccRoot(i)=0.75*(Croot(i+1,1)-Croot(i,1));
else        %if there are more than one chordwise elements
    for i=1:nc
        dzdxRoot(i)=(Croot(i+1,3)-Croot(i,3))/(Croot(i+1,1)-Croot(i,1));
        if i==nc
            ccRoot(i)=0.75*(Croot(i+1,1)-Croot(i,1));
        else
            ccRoot(i)=0.75*(Croot(i+1,1)-Croot(i,1))+0.25*(Croot(i+2,1)-Croot(i+1,1));
        end
    end
end

if plot_on
    axes(figs.root); cla;
    plot(Croot(:,1)*-3.2808,Croot(:,3)*-3.2808,'r') %Convert to feet; plot with z-axis positive up and x-axis to the left
    hold on
    plot(Troot(:,1)*-3.2808,Troot(:,3)*-3.2808,'b')  %Convert to feet; plot with z-axis positive up and x-axis to the left
    axis equal
    title('Root airfoil')
end

%% Tip Airfoil data
y=-halfSpan;     % span station; left wing
chord=inputgeo.taper*inputgeo.c_r;        % chord length
alphaTip=inputgeo.i_r+inputgeo.twist;    % geometric angle of incidence at tip in radians

%call function
[Ctip,Ttip,Atip]=NacaCoord(inputgeo.tip,y,nc);

%% scale to chord length
Ctip(:,1)=chord*Ctip(:,1);Ctip(:,3)=chord*Ctip(:,3);
Ttip(:,1)=chord*Ttip(:,1);Ttip(:,3)=chord*Ttip(:,3);
Atip=Atip*chord^2;

%Calculate slope of camber line for each panel at the tip
%Calculate the chord along each elemental panel
dzdxTip = zeros(1,nc);
ccTip = zeros(1,nc);
if nc == 1 %If there is only one chordwise element
    dzdxTip(i)=-(Ctip(i+1,3)-Ctip(i,3))/(Ctip(i+1,1)-Ctip(i,1));
    ccTip(i)=0.75*(Ctip(i+1,1)-Ctip(i,1));
else        %if there are more than one chordwise elements
    for i=1:nc
        dzdxTip(i)=(Ctip(i+1,3)-Ctip(i,3))/(Ctip(i+1,1)-Ctip(i,1));
        if i==nc
            ccTip(i)=0.75*(Ctip(i+1,1)-Ctip(i,1));
        else
            ccTip(i)=0.75*(Ctip(i+1,1)-Ctip(i,1))+0.25*(Ctip(i+2,1)-Ctip(i+1,1));
        end
    end
end

if plot_on
    axes(figs.tip); cla;
    plot(Ctip(:,1)*-3.2808,Ctip(:,3)*-3.2808,'r') %Convert to feet; plot with z-axis positive up and x-axis to the left
    hold on
    plot(Ttip(:,1)*-3.2808,Ttip(:,3)*-3.2808,'b') %Convert to feet; plot with z-axis positive up and x-axis to the left
    axis equal
    title('Tip airfoil')
end

%% Root: Transform to Aircraft-Fixed Coordinate System:
% skin:
%Skin(number of spanwise sections, number of chordwise sections, coordinates in 3 dimensions (x,y,z))
Skin(1,1:2*nc+1,1:3) = Troot*[cos(alphaRoot) 0 sin(alphaRoot); 0 1 0; -sin(alphaRoot) 0 cos(alphaRoot)];

% Mean camber line:
Camber(1,1:nc+1,1:3) = Croot*[cos(alphaRoot) 0 sin(alphaRoot); 0 1 0; -sin(alphaRoot) 0 cos(alphaRoot)];

%% Tip: Transform to Aircraft-Fixed Coordinate System:
% sweep and dihedral
% Negative sign occurs because local axes are opposite to global
xLETip = -halfSpan*(tan(sweep));% x coordinate of Leading Edge at Tip
zLETip = -halfSpan*(tan(dihedral));% z coordinate of Leading Edge at Tip
% Skin:
LETip=ones(2*nc+1,1)*[xLETip 0 zLETip];
Skin(ns+1,1:2*nc+1,1:3) = Ttip*[cos(alphaTip) 0 sin(alphaTip); 0 1 0; -sin(alphaTip) 0 cos(alphaTip)]+LETip;

% Mean camber line
LETip=ones(nc+1,1)*[xLETip 0 zLETip];
Camber(ns+1,1:nc+1,1:3) = Ctip*[cos(alphaTip) 0 sin(alphaTip); 0 1 0; -sin(alphaTip) 0 cos(alphaTip)]+LETip;

%% Intermediate stations
for k=1:ns-1 % k = 0 corresponds to root, k = ns corresponds to tip
    eta=k/ns;
    for i=1:nc+1 % along camber
        Camber(k+1,i,:) = Camber(1,i,:)+eta*(Camber(ns+1,i,:)-Camber(1,i,:));
    end
    for i=1:2*nc+1 %along surface
        Skin(k+1,i,:) = Skin(1,i,:)+eta*(Skin(ns+1,i,:)-Skin(1,i,:));
    end
end

if plot_on
    %Plot wing surface
    axes(figs.fig); cla; hold on; axis equal;
    for k=1:ns+1
        plot3(Skin(k,:,1)*-3.2808,Skin(k,:,2)*3.2808,Skin(k,:,3)*-3.2808);  %Convert to feet; plot with z-axis positive up and x-axis to the left
        plot3(Skin(k,:,1)*-3.2808,-Skin(k,:,2)*3.2808,Skin(k,:,3)*-3.2808);  %Convert to feet; plot with z-axis positive up and x-axis to the left
        plot3(Camber(k,:,1)*-3.2808,Camber(k,:,2)*3.2808,Camber(k,:,3)*-3.2808,'r')  %Convert to feet; plot with z-axis positive up and x-axis to the left
    end
    for i=1:size(Skin,2)
    plot3(Skin(:,i,1)*-3.2808,Skin(:,i,2)*3.2808,Skin(:,i,3)*-3.2808);
    plot3(Skin(:,i,1)*-3.2808,-Skin(:,i,2)*3.2808,Skin(:,i,3)*-3.2808);
    end
end

%Determine panel properties
twist = zeros(1,ns);
dzdx = zeros(ns,nc);
cc = zeros(ns,nc);
CP = zeros(ns,nc,3);
BV = zeros(ns,nc,3);
BV1 = zeros(ns,nc,3);
BV2 = zeros(ns,nc,3);
BVhalfspan = zeros(ns,nc);
sweep_c4 = zeros(ns,nc);

for k = 1:ns
    %Determine local twist at each panel, dz/dx at control points, and
    %chord along left trailing leg of elemental panel
    eta=k/(ns+1);
    twist(k)=alphaRoot+eta*(alphaTip-alphaRoot);
    dzdx(k,:)=dzdxRoot+eta*(dzdxTip-dzdxRoot);
    cc(k,:)=ccRoot+eta*(ccTip-ccRoot);  %Validated
    %Determine control point coordinates, coordinates to center of bound
    %vortex, half span of bound vortex, and quarter-chord sweep angle
    for i = 1:nc
        CP(k,i,:)=0.5*(Camber(k,i,:)+Camber(k+1,i,:))+0.75*(0.5*(Camber(k,i+1,:)+Camber(k+1,i+1,:))-0.5*(Camber(k,i,:)+Camber(k+1,i,:)));
        BV(k,i,:)=0.5*(Camber(k,i,:)+Camber(k+1,i,:))+0.25*(0.5*(Camber(k,i+1,:)+Camber(k+1,i+1,:))-0.5*(Camber(k,i,:)+Camber(k+1,i,:)));
        BV1(k,i,:)=0.75*Camber(k+1,i,:)+0.25*Camber(k+1,i+1,:);  %Left bound vortex coordinate
        BV2(k,i,:)=0.75*Camber(k,i,:)+0.25*Camber(k,i+1,:);  %Right bound vortex coordinate
        BVhalfspan(k,i)= 0.5*(0.75*norm(shiftdim(Camber(k,i,2:3)-Camber(k+1,i,2:3)))+0.25*norm(shiftdim(Camber(k,i+1,2:3)-Camber(k+1,i+1,2:3))));
        %Only consider [Y,Z] distances due to definition of halfspan in
        %NASA paper; considering X distance as well makes halfspan increase
        %greatly as you increase sweep and invalidates calculations
        %Validated BVhalfspan calculation through (1:ns-1,1:nc)
        sweep_c4(k,i)=atan((Camber(k,i,1)-Camber(k+1,i,1))/(Camber(k,i,2)-Camber(k+1,i,2))); %Validated
    end
end

%Prandtl-Glauert corrections
CPprime = CP(:,:,1)/beta;
BVprime = BV(:,:,1)/beta;
BV1prime = BV1(:,:,1)/beta;
BV2prime = BV2(:,:,1)/beta;
sweep_c4_prime = atan(tan(sweep_c4)/beta);

for k = 1:ns
    for i  = 1:nc
        panel(k,i).CP=shiftdim(CP(k,i,:));  %Removing singleton dimensions
        panel(k,i).BV=shiftdim(BV(k,i,:)); %Removing singleton dimensions
        panel(k,i).BV1=shiftdim(BV1(k,i,:)); %Removing singleton dimensions
        panel(k,i).BV2=shiftdim(BV2(k,i,:)); %Removing singleton dimensions
        panel(k,i).dzdx=dzdx(k,i);
        panel(k,i).twist=twist(k);
        panel(k,i).s=BVhalfspan(k,i);
        panel(k,i).sweep=sweep_c4(k,i);
        panel(k,i).sweepprime=sweep_c4_prime(k,i);
        panel(k,i).CPprime=CPprime(k,i);
        panel(k,i).BVprime=BVprime(k,i);
        panel(k,i).BV1prime=BV1prime(k,i);
        panel(k,i).BV2prime=BV2prime(k,i);
        panel(k,i).cc=cc(k,i);
    end
end

%Determine wing volume
Vol = 1/3*(Aroot +sqrt(Aroot*Atip) + Atip)*inputgeo.b;


function Mz = ir_equations(params, seqFlag, approxFlag)
%IR_EQUATIONS Analytical equations for the longitudinal magnetization of
%steady-state inversion recovery experiments with either a gradient echo
%(GRE-IR) or spin-echo (SE-IR) readouts.
%   Reference: Barral, J. K., Gudmundson, E. , Stikov, N. , Etezadi?Amoli,
%   M. , Stoica, P. and Nishimura, D. G. (2010), A robust methodology for
%   in vivo T1 mapping. Magn. Reson. Med., 64: 1057-1067.
%   doi:10.1002/mrm.22497
%
%   params: struct with the required parameters for the sequence and
%   approximation. See below for list.
%
%   seqFlag: String. Either 'GRE-IR' or 'SE-IR'
%   approxFlag: Integer between 1 and 4.
%       1: General equation (no approximation).
%       2: Ideal 180 degree pulse approximation of case 1.
%       3: Ideal 90 degree pulse approximation of case 2, and readout term
%          absorbed into constant.
%       4: Long TR (TR >> T1) approximation of case 3.
%
%   **PARAMS PROPERTIES**
%   All times in seconds, all angles in degrees.
%  'GRE-IR'
%       case 1: T1, TR, TI, EXC_FA, INV_FA, constant (optional)
%       case 2: T1, TR, TI, EXC_FA, constant (optional)
%       case 3: T1, TR, TI, constant (optional)
%       case 4: T1, TI, constant (optional)
%
%  'SE-IR'
%       case 1: Same as 'GRE-IR' case + SE_FA, TE
%       case 2: Same as 'GRE-IR' case + TE
%       case 3: Same as 'GRE-IR' case + TE
%       case 4: Same as 'GRE-IR' case
%

switch seqFlag
    case 'GRE-IR'
        switch approxFlag
            case 1 % General
                try
                    T1 = params.T1;
                    TR = params.TR;
                    TI = params.TI;

                    EXC_FA = params.EXC_FA; % Excitation pulse in deg
                    INV_FA = params.INV_FA; % Inversion pulse in deg

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* ( (1-cosd(INV_FA).*exp(-TR/T1) - (1-cosd(INV_FA)).*exp(-TI./T1)) ./ (1-cosd(INV_FA).*cosd(EXC_FA).*exp(-TR./T1)) );
                catch
                    error('ir_equations.GRE-IR.case1: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end
            case 2 % Ideal 180 pulse
                try
                    T1 = params.T1;
                    TR = params.TR;
                    TI = params.TI;

                    EXC_FA = params.EXC_FA; % in deg

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* (1 - 2*exp(-TI./T1) + exp(-TR./T1));
                catch
                    error('ir_equations.GRE-IR.case2: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end
            case 3 % Ideal 90 pulse.
                try
                    T1 = params.T1;
                    TR = params.TR;
                    TI = params.TI;

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* (1 - 2*exp(-TI./T1) + exp(-TR./T1));
                catch
                    error('ir_equations.GRE-IR.case3: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end

            case 4 % Long TR (TR >> T1)
                try
                    T1 = params.T1;
                    TI = params.TI;

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* (1 - 2*exp(-TI./T1));
                catch
                    error('ir_equations.GRE-IR.case4: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end

            otherwise
                error('ir_equations: Unknown flag. Run `help ir_equations` for more info.')
        end
    case 'SE-IR'
        switch approxFlag
            case 1 % General equation
                try
                    T1 = params.T1;
                    TR = params.TR;
                    TI = params.TI;

                    TE = params.TE;

                    EXC_FA = params.EXC_FA; % Excitation pulse in deg
                    INV_FA = params.INV_FA; % Inversion pulse in deg
                    SE_FA = params.SE_FA; % Inversion pulse in deg

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* ( (1-cosd(INV_FA).*cosd(SE_FA).*exp(-TR/T1) - cosd(INV_FA).*(1-cosd(SE_FA)).*exp(-(TR-(TE/2))./T1) - (1-cosd(INV_FA)).*exp(-TI./T1)) ./ (1-cosd(INV_FA).*cosd(EXC_FA).*cosd(SE_FA).*exp(-TR./T1)) );
                catch
                    error('ir_equations.SE-IR.case1: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end
            case 2 % Ideal 180 pulses
                try
                    T1 = params.T1;
                    TR = params.TR;
                    TI = params.TI;

                    TE = params.TE;

                    EXC_FA = params.EXC_FA; % Excitation pulse in deg

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* ( (1-exp(-TR/T1) + 2.*exp(-(TR-(TE/2))./T1) - 2.*exp(-TI./T1)) ./ (1-cosd(EXC_FA).*exp(-TR./T1)) );
                catch
                    error('ir_equations.SE-IR.case2: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end
            case 3 % Ideal 90 pulse
                try
                    T1 = params.T1;
                    TR = params.TR;
                    TI = params.TI;

                    TE = params.TE;

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* (1-exp(-TR/T1) + 2.*exp(-(TR-(TE/2))./T1) - 2.*exp(-TI./T1));
                catch
                    error('ir_equations.SE-IR.case3: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end
            case 4 % Long TR
                try
                    T1 = params.T1;
                    TI = params.TI;

                    try
                        constant = params.constant;
                    catch
                        constant = 1;
                    end

                    Mz= constant .* (1 - 2.*exp(-TI./T1));
                catch
                    error('ir_equations.SE-IR.case4: Incorrect parameters for given flag.  Run `help ir_equations` for more info.')
                end
        end
    otherwise
        error('ir_equations.seqFlag: Incorrect seqFlag arguement. Must be either GRE-IR or SE-IR.')
end


function value = r8_tanh ( x )

%*****************************************************************************80
%
%% R8_TANH evaluates the hyperbolic tangent of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the hyperbolic tangent of X.
%
  persistent nterms
  persistent sqeps
  persistent tanhcs
  persistent xmax

  if ( isempty ( nterms ) )
    tanhcs = [ ...
      -0.25828756643634710438338151450605, ...
      -0.11836106330053496535383671940204, ...
      +0.98694426480063988762827307999681E-02, ...
      -0.83579866234458257836163690398638E-03, ...
      +0.70904321198943582626778034363413E-04, ...
      -0.60164243181207040390743479001010E-05, ...
      +0.51052419080064402965136297723411E-06, ...
      -0.43320729077584087216545467387192E-07, ...
      +0.36759990553445306144930076233714E-08, ...
      -0.31192849612492011117215651480953E-09, ...
      +0.26468828199718962579377758445381E-10, ...
      -0.22460239307504140621870997006196E-11, ...
      +0.19058733768288196054319468396139E-12, ...
      -0.16172371446432292391330769279701E-13, ...
      +0.13723136142294289632897761289386E-14, ...
      -0.11644826870554194634439647293781E-15, ...
      +0.98812684971669738285540514338133E-17, ...
      -0.83847933677744865122269229055999E-18, ...
      +0.71149528869124351310723506176000E-19, ...
      -0.60374242229442045413288837119999E-20, ...
      +0.51230825877768084883404663466666E-21, ...
      -0.43472140157782110106047829333333E-22, ...
      +0.36888473639031328479423146666666E-23, ...
      -0.31301874774939399883325439999999E-24, ...
      +0.26561342006551994468488533333333E-25, ...
      -0.22538742304145029883494399999999E-26, ...
      +0.19125347827973995102208000000000E-27, ...
      -0.16228897096543663117653333333333E-28, ...
      +0.13771101229854738786986666666666E-29, ...
      -0.11685527840188950118399999999999E-30, ...
      +0.99158055384640389120000000000000E-32 ]';
    nterms = r8_inits ( tanhcs, 31, 0.1 * r8_mach ( 3 ) );
    sqeps = sqrt ( 3.0 * r8_mach ( 3 ) );
    xmax = - 0.5 * log ( r8_mach ( 3 ) );
  end

  y = abs ( x );

  if ( y <= sqeps )

    value = x;

  elseif ( y <= 1.0 )

    value = x * ( 1.0 + r8_csevl ( 2.0 * x * x - 1.0, tanhcs, nterms ) );

  elseif ( y <= xmax )

    y = exp ( y );
    yrec = 1.0 / y;
    value = ( y - yrec ) / ( y + yrec );

    if ( x < 0.0 )
      value = - value;
    end

  else

    if ( x < 0.0 )
      value = - 1.0;
    else
      value = + 1.0;
    end

  end

  return
end


function line_nco_rule_test01 ( )

%*****************************************************************************80
%
%% LINE_NCO_RULE_TEST01 computes and prints NCO rules.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2014
%
%  Author:
%
%    John Burkardt
%
  a = -1.0;
  b = +1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINE_NCO_RULE_TEST01\n' );
  fprintf ( 1, '  LINE_NCO_RULE computes the Newton-Cotes Open rule\n' );
  fprintf ( 1, '  using N equally spaced points for an interval [A,B].\n' );

  for n = 1 : 12

    [ x, w ] = line_nco_rule ( n, a, b );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Newton-Cotes Open Rule #%d\n', n );
    fprintf ( 1, '   I       X(I)            W(I)\n' );
    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %2d  %14.6g  %14.6g\n', i, x(i), w(i) );
    end
    fprintf ( 1, '        Sum(|W)|) =  %14.6g\n', sum ( abs ( w(1:n) ) ) );

  end

  return
end


function dtr = sv_clock_bias(t, toc, a0, a1, a2, e, sqrtA, toe, Delta_n, M0)
% 输入:
% a0 a1 a2 toc: 卫星时钟校正模型方程中3个参数，  toc: 第一数据块参考时间, 被用作时钟校正模型中时间参考点

% dtr： 卫星时钟偏差

dtr = a0+a1*(t-toc)+a2*(t-toc).^2;%t为未做钟差改正的观测时刻

F = -4.442807633e-10;
mu = 3.986005e14;
A = sqrtA^2;
cmm = sqrt(mu/A^3); % computed mean motion
tk = t - toe;
% account for beginning or end of week crossover
if (tk > 302400)
    tk = tk-604800;
end
if (tk < -302400)
    tk = tk+604800;
end
% apply mean motion correction
n = cmm + Delta_n;

% Mean anomaly
mk = M0 + n*tk;

% solve for eccentric anomaly
Ek = mk;
Ek = mk + e*sin(Ek);
Ek = mk + e*sin(Ek);
Ek = mk + e*sin(Ek);

% dsv 时间为s
dtr = dtr + F*e*sqrtA*sin(Ek);

end


function SNR = Convert_SNR(SNR_dB)

% Convert back from dBs to SNR value
%SNR_dB = 10 * Log10(SNR)

SNR = 10^(SNR_dB/10);


function euler = quatern2euler(q)
%QUATERN2EULER Converts a quaternion orientation to ZYX Euler angles
%
%   q = quatern2euler(q)
%
%   Converts a quaternion orientation to ZYX Euler angles where phi is a
%   rotation around X, theta around Y and psi around Z.
%
%   For more information see:
%   http://www.x-io.co.uk/node/8#quaternions
%
%	Date          Author          Notes
%	27/09/2011    SOH Madgwick    Initial release

    R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
    R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
    R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
    R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
    R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;

    phi = atan2(R(3,2,:), R(3,3,:) );
    theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) );
    psi = atan2(R(2,1,:), R(1,1,:) );

    euler = [phi(1,:)' theta(1,:)' psi(1,:)'];
end


function err = getH1error(node,elem,Du,uh,K,quadOrder)
%% GETH1ERROR H1 norm of the approximation error.
%
%  err = getH1error(node,elem,@Du,uh,K) computes the H1 norm of the
%  error between the exact solution Du and finite element approximation
%  uh on a mesh described by node and elem.
%
%  The input parameter Du is a function handle and uh is either a column
%  array with length N representing a piecewise linear function on the mesh
%  or a (NT,2) array representing the gradient of a linear function. The
%  diffusion coefficient K is either a NT by 1 array or a function handle.
%
%  err = getH1error(node,elem,@Du,uh,d,quadOrder) computes error
%  using the quadrature rule with order quadOrder (up to 9). The default
%  order is 3.
%
%  Example: compute H1 error of piecewise linear interpolation
%
%     [node,elem] = squaremesh([0,1,0,1],0.25);
%     for k = 1:4
%         exactu = inline('sin(pi*pxy(:,1)).*sin(pi*pxy(:,2))','pxy');
%         Du = inline('[pi*cos(pi*pxy(:,1)).*sin(pi*pxy(:,2)) pi*sin(pi*pxy(:,1)).*cos(pi*pxy(:,2))]','pxy');
%         uI = exactu(node);
%         N(k) = size(node,1);
%         err(k) = getH1error(node,elem,Du,uI);
%         [node,elem] = uniformrefine(node,elem);
%     end
%     showrate(N,err);
%
% See also getH1error3, getL2error, getL2error3, quadpts.
%
% The quadratic element is added by Ming Wang. The cubic element is added by Jie Zhou.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

Nu = size(uh,1);    N = size(node,1);   NT = size(elem,1);
% Euler formula N-NE+NT = c % rough estimateus using Euler formula
NE = N + NT;    NP2 = N + NE;   NP3 = N + 2*NE + NT;
if Nu > N+NT-5   % Euler formula N-NE+NT = c
    elem2dof = dofP2(elem);
    NP2 = max(elem2dof(:));
    NE = NP2 - N;
    NP3 = N+2*NE+NT;
end

%% Default quadrature orders for different elements
if ~exist('quadOrder','var')
    switch Nu
        case NT     % piecewise constant vector (uh is Duh)
            quadOrder = 3;
        case N      % piecewise linear function P1 element
            quadOrder = 3;
        case NE     % piecewise linear function CR element
            quadOrder = 3;
        case NP2    % piecewise quadratic function
            quadOrder = 4;
        case NE + NT % WG element
            quadOrder = 3;
        case NP3    % P3 element
            quadOrder = 5;
    end
end

%% compute gradient of finite element function uh
if (size(uh,2) == 2) && (Nu == NT)      % uh is a piecewise constant vector
    Duh = uh;
    area = abs(simplexvolume(node,elem));
elseif size(uh,2) == 1   % scalar function uh
    switch Nu
        case N      % piecewise linear function P1 element
            [Duh,area] = gradu(node,elem,uh);
        case NE     % piecewise linear function CR element
            elem2edge = elem2dof(:,4:6) - N;
            [Duh,area] = graduCR(node,elem,elem2edge,uh);
        case NE + NT % weak Galerkin element
            elem2edge = elem2dof(:,4:6) - N;
            [Duh,area] = graduWG(node,elem,elem2edge,uh);
        case NP2    % piecewise quadratic function
            [Dlambda,area] = gradbasis(node,elem);
        case NP3
            [Dlambda,area] = gradbasis(node,elem);
            elem2dof = dofP3(elem);
    end
end

%% compute H1 error element-wise using quadrature rule with order quadOrder
[lambda,weight] = quadpts(quadOrder);
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:);
    if Nu == NP2 % piecewise quadratic function
        Dphip1 = (4*lambda(p,1)-1).*Dlambda(:,:,1);
        Dphip2 = (4*lambda(p,2)-1).*Dlambda(:,:,2);
        Dphip3 = (4*lambda(p,3)-1).*Dlambda(:,:,3);
        Dphip4 = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
        Dphip5 = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
        Dphip6 = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
        Duh = repmat(uh(elem2dof(:,1)),1,2).*Dphip1 + ...
              repmat(uh(elem2dof(:,2)),1,2).*Dphip2 + ...
              repmat(uh(elem2dof(:,3)),1,2).*Dphip3 + ...
              repmat(uh(elem2dof(:,4)),1,2).*Dphip4 + ...
              repmat(uh(elem2dof(:,5)),1,2).*Dphip5 + ...
              repmat(uh(elem2dof(:,6)),1,2).*Dphip6;
    end
    if Nu == NP3 % piecewise cubic function
        Dphip1 = (27/2*lambda(p,1)*lambda(p,1)-9*lambda(p,1)+1).*Dlambda(:,:,1);
        Dphip2 = (27/2*lambda(p,2)*lambda(p,2)-9*lambda(p,2)+1).*Dlambda(:,:,2);
        Dphip3 = (27/2*lambda(p,3)*lambda(p,3)-9*lambda(p,3)+1).*Dlambda(:,:,3);
        Dphip4 = 9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,3)+...
                lambda(p,3)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
        Dphip5 = 9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,2)+...
                 lambda(p,2)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
        Dphip6 = 9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,1)+...
                 lambda(p,1)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
        Dphip7 = 9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,3)+...
                 lambda(p,3)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
        Dphip8 = 9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,2)+...
                 lambda(p,2)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
        Dphip9 = 9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,1)+...
                 lambda(p,1)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
        Dphip10= 27*(lambda(p,1)*lambda(p,2)*Dlambda(:,:,3)+lambda(p,1)*lambda(p,3)*Dlambda(:,:,2)+...
                 lambda(p,3)*lambda(p,2)*Dlambda(:,:,1));
        Duh = repmat(uh(elem2dof(:,1)),1,2).*Dphip1 + ...
              repmat(uh(elem2dof(:,2)),1,2).*Dphip2 + ...
              repmat(uh(elem2dof(:,3)),1,2).*Dphip3 + ...
              repmat(uh(elem2dof(:,4)),1,2).*Dphip4 + ...
              repmat(uh(elem2dof(:,5)),1,2).*Dphip5 + ...
              repmat(uh(elem2dof(:,6)),1,2).*Dphip6 + ...
              repmat(uh(elem2dof(:,7)),1,2).*Dphip7 + ...
              repmat(uh(elem2dof(:,8)),1,2).*Dphip8 + ...
              repmat(uh(elem2dof(:,9)),1,2).*Dphip9 + ...
              repmat(uh(elem2dof(:,10)),1,2).*Dphip10;
    end
    if exist('K','var') && ~isempty(K) && ~isnumeric(K) % K is a function
        err = err + weight(p)*K(pxy).*sum((Du(pxy)-Duh).^2,2);
    else
        err = err + weight(p)*sum((Du(pxy)-Duh).^2,2);
    end
end
if exist('K','var') && ~isempty(K) && isnumeric(K) && size(K,1) == NT
    err = K.*err;    % K is piecewise constant
end
err = area.*err;
err(isnan(err)) = 0; % singular values are excluded
err = sqrt(sum(err));


function y = naca4_symmetric ( t, c, x )

%*****************************************************************************80
%
%% NACA4_SYMMETRIC evaluates y(x) for a NACA symmetric 4-digit airfoil.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Eastman Jacobs, Kenneth Ward, Robert Pinkerton,
%    "The characteristics of 78 related airfoil sections from tests in
%    the variable-density wind tunnel",
%    NACA Report 460, 1933.
%
%  Parameters:
%
%    Input, real T, the maximum relative thickness.
%
%    Input, real C, the chord length.
%
%    Input, real X(*), points along the chord length.
%    0.0 <= X(*) <= C.
%
%    Output, real Y(*), for each value of X, the corresponding value of Y
%    so that (X,Y) is on the upper wing surface, and (X,-Y) is on the
%    lower wing surface.
%
  y = 5.0 * t * c * ( ...
    0.2969 * sqrt ( x / c ) ...
    + (((( ...
      - 0.1015 ) .* ( x / c ) ...
      + 0.2843 ) .* ( x / c ) ...
      - 0.3516 ) .* ( x / c ) ...
      - 0.1260 ) .* ( x / c ) );

  return
end


function q = fromTransfMatrixToPosQuat(H)

    % FROMTRANSFMATRIXTOQUATERNIONS computes a link pose using position
    %                               + quaternion rapresentation. The input is
    %                               the link pose represented by a transformation matrix.
    %
    % FORMAT: q = fromTransfMatrixToPosQuat(H)
    %
    % INPUT:  - H = [4 * 4] transf matrix representing link pose
    %
    % OUTPUT: - q = [7 * 1] position + quaternions representing link pose
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    % Separate the rotation matrix
    R       = H(1:3,1:3);
    qt_b    = zeros(4,1);

    % min. value to treat a number as zero
    epsilon = 1e-12;

    % Compute the corresponding (unit) quaternion from a given rotation matrix R:
    %
    % The transformation uses the computational efficient algorithm of Stanley.
    % To be numerically robust, the code determines the set with the maximum
    % divisor for the calculation.
    %
    % For further details about the Stanley Algorithm, see:
    %   [1] Optimal Spacecraft Rotational Maneuvers, John L. Junkins & James D. Turner, Elsevier, 1986, pp. 28-29, eq. (2.57)-(2.59).
    %   [2] Theory of Applied Robotics: Kinematics, Dynamics, and Control, Reza N. Jazar, 2nd Edition, Springer, 2010, p. 110, eq. (3.149)-(3.152).
    % Note: There exist also an optimized version of the Stanley method and is the fastest
    %       possible computation method for Matlab, but it does not cover all special cases.
    % Further details about the fast calculation can be found at:
    %   [3] Modelling and Control of Robot Manipulators, L. Sciavicco & B. Siciliano, 2nd Edition, Springer, 2008,
    %       p. 36, formula (2.30).
    %
    tr = R(1,1) + R(2,2) + R(3,3);

    if (tr > epsilon)

            % scalar part:
            qt_b(1,1) = 0.5*sqrt(tr + 1);
            s_inv     = 1/(qt_b(1,1)*4);

            % vector part:
            qt_b(2,1) = (R(3,2) - R(2,3))*s_inv;
            qt_b(3,1) = (R(1,3) - R(3,1))*s_inv;
            qt_b(4,1) = (R(2,1) - R(1,2))*s_inv;
    else
        % if tr <= 0, find the greatest diagonal element for calculating
        % the scale factor s and the vector part of the quaternion
        if ((R(1,1) > R(2,2)) && (R(1,1) > R(3,3)))

            qt_b(2,1) = 0.5*sqrt(R(1,1) - R(2,2) - R(3,3) + 1);
            s_inv     = 1/(qt_b(2,1)*4);

            qt_b(1,1) = (R(3,2) + R(2,3))*s_inv;
            qt_b(3,1) = (R(2,1) + R(1,2))*s_inv;
            qt_b(4,1) = (R(3,1) + R(1,3))*s_inv;

        elseif (R(2,2) > R(3,3))

            qt_b(3,1) = 0.5*sqrt(R(2,2) - R(3,3) - R(1,1) + 1);
            s_inv     = 1/(qt_b(3,1)*4);

            qt_b(1,1) = (R(1,3) - R(3,1))*s_inv;
            qt_b(2,1) = (R(2,1) + R(1,2))*s_inv;
            qt_b(4,1) = (R(3,2) + R(2,3))*s_inv;
        else
            qt_b(4,1) = 0.5*sqrt(R(3,3) - R(1,1) - R(2,2) + 1);
            s_inv     = 1/(qt_b(4,1)*4);

            qt_b(1,1) = (R(2,1) - R(1,2))*s_inv;
            qt_b(2,1) = (R(3,1) + R(1,3))*s_inv;
            qt_b(3,1) = (R(3,2) + R(2,3))*s_inv;
        end
    end

    % Final state converted into quaternion rapresentation
    q = [H(1:3,4); qt_b];
end