function [e,E_q] = q2e(q)

% Q2E  Quaternion to Euler angles conversion.
%   Q2E(Q) returns an Euler angles vector [roll;pitch;yaw] corresponding to
%   the orientation quaternion Q.
%
%   [E,Jq] = Q2E(Q) returns also the Jacobian matrix.
%
%   See also QUATERNION, EULERANGLES, R2Q, E2Q, Q2V.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.
%   Revised 2018 Joan Sola - handle full gimbal range (2pi, pi, 2pi)

a  = q(1); % a: real part
b  = q(2);
c  = q(3);
d  = q(4);

r32 =  2*c*d + 2*a*b;
r33 =  a^2   - b^2   - c^2 + d^2;
r31 =  2*b*d - 2*a*c;
r21 =  2*b*c + 2*a*d;
r11 =  a^2   + b^2   - c^2 - d^2;

n   = sqrt(r11^2+r21^2);

e   = [ atan2( r32,r33)
        atan2(-r31, n)
        atan2( r21,r11) ];

if nargout >1

    % partials of R wrt q
    dr11dq  = [  2*a,  2*b, -2*c, -2*d];
    dr21dq  = [  2*d,  2*c,  2*b,  2*a];
    dr31dq  = [ -2*c,  2*d, -2*a,  2*b];
    dr32dq  = [  2*b,  2*a,  2*d,  2*c];
    dr33dq  = [  2*a, -2*b, -2*c,  2*d];

    % partials of n wrt R elements
    dndr11 = r11/n;
    dndr21 = r21/n;

    % partials of e wrt n
    de2dn   = r31/(n^2 + r31^2);

    % partials of e wrt R elements
    de1dr32 =  r33/(r33^2 + r32^2);
    de1dr33 = -r32/(r33^2 + r32^2);
    de2dr31 = -n /(n^2 + r31^2);
    de3dr11 = -r21/(r11^2 + r21^2);
    de3dr21 =  r11/(r11^2 + r21^2);

    % chain rule: rows of Jacobian
    de1dq  = de1dr33*dr33dq + de1dr32*dr32dq;
    de2dq  = de2dr31*dr31dq + de2dn*(dndr11*dr11dq+dndr21*dr21dq);
    de3dq  = de3dr11*dr11dq + de3dr21*dr21dq;

    % assemble Jacobian
    E_q    = [de1dq;de2dq;de3dq];
end

return

%% jac

syms a b c d real
q=[a;b;c;d];
[e,E_q] = q2e(q);
simplify(E_q-jacobian(e,q))

%% test
for i = 1 : 1000
    e = [2/3;1/3;2/3] .* randn(3,1);
    eo = q2e(e2q(e));
    if any(e-eo > 1e-10) % Should never enter this IF
        e
        eo
        disp('###########')
    end
end

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

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

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

%   # END GPL LICENSE


function F = CalHV(points,bounds,k,nSample)
% Calculate the hypervolume-based fitness value of each solution

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is modified from the code in
% http://www.tik.ee.ethz.ch/sop/download/supplementary/hype/

    [N,M] = size(points);
    if M > 2
        % Use the estimated method for three or more objectives
        alpha = zeros(1,N);
        for i = 1 : k
            alpha(i) = prod((k-[1:i-1])./(N-[1:i-1]))./i;
        end
        Fmin = min(points,[],1);
        S    = unifrnd(repmat(Fmin,nSample,1),repmat(bounds,nSample,1));
        PdS  = false(N,nSample);
        dS   = zeros(1,nSample);
        for i = 1 : N
            x        = sum(repmat(points(i,:),nSample,1)-S<=0,2) == M;
            PdS(i,x) = true;
            dS(x)    = dS(x) + 1;
        end
        F = zeros(1,N);
        for i = 1 : N
            F(i) = sum(alpha(dS(PdS(i,:))));
        end
        F = F.*prod(bounds-Fmin)/nSample;
    else
        % Use the accurate method for two objectives
        pvec  = 1:size(points,1);
        alpha = zeros(1,k);
        for i = 1 : k
            j = 1 : i-1;
            alpha(i) = prod((k-j)./(N-j))./i;
        end
        F = hypesub(N,points,M,bounds,pvec,alpha,k);
    end
end

function h = hypesub(l,A,M,bounds,pvec,alpha,k)
% The recursive function for the accurate method

    h     = zeros(1,l);
    [S,i] = sortrows(A,M);
    pvec  = pvec(i);
    for i = 1 : size(S,1)
        if i < size(S,1)
            extrusion = S(i+1,M) - S(i,M);
        else
            extrusion = bounds(M) - S(i,M);
        end
        if M == 1
            if i > k
                break;
            end
            if alpha >= 0
                h(pvec(1:i)) = h(pvec(1:i)) + extrusion*alpha(i);
            end
        elseif extrusion > 0
            h = h + extrusion*hypesub(l,S(1:i,:),M-1,bounds,pvec(1:i),alpha,k);
        end
    end
end


function [ r, w ] = oned_gauss ( rule )

%*****************************************************************************80
%
%% ONED_GAUSS sets up a Gauss quadrature rule in [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2011
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, integer RULE, the number of Gauss points desired.
%
%    Output, real R(RULE), Gauss points located between (-1,1).
%
%    Output, real W(RULE), Gauss weights corresponding to r.
%
  r = zeros(rule,1);
  w = zeros(rule,1);

  if ( rule == 1 )
    r(1) = 0;
    w(1) = 2;
  elseif ( rule == 2 )
    r(1) =-1.0 / sqrt(3.0);
    r(2) =-r(1);
    w(1) = 1.0;
    w(2) = 1.0;
  elseif ( rule == 3 )
    r(1) =-sqrt(3.0/5.0);
    r(2) = 0.0;
    r(3) =-r(1);
    w(1) = 5.0 / 9.0;
    w(2) = 8.0 / 9.0;
    w(3) = w(1);
  elseif ( rule == 4 )
    r(1) =-sqrt((3.0+2.0*sqrt(6.0/5.0))/7.0);
    r(2) =-sqrt((3.0-2.0*sqrt(6.0/5.0))/7.0);
    r(3) =-r(2);
    r(4) =-r(1);
    w(1) = 0.5 - 1.0 / ( 6.0 * sqrt(6.0/5.0) );
    w(2) = 0.5 + 1.0 / ( 6.0 * sqrt(6.0/5.0) );
    w(3) = w(2);
    w(4) = w(1);
  elseif ( rule == 5 )
    r(1) =-sqrt(5.0+4.0*sqrt(5.0/14.0)) / 3.0;
    r(2) =-sqrt(5.0-4.0*sqrt(5.0/14.0)) / 3.0;
    r(3) = 0.0;
    r(4) =-r(2);
    r(5) =-r(1);
    w(1) = 161.0/450.0-13.0/(180.*sqrt(5.0/14.0));
    w(2) = 161.0/450.0+13.0/(180.*sqrt(5.0/14.0));
    w(3) = 128.0/225.0;
    w(4) = w(2);
    w(5) = w(1);
  elseif ( rule == 6 )
    r(1) = -0.2386191861;
    r(2) = -0.6612093865;
    r(3) = -0.9324695142;
    r(4) = - r(1);
    r(5) = - r(2);
    r(6) = - r(3);
    w(1) = .4679139346;
    w(2) = .3607615730;
    w(3) = .1713244924;
    w(4) = w(1);
    w(5) = w(2);
    w(6) = w(3);
  else
    error('Quadrature rule not supported')
    keyboard
  end

  return
end


function [out] = score_ridge(mdv, hilo, lambda, crossval)
% Calculates ridge score
%
% USAGE:
%
%    [out] = score_ridge(mdv, hilo, lambda, crossval)
%
% INPUTS:
%    mdv:         structure
%    hilo:        (0's and 1's), ideally there will be a similar # of each.
%
% OPTIONAL INPUTS:
%    lambda:      ridge parameter, default = .01
%    crossval:    whether to do cross validation. This severely slows
%                 down the computation, default is no.
%
% OUTPUT:
%    out:         score

if nargin < 4
    crossval = 0;  % don't cross validate by default
end
if nargin < 3
    lambda = .01;
end

hilo(hilo ==0) = -1; % make 0's into ones.
[nvars, npoints] = size(mdv);

if crossval == 0
    ybar = mean(hilo);
    b = inv(mdv*mdv' + lambda*eye(nvars))*mdv*hilo;

    yhat = mdv'*b+ybar;
    out = sum(sign(yhat) == sign(hilo))/npoints;
else
    results = zeros(npoints,1);
    for i = 1:npoints
        hilot = hilo([1:i-1,i+1:npoints]);
        mdvt = mdv(:,[1:i-1,i+1:npoints]);

        ybart = mean(hilot);
        b = inv(mdvt*mdvt' + lambda*eye(nvars))*mdvt*hilot;

        yhat = mdv(:,i)'*b+ybart;
        results(i)  = sign(yhat) == sign(hilo(i));
    end
    out = sum(results)/npoints;
end


function x = equidistant1 ( n )

%*****************************************************************************80
%
%% EQUIDISTANT1 returns the Type 1 Equidistant points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2018
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the points.
%
  x(1:n) = ( 1 - n : 2 : n - 1 ) / ( n + 1 );

  return
end


function rank = bal_seq_rank ( n, t )

%*****************************************************************************80
%
%% BAL_SEQ_RANK ranks a balanced sequence.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of 0's (and 1's) in the sequence.
%    N must be positive.
%
%    Input, integer T(2*N), a balanced sequence.
%
%    Output, integer RANK, the rank of the balanced sequence.
%

%
%  Check.
%
  ierror = bal_seq_check ( n, t );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BAL_SEQ_RANK - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'BAL_SEQ_RANK - Fatal error!' );
  end

  y = 0;
  rank = 0;

  for x = 1 : 2 * n - 1

    if ( t(x) == 0 )
      y = y + 1;
    else
      mxy = mountain ( n, x, y + 1 );
      rank = rank + mxy;
      y = y - 1;
    end

  end

  return
end


%--- help for sym/collect ---
%
% COLLECT Collect coefficients.
%    COLLECT(S,v) regards each element of the symbolic matrix S as a
%    polynomial in v and rewrites S in terms of the powers of v.
%    COLLECT(S) uses the default variable determined by SYMVAR.
%    COLLECT(S, 'f') uses all symbolic calls to the function f as variables.
%    COLLECT(S, {'f1', ..., 'fk'}) uses all symbolic calls to any of the
%    functions f1, ..., fk as variables.
%
%    Examples:
%       syms x y
%
%       collect(x^2*y + y*x - x^2 - 2*x)  returns (y - 1)*x^2 + (y - 2)*x
%
%       f = -1/4*x*exp(-2*x)+3/16*exp(-2*x)
%       collect(f,exp(-2*x))  returns -(x/4 - 3/16)/exp(2*x)
%
%       f = x*sin(x) + sin(x) + x*sin(2*x) - sin(2*x)
%       collect(f, 'sin')  returns (x + 1)*sin(x) + (x - 1)*sin(2*x)
%
%    See also SYM/SIMPLIFY, SYM/FACTOR, SYM/EXPAND, SYM/SYMVAR.
%
%    Other functions named collect
%
%       ts/collect
%


function v = mono_value ( d, nx, f, x )

%*****************************************************************************80
%
%% MONO_VALUE evaluates a monomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 December 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D, the spatial dimension.
%
%    Input, integer NX, the number of evaluation points.
%
%    Input, integer F(D), the exponents of the monomial.
%
%    Input, real X(D,NX), the coordinates of the evaluation points.
%
%    Output, real V(NX), the value of the monomial at X.
%
  v = ones ( nx, 1 );

  for i = 1 : d
    v(1:nx,1) = v(1:nx,1) .* x(i,1:nx)' .^ f(i);
  end

  return
end


function out = SY_StatAv(y,whatType,n)
% SY_StatAv     Simple mean-stationarity metric, StatAv.
%
% The StatAv measure divides the time series into non-overlapping subsegments,
% calculates the mean in each of these segments and returns the standard deviation
% of this set of means.
%
% Empirically mean-stationary data would display StatAv approaching to zero.
%
% cf. "Heart rate control in normal and aborted-SIDS infants", S. M. Pincus et al.
% Am J. Physiol. Regul. Integr. Comp. Physiol. 264(3) R638 (1993)
%
%---INPUTS:
%
% y, the input time series
%
% whatType, the type of StatAv to perform:
%           (i) 'seg': divide the time series into n segments
%           (ii) 'len': divide the time series into segments of length n
%
% n, either the number of subsegments ('seg') or their length ('len')

% Might be nicer to use the 'buffer' function for this...?
% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
% Check Inputs
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(whatType)
    whatType = 'seg'; % divide into n segments by default
end

if nargin < 3 || isempty(n)
    n = 5; % use 5 segments
end

N = length(y); % Time-series length

% ------------------------------------------------------------------------------
% Compute the means in local time-series segments
% ------------------------------------------------------------------------------

switch whatType
case 'seg'
    % divide time series into n segments
    M = zeros(n,1);
    p = floor(N/n);% lose the last N mod n data points

    for j = 1:n
        M(j) = mean(y(p*(j-1)+1:p*j));
    end
case 'len'
    if N > 2*n
        pn = floor(N/n);
        M = zeros(pn,1);
        for j = 1:pn
            M(j) = mean(y((j-1)*n+1:j*n));
        end
    else
        fprintf(1,'This time series (N = %u) is too short for StatAv(%s,''%u'')\n',N,whatType,n);
        out = NaN; return
    end
otherwise
    error('Error evaluating StatAv of type ''%s'', please select either ''seg'' or ''len''',whatType)
end

% ------------------------------------------------------------------------------
% Compute the StatAv statistic

s = std(y); % should be 1 (for a z-scored time-series input)
sdav = std(M);
out = sdav/s;

end


%DEMEV2	Demonstrate Bayesian classification for the MLP.
%
%	Description
%	A synthetic two class two-dimensional dataset X is sampled  from a
%	mixture of four Gaussians.  Each class is associated with two of the
%	Gaussians so that the optimal decision boundary is non-linear. A 2-
%	layer network with logistic outputs is trained by minimizing the
%	cross-entropy error function with isotroipc Gaussian regularizer (one
%	hyperparameter for each of the four standard weight groups), using
%	the scaled conjugate gradient optimizer. The hyperparameter vectors
%	ALPHA and BETA are re-estimated using the function EVIDENCE. A graph
%	is plotted of the optimal, regularised, and unregularised decision
%	boundaries.  A further plot of the moderated versus unmoderated
%	contours is generated.
%
%	See also
%	EVIDENCE, MLP, SCG, DEMARD, DEMMLP2
%

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

clc;

disp('This program demonstrates the use of the evidence procedure on')
disp('a two-class problem.  It also shows the improved generalisation')
disp('performance that can be achieved with moderated outputs; that is')
disp('predictions where an approximate integration over the true')
disp('posterior distribution is carried out.')
disp(' ')
disp('First we generate a synthetic dataset with two-dimensional input')
disp('sampled from a mixture of four Gaussians.  Each class is')
disp('associated with two of the Gaussians so that the optimal decision')
disp('boundary is non-linear.')
disp(' ')
disp('Press any key to see a plot of the data.')
pause;

% Generate the matrix of inputs x and targets t.

rand('state', 423);
randn('state', 423);

ClassSymbol1 = 'r.';
ClassSymbol2 = 'y.';
PointSize = 12;
titleSize = 10;

fh1 = figure;
set(fh1, 'Name', 'True Data Distribution');
whitebg(fh1, 'k');

%
% Generate the data
%
n=200;

% Set up mixture model: 2d data with four centres
% Class 1 is first two centres, class 2 from the other two
mix = gmm(2, 4, 'full');
mix.priors = [0.25 0.25 0.25 0.25];
mix.centres = [0 -0.1; 1.5 0; 1 1; 1 -1];
mix.covars(:,:,1) = [0.625 -0.2165; -0.2165 0.875];
mix.covars(:,:,2) = [0.25 0; 0 0.25];
mix.covars(:,:,3) = [0.2241 -0.1368; -0.1368 0.9759];
mix.covars(:,:,4) = [0.2375 0.1516; 0.1516 0.4125];

[data, label] = gmmsamp(mix, n);

%
% Calculate some useful axis limits
%
x0 = min(data(:,1));
x1 = max(data(:,1));
y0 = min(data(:,2));
y1 = max(data(:,2));
dx = x1-x0;
dy = y1-y0;
expand = 5/100;			% Add on 5 percent each way
x0 = x0 - dx*expand;
x1 = x1 + dx*expand;
y0 = y0 - dy*expand;
y1 = y1 + dy*expand;
resolution = 100;
step = dx/resolution;
xrange = [x0:step:x1];
yrange = [y0:step:y1];
%
% Generate the grid
%
[X Y]=meshgrid([x0:step:x1],[y0:step:y1]);
%
% Calculate the class conditional densities, the unconditional densities and
% the posterior probabilities
%
px_j = gmmactiv(mix, [X(:) Y(:)]);
px = reshape(px_j*(mix.priors)',size(X));
post = gmmpost(mix, [X(:) Y(:)]);
p1_x = reshape(post(:, 1) + post(:, 2), size(X));
p2_x = reshape(post(:, 3) + post(:, 4), size(X));

plot(data((label<=2),1),data(label<=2,2),ClassSymbol1, 'MarkerSize', ...
PointSize)
hold on
axis([x0 x1 y0 y1])
plot(data((label>2),1),data(label>2,2),ClassSymbol2, 'MarkerSize', ...
    PointSize)

% Convert targets to 0-1 encoding
target=[label<=2];
disp(' ')
disp('Press any key to continue')
pause; clc;

disp('Next we create a two-layer MLP network with 6 hidden units and')
disp('one logistic output.  We use a separate inverse variance')
disp('hyperparameter for each group of weights (inputs, input bias,')
disp('outputs, output bias) and the weights are optimised with the')
disp('scaled conjugate gradient algorithm.  After each 100 iterations')
disp('the hyperparameters are re-estimated twice.  There are eight')
disp('cycles of the whole algorithm.')
disp(' ')
disp('Press any key to train the network and determine the hyperparameters.')
pause;

% Set up network parameters.
nin = 2;		% Number of inputs.
nhidden = 6;		% Number of hidden units.
nout = 1;		% Number of outputs.
alpha = 0.01;		% Initial prior hyperparameter.
aw1 = 0.01;
ab1 = 0.01;
aw2 = 0.01;
ab2 = 0.01;

% Create and initialize network weight vector.
prior = mlpprior(nin, nhidden, nout, aw1, ab1, aw2, ab2);
net = mlp(nin, nhidden, nout, 'logistic', prior);

% Set up vector of options for the optimiser.
nouter = 8;			% Number of outer loops.
ninner = 2;			% Number of innter loops.
options = foptions;		% Default options vector.
options(1) = 1;			% This provides display of error values.
options(2) = 1.0e-5;		% Absolute precision for weights.
options(3) = 1.0e-5;		% Precision for objective function.
options(14) = 100;		% 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, data, target, 'scg');
  [net, gamma] = evidence(net, data, target, ninner);
  fprintf(1, '\nRe-estimation cycle %d:\n', k);
  disp(['  alpha = ', num2str(net.alpha')]);
  fprintf(1, '  gamma =  %8.5f\n\n', gamma);
  disp(' ')
  disp('Press any key to continue.')
  pause;
end

disp(' ')
disp('Network training and hyperparameter re-estimation are now complete.')
disp('Notice that the final error value is close to the number of data')
disp(['points (', num2str(n), ') divided by two.'])
disp('Also, the hyperparameter values differ, which suggests that a single')
disp('hyperparameter would not be so effective.')
disp(' ')
disp('First we train an MLP without Bayesian regularisation on the')
disp('same dataset using 400 iterations of scaled conjugate gradient')
disp(' ')
disp('Press any key to train the network by maximum likelihood.')
pause;
% Train standard network
net2 = mlp(nin, nhidden, nout, 'logistic');
options(14) = 400;
net2 = netopt(net2, options, data, target, 'scg');
y2g = mlpfwd(net2, [X(:), Y(:)]);
y2g = reshape(y2g(:, 1), size(X));

disp(' ')
disp('We can now plot the function represented by the trained networks.')
disp('We show the decision boundaries (output = 0.5) and the optimal')
disp('decision boundary given by applying Bayes'' theorem to the true')
disp('data model.')
disp(' ')
disp('Press any key to add the boundaries to the plot.')
pause;

% Evaluate predictions.
[yg, ymodg] = mlpevfwd(net, data, target, [X(:) Y(:)]);
yg = reshape(yg(:,1),size(X));
ymodg = reshape(ymodg(:,1),size(X));

% Bayesian decision boundary
[cB, hB] = contour(xrange,yrange,p1_x,[0.5 0.5],'b-');
[cNb, hNb] = contour(xrange,yrange,yg,[0.5 0.5],'r-');
[cN, hN] = contour(xrange,yrange,y2g,[0.5 0.5],'g-');
set(hB, 'LineWidth', 2);
set(hNb, 'LineWidth', 2);
set(hN, 'LineWidth', 2);
Chandles = [hB(1) hNb(1) hN(1)];
legend(Chandles, 'Bayes', ...
  'Reg. Network', 'Network', 3);

disp(' ')
disp('Note how the regularised network predictions are closer to the')
disp('optimal decision boundary, while the unregularised network is')
disp('overtrained.')

disp(' ')
disp('We will now compare moderated and unmoderated outputs for the');
disp('regularised network by showing the contour plot of the posterior')
disp('probability estimates.')
disp(' ')
disp('The first plot shows the regularised (moderated) predictions')
disp('and the second shows the standard predictions from the same network.')
disp('These agree at the level 0.5.')
disp('Press any key to continue')
pause
levels = 0:0.1:1;
fh4 = figure;
set(fh4, 'Name', 'Moderated outputs');
hold on
plot(data((label<=2),1),data(label<=2,2),'r.', 'MarkerSize', PointSize)
plot(data((label>2),1),data(label>2,2),'y.', 'MarkerSize', PointSize)

[cNby, hNby] = contour(xrange, yrange, ymodg, levels, 'k-');
set(hNby, 'LineWidth', 1);

fh5 = figure;
set(fh5, 'Name', 'Unmoderated outputs');
hold on
plot(data((label<=2),1),data(label<=2,2),'r.', 'MarkerSize', PointSize)
plot(data((label>2),1),data(label>2,2),'y.', 'MarkerSize', PointSize)

[cNbm, hNbm] = contour(xrange, yrange, yg, levels, 'k-');
set(hNbm, 'LineWidth', 1);

disp(' ')
disp('Note how the moderated contours are more widely spaced.  This shows')
disp('that there is a larger region where the outputs are close to 0.5')
disp('and a smaller region where the outputs are close to 0 or 1.')
disp(' ')
disp('Press any key to exit')
pause
close(fh1);
close(fh4);
close(fh5);


function colors = RainbowColors_ss(numcolors)
% Generates RGB Triplets based on the idealized colors below:
% 0. red    [1 0 0]
% 1. orange [1 .5 0]
% 2. yellow [1 1 0]
% 2.5. green  [0 1 0]
% 3. cyan   [0 1 1]
% 4. blue   [0 0 1]
% 5. purple [.5 0 .5]
%
% INPUT
% -numcolors: number of colors you want out
% OUTPUT
% - colors a 3 column matrix signifying Red Green and Blue values with each
% column, number of rows = numcolors
%
% Brendon Watson 2015

cidxs = linspace(0,1,numcolors);
r = redfunc(cidxs);
g = greenfunc(cidxs);
b = bluefunc(cidxs);

colors = [r g b];

1;

function y = redfunc(x)
for a = 1:length(x)
    if x(a)<2/5
        y(a) = 1;
    elseif x(a)>=2/5 && x(a)<2.5/5
        y(a) = 1-5*(x(a)-2/5);
    elseif x(a)>=2.5/5 && x(a)<4/5
        y(a) = 0;
    elseif x(a)>=4/5
        y(a) = 0+.5*5*(x(a)-4/5);
    end
end
y = y';

function y = greenfunc(x)
for a = 1:length(x)
    if x(a)<2/5
        y(a) = .5*5*(x(a));
    elseif x(a)>=2/5 && x(a)<3/5
        y(a) = 1;
    elseif x(a)>=3/5 && x(a)<4/5
        y(a) = 1-5*(x(a)-3/5);
    elseif x(a)>=4/5
        y(a) = 0;
    end
end
y = y';

function y = bluefunc(x)
for a = 1:length(x)
    if x(a)<3/5
        y(a) = 0;
    elseif x(a)>=2.5/5 && x(a)<3/5
        y(a) = 5*(x(a)-2.5/5);
    elseif x(a)>=3/5 && x(a)<4/5
        y(a) = 1;
    elseif x(a)>=4/5
        y(a) = 1-.5*5*(x(a)-4/5);
    end
end
y = y';


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Compare different clustering methods of event sequences
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear

options.N = 100; % the number of sequences
options.Nmax = 100; % the maximum number of events per sequence
options.Tmax = 50; % the maximum size of time window
options.tstep = 0.1;
options.dt = 0.1;
options.M = 250;
options.GenerationNum = 10;
D = 3; % the dimension of Hawkes processes
K = 2; % the number of clusters
nTest = 5;
nSeg = 5;
nNum = options.N/nSeg;

disp('Approximate simulation of Hawkes processes via branching process')
% the first cluster contains the event sequences generated by a Hawkes process with exponential kernel
disp('Simple exponential kernel')
para1.kernel = 'exp';
para1.landmark = 0;
L = length(para1.landmark);
para1.mu = rand(D,1)/D;
para1.A = zeros(D, D, L);
for l = 1:L
    para1.A(:,:,l) = (0.7^l)*rand(D);
end
para1.A = 0.5 * para1.A./max(abs(eig(para1.A)));
para1.w = 0.5;
Seqs1 = Simulation_Branch_HP(para1, options);

% the second cluster contains the event sequences generated by a Hawkes process with gaussian kernel
disp('Complicated gaussian kernel')
para2.kernel = 'gauss';
para2.landmark = 0:3:12;
L = length(para2.landmark);
para2.mu = rand(D,1)/D;
para2.A = zeros(D, D, L);
for l = 1:L
    para2.A(:,:,l) = (0.9^l)*rand(D);
end
para2.A = 0.25 * para2.A./max(abs(eig(sum(para2.A,3))));
para2.A = reshape(para2.A, [D, L, D]);
para2.w = 1;
Seqs2 = Simulation_Branch_HP(para2, options);

SeqsMix = [Seqs1,Seqs2];

% ground truth: the similarity matrix of event sequences.
% GT(i,j)=1 if the sequence i and the sequence j belong to a same cluster
GT = blkdiag(ones(options.N), ones(options.N));

% Learning a mixture model of Hawkes processes
% initialize
model = Initialization_Cluster_Basis(SeqsMix, 2);
alg.outer = 8;
alg.rho = 0.1;
alg.inner = 5;
alg.thres = 1e-5;
alg.Tmax = [];
model = Learning_Cluster_Basis(SeqsMix, model, alg);
[~, labels1] = max(model.R, [],2);
% estimate similarity matrix
Est1 = zeros(length(labels1));
for i = 1:length(labels1)
    for j = 1:length(labels1)
        if labels1(i)==labels1(j)
            Est1(i,j) = 1;
        end
    end
end

% Learning a distance metric of marked point processes
configure.M = [options.Tmax; D];
configure.id = {1,2,[1,2]};
configure.tau = 1;
configure.W = 5;
configure.epoch = 15;
configure.lr = 1e-4;
[configure, obj] = Estimate_Weight(configure, SeqsMix);
Dis = zeros(length(SeqsMix));
tic
for n = 1:length(SeqsMix)-1
    Xn = [SeqsMix(n).Time; SeqsMix(n).Mark];
    for m = n+1:length(SeqsMix)
        Xm = [SeqsMix(m).Time; SeqsMix(m).Mark];
        [Dis(n,m), ~] = DistanceSum_MPP(Xn, Xm, configure);
    end
    toc
end
Dis = Dis + Dis';
% estimate similarity matrix
SimilarMat = exp(-Dis.^2./(2*var(Dis(:))));

% visualize clustering results
figure
subplot(131)
imshow(GT,[])
title('Ground truth of clusters')
subplot(132)
imshow(Est1,[])
title('Mixture of Hawkes processess')
subplot(133)
imshow(SimilarMat,[])
title('Distance metrics of MPP')


function CRB = crb_general_sto_1d(design, wavelength, doas, P, noise_var, snapshot_count)
%CRB_GENERAL_STO_1D CRB for general 1D arrays based on the stochastic
%(unconditional) model, in radians.
%Inputs:
%   design - Array design.
%   wavelength - Wavelength.
%   doas - DOA vector in radians.
%   P - Source covariance matrix. If all sources are uncorrelated and
%       shares the same power, you can just pass in a scalar. If all
%       sources are uncorrelated but have different powers, you can just
%       pass in a vector.
%   noise_var - Noise power.
%   snapshot_count - (Optional) number of snapshots. Default is one.
%Reference:
% [1] P. Stoica and A. Nehorai, "Performance study of conditional and
%     unconditional direction-of-arrival estimation," IEEE Transactions on
%     Acoustics, Speech and Signal Processing, vol. 38, no. 10,
%     pp. 1783-1795, Oct. 1990.
if design.dim ~= 1
    error('1D array expected.');
end
if nargin <= 5
    snapshot_count = 1;
end
m = design.element_count;
k = length(doas);
P = unify_source_power_matrix(P, k);
[A, D] = steering_matrix(design, wavelength, doas);
R = A*P*A' + noise_var * eye(m);
H = D'*(eye(m) - A/(A'*A)*A')*D;
CRB = real(H .* ((P*A'/R*A*P).'));
CRB = eye(k) / CRB * (noise_var / snapshot_count / 2);
end


function particles = correction_step(particles, z)

% Weight the particles according to the current map of the particle
% and the landmark observations z.
% z: struct array containing the landmark observations.
% Each observation z(j) has an id z(j).id, a range z(j).range, and a bearing z(j).bearing
% The vector observedLandmarks indicates which landmarks have been observed
% at some point by the robot.

% Number of particles
numParticles = length(particles);

% Number of measurements in this time step
m = size(z, 2);

% TODO: Construct the sensor noise matrix Q_t (2 x 2)
Q_t = 0.01*eye(2);
% process each particle
for i = 1:numParticles
  robot = particles(i).pose;
  % process each measurement
  for j = 1:m
    % Get the id of the landmark corresponding to the j-th observation
    % particles(i).landmarks(l) is the EKF for this landmark
    l = z(j).id;

    % The (2x2) EKF of the landmark is given by
    % its mean particles(i).landmarks(l).mu
    % and by its covariance particles(i).landmarks(l).sigma

    % If the landmark is observed for the first time:
    if (particles(i).landmarks(l).observed == false)

      % TODO: Initialize its position based on the measurement and the current robot pose:

      particles(i).landmarks(l).mu = [robot(1)+z(j).range*cos(z(j).bearing + robot(3)); robot(2) + z(j).range*sin(z(j).bearing + robot(3))];

      % get the Jacobian with respect to the landmark position
      [h, H] = measurement_model(particles(i), z(j));

      % TODO: initialize the EKF for this landmark

      particles(i).landmarks(l).sigma = H\Q_t*inv(H)';

      % Indicate that this landmark has been observed
      particles(i).landmarks(l).observed = true;

    else

      % get the expected measurement
      [expectedZ, H] = measurement_model(particles(i), z(j));

      % TODO: compute the measurement covariance

	Q = H*particles(i).landmarks(l).sigma*H' + Q_t;

      % TODO: calculate the Kalman gain

	K = particles(i).landmarks(l).sigma*H'/Q;

      % TODO: compute the error between the z and expectedZ (remember to normalize the angle)

	z_diff = [z(j).range;z(j).bearing] - expectedZ;
	z_diff(2) = normalize_angle(z_diff(2));

      % TODO: update the mean and covariance of the EKF for this landmark

	particles(i).landmarks(l).mu = particles(i).landmarks(l).mu + K*z_diff;
	particles(i).landmarks(l).sigma = (eye(2) - K*H)*particles(i).landmarks(l).sigma;

      % TODO: compute the likelihood of this observation, multiply with the former weight
      %       to account for observing several features in one time step
	particles(i).weight = particles(i).weight*1/sqrt(det(2*pi*Q))*exp(-1/2*z_diff'/Q*z_diff);
    end

  end % measurement loop
end % particle loop

end


function [X, objV] = proxF_tSVD_1(Y,rho,opts)
% proximal function of tensor nuclear norm
% Authors: G. Ely, S. Aeron, Z. Zhang, ECE, Tufts Univ. 03/16/2015

%% Input checking
parOP = false;
if ~exist('opts','var') && ~isempty(opts)
    nARG = length(opts);
    if nARG > 0
        parOP=opts{1};
    end
end

%%
sa = size(Y);
la = length(sa);
[n1,n2,n3] = size(Y);

% this returned this STILL FFT'd version
[U,S,V]=ntsvd(Y,1,parOP);

sTrueV =zeros(min(n1,n2),n3);
for i = 1:n3
    s = S(:,:,i);  % tube
    s = diag(s);
    sTrueV(:,i) = s;
end
%%
[sTrueV] = proxF_l1(sTrueV,rho);
%objV = sum(sTrueV(:));
objV = sum(sTrueV(:));
%%
for i = 1:min(n1,n2)
    for j = 1:n3
        S(i,i,j) = sTrueV(i,j);
    end
end

for i = la:-1:3
    U = ifft(U,[],i);
    S = ifft(S,[],i);
    V = ifft(V,[],i);
end

% X = tprod( tprod(U,S), ttrans_HO(V));

X = tprod( tprod(U,S), tran(V));


function rad = GB_deg2rad(deg)
%converts from degrees to rad
rad = deg*pi/180;


function S = slsum(A, d)
%SLSUM Compute the sum of values in subarrays
%
% $ Syntax $
%   - S = slsum(A)
%   - S = slsum(A, d)
%   - S = slsum(A, [d1 d2 ... dk])
%
% $ Arguments $
%   - A:        the input array
%   - d:        the dimensions along which the sum is performed
%   - S:        the resultant sum matrix
%
% $ Description $
%   - S = slsum(A) sums up the values in column vectors of A. It is
%     equivalent to S = sum(A)
%
%   - S = slsum(A, d) sums up the values along dimension d. It is
%     equivalent to S = sum(A, d)
%
%   - S = slsum(A, [d1 d2 ... dk]) sums up the values along dimension
%     d1, d2, ... dk.
%
% $ History $
%   - Created by Dahua Lin on Nov 18th, 2005
%

%% parse and verify input arguments
if nargin < 2 || isempty(d)
    d = 1;
end

%% compute
if isscalar(d)
    S = sum(A, d);
else
    k = length(d);
    S = A;
    for i = 1 : k
        S = sum(S, d(i));
    end
end


function [ c, seed ] = c8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% C8VEC_UNIFORM_01 returns a unit pseudorandom C8VEC.
%
%  Discussion:
%
%    The angles should be uniformly distributed between 0 and 2 * PI,
%    the square roots of the radius uniformly distributed between 0 and 1.
%
%    This results in a uniform distribution of values in the unit circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer N, the number of values to compute.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, complex C(N), the pseudorandom complex vector.
%
%    Output, integer SEED, a seed for the random number generator.
%
  c = zeros ( n, 1 );

  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'C8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'C8VEC_UNIFORM_01 - Fatal error!' );
  end

  for j = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    r = sqrt ( seed * 4.656612875E-10 );

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    theta = 2.0 * pi * seed * 4.656612875E-10;

    c(j) = r * ( cos ( theta ) + sin ( theta ) * i );

  end

  return
end


function [points1, points2] = get_uniform_points_boundary(ellipse1, ellipse2, nr_points)
% GET_UNIFORM_POINTS_BOUNDARY gives the sets of points that are
% equidistantly chosen on the boudary of two ellipses, so that future OSPA
% could be calculated
%
% Input:
%        ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2]
%        ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2]
%        nr_points, nr of points that are uniformly chosen on the boundary
%                   to calculate OPSA distance
% Output:
%       points1, 2xnr_points, points that are chosen on the ellipse1
%       poitns2, 2xnr_points, points that are chosen on the ellispe2
% Written by Shishan Yang

theta = (0:2*pi/nr_points:2*pi-2*pi/nr_points);

alpha1 = ellipse1(3);
center1 = ellipse1(1:2)';
l1 = ellipse1(4:5);
gt_rotation_mat = [cos(alpha1) -sin(alpha1); sin(alpha1) cos(alpha1)];
points1(1,:) = l1(1)*cos(theta);
points1(2,:) = l1(2)*sin(theta);

alpha2 = ellipse2(3);
center2 = ellipse2(1:2)';
l2 = ellipse2(4:5);
est_rotation_mat = [cos(alpha2) -sin(alpha2); sin(alpha2) cos(alpha2)];
points2(1,:) = l2(1)*cos(theta);
points2(2,:) = l2(2)*sin(theta);

points1 = gt_rotation_mat*points1 + repmat(center1,1,size(points1,2));
points2 = est_rotation_mat*points2 + repmat(center2,1,size(points2,2));
end


function value = r8_aid ( x )

%*****************************************************************************80
%
%% R8_AID evaluates the derivative of the Airy function Ai of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 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 derivative of the Airy function
%    Ai of X.
%
  persistent aifcs
  persistent aigcs
  persistent naif
  persistent naig
  persistent x2sml
  persistent x3sml

  if ( isempty ( naif ) )
    aifcs = [ ...
       0.105274612265314088088970057325134114, ...
       0.011836136281529978442889292583980840, ...
       0.000123281041732256643051689242469164, ...
       0.000000622612256381399016825658693579, ...
       0.000000001852988878441452950548140821, ...
       0.000000000003633288725904357915995625, ...
       0.000000000000005046217040440664768330, ...
       0.000000000000000005223816555471480985, ...
       0.000000000000000000004185745090748989, ...
       0.000000000000000000000002672887324883, ...
       0.000000000000000000000000001392128006, ...
       0.000000000000000000000000000000602653, ...
       0.000000000000000000000000000000000220 ]';
    aigcs = [ ...
       0.0212338781509186668523122276848937, ...
       0.0863159303352144067524942809461604, ...
       0.0017975947203832313578033963225230, ...
       0.0000142654998755506932526620687495, ...
       0.0000000594379952836832010488787064, ...
       0.0000000001524033664794478945214786, ...
       0.0000000000002645876603490435305100, ...
       0.0000000000000003315624296815020591, ...
       0.0000000000000000003139789757594792, ...
       0.0000000000000000000002325767379040, ...
       0.0000000000000000000000001384384231, ...
       0.0000000000000000000000000000676629, ...
       0.0000000000000000000000000000000276 ]';
    eta = 0.1 * r8_mach ( 3 );
    naif = r8_inits ( aifcs, 13, eta );
    naig = r8_inits ( aigcs, 13, eta );
    x3sml = r8_mach ( 3 )^0.3334;
    x2sml = sqrt ( r8_mach ( 3 ) );
  end

  if ( x < - 1.0 )
    [ xn, phi ] = r8_admp ( x );
    value = xn * cos ( phi );
  elseif ( abs ( x ) <= x2sml )
    x2 = 0.0;
    x3 = 0.0;
    value = ( x2 * ( 0.125 + r8_csevl ( x3, aifcs, naif ) ) ...
      - r8_csevl ( x3, aigcs, naig ) ) - 0.25;
  elseif ( abs ( x ) <= x3sml )
    x2 = x * x;
    x3 = 0.0;
    value = ( x2 * ( 0.125 + r8_csevl ( x3, aifcs, naif ) ) ...
      - r8_csevl ( x3, aigcs, naig ) ) - 0.25;
  elseif ( x <= 1.0 )
    x2 = x * x;
    x3 = x * x * x;
    value = ( x2 * ( 0.125 + r8_csevl ( x3, aifcs, naif ) ) ...
      - r8_csevl ( x3, aigcs, naig ) ) - 0.25;
  else
    value = r8_aide ( x ) ...
      * exp ( - 2.0 * x * sqrt ( x ) / 3.0 );
  end

  return
end


function pols = kjacopols ( x, a, b, n )

%*****************************************************************************80
%
%% KJACOPOLS evaluates Jacobi polynomials.
%
%  Discussion:
%
%    This routine evaluates Jacobi polynomials.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    26 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, real A, B, the parameter values.
%
%    Input, integer N, the highest degree to be evaluated.
%
%    Output, real POLS(N+1), the polynomial values.
%
  pols = zeros(n+1,1);

  pkp1 = 1.0;
  pols(1) = pkp1;

  if ( n == 0 )
    return
  end

  pk = pkp1;
  pkp1 = ( a / 2.0 - b / 2.0 ) ...
    + ( 1.0 + a / 2.0 + b / 2.0 ) * x;
  pols(2) = pkp1;

  if ( n == 1 )
    return
  end

  for k = 2 : n

    pkm1 = pk;
    pk = pkp1;

    alpha = ( 2.0 * k + a + b - 1.0 ) ...
      * ( a * a - b * b + ( 2.0 * k + a + b - 2.0 ) ...
      * ( 2.0 * k + a + b ) * x );

    beta = 2.0 * ( k + a - 1.0 ) * ( k + b - 1.0 ) ...
      * ( 2.0 * k + a + b );

    pkp1 = ( alpha * pk - beta * pkm1 ) ...
      / ( 2.0 * k * ( k + a + b ) ...
      * ( 2.0 * k + a + b - 2.0 ) );

    pols(k+1) = pkp1;

  end

  return
end


function point = intersectPlaneLine(plane, line)
%INTERSECTPLANELINE return intersection between a plane and a line
%
%   PT = intersectPlaneLine(PLANE, LINE) return the intersection point of
%   the given line and the given plane.
%   PLANE : [x0 y0 z0 dx1 dy1 dz1 dx2 dy2 dz2]
%   LINE :  [x0 y0 z0 dx dy dz]
%   PT :    [xi yi zi]
%
%   Note: deprecated. Replaced by function 'intersectLinePlane'
%
%  Songbai Ji (6/23/2006). Bug fixed; also allow one plane, many lines;
% many planes one line; or N planes and N lines configuration in the input.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 17/02/2005.
%

%   HISTORY
%   24/11/2005 add support for multiple input
%   23/06/2006 correction from Songbai Ji
%   14/12/2006 correction for parallel lines and plane normals
%   05/01/2007 fixup for parallel lines and plane normals
%   17/10/2008 add warning for deprecation

warning('IMAEL:deprecatedFunction', ...
    'This function is deprecated, use ''intersectLinePlane'' instead');

% unify sizes of data
if size(plane, 1) == 1;     % one plane possible many lines
    plane = repmat(plane, size(line,1), 1);
elseif size(line,1) == 1;   % one line and many planes
    line = repmat(line, size(plane,1), 1);
elseif (size(plane,1) ~= size(line,1)) ; % N planes and M lines, not allowed for now.
    error('input size not correct, either one/many plane and many/one line, or same # of planes and lines!');
end

% initialize empty array
point = zeros(size(plane, 1), 3);

% plane normal
n = cross(plane(:,4:6), plane(:, 7:9), 2);

% get indices of line and plane which are parallel
par = abs(dot(n, line(:,4:6), 2))<1e-14;
point(par,:) = NaN;
% old version:
% II = find(abs(dot(n, line(:,4:6), 2))<1e-14);
% if ~isempty(II)
%     point(II,:) = [NaN NaN NaN];
% end

% difference between origins of plane and line
dp = plane(:,1:3) - line(:,1:3);

% relative position of intersection on line
% Should be Array multiply, original file had a bug. (songbai ji
% 6/23/2006).
% Divide only for non parallel vectors (DL,
t = dot(n(~par,:), dp(~par,:), 2)./dot(n(~par,:), line(~par,4:6), 2);
%t = dot(n, dp, 2)./dot(n, line(:,4:6), 2);

% compute coord of intersection point
% point = line(:,1:3) + t*line(:,4:6);
% NOTE: original m file had a bug (in the above line).  It should be
% corrected as follows.  (Songbai Ji 6/23/2006).
%point = line(~par,1:3) + repmat(t,1,3).*line(~par,4:6);
%point = line(:,1:3) + repmat(t,1,3).*line(:,4:6);
point(~par, :) = line(~par,1:3) + repmat(t,1,3).*line(~par,4:6);


function [ t, wts ] = h_quadrature_rule ( nt )

%*****************************************************************************80
%
%% H_QUADRATURE_RULE: quadrature for H(i,x).
%
%  Discussion:
%
%    H(i,x) is the physicist's Hermite polynomial of degree I.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NT, the order of the rule.
%
%    Output, real T(NT,1), WTS(NT,1), the points and weights of the rule.
%
  aj = zeros ( nt, 1 );
  bj = sqrt ( ( 1 : nt )' / 2.0 );
  wts = zeros ( nt, 1 );
  wts(1,1) = sqrt ( sqrt ( pi ) );

  [ t, wts ] = imtqlx ( nt, aj, bj, wts );

  wts(1:nt,1) = wts(1:nt,1).^2;

  return
end


function v = dquat2pos(dq)

% DQUAT2POS  Transforms a double quaternion point position representation
%            into a vector representation.
%
%     V = DQUAT2POS(DQ) transforms the double quaternion representation of
%        a point position (in the euclidean space) into a vector v, which
%        represents the point coordinates. DQ is either a vector of size 8
%        or an array of size 8*N (each column represents a point position
%        dual quaternion) where N is the number of point locations. V is a
%        vector of size 3 or an array of size 3*N depending on the input
%        format.
%
% See also POS2DQUAT, DQUAT2VEL, DQUAT2LINE, DQUAT2LINEVEL

sdq = size(dq);
if sdq == [1 8], dq = dq'; sdq = size(dq); end

% wrong format
if sdq(1) ~= 8
    error('DualQuaternion:dquat2pos:wrongsize',...
        '%d rows in the DQ array. It should be 8.',sdq(1));
end

% check point position dual quaternion
tol = 1e-6;
[maxval1,imax1] = max(abs(dq(1,:)-1));
[maxval2,imax2] = max(max(abs(dq(2:5,:))));
imax = union(imax1,imax2);
if maxval1 > tol || maxval2 > tol
    warning('DualQuaternion:dquat2pos:wrongFormat',...
        'At least one dual quaternion is not a point position dual quaternion (tol = %.1e).\n Indices of max values: %d',...
        tol,imax);
end

% extraction of the point position coordinates
v = dq(6:8,:);


function [gamma]=wildual(g,M,L)
%WILDUAL  Wilson dual window
%   Usage:  gamma=wildual(g,M);
%           gamma=wildual(g,M,L);
%
%   Input parameters:
%         g     : Gabor window.
%         M     : Number of modulations.
%         L     : Length of window. (optional)
%   Output parameters:
%         gamma : Canonical dual window.
%
%   `wildual(g,M)` returns the dual window of the Wilson or WMDCT basis with
%   window *g*, parameter *M* and length equal to the length of the window *g*.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |wilwin| for more details.
%
%   If the length of *g* is equal to $2\cdot M$ then the input window is
%   assumed to be an FIR window. In this case, the dual window also has
%   length of $2\cdot M$. Otherwise the smallest possible transform length is
%   chosen as the window length.
%
%   `wildual(g,M,L)` does the same, but now *L* is used as the length of the
%   Wilson basis.
%
%   The input window *g* must be real and whole-point even. If *g* is not
%   whole-point even, then reconstruction using the dual window will not be
%   perfect. For a random window *g*, the window closest to *g* that satisfies
%   these restrictions can be found by ::
%
%     g_wpe = real(peven(g));
%
%   All windows in the toolbox satisfies these restrictions unless
%   clearly stated otherwise.
%
%   See also:  dwilt, wilwin, wmdct, wilorth, isevenfunction

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DWILT
%   REFERENCE: OK

complainif_argnonotinrange(nargin,2,3,mfilename);

if nargin==2
    L=[];
end;

%% ------ step 2: Verify a, M and L
if isempty(L)
    if isnumeric(g)
        % Use the window length
        Ls=length(g);
    else
        % Use the smallest possible length
        Ls=1;
    end;

    % ----- step 2b : Verify M and get L from the window length ----------
    L=dwiltlength(Ls,M);

else

    % ----- step 2a : Verify M and get L

    Luser=dwiltlength(L,M);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DWILTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser);
    end;

end;

%% ----- step 3 : Determine the window

[g,info]=wilwin(g,M,L,upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

%% ----- call gabdual ----------------
a=M;

g=fir2long(g,L);
gamma=2*comp_gabdual_long(g,a,2*M);


function result = torus_6s2 ( func, r1, r2, result )

%*****************************************************************************80
%
%% TORUS_6S2 approximates an integral inside a torus in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      ( SQRT ( X**2 + Y**2 ) - R1 )**2 + Z**2 <= R2**2.
%
%  Discussion:
%
%    An 84 point 6-th degree formula is used, Stroud number TOR3-S2:6-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function of three variables which is to be integrated,
%    of the form:
%      function value = func ( x, y, z )
%
%    Input, real R1, R2, the two radii that define the torus.
%
%    Output, real RESULT, the approximate integral of the function.
%
  norder = 2;
  s = [ 0.322914992E+00, 0.644171310E+00 ];
  weight = [ 0.387077796E+00, 0.165609800E+00 ];

  w = 1.0E+00 / ( 7.0E+00 * r1 * pi );

  quad = 0.0E+00;

  for n = 1 : 7

    u = 0.5E+00 * sqrt ( 3.0E+00 ) * r2;
    cth = cos ( 2.0E+00 * pi * n / 7.0E+00 );
    sth = sin ( 2.0E+00 * pi * n / 7.0E+00 );

    for i = 1 : 2

      u = -u;

      x = ( r1 + u ) * cth;
      y = ( r1 + u ) * sth;
      z = 0.0E+00;
      quad = quad + 0.232710567E+00 * w * ( r1 + u ) * feval ( func, x, y, z );

      x = r1 * cth;
      y = r1 * sth;
      z = u;
      quad = quad + 0.232710567E+00 * w * r1 * feval ( func, x, y, z );

    end

    for k = 1 : norder

      u = s(k) * r2;
      v = u;

      for i = 1 : 2

        u = -u;

        for j = 1 : 2

          v = -v;

          x = ( r1 + u ) * cth;
          y = ( r1 + u ) * sth;
          z = v;
          quad = quad + weight(k) * w * ( r1 + u ) * feval ( func, x, y, z );

        end
      end
    end
  end

  volume = torus_volume_3d ( r1, r2 );
  result = quad * volume;

  return
end


function u = nvecs(X,n,r,opts)
%NVECS Compute the leading mode-n vectors for a tensor.
%
%   U = NVECS(X,n,r) computes the r leading eigenvalues of Xn*Xn'
%   (where Xn is the mode-n matricization of X), which provides
%   information about the mode-n fibers. In two-dimensions, the r
%   leading mode-1 vectors are the same as the r left singular vectors
%   and the r leading mode-2 vectors are the same as the r right
%   singular vectors.
%
%   U = NVECS(X,n,r,OPTS) specifies options:
%   OPTS.eigsopts: options passed to the EIGS routine [struct('disp',0)]
%   OPTS.flipsign: make each column's largest element positive [true]
%
%   See also TENSOR, TENMAT, EIGS.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

if ~exist('opts','var')
    opts = struct;
end

if isfield(opts,'eigsopts')
    eigsopts = opts.eigsopts;
else
    eigsopts.disp = 0;
end

Xn = double(tenmat(X,n));
Y = Xn*Xn';
warning off;
[u,d] = eigs(Y, r, 'LM', eigsopts);
warning on;
if isfield(opts,'flipsign')
    flipsign = opts.flipsign;
else
    flipsign = true;
end

if flipsign
    % Make the largest magnitude element be positive
    [val,loc] = max(abs(u));
    for i = 1:r
        if u(loc(i),i) < 0
            u(:,i) = u(:,i) * -1;
        end
    end
end


function	[out,dout]=rodrigues(in)

% RODRIGUES	Transform rotation matrix into rotation vector and viceversa.
%
%		Sintax:  [OUT]=RODRIGUES(IN)
% 		If IN is a 3x3 rotation matrix then OUT is the
%		corresponding 3x1 rotation vector
% 		if IN is a rotation 3-vector then OUT is the
%		corresponding 3x3 rotation matrix
%

%%
%%		Copyright (c) March 1993 -- Pietro Perona
%%		California Institute of Technology
%%

%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!

%% BUG when norm(om)=pi fixed -- April 6th, 1997;
%% Jean-Yves Bouguet

%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
%% Jean-Yves Bouguet

% BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007

[m,n] = size(in);
%bigeps = 10e+4*eps;
bigeps = 10e+20*eps;

if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
    theta = norm(in);
    if theta < eps
        R = eye(3);

        %if nargout > 1,

        dRdin = [0 0 0;
            0 0 1;
            0 -1 0;
            0 0 -1;
            0 0 0;
            1 0 0;
            0 1 0;
            -1 0 0;
            0 0 0];

        %end;

    else
        if n==length(in)  in=in'; end; 	%% make it a column vec. if necess.

        %m3 = [in,theta]

        dm3din = [eye(3);in'/theta];

        omega = in/theta;

        %m2 = [omega;theta]

        dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];

        alpha = cos(theta);
        beta = sin(theta);
        gamma = 1-cos(theta);
        omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
        A = omega*omega';

        %m1 = [alpha;beta;gamma;omegav;A];

        dm1dm2 = zeros(21,4);
        dm1dm2(1,4) = -sin(theta);
        dm1dm2(2,4) = cos(theta);
        dm1dm2(3,4) = sin(theta);
        dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
            0 0 -1 0 0 0 1 0 0;
            0 1 0 -1 0 0 0 0 0]';

        w1 = omega(1);
        w2 = omega(2);
        w3 = omega(3);

        dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
        dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
        dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];

        R = eye(3)*alpha + omegav*beta + A*gamma;

        dRdm1 = zeros(9,21);

        dRdm1([1 5 9],1) = ones(3,1);
        dRdm1(:,2) = omegav(:);
        dRdm1(:,4:12) = beta*eye(9);
        dRdm1(:,3) = A(:);
        dRdm1(:,13:21) = gamma*eye(9);

        dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;

    end;
    out = R;
    dout = dRdin;

    %% it is prob. a rot matr.
elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
        & (abs(det(in)-1) < bigeps))
    R = in;

    % project the rotation matrix to SO(3);
    [U,S,V] = svd(R);
    R = U*V';

    tr = (trace(R)-1)/2;
    dtrdR = [1 0 0 0 1 0 0 0 1]/2;
    theta = real(acos(tr));

    if sin(theta) >= 1e-4,

        dthetadtr = -1/sqrt(1-tr^2);

        dthetadR = dthetadtr * dtrdR;
        % var1 = [vth;theta];
        vth = 1/(2*sin(theta));
        dvthdtheta = -vth*cos(theta)/sin(theta);
        dvar1dtheta = [dvthdtheta;1];

        dvar1dR =  dvar1dtheta * dthetadR;

        om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';

        dom1dR = [0 0 0 0 0 1 0 -1 0;
            0 0 -1 0 0 0 1 0 0;
            0 1 0 -1 0 0 0 0 0];

        % var = [om1;vth;theta];
        dvardR = [dom1dR;dvar1dR];

        % var2 = [om;theta];
        om = vth*om1;
        domdvar = [vth*eye(3) om1 zeros(3,1)];
        dthetadvar = [0 0 0 0 1];
        dvar2dvar = [domdvar;dthetadvar];

        out = om*theta;
        domegadvar2 = [theta*eye(3) om];

        dout = domegadvar2 * dvar2dvar * dvardR;

    else
        if tr > 0; 			% case norm(om)=0;

            out = [0 0 0]';

            dout = [0 0 0 0 0 1/2 0 -1/2 0;
                0 0 -1/2 0 0 0 1/2 0 0;
                0 1/2 0 -1/2 0 0 0 0 0];
        else

            % case norm(om)=pi;
            if(0)

                %% fixed April 6th by Bouguet -- not working in all cases!
                out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
                %keyboard;

            else

                % Solution by Mike Burl on Feb 27, 2007
                % This is a better way to determine the signs of the
                % entries of the rotation vector using a hash table on all
                % the combinations of signs of a pairs of products (in the
                % rotation matrix)

                % Define hashvec and Smat
                hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11];
                Smat = [1,1,1; 1,0,-1; 0,1,-1; 1,-1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,-1;
                    1,-1,-1; 1,-1,1];

                M = (R+eye(3,3))/2;
                uabs = sqrt(M(1,1));
                vabs = sqrt(M(2,2));
                wabs = sqrt(M(3,3));

                mvec = [M(1,2), M(2,3), M(1,3)];
                syn  = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function
                hash = syn * [9; 3; 1];
                idx = find(hash == hashvec);
                svec = Smat(idx,:)';

                if ( isempty(svec) )
                    out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
                else
                    out = theta * [uabs; vabs; wabs] .* svec;
                end

            end;

            if nargout > 1,
                fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
                dout = NaN*ones(3,9);
            end;
        end;
    end;

else
    error('Neither a rotation matrix nor a rotation vector were provided');
end;

return;

%% test of the Jacobians:

%%%% TEST OF dRdom:
om = randn(3,1);
dom = randn(3,1)/1000000;

[R1,dR1] = rodrigues(om);
R2 = rodrigues(om+dom);

R2a = R1 + reshape(dR1 * dom,3,3);

gain = norm(R2 - R1)/norm(R2 - R2a)

%%% TEST OF dOmdR:
om = randn(3,1);
R = rodrigues(om);
dom = randn(3,1)/10000;
dR = rodrigues(om+dom) - R;

[omc,domdR] = rodrigues(R);
[om2] = rodrigues(R+dR);

om_app = omc + domdR*dR(:);

gain = norm(om2 - omc)/norm(om2 - om_app)

%%% OTHER BUG: (FIXED NOW!!!)

omu = randn(3,1);
omu = omu/norm(omu)
om = pi*omu;
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

%%% NORMAL OPERATION

om = randn(3,1);
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

return

% Test: norm(om) = pi

u = randn(3,1);
u = u / sqrt(sum(u.^2));
om = pi*u;
R = rodrigues(om);

R2 = rodrigues(rodrigues(R));

norm(R - R2)


function [r, v] = orb2eci(mu, oev)

% convert classical orbital elements to eci state vector

% input

%  mu     = gravitational constant (km**3/sec**2)
%  oev(1) = semimajor axis (kilometers)
%  oev(2) = orbital eccentricity (non-dimensional)
%           (0 <= eccentricity < 1)
%  oev(3) = orbital inclination (radians)
%           (0 <= inclination <= pi)
%  oev(4) = argument of perigee (radians)
%           (0 <= argument of perigee <= 2 pi)
%  oev(5) = right ascension of ascending node (radians)
%           (0 <= raan <= 2 pi)
%  oev(6) = true anomaly (radians)
%           (0 <= true anomaly <= 2 pi)

% output

%  r = eci position vector (kilometers)
%  v = eci velocity vector (kilometers/second)

% Orbital Mechanics with MATLAB

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

r = zeros(3, 1);
v = zeros(3, 1);

% unload orbital elements array

sma = oev(1);
ecc = oev(2);
inc = oev(3);
argper = oev(4);
raan = oev(5);
tanom = oev(6);

slr = sma * (1 - ecc * ecc);

rm = slr / (1 + ecc * cos(tanom));

arglat = argper + tanom;

sarglat = sin(arglat);
carglat = cos(arglat);

c4 = sqrt(mu / slr);
c5 = ecc * cos(argper) + carglat;
c6 = ecc * sin(argper) + sarglat;

sinc = sin(inc);
cinc = cos(inc);

sraan = sin(raan);
craan = cos(raan);

% position vector

r(1) = rm * (craan * carglat - sraan * cinc * sarglat);
r(2) = rm * (sraan * carglat + cinc * sarglat * craan);
r(3) = rm * sinc * sarglat;

% velocity vector

v(1) = -c4 * (craan * c6 + sraan * cinc * c5);
v(2) = -c4 * (sraan * c6 - craan * cinc * c5);
v(3) = c4 * c5 * sinc;


function Stall_margin = retreating_stall_envelope(mu_act,Cts_act)
% Datum Rotor Coefficients (mu verus Cw/s)      High Speed Rotor Coefficients (mu verus Cw/s)
% a:	-7.6568	Squared term                    a:	-7.4029	Squared term
% b:	0.2992	Linear term                     b:	1.2392	Linear term
% c:	0.5357	Constant                        c:	0.4827	Constant

highspeed = 0; %0: datum; 1: high speed

% Datum Rotor Coefficients (mu verus Cw/s)
Cdat=[-7.6568   ;
0.2992;
0.5357];

% High Speed Rotor Coefficients (mu verus Cw/s)
Chs=[-7.4029	;
1.2392	;
0.4827	];

C = highspeed*Chs+(1-highspeed)*Cdat;
a = C(1); b = C(2); c = C(3);

Cts_act=Cts_act*2; %because of the way to find Ct in the place where this data comes from (with a 1/2 in Ct calc)

mu_all = a*Cts_act.^2+Cts_act*b+c;

Cts_all = (-b-sqrt(b^2-4*a*(c-mu_act)))/(2*a);

Stall_margin = (mu_all-mu_act)/abs(mu_all);
if isreal(Cts_all)
    Ct_margin = (Cts_all-Cts_act)/abs(Cts_all);
    Stall_margin = (Stall_margin+Ct_margin)/2;
else
    Stall_margin = -abs(Stall_margin);
end


function kf = polynomial_correlation(xf, yf, a, b)
%POLYNOMIAL_CORRELATION Polynomial Kernel at all shifts, i.e. kernel correlation.
%   Evaluates a polynomial kernel with constant A and exponent B, for all
%   relative shifts between input images XF and YF, which must both be MxN.
%   They must also be periodic (ie., pre-processed with a cosine window).
%   The result is an MxN map of responses.
%
%   Inputs and output are all in the Fourier domain.
%
%   Joao F. Henriques, 2014
%   http://www.isr.uc.pt/~henriques/

	%cross-correlation term in Fourier domain
	xyf = xf .* conj(yf);
	xy = sum(real(ifft2(xyf)), 3);  %to spatial domain

	%calculate polynomial response for all positions, then go back to the
	%Fourier domain
	kf = fft2((xy / numel(xf) + a) .^ b);

end


function d = DistanceToSE3(mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes mat: A 4x4 matrix.
% Returns the Frobenius norm to describe the distance of mat from the SE(3)
% manifold.
% Compute the determinant of matR, the top 3x3 submatrix of mat. If
% det(matR) <= 0, return a large number. If det(matR) > 0, replace the top
% 3x3 submatrix of mat with matR' * matR, and set the first three entries
% of the fourth column of mat to zero. Then return norm(mat - I).
% Example Inputs:
%
% clear; clc;
% mat = [1.0, 0.0,   0.0,  1.2;
%        0.0, 0.1, -0.95,  1.5;
%        0.0, 1.0,   0.1, -0.9;
%        0.0, 0.0,   0.1, 0.98];
% d = DistanceToSE3(mat)
%
% Output:
% d =
%     0.1349

[R, p] = TransToRp(mat);
if det(R) > 0
	d = norm([R' * R, [0; 0; 0]; mat(4, :)] - eye(4), 'fro');
else
    d = 1e+9;
end
end


function  Metric_table  = MFSK_DEMOD(Data_in,M,E_s,SNR_dB)
%*** MFSK DEMOD ***
%function  Metric_table  = MFSK_DEMOD(Data_in,M,E_s,SNR_dB)
% Create a (M x n) matrix containing the metrics of the M-FSK demodulator,
% simulating an AWGN channel
% Data_in is random values [0 .. M-1], representing the data that must be
% transmitted
% M is the number of frequencies used (M-FSK)
% E_s is the transmitted signal energy
% SNR_dB is the signal-to-noise ratio, measured in dBs

    n = length(Data_in);

    %Calculate the mu and sigma_2 values from the SNR

    SNR = Convert_SNR(SNR_dB);
    N_o = E_s / SNR;        % Noise Power on channel in dB (Proakis - MFSK)

    mu = 0;
    sigma_2 = (1/2)*N_o;
    sigma = sqrt(sigma_2);

    %******************************
    %*** Create Decision matrix ***
    %******************************

    Metric_table = [];
    T_Metrics =[];

    %Create 2 x M metric outputs for timeslot i (M equals number of frequencies in M-FSK)
    for num = 1:n
        Signal = [];
        for i = 0:M-1
            if (i == Data_in(num))
                %It is the symbol that was transmitted

                %Determine a value for phi
                phi = 2 * pi * rand;

                % Compute the COS Component:
                Signal(i+1,1) = sqrt(E_s) * cos(phi) + normrnd(mu,sigma);

                % Compute the SIN Component
                Signal(i+1,2) = sqrt(E_s) * sin(phi) + normrnd(mu,sigma);

            else
                Signal(i+1,1) = normrnd(mu,sigma);
                Signal(i+1,2) = normrnd(mu,sigma);
            end
        end

        % Compute the Envelope metrics for each symbol
        Metrics = [];  % Vector containing the computed envelope metrics

        for i = 1:M
            Metrics(i) = Decision_metric(Signal(i,1),Signal(i,2));
        end

        Metric_table = [Metric_table transpose(Metrics)];
    end


function plotFit(min_x, max_x, mu, sigma, theta, p)
%PLOTFIT Plots a learned polynomial regression fit over an existing figure.
%Also works with linear regression.
%   PLOTFIT(min_x, max_x, mu, sigma, theta, p) plots the learned polynomial
%   fit with power p and feature normalization (mu, sigma).

% Hold on to the current figure
hold on;

% We plot a range slightly bigger than the min and max values to get
% an idea of how the fit will vary outside the range of the data points
x = (min_x - 15: 0.05 : max_x + 25)';

% Map the X values
X_poly = polyFeatures(x, p);
X_poly = bsxfun(@minus, X_poly, mu);
X_poly = bsxfun(@rdivide, X_poly, sigma);

% Add ones
X_poly = [ones(size(x, 1), 1) X_poly];

% Plot
plot(x, X_poly * theta, '--', 'LineWidth', 2)

% Hold off to the current figure
hold off

end


% Demo of use of TVDIP - total variation denoising algorithm
clear all;

% Generate test signal: piecewise smooth with noise corruption
y = [1.5*ones(10000,1); -0.7*ones(2000,1); 1.3*ones(100,1); -0.3*ones(5000,1)];
N = length(y);
y = y + 0.5*randn(N,1);

% Find the value of lambda greater than which the TVD solution is just the
% mean
lmax = tvdiplmax(y);

% Perform TV denoising for lambda across a range of values up to a small
% fraction of the maximum found above
lratio = [1e-4 1e-3 1e-2 1e-1];
L = length(lratio);
[x, E, status] = tvdip(y,lmax*lratio,1,1e-3,100);

% Plots - display original test signal y, and TVD results
close all;
figure;
for l = 1:L
    subplot(L,1,l);
    hold on;
    plot(y,'-','Color',0.8*[1 1 1]);
    plot(x(:,l),'k-');
    axis tight;
    title(sprintf('\\lambda=%5.2e, \\lambda/\\lambda_{max}=%5.2e',lmax*lratio(l),lratio(l)));
end
xlabel('n');
legend({'Input y_n','TVD x_n'});


function p=v_sone2phon(s)
%V_PHON2SONE convert SONE loudness values to PHONs p=(s)
%Inputs:    s is a matrix of sone values
%
%Outputs:   p is a matrix, the same size as s, of phon values
%
% The phon scale measures perceived loudness in dB; at 1 kHz it is identical to dB SPL
% relative to 20e-6 Pa sound pressure. The sone scale is proportional to apparent loudness
% and, by definition, equals 1 at 40 phon. The form of the loudness curve is taken from [1].
% The hearing threshold at 1 kHz for 18 to 25 year olds with normal hearing is taken from [2].
%
% Refs: [1]	J. Lochner and J. Burger. Form of the loudness function in the presence of masking noise.
%           The Journal of the Acoustical Society of America, 33: 1705, 1961.
%       [2]	ISO/TC43. Acoustics, Normal equal-loudness-level contours.
%           Standard ISO 226:2003, Aug. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: v_sone2phon.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b d
if isempty(a)
    b=1/(log(10)*0.1*0.27); % 0.27 is the exponent from [1] and [2]
    d=exp(2.4/b); % 2.4 dB is teh hearing threshold from [2]
    a=exp(40/b)-d; % scale factor to make p=40 give s=1
end
if nargout>0

    p=b*log(a*s+d);
else
    if nargin<1 || isempty(s)
        pp=linspace(5,90,100)'; % phon values
        ss=v_phon2sone(pp);
    else
        ss=s;
    end
    semilogx(ss,v_sone2phon(ss));
    v_axisenlarge(-1);
    v_xticksi;
    ylabel('phon = dB SPL @ 1 kHz');
    xlabel('sone');
end


%% (Internal) Find unique elements in a vector within a win_size tolerance window
%
%     [unique_elements idx ] = unique_w_tolerance( val1, win_size )
%
%
% Arguments:
%
%   + val1: data elements,
%
%   + win_size: tolerance to consider val1(i) == val1(i+1).
%
% Output:
%
%   + sft_intersect1: the elements in the soft intersection
%
%   + idx1, idx2: the indexes of val1(idx1) which are in the soft
%   intersection.
%
% Example:
%
%
% See also soft_set_intersect
%
% Author: Mariano Llamedo Soria (llamedom at frba.utn.edu.ar)
% Version: 0.1 beta
% Birthdate: 14/05/2016
% Last update: 14/05/2016
% Copyright 2008-2016
%
function [unique_elements, idx ] = unique_w_tolerance( val1, win_size )

if( nargin < 1 || isempty(val1) )
    unique_elements = [];
    idx = [];
    return;
end

if( nargin < 2 || isempty(win_size) )
    win_size = 1;
end

[val1, idx] = unique(val1);

if( length( val1) == 1 )
    unique_elements = val1;
    return;
end

bAux = true;
while( any(bAux) )
    bAux = abs(val1(1:end-1) - val1(2:end)) <= win_size;
    bAux = [bAux; bAux(end)];
    % keep first occurrence
    % avoid removing consecutive occurrences, keep only the first.
    % Use a logic function to do this.
    bAux1 = toeplitz(double([ false(3,1); bAux ]));
    bAux1 = logical(bAux1(3:end-1,1:4));
    bAux1 = bAux1(:,1) & (~bAux1(:,2)) | bAux1(:,1) & bAux1(:,3) & (~bAux1(:,4));

    val1(bAux1) = [];
    idx(bAux1) = [];

end

unique_elements = val1;


function hammersley_dataset ( )

%*****************************************************************************80
%
%% HAMMERSLEY_DATASET generates a Hammersley dataset and writes it to a file.
%
%  Discussion:
%
%    This program is meant to be used interactively.  It's also
%    possible to prepare a simple input file beforehand and use it
%    in batch mode.
%
%    The program requests input values from the user:
%
%    * NDIM, the spatial dimension,
%    * N, the number of points to generate,
%    * STEP, the index of the first subsequence element to be computed.
%    * SEED(1:NDIM), the Hammersley sequence index corresponding to STEP = 0.
%    * LEAP(1:NDIM), the successive jumps in the Hammersley sequence.
%    * BASE(1:NDIM), the bases (usually distinct primes or -N).
%
%    The program generates the data, writes it to the file
%
%      hammersley_NDIM_N.txt
%
%    where "NDIM" and "N" are the numeric values specified by the user,
%    and then asks the user for more input.   To indicate that no further
%    computations are desired, it is enough to input a nonsensical
%    value, such as -1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 September 2004
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Generate a Hammersley dataset.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This program is meant to be used interactively.\n' );
  fprintf ( 1, '  It is also possible to prepare a simple input \n' );
  fprintf ( 1, '  file beforehand and use it in batch mode.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The program requests input values from the user:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  * NDIM, the spatial dimension,\n' );
  fprintf ( 1, '  * N, the number of points to generate,\n' );
  fprintf ( 1, '  * STEP, the index of the first subsequence element.\n' );
  fprintf ( 1, '  * SEED(1:NDIM), the sequence element\n' );
  fprintf ( 1, '    corresponding to STEP = 0\n' );
  fprintf ( 1, '  * LEAP(1:NDIM), the successive jumps in the sequence.\n' );
  fprintf ( 1, '  * BASE(1:M), the bases, usually distinct primes\n' );
  fprintf ( 1, '    or -N (to generate values like J/N).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The program generates the data, writes it to the file\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    hammersley_NDIM_N.txt\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  where "NDIM" and "N" are the numeric values specified\n' );
  fprintf ( 1, '  by the user, and then asks the user for more input.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  To indicate that no further computations are \n' );
  fprintf ( 1, '  desired, it is enough to input a nonsensical value, \n' );
  fprintf ( 1, '  such as -1.\n' );

  while ( 1 )

    fprintf ( 1, '  *\n' );
    fprintf ( 1, ' *\n' );
    fprintf ( 1, '*  Ready to generate a new dataset:\n' );
    fprintf ( 1, ' *\n' );
    fprintf ( 1, '  *\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NDIM is the spatial dimension.\n' );
    fprintf ( 1, '  (Try "2" if you have no preference.\n' );
    fprintf ( 1, '  Any value less than 1 terminates execution.\n' );
    ndim = [];
    ndim = input ( '  Enter NDIM:  ' );

    fprintf ( 1, '  User input NDIM = %d\n', ndim );

    if ( ~halham_ndim_check ( ndim ) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
      fprintf ( 1, '  The input value of NDIM = %d\n', ndim );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  N is the number of points.\n' );
    fprintf ( 1, '  (Try "25" if you have no preference.\n' );
    fprintf ( 1, '  (Any value less than 1 terminates execution.)\n' );
    n = [];
    n = input ( '  Enter N:  ' );

    fprintf ( 1, '  User input N = %d\n', n );

    if ( ~halham_n_check ( n ) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
      fprintf ( 1, '  The input value of N = %d\n', n );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  STEP is the index of the first subsequence element.\n' );
    fprintf ( 1, '  (Try "0" or "1" if you have no preference.\n' );
    fprintf ( 1, '  (Any value less than 0 terminates execution.)\n' );
    step = [];
    step = input ( '  Enter STEP:  ' );

    fprintf ( 1, '  User input STEP = %d\n', step );

    if ( ~halham_step_check ( step ) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
      fprintf ( 1, '  The input value of STEP = %d\n', step );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  SEED(1:NDIM) is the starting element index\n' );
    fprintf ( 1, '  for each coordinate.\n' );
    fprintf ( 1, '  (Try "[0,0,...,0]" if you have no preference.\n' );
    fprintf ( 1, '  (Any value less than 0 terminates execution.)\n' );
    seed = [];
    seed(1:ndim) = input ( '  Enter SEED(1:NDIM):  ' );

    fprintf ( 1, '\n' );
    i4vec_transpose_print ( ndim, seed, 'SEED:' );

    if ( ~halham_seed_check ( ndim, seed ) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
      fprintf ( 1, '  The negative input value of at least one entry of\n' );
      fprintf ( 1, '  SEED is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  LEAP(1:NDIM) is the leaping multiplier\n' );
    fprintf ( 1, '  for each coordinate.\n' );
    fprintf ( 1, '  (Try "[1,1,...,1]" if you have no preference.\n' );
    fprintf ( 1, '  (Another choice is a prime bigger than all the bases.)\n' );
    fprintf ( 1, '  (Any value less than 1 terminates execution.)\n' );
    leap = [];
    leap(1:ndim) = input ( '  Enter LEAP(1:NDIM):  ' );

    fprintf ( 1, '\n' );
    i4vec_transpose_print ( ndim, leap, 'LEAP:' );

    if ( ~halham_leap_check ( ndim, leap ) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
      fprintf ( 1, '  The input value of at least one entry of\n' );
      fprintf ( 1, '  LEAP is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  BASE(1:NDIM) is the base for each coordinate,\n' );
    fprintf ( 1, '  usually distinct primes or -N for values like J/N.\n' );
    fprintf ( 1, '  (Try "[-N,2,3,5,7,11,13,...]" if you have no preference.\n' );
    fprintf ( 1, '  (Any value of 0 or 1 terminates execution.)\n' );
    base = [];
    base(1:ndim) = input ( '  Enter BASE:  ' );

    fprintf ( 1, '\n' );
    i4vec_transpose_print ( ndim, base, 'BASE:' );

    if ( ~hammersley_base_check ( ndim, base ) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'HAMMERSLEY_DATASET\n' );
      fprintf ( 1, '  The input value of at least one entry of\n' );
      fprintf ( 1, '  BASE is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
    end

    r = i4_to_hammersley_sequence ( ndim, n, step, seed, leap, base );

    file_out_name = ...
      strcat ( 'hammersley_', num2str ( ndim ), '_', num2str ( n ), '.txt' );

    halham_write ( ndim, n, step, seed, leap, base, r, file_out_name );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  The data was written to the file "%s".\n', ...
      file_out_name );

  end

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

  return
end


disp(' ');
disp('fdct_wrapping_demo_basic.m -- Displays a curvelet')
disp ('both in the spatial and frequency domains.');
disp(' ');
disp(['This is achieved by setting all the coefficients in the curvelet'])
disp(['domain to zero except that at the required location (which'])
disp(['is set to one). The curvelet is obtained by taking the'])
disp(['adjoint curvelet transform. Notice how the curvelet is sharply '])
disp(['localized in both space and frequency.']);
disp(' ');

% fdct_wrapping_demo_basic.m -- Displays a curvelet both in the spatial and frequency domains.

% m = 512;
% n = 512;
%
% X = zeros(m,n);

%forward curvelet transform
disp('Take curvelet transform: fdct_wrapping');
tic; C = fdct_wrapping(X,0); toc;

%specify one curvelet
s = 5;
w = 1;
[A,B] = size(C{s}{w});6
a = ceil((A+1)/2);
b = ceil((B+1)/2);
C{s}{w}(a,b) = 1;

%adjoint curvelet transform
disp('Take adjoint curvelet transform: ifdct_wrapping');
tic; Y = ifdct_wrapping(C,0); toc;

%display the curvelet
F = ifftshift(fft2(fftshift(Y)));
subplot(1,2,1); colormap gray; imagesc(real(Y)); axis('image'); ...
    title('a curvelet: spatial viewpoint');
subplot(1,2,2); colormap gray; imagesc(abs(F)); axis('image'); ...
    title('a curvelet: frequency viewpoint');

%get parameters
[SX,SY,FX,FY,NX,NY] = fdct_wrapping_param(C);


function [sorted_vertices, ...
          h_fes, h_bnd, h_fill, h_vert, h_int, h_max, g_labels] = ...
    plot_feasible(A, b, c, lower_b, upper_b, varargin)
% Plot the feasible region of a linear program
%
% file:      	plot_feasible.m, (c) Matthew Roughan, 2009
% created: 	Mon Jun 22 2009
% author:  	Matthew Roughan
% email:   	matthew.roughan@adelaide.edu.au
%
% Plot the feasible region of the 2D linear program
%           maximize            f = c'*x
%           subject to       A x <= b
% on the region bounded by lower_b and upper_b. Note that the inputs
%     A  is a mx2 matrix, where there are m constraints
%     b  is a mx1 column vector, where there are m constraints
%     c  is a 2x1 column vector
%     lower_b, upper_b are 2x1 column vectors
%
% If the problem includes non-negativity bounds, i.e.,
%               x >= 0
% which is quite common, then these need to be explicitly included into
% the constraints:  A x <= b, not implicitly through the bounds, which are mainly there to
% allow us to make a nice plot.
%
% The feasible region is filled with lines at a setable angle and separation, much as fill
% would do with a solid colour, though options allow you to fill with a color, or both.
%
% This code is primarily (for me) useful for teaching about linear programming, and only
% works for 2D problems (for the obvious reason that problems with more than two variables
% are hard to plot).
%
% Note that if you want to cross-hatch just call this routine twice, with a change in angle
% of 90 degrees.
%
% There are a bunch of optional inputs in pairs, as for the plot command
%    'linecolor'         color of feasible region boundary lines (default black)
%    'backgroundcolor'   background color of feasible region (default no background)
%    'linewidth'         width of lines (default 1)
%    'linestyle'         style of the boundary lines
%    'filllinestyle'     style of the fill lines
%    'linesep'           separation of cross-hatch lines (default 1)
%                        if this is set to -1, don't put any fill lines in
%    'lineangle'         angle (in degrees) of cross-hatch lines
%                           the default is to put them along lines of equal values of the
%                           objective function
%    'hold'              hold_n=1 means keep previous plot
%                        hold_n=0 (default) means clear previous figure
%                        use this to plot multiple feasible regions on same plot, or to do
%                        crosshatching.
%
%    'plot_vertices'     if defined indicates that we should plot the vertices of the feasible region
%                        the value will determine the symbol to use to plot the vertices,
%                        e.g., 'rx', or 'o+'
%    'label_vertices'    if not 0, then label the vertices with their position (label_vertices=1)
%                        or value (label_vertices=1) or both (label_vertices=3)
%    'label_vertices_size'  size of vertex labels, default = 12
%    'label_vertices_color' size of vertex labels, default = 'k'
%    'label_vertices_prec'  precision of vertex labels (default = 2 decimal places)
%
%    'plot_intersections' if defined indicates that we should plot the intersection points of boundaries
%                        the value will determine the symbol to use to plot the vertices,
%                        e.g., 'rx', or 'o+'
%                        Note that vertices are also intersection points, and will be double
%                        plotted if used with the plot_vertices option
%
%    'plot_max'          if not 0, then plot the location of the maximum, using the defined symbol
%
%    'extend_boundaries' if defined, plot each of the constraint lines past the feasible region
%                        using the linewidth and color defined above. The style of the line can
%                        be given as the value, e.g., ':'
%
% OUTPUTS:
%     sorted_vertices = a 2x(n+1) vector of the vertices of the feasible region
%                        note the last one is a repeat of the first to aid in plotting the region
%
%
%     h_fes    = handles to boundary lines of the linear program
%     h_bnd    = handles to the boundary lines
%     h_fill   = handles to the fill lines
%     h_vert   = handles to the vertices (if plotted)
%     h_int    = handles to the intersection points (if plotted)
%     h_max    = a handle to the plotted maximum point
%     g_labels = handles to vertex labels if used
%
% version 0.1, July 22nd 2009
%
% TODO
%     speckled fill
%     testing for boundedness and autosizing the figure
%     plotting bounds with cross-hatch on feasible side
%     at present the code uses the optimization toolkit (through the linprog) function
%       which we use to solve the linear program, but as this is a 2D problem we should be able to
%       remove this dependence with a little work.
%     currently the intersections with upper and lower bounds for plotting are also included
%       in intersection set, and plotted
%     automate ability to put text description of the problem in the window
%

% check sizes of inputs
sA = size(A);
sb = size(b);
if (sA(1) ~= sb(1))
  error('incorrect input sizes\n');
end
if (sA(2) ~= 2)
  error('can only plot for 2 variables\n');
end
n = sA(1); % number of constraints
if (size(upper_b,1) ~=2 | size(upper_b,2) ~= 1)
  error('upper_b is the wrong size\n');
end
if (size(lower_b,1) ~=2 | size(lower_b,2) ~= 1)
  error('lower_b is the wrong size\n');
end

% set default output values
h_fes    = [];
h_bnd    = [];
h_fill   = [];
h_vert   = [];
h_int    = [];
h_max    = [];

% parse the variable input arguments
linecolor = 'k';
backgroundcolor = 0;
linewidth = 1;
linestyle = '-';
filllinestyle = '-';
linesep = 1;
% default line angle is so that lines will be iso-objective
if (abs(c(1)) < 1.0e-12)
  lineangle = 90; % degrees
else
  lineangle = atand(c(2)/c(1));
end
hold_n = 0;
plot_vertices = 0;
label_vertices = 0;
label_vertices_size = 12;
label_vertices_color = 'k';
label_vertices_prec = 2;
plot_intersections = 0;
extend_boundaries = 0;
plot_max = 0;
if (length(varargin) > 0)
  % process variable arguments
  for k = 1:2:length(varargin)
    if (ischar(varargin{k}))
      argument = char(varargin{k});
      value = varargin{k+1};
      switch argument
       case {'linecolor','lc'}
	linecolor = value;
       case {'backgroundcolor','bgc'}
	backgroundcolor = value;
       case {'linewidth','lw'}
	linewidth = value;
       case {'linestyle','ls'}
	 linestyle = value;
       case {'filllinestyle','fls'}
	 filllinestyle = value;
       case {'linesep','ls'}
	linesep = value;
       case {'lineangle','la'}
	lineangle = value;
       case {'hold'}
	hold_n = value;
       case {'plot_vertices', 'pv'}
	plot_vertices = value;
       case {'label_vertices', 'lv'}
	label_vertices = value;
       case {'label_vertices_size', 'lvs'}
	label_vertices_size = value;
       case {'label_vertices_color', 'lvc'}
	label_vertices_color = value;
       case {'label_vertices_prec', 'lvp'}
	label_vertices_prec = value;
       case {'plot_intersections', 'pi'}
	plot_intersections = value;
       case {'extend_boundaries', 'eb'}
	extend_boundaries = value;
       case {'plot_max', 'pm'}
	 plot_max = value;
       otherwise
	error('incorrect input parameters');
      end
    end
  end
end

% set up the plot region
if (hold_n==0)
  hold off
  plot(lower_b(1), lower_b(2));
end
diff_x = (upper_b(1)-lower_b(1))/20;
diff_y = (upper_b(2)-lower_b(2))/20;
set(gca, 'xlim', [lower_b(1)-diff_x upper_b(1)+diff_x]);
set(gca, 'ylim', [lower_b(2)-diff_y upper_b(2)+diff_y]);
hold on
epsilon = 1.0e-14;

% plot the boundary line lines of the linear program across the plot region
if ~(extend_boundaries == 0)
  for i=1:n
    % find end-points of the lines along the boundaries
    constraint = A(i,:);
    k = b(i);
    if (abs(constraint(1)) < epsilon)
      % fprintf('(abs(constraint(1) < epsilon): i = %d\n', i);
      if (lower_b(1)<=k/constraint(2) & k/constraint(2)<=upper_b(1))
	x = [lower_b(1) upper_b(1)];
	y = [1 1] * k/constraint(2);
      else
	x = [];
	y = [];
      end
    elseif (abs(constraint(2)) < epsilon)
      % fprintf('(abs(constraint(2) < epsilon): i = %d\n', i);
      if (lower_b(2)<=k/constraint(1) & k/constraint(1)<=upper_b(2))
	x = [1 1] * k/constraint(1);
	y = [lower_b(2) upper_b(2)];
      else
	x = [];
	y = [];
      end
    else
      % compute all four intersections, and decide which is feasible
      % horizontal intercept points
      y_1 = [lower_b(2) upper_b(2)];
      x_1 = (k - constraint(2)*y_1) / constraint(1);
      i_1 = find(lower_b(1)<x_1 & x_1<upper_b(1));
      % vertical intercept points
      x_2 = [lower_b(1) upper_b(1)];
      y_2 = (k - constraint(1)*x_2) / constraint(2);
      i_2 = find(lower_b(2)<y_2 & y_2<upper_b(2));

      x = [x_1(i_1) x_2(i_2)];
      y = [y_1(i_1) y_2(i_2)];
    end
    % plot the line
    h_fes(i) = plot(x, y, 'color', linecolor, 'linestyle', extend_boundaries);
  end
end

% find the vertices of the lines, plus the borders
Aex = [A; -eye(2); eye(2)];
bex = [b; -lower_b; upper_b];
m = n + 4;
intersections = [];
vertices = [];
for i=1:m-1
  for j=i+1:m
    M = [Aex(i,:); Aex(j,:)];
    bm = bex([i,j]);
    x = M \ bm;
    intersections = [intersections, x];
    % test for feasibility
    if (A*x <= b)
      vertices = [vertices, x];
    end
  end
end

% find and fill the polygon defined by the vertices
k = convhull(vertices(1,:), vertices(2,:));
sorted_vertices(1,:) = vertices(1,k)';
sorted_vertices(2,:) = vertices(2,k)';
if (backgroundcolor == 0)
  h_bnd = plot(sorted_vertices(1,:), sorted_vertices(2,:),  ...
	       'color', linecolor, 'linewidth', linewidth, 'linestyle', linestyle);
else
  h_bnd = fill(sorted_vertices(1,:), sorted_vertices(2,:), backgroundcolor, ...
	       'edgecolor', linecolor, 'linewidth', linewidth, 'linestyle', linestyle);
end

% plot the intersection points and vertices
if ~(plot_intersections == 0)
  h_int = plot(intersections(1,:), intersections(2, :), plot_intersections);
end

if ~(plot_vertices == 0)
  h_vert = plot(sorted_vertices(1,:), sorted_vertices(2, :), plot_vertices);

  if (label_vertices == 1)
    % write the position of the vertex
    format_str = sprintf('(%%.%.0ff,%%.%.0ff)', label_vertices_prec, label_vertices_prec);
    for i=1:size(sorted_vertices, 2)
      temp = 10^label_vertices_prec;
      x = round(temp*sorted_vertices(1,i))/temp;
      y = round(temp*sorted_vertices(2,i))/temp;
      g_labels(i) = text(x+diff_x/2, y, sprintf(format_str, x, y), ...
			 'fontsize', label_vertices_size, 'color', label_vertices_color);
    end
  elseif (label_vertices == 2)
    % write the value of the vertex
    format_str = sprintf('f = %%.%.0ff', label_vertices_prec);
    for i=1:size(sorted_vertices, 2)
      temp = 10^label_vertices_prec;
      x = sorted_vertices(1,i);
      y = sorted_vertices(2,i);
      f = round(temp* (c' * [x; y]))/temp;
      g_labels(i) = text(x+diff_x/2, y, sprintf(format_str, f), ...
			 'fontsize', label_vertices_size, 'color', label_vertices_color);
    end
  elseif (label_vertices == 3)
    % write the position and value
    % write the position of the vertex
    format_str = sprintf('(%%.%.0ff,%%.%.0ff), f=%%.%.0ff', ...
			 label_vertices_prec, label_vertices_prec, label_vertices_prec);
    for i=1:size(sorted_vertices, 2)
      temp = 10^label_vertices_prec;
      x = sorted_vertices(1,i);
      y = sorted_vertices(2,i);
      f = round(temp* (c' * [x; y]))/temp;
      x = round(temp*x)/temp;
      y = round(temp*y)/temp;
      g_labels(i) = text(x+diff_x/2, y, sprintf(format_str, x, y, f), ...
			 'fontsize', label_vertices_size, 'color', label_vertices_color);
    end
  end

end

% plot the max if needed
if ~(plot_max == 0)
  x_max = linprog(-c, A, b, [], [], lower_b, upper_b);
  h_max = plot(x_max(1), x_max(2), plot_max);
end

% do the fill lines
if (linesep > 0)
  %   find min and max via linprog to see start and end points of lines
  %      add in implicit boundary constraints when do it
  c_line = [cosd(lineangle), sind(lineangle)];
  x_min = linprog(c_line, A, b, [], [], lower_b, upper_b);
  % plot(x_min(1), x_min(2), 'b*');

  x_max = linprog(-c_line, A, b, [], [], lower_b, upper_b);
  % plot(x_max(1), x_max(2), 'b*');

  distance = sqrt( sum((x_max-x_min).^2)  );

  %   draw lines clipping them at the boundaries
  for i=0:linesep:distance
    counter = ceil(i/linesep) + 1;

    % sigma = x_min + i*(x_max-x_min)/distance;
    sigma = x_min + i*c_line';
    % plot(sigma(1), sigma(2), '+', 'color', linecolor);

    % for each line, consider its intersepts with each possible
    % boundary, and check whether they are feasible
    edge_vertices = [];
    for j=1:m
      M = [Aex(j,:); c_line];
      d = [bex(j); sum(c_line.*sigma')];
      x = M \ d;
      % test for feasibility
      if (A*x <= b)
	edge_vertices = [edge_vertices, x];
      end
    end
    edge_vertices = round(100*edge_vertices)'/100;
    edge_vertices = unique(edge_vertices, 'rows')';
    if (size(edge_vertices,2) >= 2)
      h_fill(counter) = plot(edge_vertices(1,:), edge_vertices(2,:), ...
			     'color', linecolor, 'linestyle', filllinestyle, 'linewidth', linewidth);
    end
  end
end


function value = rodman_determinant ( n, alpha )

%*****************************************************************************80
%
%% RODMAN_DETERMINANT returns the determinant of the RODMAN matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real ALPHA, the parameter.
%
%    Output, real VALUE, the determinant.
%
  value = ( 1.0 - alpha )^( n - 1 ) * ( 1.0 + alpha * ( n - 1 ) );

  return
end


function prob_test1188 ( )

%*****************************************************************************80
%
%% PROB_TEST1188 tests NORMAL_TRUNCATED_B_CDF, NORMAL_TRUNCATED_B_CDF_INV, NORMAL_TRUNCATED_B_PDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 August 2013
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;
  b = 150.0;
  mu = 100.0;
  s = 25.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROB_TEST1188\n' );
  fprintf ( 1, '  For the Upper Truncated Normal PDF:\n' );
  fprintf ( 1, '  NORMAL_TRUNCATED_B_CDF evaluates the CDF.\n' );
  fprintf ( 1, '  NORMAL_TRUNCATED_B_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  NORMAL_TRUNCATED_B_PDF evaluates the PDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The "parent" normal distribution has\n' );
  fprintf ( 1, '    mean =               %g\n', mu );
  fprintf ( 1, '    standard deviation = %g\n', s );
  fprintf ( 1, '  The parent distribution is truncated to\n' );
  fprintf ( 1, '  the interval [-oo,%g]\n', b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n');

  for i = 1 : 10

    [ x, seed ] = normal_truncated_b_sample ( mu, s, b, seed );

    pdf = normal_truncated_b_pdf ( x, mu, s, b );

    cdf = normal_truncated_b_cdf ( x, mu, s, b );

    x2 = normal_truncated_b_cdf_inv ( cdf, mu, s, b );

    fprintf( 1, '  %14g  %14g  %14g  %14g\n', x, pdf, cdf, x2 );

  end

  return
end


function [ num_int, pint ] = halfspace_normal_triangle_int_3d ( p, normal, t )

%*****************************************************************************80
%
%% HALFSPACE_NORMAL_TRIANGLE_INT_3D: intersection ( normal halfspace, triangle ) in 3D.
%
%  Discussion:
%
%    The normal form of a halfspace in 3D may be described as the set
%    of points (X,Y,Z) on or "above" a plane described in normal form:
%
%      P is a point on the plane,
%      NORMAL is the unit normal vector, pointing "out" of the
%      halfspace.
%
%    The triangle is specified by listing its three vertices.
%
%    The intersection may be described by the number of vertices of the
%    triangle that are included in the halfspace, and by the location of
%    points between vertices that separate a side of the triangle into
%    an included part and an unincluded part.
%
%    0 vertices, 0 separators    (no intersection)
%    1 vertex, 0 separators      (point intersection)
%    2 vertices, 0 separators    (line intersection)
%    3 vertices, 0 separators    (triangle intersection)
%
%    1 vertex, 2 separators,     (intersection is a triangle)
%    2 vertices, 2 separators,   (intersection is a quadrilateral).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(3,3), the vertices of the triangle.
%
%    Input, real P(3,1), a point on the bounding plane
%    that defines the halfspace.
%
%    Input, real NORMAL(3,1), the components of the normal vector
%    to the bounding plane that defines the halfspace.  By convention, the
%    normal vector points "outwards" from the halfspace.
%
%    Output, integer NUM_INT, the number of intersection points returned,
%    which will always be between 0 and 4.
%
%    Output, real PINT(3,4), the coordinates of the NUM_INT
%    intersection points.  The points will lie in sequence on the triangle.
%    Some points will be vertices, and some may be separators.
%
  dim_num = 3;
%
%  Compute the signed distances between the vertices and the plane.
%
  d = - normal(1:dim_num,1)' * p(1:dim_num,1);
%
%  Compute the signed distances between the vertices and the plane.
%
  dist1 = d + normal(1:dim_num,1)' * t(1:dim_num,1);
  dist2 = d + normal(1:dim_num,1)' * t(1:dim_num,2);
  dist3 = d + normal(1:dim_num,1)' * t(1:dim_num,3);
%
%  Now we can find the intersections.
%
  [ num_int, pint ] = halfspace_triangle_int_3d ( t, dist1, dist2, dist3 );

  return
end


function runIntersect = mintersect(varargin)
%MINTERSECT Multiple set intersection.
%   MINTERSECT(A,B,C,...) when A,B,C... are vectors returns the values
%   common to all A,B,C... The result will be sorted.  A,B,C... can be cell
%   arrays of strings.
%
%   MINTERSECT repeatedly evaluates INTERSECT on successive pairs of sets,
%   which may not be very efficient.  For a large number of sets, this should
%   probably be reimplemented using some kind of tree algorithm.
%
%   MINTERSECT(A,B,'rows') when A,B,C... are matrices with the same
%   number of columns returns the rows common to all A,B,C...
%
%   See also INTERSECT

flag = 0;
if isempty(varargin),
    error('No inputs specified.')
else
    if isequal(varargin{end},'rows'),
        flag = 'rows';
        setArray = varargin(1:end-1);
    else
        setArray = varargin;
    end
end

runIntersect = setArray{1};
for i = 2:length(setArray),

    if isequal(flag,'rows'),
        runIntersect = intersect(runIntersect,setArray{i},'rows');
    elseif flag == 0,
        runIntersect = intersect(runIntersect,setArray{i});
    else
        error('Flag not set.')
    end

    if isempty(runIntersect),
        return
    end

end