function [coeff, score, latent, tsquared, explained, mu] = pca(x,varargin)
%PCA Principal Component Analysis (PCA) on raw data.
%   COEFF = PCA(X) returns the principal component coefficients for the N
%   by P data matrix X. Rows of X correspond to observations and columns to
%   variables. Each column of COEFF contains coefficients for one principal
%   component. The columns are in descending order in terms of component
%   variance (LATENT). PCA, by default, centers the data and uses the
%   singular value decomposition algorithm. For the non-default options,
%   use the name/value pair arguments.
%
%   [COEFF, SCORE] = PCA(X) returns the principal component score, which is
%   the representation of X in the principal component space. Rows of SCORE
%   correspond to observations, columns to components. The centered data
%   can be reconstructed by SCORE*COEFF'.
%
%   [COEFF, SCORE, LATENT] = PCA(X) returns the principal component
%   variances, i.e., the eigenvalues of the covariance matrix of X, in
%   LATENT.
%
%   [COEFF, SCORE, LATENT, TSQUARED] = PCA(X) returns Hotelling's T-squared
%   statistic for each observation in X. PCA uses all principal components
%   to compute the TSQUARED (computes in the full space) even when fewer
%   components are requested (see the 'NumComponents' option below). For
%   TSQUARED in the reduced space, use MAHAL(SCORE,SCORE).
%
%   [COEFF, SCORE, LATENT, TSQUARED, EXPLAINED] = PCA(X) returns a vector
%   containing the percentage of the total variance explained by each
%   principal component.
%
%   [COEFF, SCORE, LATENT, TSQUARED, EXPLAINED, MU] = PCA(X) returns the
%   estimated mean, MU, when 'Centered' is set to true; and all zeros when
%   set to false.
%
%   [...] = PCA(..., 'PARAM1',val1, 'PARAM2',val2, ...) specifies optional
%   parameter name/value pairs to control the computation and handling of
%   special data types. Parameters are:
%
%    'Algorithm' - Algorithm that PCA uses to perform the principal
%                  component analysis. Choices are:
%        'svd'   - Singular Value Decomposition of X (the default).
%        'eig'   - Eigenvalue Decomposition of the covariance matrix. It
%                  is faster than SVD when N is greater than P, but less
%                  accurate because the condition number of the covariance
%                  is the square of the condition number of X.
%        'als'   - Alternating Least Squares (ALS) algorithm which finds
%                  the best rank-K approximation by factoring a X into a
%                  N-by-K left factor matrix and a P-by-K right factor
%                  matrix, where K is the number of principal components.
%                  The factorization uses an iterative method starting with
%                  random initial values. ALS algorithm is designed to
%                  better handle missing values. It deals with missing
%                  values without listwise deletion (see {'Rows',
%                  'complete'}).
%
%     'Centered' - Indicator for centering the columns of X. Choices are:
%         true   - The default. PCA centers X by subtracting off column
%                  means before computing SVD or EIG. If X contains NaN
%                  missing values, NANMEAN is used to find the mean with
%                  any data available.
%         false  - PCA does not center the data. In this case, the original
%                  data X can be reconstructed by X = SCORE*COEFF'.
%
%     'Economy'  - Indicator for economy size output, when D the degrees of
%                  freedom is smaller than P. D, is equal to M-1, if data
%                  is centered and M otherwise. M is the number of rows
%                  without any NaNs if you use 'Rows', 'complete'; or the
%                  number of rows without any NaNs in the column pair that
%                  has the maximum number of rows without NaNs if you use
%                  'Rows', 'pairwise'. When D < P, SCORE(:,D+1:P) and
%                  LATENT(D+1:P) are necessarily zero, and the columns of
%                  COEFF(:,D+1:P) define directions that are orthogonal to
%                  X. Choices are:
%         true   - This is the default. PCA returns only the first D
%                  elements of LATENT and the corresponding columns of
%                  COEFF and SCORE. This can be significantly faster when P
%                  is much larger than D. NOTE: PCA always returns economy
%                  size outputs if 'als' algorithm is specifed.
%         false  - PCA returns all elements of LATENT. Columns of COEFF and
%                  SCORE corresponding to zero elements in LATENT are
%                  zeros.
%
%     'NumComponents' - The number of components desired, specified as a
%                  scalar integer K satisfying 0 < K <= P. When specified,
%                  PCA returns the first K columns of COEFF and SCORE.
%
%     'Rows'     - Action to take when the data matrix X contains NaN
%                  values. If 'Algorithm' option is set to 'als, this
%                  option is ignored as ALS algorithm deals with missing
%                  values without removing them. Choices are:
%         'complete' - The default action. Observations with NaN values
%                      are removed before calculation. Rows of NaNs are
%                      inserted back into SCORE at the corresponding
%                      location.
%         'pairwise' - If specified, PCA switches 'Algorithm' to 'eig'.
%                      This option only applies when 'eig' method is used.
%                      The (I,J) element of the covariance matrix is
%                      computed using rows with no NaN values in columns I
%                      or J of X. Please note that the resulting covariance
%                      matrix may not be positive definite. In that case,
%                      PCA terminates with an error message.
%         'all'      - X is expected to have no missing values. All data
%                      are used, and execution will be terminated if NaN is
%                      found.
%
%     'Weights'  - Observation weights, a vector of length N containing all
%                  positive elements.
%
%     'VariableWeights' - Variable weights. Choices are:
%          - a vector of length P containing all positive elements.
%          - the string 'variance'. The variable weights are the inverse of
%            sample variance. If 'Centered' is set true at the same time,
%            the data matrix X is centered and standardized. In this case,
%            PCA returns the principal components based on the correlation
%            matrix.
%
%   The following parameter name/value pairs specify additional options
%   when alternating least squares ('als') algorithm is used.
%
%      'Coeff0'  - Initial value for COEFF, a P-by-K matrix. The default is
%                  a random matrix.
%
%      'Score0'  - Initial value for SCORE, a N-by-K matrix. The default is
%                  a matrix of random values.
%
%      'Options' - An options structure as created by the STATSET function.
%                  PCA uses the following fields:
%          'Display' - Level of display output.  Choices are 'off' (the
%                      default), 'final', and 'iter'.
%          'MaxIter' - Maximum number of steps allowed. The default is
%                      1000. Unlike in optimization settings, reaching
%                      MaxIter is regarded as convergence.
%           'TolFun' - Positive number giving the termination tolerance for
%                      the cost function.  The default is 1e-6.
%             'TolX' - Positive number giving the convergence threshold
%                      for relative change in the elements of L and R. The
%                      default is 1e-6.
%
%
%   Example:
%       load hald;
%       [coeff, score, latent, tsquared, explained] = pca(ingredients);
%
%   See also PPCA, PCACOV, PCARES, BIPLOT, BARTTEST, CANONCORR, FACTORAN,
%   ROTATEFACTORS.

% References:
%   [1] Jolliffe, I.T. Principal Component Analysis, 2nd ed.,Springer,2002.
%   [2] Krzanowski, W.J., Principles of Multivariate Analysis, Oxford
%       University Press, 1988.
%   [3] Seber, G.A.F., Multivariate Observations, Wiley, 1984.
%   [4] Jackson, J.E., A User's Guide to Principal Components, Wiley, 1988.
%   [5] Sam Roweis, EM algorithms for PCA and SPCA, In Proceedings of the
%       1997 conference on Advances in neural information processing
%       systems 10 (NIPS '97), MIT Press, Cambridge, MA, USA, 626-632,1998.
%   [6] Alexander Ilin and Tapani Raiko. Practical Approaches to Principal
%       Component Analysis in the Presence of Missing Values. J. Mach.
%       Learn. Res. 11 (August 2010), 1957-2000, 2010.

%   Copyright 2012 The MathWorks, Inc.

[n, p] = size(x);
internal.stats.checkSupportedNumeric('X',x,false,false,true); % complex is accepted here

% Parse arguments and check if parameter/value pairs are valid
paramNames = {'Algorithm','Centered','Economy','NumComponents','Rows',...
    'Weights','VariableWeights','Coeff0','Score0','Options'};
defaults   = {'svd',       true,      true,    p,           'complete',...
    ones(1,n,'like',x) ,ones(1,p,'like',x),        [],      [], statset('pca')};

[vAlgorithm, vCentered, vEconomy, vNumComponents, vRows,vWeights,...
    vVariableWeights, c0, s0, opts, setFlag]...
    = internal.stats.parseArgs(paramNames, defaults, varargin{:});
% Validate String value for  Algorithm value
AlgorithmNames = {'svd','eig','als'};
vAlgorithm = internal.stats.getParamVal(vAlgorithm,AlgorithmNames,...
    '''Algorithm''');
% Validate boolean value for 'Centered' option
vCentered = internal.stats.parseOnOff(vCentered,'''Centered''');
% Validate boolean value for 'Economy' option
vEconomy = internal.stats.parseOnOff(vEconomy,'''Economy''');
% Validate the number of components option 'NumComponents'
if ~isempty(x) && ~internal.stats.isScalarInt(vNumComponents,1,p)
    error(message('stats:pca:WrongNumComponents',p));
end
% Validate value for 'Rows' option
RowsNames = {'complete','pairwise','all'};
vRows = internal.stats.getParamVal(vRows,RowsNames,'''Rows''');

switch vAlgorithm
    case 'svd'
        % Switch 'Algorithm' to 'eig' if 'Rows' set to 'pairwise'
        if strcmp(vRows,'pairwise')
            if setFlag.Algorithm
                warning(message('stats:pca:NoPairwiseSVD'));
            end
            vAlgorithm = 'eig';
        end
        % Switch algorithm to 'als' if user specify 'Coeff0' and 'Score0'.
        if setFlag.Coeff0 || setFlag.Score0
            vAlgorithm = 'als';
        end
    case 'als'
        % If 'als' is chosen, force PCA to use ALS and to ignore the
        % Rows' option
        if setFlag.Rows
            warning(message('stats:pca:NoALSRows'));
        end
end

% Validate Weights Vectors
if isvector(vWeights) && isequal(numel(vWeights),n)
    vWeights = reshape(vWeights,1,n); % make sure it is a row vector
else
    error(message('stats:pca:WrongObsWeights', n));
end

if internal.stats.isString(vVariableWeights)
    WeightsNames = {'variance'};
    internal.stats.getParamVal(vVariableWeights,WeightsNames,...
        '''VariableWeights''');
    vVariableWeights = 1./classreg.learning.internal.wnanvar(x,vWeights,1);
elseif isnumeric(vVariableWeights) && isvector(vVariableWeights)...
        && (isequal(numel(vVariableWeights), p))
    vVariableWeights = reshape(vVariableWeights,1,p);
else
    error(message('stats:pca:WrongVarWeights', p));
end

if any(vWeights <= 0) || any(vVariableWeights <= 0)
    error(message('stats:pca:NonPositiveWeights'));
end
% end of checking input arguments

% Handling special empty case
if isempty(x)
    pOrZero = ~vEconomy * p;
    coeff = zeros(p, pOrZero);
    coeff(1:p+1:end) = 1;
    score = zeros(n, pOrZero);
    latent = zeros(pOrZero, 1);
    tsquared = zeros(n, 1);
    explained = [];
    mu = [];
    return;
end

nanIdx = isnan(x);
numNaN = sum(nanIdx, 2); % number of NaNs in each row
wasNaN = any(numNaN,2); % Rows that contain NaN

% Handling special cases where X is all NaNs:
if all(nanIdx(:))
    coeff = NaN;
    score = NaN;
    latent = NaN;
    tsquared = NaN;
    explained = NaN;
    mu = NaN;
    return;
end
% Handling special scalar case;
if isscalar(x)
    coeff = 1;
    score = (~vCentered)*x;
    latent = (~vCentered)*x^2;
    tsquared = ~vCentered;
    explained = 100;
    mu = vCentered*x;
    return;
end

if strcmp(vRows,'all') && (~strcmp(vAlgorithm,'als'))
    if any(wasNaN)
        error(message('stats:pca:RowsAll'));
    else
        vRows = 'complete';
    end
end

if strcmp(vRows,'complete')
    % Degrees of freedom (DOF) is n-1 if centered and n if not centered,
    % where n is the numer of rows without any NaN element.
    DOF = max(0,n-vCentered-sum(wasNaN));
elseif strcmp(vRows,'pairwise')
    % DOF is the maximum number of element pairs without NaNs
    notNaN = double(~nanIdx);
    nanC = notNaN'*notNaN;
    nanC = nanC.*(~eye(p));
    DOF = max(nanC(:));
    DOF = DOF-vCentered;
else
    DOF = max(0,n-vCentered);
end

if vCentered
    % Weighted sample mean:
    mu = classreg.learning.internal.wnanmean(x, vWeights);
else
    mu = zeros(1,p,'like',x);
end

% Compute by EIG if no weights are given
switch vAlgorithm
case 'eig'
    % Center the data if 'Centered' is true.
    if vCentered
        x = bsxfun(@minus,x,mu);
    end

    % Use EIG to compute.
    [coeff, eigValues] = localEIG(x, vCentered, vRows, vWeights,...
        vVariableWeights);

    % When 'Economy' value is true, nothing corresponding to zero
    % eigenvalues should be returned.
    if (DOF<p)
        if vEconomy
            coeff(:, DOF+1:p) = [];
            eigValues(DOF+1:p, :) = [];
        else % make sure they are zeros.
            eigValues(DOF+1:p, :) = 0;
        end
    end

    % Check if eigvalues are all postive
    if any(eigValues<0)
        error(message('stats:pca:CovNotPositiveSemiDefinite'));
    end

    if nargout > 1
        score = x/coeff';
        latent = eigValues; % Output Eigenvalues
        if nargout > 3
            tsquared = localTSquared(score, latent, n, p);
        end
    end

case 'svd' % Use SVD to compute
    % Center the data if 'Centered' is true.
    if vCentered
        x = bsxfun(@minus,x,mu);
    end

    [U,sigma, coeff, wasNaN] = localSVD(x, n,...
        vEconomy, vWeights, vVariableWeights);
    if nargout > 1
        score =  bsxfun(@times,U,sigma');
        latent = sigma.^2./DOF;
        if nargout > 3
            tsquared = localTSquared(score,latent,DOF,p);
        end
        %Insert NaNs back
        if any(wasNaN)
            score = internal.stats.insertnan(wasNaN, score);
            if nargout >3
                tsquared = internal.stats.insertnan(wasNaN,tsquared);
            end
        end
    end

    if DOF < p
        % When 'Economy' value is true, nothing corresponding to zero
        % eigenvalues should be returned.
        if vEconomy
            coeff(:, DOF+1:end) = [];
            if nargout > 1
                score(:, DOF+1:end)=[];
                latent(DOF+1:end, :)=[];
            end
        elseif nargout > 1
        % otherwise, eigenvalues and corresponding outputs need to pad
        % zeros because svd(x,0) does not return columns of U corresponding
        % to components of (DOF+1):p.
            score(:, DOF+1:p) = 0;
            latent(DOF+1:p, 1) = 0;
        end
    end

case 'als' % Alternating Least Square Algorithm

    vNumComponents = min([vNumComponents,n-vCentered,p]);  % ALS always return economy sized outputs

    if isempty(s0);
        s0 = randn(n,vNumComponents,'like',x);
    elseif ~isequal(size(s0),[n,vNumComponents])|| any(isnan(s0(:)))
        error(message('stats:pca:BadInitialValues','Score0',n,vNumComponents));
    end
    if isempty(c0);
        c0 = randn(p,vNumComponents,'like',x);
    elseif ~isequal(size(c0),[p,vNumComponents]) || any(isnan(c0(:)))
        error(message('stats:pca:BadInitialValues','Coeff0',p,vNumComponents));
    end

    [score,coeff,mu,latent]=alsmf(x,vNumComponents,'L0',s0,'R0',c0,...
        'Weights',vWeights,'VariableWeights',vVariableWeights,...
        'Orthonormal',true,'Centered',vCentered,'Options',opts);

    if nargout > 3
        % T-squared values are in reduced space.
        tsquared = localTSquared(score, latent,n-vCentered, vNumComponents);
    end
end % end of switch vAlgorithm

% Calcuate the percentage of the total variance explained by each principal
% component.
if nargout > 4
    explained = 100*latent/sum(latent);
end

% Output only the first k principal components
if (vNumComponents<DOF)
    coeff(:, vNumComponents+1:end) = [];
    if nargout > 1
    score(:, vNumComponents+1:end) = [];
    end
end

% Enforce a sign convention on the coefficients -- the largest element in
% each column will have a positive sign.
[~,maxind] = max(abs(coeff), [], 1);
[d1, d2] = size(coeff);
colsign = sign(coeff(maxind + (0:d1:(d2-1)*d1)));
coeff = bsxfun(@times, coeff, colsign);
if nargout > 1
    score = bsxfun(@times, score, colsign); % scores = score
end

end % End of main function

%----------------Subfucntions--------------------------------------------

function [coeff, eigValues]=localEIG(x,vCentered, vRows,vWeights,...
        vVariableWeights)
% Compute by EIG. vRows are the options of handing NaN when compute
% covariance matrix

% Apply observation and variable weights
OmegaSqrt = sqrt(vWeights);
PhiSqrt = sqrt(vVariableWeights);
x = bsxfun(@times, x, OmegaSqrt');
x = bsxfun(@times, x, PhiSqrt);

xCov = ncnancov(x, vRows, vCentered);

[coeff, eigValueDiag] = eig(xCov);
[eigValues, idx] = sort(diag(eigValueDiag), 'descend');
coeff = coeff(:, idx);

coeff = bsxfun(@times, coeff,1./PhiSqrt');
end

function [U,sigma, coeff, wasNaN] = localSVD(x, n,...,
    vEconomy, vWeights, vVariableWeights)
% Compute by SVD. Weights are supplied by vWeights and vVariableWeights.

% Remove NaNs missing data and record location
[~,wasNaN,x] = internal.stats.removenan(x);
if n==1  % special case because internal.stats.removenan treats all vectors as columns
    wasNaN = wasNaN';
    x = x';
end

% Apply observation and variable weights
vWeights(wasNaN) = [];
OmegaSqrt = sqrt(vWeights);
PhiSqrt = sqrt(vVariableWeights);
x = bsxfun(@times, x, OmegaSqrt');
x = bsxfun(@times, x, PhiSqrt);

if vEconomy
    [U,sigma,coeff] = svd(x,'econ');
else
    [U,sigma, coeff] = svd(x, 0);
end

U = bsxfun(@times, U, 1./OmegaSqrt');
coeff = bsxfun(@times, coeff,1./PhiSqrt');

if n == 1     % sigma might have only 1 row
    sigma = sigma(1);
else
    sigma = diag(sigma);
end
end

function tsquared = localTSquared(score, latent,DOF, p)
% Subfunction to calulate the Hotelling's T-squared statistic. It is the
% sum of squares of the standardized scores, i.e., Mahalanobis distances.
% When X appears to have column rank < r, ignore components that are
% orthogonal to the data.

if isempty(score)
    tsquared = score;
    return;
end

r = min(DOF,p); % Max possible rank of x;
if DOF > 1
    q = sum(latent > max(DOF,p)*eps(latent(1)));
    if q < r
        warning(message('stats:pca:ColRankDefX', q));
    end
else
    q = 0;
end
standScores = bsxfun(@times, score(:,1:q), 1./sqrt(latent(1:q,:))');
tsquared = sum(standScores.^2, 2);
end

function c = ncnancov(x,Rows,centered)
%   C = NCNANCOV(X) returns X'*X/N, where N is the number of observations
%   after removing rows missing values.
%
%   C = NCNANCOV(...,'pairwise') computes C(I,J) using rows with no NaN
%   values in columns I or J.  The result may not be a positive definite
%   matrix. C = NCNANCOV(...,'complete') is the default, and it omits rows
%   with any NaN values, even if they are not in column I or J.
%
%   C = NCNANCOV(...,true), C is normalized by N-1 if data X is already
%   centered. The default is false.

if nargin <2
    Rows = 'complete';
end

d = 0;
if nargin>2
    d =  d + centered;
end

idxnan = isnan(x);

[n, p] = size(x);

if ~any(any(idxnan))
    c = x'*x/(n-d);
elseif strcmp(Rows,'complete')
    nanrows = any(idxnan,2);
    xNotNaN = x((~nanrows),:);
    denom = max(0, (size(xNotNaN,1)-d) );
    c = xNotNaN'*xNotNaN/denom;
elseif strcmp(Rows,'pairwise')
    c = zeros(p,class(x));
    for i = 1:p
        for j = 1:i
            NonNaNRows = ~any(idxnan(:,[i, j]),2);
            denom = max(0,(sum(NonNaNRows)-d));
            c(i,j) = x(NonNaNRows,i)'*x(NonNaNRows,j)/denom;
        end
    end
    c = c + tril(c,-1)';
end
end


function rank = cel2rank(cel)
%CEL2RANK Convert temperature from celsius to rankine
%
%  rank = CEL2RANK(cel) converts temperature from celsius to rankine.
%
%  See also RANK2CEL, CEL2FAR, CEL2KEL.

rank = (cel + 273.15) * 1.8;


function partners=assignStableRoommates(prefLists)
%%ASSIGNSTABLEROOMMATES Solve the basic form of the stable roommates
%        problem. In this problem, an even number of individuals rank their
%        preferences for each other. The algorithm then finds a stable
%        pairing of individuals, if one exists. An assignment is stable if
%        there is no pair of (non-assigned) members where BOTH prefer each
%        other over their current assignment. This function finds one
%        solutions; though multiple solutions may exist. If no solutions
%        exist, an empty matrix is returned.
%
%INPUTS: prefLists An NX(N-1) matrix where prefLists(n,i) is the ith
%                  preferred roommate for person n. N must be an even
%                  number. The "roommate" is just a number from 1 to N that
%                  cannot equal n. Alternatively, an NXN matrix can be
%                  passed, where the last column is equal to the row
%                  number. This is allowed as this algorithm just augments
%                  any NX(N-1) matrix to have that format before proceeding
%                  (it plays a role in identifying infeasible assignments).
%
%OUTPUTS: partners An NX1 vector listing which each partner of the N
%                  individuals is assigned to share a room in a stable
%                  assignment (thus, partners is a permutation vector). If
%                  no stable assignment exists, then an empty matrix is
%                  returned.
%
%The O(N^2) algorithm of [1] is used to solve the problem.
%
%Example with a solution (from [1]):
% prefLists=[4,6,2,5,3;
%            6,3,5,1,4;
%            4,5,1,6,2;
%            2,6,5,1,3;
%            4,2,3,6,1;
%            5,1,4,2,3];
% partners=assignStableRoommates(prefLists)
%The solution should be partners=[6;3;2;5;4;1] for pairs of (1,6), (2,3),
%and (4,5)
%
%Example without a solution (from [1]):
% prefLists=[2,6,4,3,5;
%            3,5,1,6,4;
%            1,6,2,5,4;
%            5,2,3,6,1;
%            6,1,3,4,2;
%            4,2,5,1,3];
% partners=assignStableRoommates(prefLists)
%The solution in this instance is an empty matrix, because no stable
%solution exists.
%
%Example with multiple solutions (from [1]). Only 1 solution is found by
%this function, though.
% prefLists=[2,5,4,6,7,8,3;
%            3,6,1,7,8,5,4;
%            4,7,2,8,5,6,1;
%            1,8,3,5,6,7,2;
%            6,1,8,2,3,4,7;
%            7,2,5,3,4,1,8;
%            8,3,6,4,1,2,5;
%            5,4,7,1,2,3,6];
% partners=assignStableRoommates(prefLists)
%The solution that will be found by this algorithm is
%partners=[4;3;2;1;6;5;8;7]. However, the other two possible solutions are
%[1,2,3,4,8,7,6,5] and [5,6,7,8,1,2,3,4].
%
%REFERENCES:
%[1] R. W. Irving, "An Efficient Algorithm for the 'Stable Rommates'
%    Problem," Journal of Algorithms, vol. 6, no. 4, pp. 577-595, Dec.
%    1985.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    numPeople=size(prefLists,1);

    %If the preference lists do not already contain "self" (sentinel) as
    %the last column, then add it.
    if(size(prefLists,2)<numPeople)
        prefLists=[prefLists,(1:numPeople)'];
    end

    firstUnmatched=1;
    firstInCycle=1;

    %Create the ranking matrix to quickly compare preferences and
    %initialize leftmost and rightmost matrices
    ranking=zeros(numPeople,numPeople);
    %Extra entries are sentinels for procedure
    %find1stwithMoteThan1Potential
    leftmost=ones(numPeople+1,1);
    rightmost=numPeople*ones(numPeople+1,1);
    for curPerson=1:numPeople
        for rank=1:numPeople
            ranking(curPerson,prefLists(curPerson,rank))=rank;
        end
    end

    [solutionPossible,leftmost,rightmost]=phase1Reduce(prefLists,ranking,leftmost,rightmost);

    %Proper initialization unnecessary
    second(1:numPeople)=leftmost(1:numPeople)+1;

    solutionFound=false;
    tail=zeros(numPeople,1);%Initially, nothing in tail (all zeros)
    cycle=zeros(numPeople,1);
    while(solutionPossible&&solutionFound==false)
        firstUnmatched=find1stwithMoteThan1Potential(firstUnmatched,leftmost,rightmost);
        if(firstUnmatched>numPeople)
            solutionFound=true;
            break;
        else
            [cycle,firstInCycle,lastInCycle,firstUnmatched,second,tail]=seekCycle(prefLists,ranking,cycle,firstInCycle,firstUnmatched,second,tail,rightmost);

            [solutionPossible,leftmost,second,rightmost]=phase2Reduce(prefLists,ranking,cycle,leftmost,second,rightmost,firstInCycle,lastInCycle);
        end
    end

    if(solutionFound)
        partners=zeros(numPeople,1);

        for curPerson=1:numPeople
            partners(curPerson)=prefLists(curPerson,leftmost(curPerson));
        end
    else
        partners=[];%No solution found.
    end
end

function [solutionPossible,leftmost,rightmost]=phase1Reduce(prefLists,ranking,leftmost,rightmost)
    numPeople=size(prefLists,1);

    %Initially, no one proposed to (all zeros).
    setProposedTo=zeros(numPeople,1);
    for curPerson=1:numPeople
        proposer=curPerson;
        while(1)
            %Best potential partner
            nextChoice=prefLists(proposer,leftmost(proposer));
            %nextChoice holds current
            current=prefLists(nextChoice,rightmost(nextChoice));

            while(ranking(nextChoice,proposer)>ranking(nextChoice,current))
                %Proposer is rejected by nextChoice
                leftmost(proposer)=leftmost(proposer)+1;
                nextChoice=prefLists(proposer,leftmost(proposer));
                current=prefLists(nextChoice,rightmost(nextChoice));
            end
            %nextChoice holds proposer
            rightmost(nextChoice)=ranking(nextChoice,proposer);
            %and rejects current
            proposer=current;

            if(setProposedTo(nextChoice)==0)
                break;
            end
        end
        setProposedTo(nextChoice)=1;
    end

    solutionPossible=(proposer==nextChoice);
end

function firstUnmatched=find1stwithMoteThan1Potential(firstUnmatched,leftmost,rightmost)
%%Find the first person with more than one potential partner.
    while(leftmost(firstUnmatched)==rightmost(firstUnmatched))
        firstUnmatched=firstUnmatched+1;
    end
end

function [cycle,firstInCycle,lastInCycle,firstUnmatched,second,tail]=seekCycle(prefLists,ranking,cycle,firstInCycle,firstUnmatched,second,tail,rightmost)
    numPeople=size(prefLists,1);

    if(firstInCycle>1)
        %Last person in previous tail.
        person=cycle(firstInCycle-1);
        %His second choice may have to be updated
        posInCycle=firstInCycle-1;
        cycleSet=tail;
    else
        cycleSet=zeros(numPeople,1);%Nothing in the cycle set
        posInCycle=1;
        person=firstUnmatched;
    end

    %Generate sequence
    while(1)
        cycleSet(person)=1;%Add person to the cycle set.

        cycle(posInCycle)=person;
        posInCycle=posInCycle+1;
        posInList=second(person);

        while(1)%Update second choice for current person
            nextChoice=prefLists(person,posInList);
            posInList=posInList+1;

            if(ranking(nextChoice,person)<=rightmost(nextChoice))
               break;
            end;
        end

        second(person)=posInList-1;
        person=prefLists(nextChoice,rightmost(nextChoice));

        %If sequence starts to cycle.
        if(cycleSet(person)==1)
            break;
        end
    end
    lastInCycle=posInCycle-1;
    tail=cycleSet;

    %Work back to the beginning of the cycle
    while(1)
       posInCycle=posInCycle-1;
       tail(cycle(posInCycle))=0;

       if(cycle(posInCycle)==person)
           break;
       end
    end
    firstInCycle=posInCycle;
end

function [solutionPossible,leftmost,second,rightmost]=phase2Reduce(prefLists,ranking,cycle,leftmost,second,rightmost,firstInCycle,lastInCycle)
    for rank=firstInCycle:lastInCycle
        %Allow next person in cycle to be rejected
        proposer=cycle(rank);
        leftmost(proposer)=second(proposer);
        %Proper update unnaecessary at this stage
        second(proposer)=leftmost(proposer)+1;

        nextChoice=prefLists(proposer,leftmost(proposer));
        rightmost(nextChoice)=ranking(nextChoice,proposer);
        %nextChoice holds proposer.
    end

    rank=firstInCycle;
    solutionPossible=true;
    while(rank<=lastInCycle&&solutionPossible)
        %Check that no one has run out of potential partners
        proposer=cycle(rank);
        solutionPossible=(leftmost(proposer)<=rightmost(proposer));
        rank=rank+1;
    end
end

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


function prob_test095 ( )

%*****************************************************************************80
%
%% TEST095 tests LAPLACE_CDF, LAPLACE_CDF_INV, LAPLACE_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST095\n' );
  fprintf ( 1, '  For the Laplace PDF:\n' );
  fprintf ( 1, '  LAPLACE_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  LAPLACE_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  LAPLACE_PDF evaluates the PDF;\n' );

  a = 1.0;
  b = 2.0;

  check = laplace_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST095 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );

  seed = 123456789;

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

  for i = 1 : 10

    [ x, seed ] = laplace_sample ( a, b, seed );

    pdf = laplace_pdf ( x, a, b );

    cdf = laplace_cdf ( x, a, b );

    x2 = laplace_cdf_inv ( cdf, a, b );

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

  end

  return
end


function zscoredData = BF_zscore(inputData)
% BF_zscore     z-score the input data vector (without a Statistics Toolbox licence)
%
%---INPUT:
% inputData, the input time series (or any vector).
%
%---OUTPUT:
% zscoredData, the z-scored transformation of the input.

% ------------------------------------------------------------------------------
% 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 for NaNs:
if any(isnan(inputData))
    error('inputData contains NaNs');
end

% By default, z-score twice to reduce the numerical error:
zscoredData = (inputData - mean(inputData)) / std(inputData);
zscoredData = (zscoredData - mean(zscoredData)) / std(zscoredData);

end


function [res,err] = Dot_(varargin)
%DOT_         Dot products in K-fold precision
%
%   res = Dot_(A1,x1,...,An,xn,K)
%
%   [res,err] = Dot_(A1,x1,...,An,xn,K)
%
% res or res+err is sum(Ai*xi)
%     K optional, default K=2
%     K   >0  approximate result as if computed in K-fold precision
%         <0  inclusion of result as if computed in |K|-fold precision
%
% First factor Ai may be either matrix or scalar; in the latter case
%   res is of the size of xi.
% Second factor xi must be vector or scalar.
% All factors may be complex, but must be non-interval.
% Sizes of every product Ai*xi must be the same.
%
% Precision K must be at least 2.
%
% Interval input makes only sense, if all intervals are degenerated (point intervals);
%   therefore omitted.
%
% For speed and to fight interpretation overhead, 2 extra matrices are necessary; for
%   the product of to nxn matrices this would be n^3 memory, therefore this is not allowed.
%
% Examples are
%
%   res = Dot_(x',y);                  Approximate scalar product x'*y in quadruple precision
%   res = Dot_(A,x,-1,b);              Approximate residual A*x-b in quadruple precision
%   res = Dot_(A,x,-1,b,-2);           Inclusion of residual A*x-b in quadruple precision
%   res = Dot_(A,x1,A,x2,-1,b,3);      Approximate residual A*x1+A*x2-b in 3-fold precision
%
%For randomly generated full matrices of dimension n with b=a*randn(n,1) the following
%  table lists the computing times
%    t1   for ordinary residual  b - A*x
%    t2   with improved residual by lssresidual
%    t3   with quadruple precision residual by Dot_
%as well as the ratios to t1. All times are in seconds on a 800 MHz Pentium III Laptop.
%
%    n     t1      t2     t3    t2/t1  t3/t1     approximation of b-A*x
%--------------------------------------------
%   50   0.0002  0.001  0.003    3.1   14.6
%  100   0.0002  0.003  0.010   13.7   45.7
%  200   0.0005  0.010  0.045   20.5   89.5
%  500   0.0064  0.089  0.416   13.7   64.6
% 1000   0.0246  0.347  1.698   14.1   68.9
%
%The same table for verified inclusion of the residual b-A*x is as follows.
%
%    n     t1      t2     t3    t2/t1  t3/t1     inclusion of b-A*x
%--------------------------------------------
%   50   0.0012  0.001  0.004    0.9    3.0
%  100   0.0013  0.002  0.010    1.9    7.5
%  200   0.0020  0.012  0.048    6.1   23.8
%  500   0.0138  0.103  0.381    7.5   27.5
% 1000   0.0505  0.421  1.863    8.3   36.9
%
%Finally we display the achieved accuracy of the three methods for
%  matrices of dimension 100 and different condition numbers, a randomly
%  generated right hand side b and computed solution x=A\b.
%  We treat 100 test cases each and display the median and maximum componentwise
%  relative error of the results. Matrices are normed to 1.
%
%                A*x-b          lssresidual         Dot_
%condition  median maximum    median maximum    median maximum
%---------------------------------------------------------------
%    1e2    6.8e-16 1.0e+0   5.0e-21 8.9e-16   0.0e+0  4.7e-30
%    1e5    1.8e-01 1.0e+0   1.8e-18 3.0e-01   5.0e-29 1.7e-27
%   1e10    2.9e-01 1.0e+0   2.4e-06 6.8e-01   3.3e-24 1.8e-22
%   1e14    2.8e-01 1.0e+0   2.1e-06 1.0e+0    2.7e-20 2.6e-18
%
%The more we pay, the more we get.
%
% Implements algorithms Dot2 and DotK from
%   T. Ogita, S.M. Rump, S. Oishi: Accurate Sum and Dot Product,
%     SIAM Journal on Scientific Computing (SISC), 26(6):1955-1988, 2005
%

% written  11/08/03     S.M. Rump
% modified 11/30/03     S.M. Rump  complex data allowed
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 08/08/04     S.M. Rump  sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 12/12/05     S.M. Rump  apapted to paper, improved performance
% modified 05/30/07     S.M. Rump  typo
% modified 08/07/10     S.M. Rump  upper case Dot_
% modified 06/06/11     S.M. Rump  result in two parts
% modified 08/26/12     S.M. Rump  global variables removed, rounding
% modified 09/05/12     S.M. Rump  sparse complex (thanks to M. Lange)
% modified 10/17/12     S.M. Rump  comments
%

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

  % detect parameters
  len = length(varargin);
  if even(len)                      % default precision
    K = 2;
  else
    K = varargin{len};
  end
  idim = 0;

  % check precision K
  if abs(K)<2
    error('invalid parameter K for Dot_')
  end

  % detect dimension of result
  if prod(size(varargin{1}))==1
    [m n] = size(varargin{2});
  else
    m = size(varargin{1},1);        % m or n must be 1
    n = size(varargin{1},2);
  end

  % Initialization
  factor = 134217729;               % splitting factor 2^27+1
  if abs(K)==2
    res = zeros(m,1);
    if K>0
      s = 0;
    else
      sinf = 0;
      ssup = 0;
    end
  else
    res = [];
    S = [];
  end

  % Compute array to sum up
  for i=1:floor(nargin/2)

    % compute next product A*x
    A = varargin{2*i-1};
    x = varargin{2*i};
    sA = size(A);
    sx = size(x);
    if sx(2)~=1
      error('second factor in Dot_ must be vector')
    end

    if ( prod(sA)==1 ) & ( sx(1)~=m )
      error('dimensions of summands in Dot_ do not match')
    end

    if ( prod(sA)==1 ) & ( abs(A)==1 )   % first factor A scalar +/- 1

      % TwoProduct(A,x)
      if A==1
        [res,q] = sumK([res x],2);
      else
        [res,q] = sumK([res -x],2);
      end
      if abs(K)==2
        if K>0
          s = s + q;
        else
          setround(-1)
          sinf = sinf + q;
          setround(1)
          ssup = ssup + q;
          setround(0)
        end
      else
        S = [S q];
      end
      idim = idim + 1;

    else                                    % first factor non-scalar or ~=  +/- 1

      % matrix-vector product or dot product
      if ( prod(sA)~=1 ) & ( sA(2)~=sx(1) )
        error('inner dimensions in Dot_ do not match')
      end
      if issparse(A)
        [iA,jA,ssA] = find(A);
      end
      C = factor*A;
      A1 = C - ( C - A );                       % upper part of A
      A2 = A - A1;                              % A = A1+A2 exact splitting

      C = factor*x;
      x1 = C - ( C - x );                       % upper part of x
      x2 = x - x1;                              % x = x1+x2 exact splitting

      I = sqrt(-1);
      if sA(1)~=1                               % m~=1: matrix-vector product
        if issparse(A)                          % factors s.t. A*x = sum( (A1+A2).*(x1+x2) , 2 )
          x1 = sparse(iA,jA,x1(jA),m,sA(2));
          x2 = sparse(iA,jA,x2(jA),m,sA(2));
        else
          x1 = x1(:,ones(1,m)).';
          x2 = x2(:,ones(1,m)).';
        end
        if isreal(A) | isreal(x)                % A or x real
          if issparse(A)
            h = sparse(iA,jA,ssA.*x(jA),m,sA(2));
          else
            h = A .* ( x(:,ones(1,m)).' );
          end
          idim = idim + sx(1);
        else                                    % both A and x complex
          if issparse(A)
            h = [ sparse(iA,jA,ssA.*real(x(jA)),m,sA(2)) , sparse(iA,jA,I*(ssA.*imag(x(jA))),m,sA(2)) ];
          else
            xx = x(:,ones(1,m)).';
            h = [ A.*real(xx) , A.*(I*imag(xx)) ];
          end
          idim = idim + 2*sx(1);
        end
      elseif sA(2)~=1
        if isreal(A) | isreal(x)                % A or x real
          h = A .* x.';
          x1 = x1.';
          x2 = x2.';
          idim = idim + sx(1);
        else                                    % both A and x complex
          h = [ A.*real(x).' , A.*(I*imag(x)).' ];
          x1 = x1.';
          x2 = x2.';
          idim = idim + 2*sx(1);
        end
      else
        if isreal(A) | isreal(x)                % A or x real
          h = A * x;
          idim = idim + 1;
        else                                    % both A and x complex
          h = [ A*real(x) , A*(I*imag(x)) ];
          idim = idim + 2;
        end
      end

      % TwoProduct(A,x): A*x = h + r
      if isreal(A) | isreal(x)                % A or x real
        r = A2.*x2 - ((( h - A1.*x1 ) - A2.*x1 ) - A1.*x2 );
      else                                    % both A and x complex
        r = [A2.*real(x2),A2.*(I*imag(x2))] - ((( h - [A1.*real(x1),A1.*(I*imag(x1))] ) - ...
          [A2.*real(x1),A2.*(I*imag(x1))] ) - [A1.*real(x2),A1.*(I*imag(x2))] );
      end

      if issparse(h)                    % compress h
        h = compress(h);
      end
      if issparse(r)                    % compress r
        r = compress(r);
      end

      % Summation vector and partial summation for |K|=2
      if abs(K)==2
        [res,q] = sumK([res h],2);      % error-free
        sq2 = size(q,2);
        sr2 = size(r,2);
        if sq2>sr2
          r = [r zeros(size(r,1),sq2-sr2)];
        elseif sq2>sr2
          q = [q zeros(size(q,1),sr2-sq2)];
        end
        if K>0
          s = s + sum(q+r,2);
        else
          setround(-1)
          sinf = sinf + sum(q+r,2);
          setround(1)
          ssup = ssup + sum(q+r,2);
          setround(0)
        end
      else
        [res,q] = sumK([res h],2);
        S = [S q r];
      end

    end

  end

  % Compute result in K-fold precision
  if abs(K)==2                          % quadruple precision
    if K>0
      res = res + s;
    else
      INTLAB_INTVAL_ETA = realmin*eps;  % smallest positive denormalized fl-pt
      setround(-1)
      resinf = sinf - 5*idim*INTLAB_INTVAL_ETA;
      setround(1)
      ressup = ssup + 5*idim*INTLAB_INTVAL_ETA;
      err = infsup(resinf,ressup);
      if nargout==1
        res = res + err;
      end
    end
  else                                  % |K|-fold precision
    if K>0
      res = Sum_([S res],K,2);
    else
      INTLAB_INTVAL_ETA = realmin*eps;  % smallest positive denormalized fl-pt
      [res,q] = sumK([S res],-K);       % error-free transformation
      setround(-1)
      resinf = sum(q,2) - 5*idim*INTLAB_INTVAL_ETA;
      setround(1)
      ressup = sum(q,2) + 5*idim*INTLAB_INTVAL_ETA;
      err = infsup(resinf,ressup);
      if nargout==1
        res = res + err;
      end
    end
  end

  if rndold
    setround(rndold)
  end

function [res,q] = sumK(a,K)
%SUM          Summation of "a" in K-fold precision along 2nd dimension
%             res is approximate sum, sum of errors in q
%             res + sum(q,2) = sum(a,2) in exact arithmetic

  % number of summands
  N = size(a,2);

  % summation (i.e. quadruple for K==2)
  for i=1:K-1
    pi = cumsum(a,2);
    z = diff(pi,1,2);
    q = ( pi(:,1:N-1) - ( pi(:,2:N) - z ) ) + ( a(:,2:N) - z );
    res = pi(:,N);
    if i~=K-1
      a = [q res];
    end
  end

function a = compress(a)
%COMPRESS     horizontal compress of array "a" such that sum(a,2) remains unchanged

  [i,j,s]=find(a.');
  rows = sum(spones(a),2);
  m = max(rows);
  if m==0
    a = sparse([],[],[],size(a,1),m);
    return
  end
  rows = nonzeros(rows);
  index = cumsum([1;rows(1:end-1)]);
  h = ones(size(i));
  h(index(2:end))=-rows(1:end-1)+1;
  a = sparse(j,cumsum(h),s,size(a,1),m);


% HOMOTRANS - 2D homogeneous transformation of points/lines.
%
% Function to perform a transformation on 2D homogeneous points/lines
% The resulting points/lines are normalised to lie in the z = 1 plane
%
% Usage:
%           t = homoTrans(P,v);
%
% Arguments:
%           P  - 3 x 3 transformation matrix
%           v  - 3 x n matrix of points/lines

%  Peter Kovesi
%  School of Computer Science & Software Engineering
%  The University of Western Australia
%  pk @ csse uwa edu au
%  http://www.csse.uwa.edu.au/~pk
%
%  April 2000

function t = homoTrans(P,v);

t = P*v;
t(1,:) = t(1,:)./t(3,:);   %  Now normalise
t(2,:) = t(2,:)./t(3,:);
t(3,:) = ones(1,size(v,2));


%TPOLY Generate scalar polynomial trajectory
%
% [S,SD,SDD] = TPOLY(S0, SF, M) is a scalar trajectory (Mx1) that varies
% smoothly from S0 to SF in M steps using a quintic (5th order) polynomial.
% Velocity and acceleration can be optionally returned as SD (Mx1) and SDD
% (Mx1) respectively.
%
% TPOLY(S0, SF, M) as above but plots S, SD and SDD versus time in a single
% figure.
%
% [S,SD,SDD] = TPOLY(S0, SF, T) as above but the trajectory is computed at
% each point in the time vector T (Mx1).
%
% [S,SD,SDD] = TPOLY(S0, SF, T, QD0, QD1) as above but also specifies the
% initial and final velocity of the trajectory.
%
% Notes::
% - If M is given
%   - Velocity is in units of distance per trajectory step, not per second.
%   - Acceleration is in units of distance per trajectory step squared, not
%     per second squared.
% - If T is given then results are scaled to units of time.
% - The time vector T is assumed to be monotonically increasing, and time
%   scaling is based on the first and last element.
%
% Reference:
%  Robotics, Vision & Control
%  Chap 3
%  Springer 2011
%
% See also LSPB, JTRAJ.

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

% [S,SD,SDD] = TPOLY(S0, SF, N, SD0, SDF) as above but specifies initial
% and final joint velocity for the trajectory.
%
% [S,SD,SDD] = TPOLY(S0, SF, T, SD0, SDF) as above but specifies initial
% and final joint velocity for the trajectory and time vector T.
%
% Notes::
% - In all cases if no output arguments are specified S, SD, and SDD are plotted
%   against time.
%
% See also LSPB, JTRAJ.

function [s,sd,sdd] = tpoly(q0, qf, t, qd0, qdf)

    t0 = t;
    if isscalar(t)
		t = (0:t-1)';
    else
        t = t(:);
    end
    if nargin < 4
        qd0 = 0;
    end
    if nargin < 5
        qdf = 0;
    end

    plotsargs = {'.-', 'Markersize', 16};

    tf = max(t);
    % solve for the polynomial coefficients using least squares
    X = [
        0           0           0         0       0   1
        tf^5        tf^4        tf^3      tf^2    tf  1
        0           0           0         0       1   0
        5*tf^4      4*tf^3      3*tf^2    2*tf    1   0
        0           0           0         2       0   0
        20*tf^3     12*tf^2     6*tf      2       0   0
    ];
    coeffs = (X \ [q0 qf qd0 qdf 0 0]')';

    % coefficients of derivatives
    coeffs_d = coeffs(1:5) .* (5:-1:1);
    coeffs_dd = coeffs_d(1:4) .* (4:-1:1);

    % evaluate the polynomials
    p = polyval(coeffs, t);
    pd = polyval(coeffs_d, t);
    pdd = polyval(coeffs_dd, t);

    switch nargout
        case 0
            if isscalar(t0)
                % for scalar time steps, axis is labeled 1 .. M
                xt = t+1;
            else
                % for vector time steps, axis is labeled by vector M
                xt = t;
            end

            clf
            subplot(311)
            plot(xt, p, plotsargs{:}); grid; ylabel('$s$', 'FontSize', 16, 'Interpreter','latex');

            subplot(312)
            plot(xt, pd, plotsargs{:}); grid;
            if isscalar(t0)
                ylabel('$ds/dk$', 'FontSize', 16, 'Interpreter','latex');
            else
                ylabel('$ds/dt$', 'FontSize', 16, 'Interpreter','latex');
            end

            subplot(313)
            plot(xt, pdd, plotsargs{:}); grid;
            if isscalar(t0)
                ylabel('$ds^2/dk^2$', 'FontSize', 16, 'Interpreter','latex');
            else
                ylabel('$ds^2/dt^2$', 'FontSize', 16, 'Interpreter','latex');
            end

            if ~isscalar(t0)
                xlabel('t (seconds)')
            else
                xlabel('k (step)');
                for c=findobj(gcf, 'Type', 'axes')
                    set(c, 'XLim', [1 t0]);
                end
            end
            shg
        case 1
            s = p;
        case 2
            s = p;
            sd = pd;
        case 3
            s = p;
            sd = pd;
            sdd = pdd;
    end


% Calculate Pearson cross correlation between two firing rate maps
%
% The size of return variable corrValue depends on input parameter 'output':
% *) 'single', corrValue is a single number. Maps are reshaped in a vector and
%     correlation is calculated.
% *) 'vector', corrValue is a vector. Maps are processed row- or column-wise.
%    corrValue(i) is a correlation coeeficient for row/column i.
% Row/column selection is done by parameter 'processBy'.
%
%  USAGE
%   corrValue = analyses.spatialCrossCorrelation(map1, map2, <options>)
%   map1      2D matrix, rate map
%   map2      2D matrix, rate map
%   <options>   optional list of property-value pairs (see table below)
%
%   =========================================================================
%    Properties    Values
%   -------------------------------------------------------------------------
%   'output'       'single' or 'vector'. Default is 'single'.
%
%   'processBy'     'r' or 'c'. The parameter specifies how maps are processed.
%                   'r' means iteration over y-axis (rows) and calculation of correlation per row.
%                   'c' means iteration over x-axis (columns) and calculation of correlation
%                   per column. Default is 'c'. This only makes difference for 'vector' output.
%                   Single value is independent of processing order.
%   =========================================================================
%
%   corrValue       Correlation value(s). Can be either a single number or a vector.
%
function corrValue = spatialCrossCorrelation(map1, map2, varargin)
    columnWise = 0;
    rowWise = 1;
    single = 'single';

    inp = inputParser;
    defaultProcessBy = 'c';
    defaultReturnFormat = single;

    checkProcessBy = @(x) strcmpi(x, 'c') || strcmpi(x, 'r');
    checkReturnFormat = @(x) strcmpi(x, 'single') || strcmpi(x, 'vector');

    addRequired(inp, 'map1');
    addRequired(inp, 'map2');
    addParamValue(inp, 'processBy', defaultProcessBy, checkProcessBy);
    addParamValue(inp, 'output', defaultReturnFormat, checkReturnFormat);

    parse(inp, map1, map2, varargin{:});

    % get parsed arguments
    if strcmpi(inp.Results.processBy, 'c')
        orientation = columnWise;
    else
        orientation = rowWise;
    end
    returnFormat = inp.Results.output;

    if ~isequal(size(map1), size(map2))
        warning('BNT:mapsSize', 'Maps are of different size. This is suspicious. Correlation won''t be calculated');
        corrValue = nan;
        return;
    end

    if strcmpi(returnFormat, single)
        % Transform the 2-D maps to 1-D arrays by assembling the columns/rows from the maps
        A = reshape(map1, numel(map1), 1);
        B = reshape(map2, numel(map2), 1);

        % Remove the bins with NaN in A from both maps
        nansA = isnan(A);
        A = A(~nansA);
        B = B(~nansA);

        % Remove the bins with NaN in B from both maps
        nansB = isnan(B);
        A = A(~nansB);
        B = B(~nansB);

        % Calculate the correlation for the two rate maps.
        if isempty(A) || isempty(B)
            corrValue = NaN; % Cannot do calculation on empty arrays
        else
            corrValue = corr(A, B);
        end
    else
        nansLeft = isnan(map1);
        map1(nansLeft) = [];
        map2(nansLeft) = [];

        nansRight = isnan(map2);
        map1(nansRight) = [];
        map2(nansRight) = [];

        if orientation == columnWise
            corrMatrix = corr(map1, map2);
            corrValue = diag(corrMatrix)'; % make a row
        else
            corrMatrix = corr(map1', map2');
            corrValue = diag(corrMatrix); % make a column
        end
    end
end


function value = r8_cbrt ( x )

%*****************************************************************************80
%
%% R8_CBRT computes the cube root of an R8.
%
%  Discussion:
%
%    The approximation is a generalized Chebyshev series converted
%    to polynomial form.  The approximation is nearly best in the
%    sense of relative error with 4.085 digits accuracy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 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 number whose square root is desired.
%
%    Output, real VALUE, the cube root of X.
%
  persistent cbrt2
  persistent niter

  if ( isempty ( niter ) )

    cbrt2 = [ ...
      0.62996052494743658238360530363911, ...
      0.79370052598409973737585281963615, ...
      1.0, ...
      1.25992104989487316476721060727823, ...
      1.58740105196819947475170563927231 ]';

    niter = r8_aint ( 1.443 * log ( - 0.106 ...
      * log ( 0.1 * r8_mach ( 3 ) ) ) + 1.0 );

  end

  value = 0.0;

  if ( x ~= 0.0 )

    [ y, n ] = r8_upak ( abs ( x ) );
    ixpnt = floor ( n / 3 );
    irem = n - 3 * ixpnt + 3;

    value = 0.439581 + y * ( ...
            0.928549 + y * ( ...
          - 0.512653 + y * ...
            0.144586 ) );

    for iter = 1 : niter
      vsq = value * value;
      value = value + ( y - value * vsq ) / ( 3.0 * vsq );
    end

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

    value = r8_pak ( cbrt2(irem) * value, ixpnt );

  end

  return
end


function [X,para1,para2,Problem]=pretreat(X,method,para1,para2)
%   data pretreatment
Problem=0;
if nargin==2
  [Mx,Nx]=size(X);
   if strcmp(method,'autoscaling')
    para1=mean(X);para2=std(X);
   elseif strcmp(method,'center')
    para1=mean(X);para2=ones(1,Nx);
   elseif strcmp(method,'minmax')
    para1=min(X);maxv=max(X);para2=maxv-para1;
   elseif strcmp(method,'pareto')
    para1=mean(X);para2=sqrt(std(X));
   elseif strcmp(method,'none')
    para1=zeros(1,Nx);para2=ones(1,Nx);
   else
    display('Wrong data pretreat method!');
   end

   if length(find(para2==0))<1
     for i=1:Nx; X(:,i)=(X(:,i)-para1(i))/para2(i);end
   else
     Problem=1;
   end

elseif nargin==4
   [Mx,Nx]=size(X);
   for i=1:Nx
     X(:,i)=(X(:,i)-para1(i))/para2(i);
   end
end


%% Laplacian Eigen maps and MDS
    close all; clear all;
	%[X, labels] = generate_data('swiss', 2000);

    % 1st group of points
    X1=[1 1
        1 0.5
        1 1.5
        0.5 1
        1.5 1
        ]
    [N1,r]=size(X1);
    labels([1:N1])=1;  % Labels of the class - 3 classes here

    % 2nd group of points
    X2=[-1 -1
        -1 -0.8
        -1 -1.2
        -0.8 -1
        -1.2 -1
        ]
     [N2,r]=size(X2);
       labels([N1+1:N1+N2])=2;

    % 3rd group of points (overlap with one and two)
%         X3=[-0.9 -1
%         -0.2 -0.8
%         -0.5 -0.5
%          0.5 0.8
%          0.2 0.0
%          0.1 -0.1
%          0.2 -0.2
% %         ]
        X3=X1;
     [N3,r]=size(X3);
   labels([N1+N2+1:N1+N2+N3])=3;

 figure; hold on;
 plot(X1(:,1),X1(:,2),'b*');
 plot(X2(:,1),X2(:,2),'r*');
 plot(X3(:,1),X3(:,2),'k*');
 legend('Class 1', 'Class 2', 'Class 3');
axis([-2 2 -2 2]);

X=[X1' X2' X3']';  %% Construct the entire matrix made of the 3 subgroups of points

[L,r]=size(X);
for i=1:L
 if labels(i)==1 c(i,:)=[0 0 1];
 elseif labels(i)==2; c(i,:)=[1 0 0];
 else c(i,:)=[0 0 0];
 end;
end;
%%
figure; hold on;

%% Perform MDS
   % [mappedX, mapping] = compute_mapping(X, 'MDS', 3);

   disp('Computing kernel matrix...');
   kernel       = 'gauss';
   param1       = 1; % sigma
   param2       = 0; % not used
   K            = gram(X, X, kernel, param1, param2);
   dims_keep    = 2;

   [mappedX, mapping] = cmdscale(K,dims_keep);

   subplot(2,2,1), scatter(mappedX(:,1), mappedX(:,2), 25, c, 'filled'); title(['Result of MDS']); drawnow

%% Perform PCA
    [mappedX, mapping] = compute_mapping(X, 'PCA', 2);
	subplot(2,2,2), scatter(mappedX(:,1), mappedX(:,2), 25, c, 'filled'); title(['Result of ' mapping.name]); drawnow

    %% Perform Laplacian
    [mappedX, mapping] = compute_mapping(X, 'Laplacian', 4);
	subplot(2,2,3), scatter(mappedX(:,1), mappedX(:,2), 25, c, 'filled'); title(['Result of ' mapping.name]); drawnow
    %% Isomap
    [mappedX, mapping] = compute_mapping(X, 'Isomap', 2);
	subplot(2,2,4), scatter(mappedX(:,1), mappedX(:,2), 25, c, 'filled'); title(['Result of ' mapping.name]); drawnow
return;

	figure, scatter3(X(:,1), X(:,2), X(:,3), 5, labels); title('Original dataset'), drawnow
	no_dims = round(intrinsic_dim(X, 'MLE'));
	disp(['MLE estimate of intrinsic dimensionality: ' num2str(no_dims)]);

    % This does not seem to be implemented in the toolbox, -> use matlab
    % dimensionality reduction: http://uk.mathworks.com/help/stats/dimensionality-reduction.html
    [mappedX, mapping] = compute_mapping(X, 'MDS', 2);
	figure, scatter(mappedX(:,1), mappedX(:,2), 5); title(['Result of ' mapping.name]); drawnow

    [mappedX, mapping] = compute_mapping(X, 'LaplacianEigenmaps', no_dims, 7);
	figure, scatter(mappedX(:,1), mappedX(:,2), 5, labels(mapping.conn_comp)); title('Result of Laplacian Eigenmaps'); drawnow

    %% Multi-dimensional scaling from matlab

    % Generate some points in 4-D space, but close to 3-D space, then reduce them to distances only.
    X = [normrnd(0,1,10,3) normrnd(0,.1,10,1)];
    D = pdist(X,'euclidean');

    [Y,e] = cmdscale(D);

    % Four, but fourth one small
    dim = sum(e > eps^(3/4));

    % Poor reconstruction
    maxerr2 = max(abs(pdist(X)-pdist(Y(:,1:2))))

    % Good reconstruction
    maxerr3 = max(abs(pdist(X)-pdist(Y(:,1:3))))

    % Exact reconstruction
    maxerr4 = max(abs(pdist(X)-pdist(Y)))

    %% Try matlab MDS with Swiss role data set

    [X, labels] = generate_data('swiss', 1000);

    % Use the euclidean distance metrix, rows of X should correspond to
    % observations
    D          = pdist(X,'euclidean');

    disp(['D: ' num2str(size(D,1)) ' x ' num2str(size(D,2))]);

    [Y,e]       = cmdscale(D);
  	figure, scatter(Y(:,1), Y(:,2), 5,labels); title(['Result of Matlab MDS']); drawnow


%% HODGELAPMGRATE test multigrid solver for Hodge Laplacian
%
% Reference
%
% L. Chen, Y. Wu, L. Zhong and J. Zhou. Multigrid Preconditioners for Mixed
% Finite Element Methods of Vector Laplacian. Journal of Scientific
% Computing, 77:101?128, 2018. https://doi.org/10.1007/s10915-018-0697-7
%
% See also HodgeLap3mgrate
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%%
close all; clear variables;
pde = HodgeLaplacianEdata1;
% bdFlag = setboundary(node,elem,'Dirichlet','x==0','Neumann','~(x==0)');
option.elemType = 'ND0';

%% Options
% option.N0 = 10;  % number of nodes in the coarsest mesh
option.maxIt = 4;
option.rateflag = 0;
% option.mg.solver = 'VCYCLE';    % V,W;
% option.mg.coarsematrix = 'G'; % Galerkin formulation
option.mg.smoothingstep = 2;    % Smoothing step.
option.mg.smoothingratio = 1.5; % ratio of variable smoothing
option.mg.Vit = 1;      % number of cycles for Schur complement

%% Square
disp('Square Domain')
option.L0 = 3;  % refine to a finer mesh
[node,elem] = squaremesh([0,1,0,1],1/4);
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);

%% Diagonal Preconditioner
option.solver = 'sdiag';
mfemHodgeLap(mesh,pde,option);

%% Triangular Preconditioner
option.solver = 'tri';
mfemHodgeLap(mesh,pde,option);

%% Approximated Factorization Preconditioner
option.solver = 'appf';
mfemHodgeLap(mesh,pde,option);

%% Lshape domain
disp('Lshape Domain')
option.L0 = 2;  % refine to a finer mesh
pde = fveconedata;
[node,elem] = squaremesh([-1,1,-1,1],0.25);
[node,elem] = delmesh(node,elem,'x>0 & y<0');
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);

%% Diagonal Preconditioner
option.solver = 'sdiag';
mfemHodgeLap(mesh,pde,option);

%% Triangular Preconditioner
option.solver = 'tri';
mfemHodgeLap(mesh,pde,option);

%% Approximated Factorization Preconditioner
option.solver = 'appf';
mfemHodgeLap(mesh,pde,option);

%% Crack domain
disp('Crack Domain')
pde = fveconedata;
node = [1,0; 0,1; -1,0; 0,-1; 0,0; 1,0];        % nodes
elem = [5,1,2; 5,2,3; 5,3,4; 5,4,6];            % elements
elem = label(node,elem);                        % label the mesh
[node,elem ] = uniformrefine(node,elem);
bdFlag = setboundary(node,elem,'Dirichlet');
option.L0 = 3;  % refine to a finer mesh
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);

%% Diagonal Preconditioner
option.solver = 'sdiag';
mfemHodgeLap(mesh,pde,option);

%% Triangular Preconditioner
option.solver = 'tri';
mfemHodgeLap(mesh,pde,option);

%% Approximated Factorization Preconditioner
option.solver = 'appf';
mfemHodgeLap(mesh,pde,option);


% Fugacity coeficient calculation using Soave equation of state
% Author: Arnulfo Rosales-Quintero
% Email: arnol122@gmail.com
% This function calculates either liquid or vapor fugacity coeficients
%-------------------------------------------------------------------------%
function [Phi,Vmix]=Phi(P,T,xy,a,b,phase01)
%-------------------------------------------------------------------------%
    global delta1 delta2 R
    global Kij
%-----------------------------------------------------------------------%
nc = length(xy);
%Mixture rule
amix=0.;	bmix=0.;
for i=1:nc
    dda=0.;
    for j=1:nc
        aij(i,j)= sqrt(a(i)*a(j))*(1.0-Kij(i,j));
        dda     = 2.0 * xy(j) * aij(i,j) + dda;
        amix    = xy(i)*xy(j) * aij(i,j) + amix;	%"a" mixture constant
    end
    dna(i) = dda;						%First derivative   d(an)/dni
    dnb(i) = b(i);						%First derivative   d(bn)/dni
    bmix   = xy(i)*b(i) + bmix;         %"b" mixture constant
end
%-------------------------------------------------------------------------%
%Mixture Volume
    Bb = (P*bmix*(delta1+delta2-1.0) - R*T)/P;
    Cc = (amix - (P*bmix^2+R*T*bmix)*(delta1+delta2) + P*bmix^2*delta1*delta2)/P;
    Dd = (-amix*bmix - (P*bmix^3 + R*T*bmix^2)*delta1*delta2)/P;

switch phase01
    case ('vapor')
        Vmix = max(roots([1 Bb Cc Dd]));
    case ('liquid')
        Vmix = min(roots([1 Bb Cc Dd]));
end
%-------------------------------------------------------------------------%
%Fugacity calculation
for i=1:nc
	Phi(i) = dnb(i)/bmix*(P*Vmix/R/T-1.) - log(P*(Vmix-bmix)/R/T);
    Phi(i) = Phi(i) - amix/bmix*(dna(i)/amix-dnb(i)/bmix)/R/T*log((Vmix+delta1*bmix)/(Vmix+delta2*bmix))/(delta1-delta2);
end
	Phi = exp(Phi);
%-------------------------------------------------------------------------%


classdef SMMOP6 < PROBLEM
% <multi/many> <real> <large/none> <multimodal> <sparse/none>
% Sparse multi-modal multi-objective optimization problem
% theta --- 0.1 --- Sparsity of the Pareto sets
% np    ---   4 --- Number of the Pareto sets

%------------------------------- Reference --------------------------------
% Y. Tian, R. Liu, X. Zhang, H. Ma, K. C. Tan, and Y. Jin, A
% multipopulation evolutionary algorithm for solving large-scale multimodal
% multiobjective optimization problems, IEEE Transactions on Evolutionary
% Computation, 2021, 25(3): 405-418.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties(Access = private)
        theta = 0.1;    % Sparsity of the Pareto sets
        np    = 4;    	% Number of the Pareto sets
        POS;            % Pareto optimal set for IGDX calculation
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.theta,obj.np] = obj.ParameterSet(0.1,4);
            if isempty(obj.M); obj.M = 2; end
            if isempty(obj.D); obj.D = 100; end
            obj.lower    = [zeros(1,obj.M-1)+0,zeros(1,obj.D-obj.M+1)-1];
            obj.upper    = [zeros(1,obj.M-1)+1,zeros(1,obj.D-obj.M+1)+2];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            [N,D] = size(X);
            M     = obj.M;
            S     = ceil(obj.theta*(D-M));
            g     = zeros(N,obj.np);
            for i = 1 : obj.np
                g(:,i) = sum(g2(X(:,M+(i-1)*S:M+i*S-1),pi/3),2)+sum(g4(X(:,[M:M+(i-1)*S-1,M+i*S:end]),0),2);
            end
            PopObj = repmat(1+min(g,[],2)/(D-M+1),1,M).*fliplr(cumprod([ones(N,1),1-cos(X(:,1:M-1)*pi/2)],2)).*[ones(N,1),1-sin(X(:,M-1:-1:1)*pi/2)];
        end
        %% Generate Pareto optimal solutions
        function R = GetOptimum(obj,N)
            % Generate points in Pareto optimal set
            A       = GetPS(obj.D-obj.M+1,obj.np,ceil(obj.theta*(obj.D-obj.M)));
            X       = UniformPoint(N/size(A,1),obj.M-1,'grid');
            obj.POS = [repmat(X,size(A,1),1),A(repmat(1:end,size(X,1),1),:)];
            % Generate points on Pareto front
            R = UniformPoint(N,obj.M);
            c = ones(size(R,1),obj.M);
            for i = 1 : size(R,1)
                for j = 2 : obj.M
                    temp = R(i,j)/R(i,1)*prod(1-c(i,obj.M-j+2:obj.M-1));
                    c(i,obj.M-j+1) = (temp^2-temp+sqrt(2*temp))/(temp^2+1);
                end
            end
            x = acos(c)*2/pi;
            R = fliplr(cumprod([ones(size(x,1),1),1-cos(x(:,1:end-1)*pi/2)],2)).*[ones(size(x,1),1),1-sin(x(:,end-1:-1:1)*pi/2)];
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                a = linspace(0,pi/2,100)';
                R = [1-cos(a),1-sin(a)];
            elseif obj.M == 3
                a = linspace(0,pi/2,10)';
                R = {(1-cos(a))*(1-cos(a')),(1-cos(a))*(1-sin(a')),(1-sin(a))*ones(size(a'))};
            else
                R = [];
            end
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case 'IGDX'
                    score = feval(metName,Population,obj.POS);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopDec = Population.decs;
            A      = GetPS(obj.D-obj.M+1,obj.np,ceil(obj.theta*(obj.D-obj.M)));
            [~,Label]  = min(pdist2(PopDec(:,obj.M:end),A),[],2);
            tempStream = RandStream('mlfg6331_64','Seed',2);
            if obj.M == 2
                for i = 1 : size(A,1)
                    color = rand(tempStream,1,3);
                    Draw(Population(Label==i).objs+(i-1)*0.05,'o','MarkerSize',6,'Marker','o','Markerfacecolor',sqrt(color),'Markeredgecolor',color,{'\it f\rm_1','\it f\rm_2',[]});
                    Draw(obj.PF+(i-1)*0.05,'-','LineWidth',1,'Color',color);
                end
            elseif obj.M == 3
                for i = 1 : size(A,1)
                    color = rand(tempStream,1,3);
                    ax = Draw(Population(Label==i).objs+(i-1)*0.05,'o','MarkerSize',8,'Marker','o','Markerfacecolor',sqrt(color),'Markeredgecolor',color,{'\it f\rm_1','\it f\rm_2','\it f\rm_3'});
                    surf(ax,obj.PF{1}+(i-1)*0.05,obj.PF{2}+(i-1)*0.05,obj.PF{3}+(i-1)*0.05,'EdgeColor',color,'FaceColor','none');
                end
            else
                for i = 1 : size(A,1)
                    Draw(Population(Label==i).objs,'-','Color',rand(tempStream,1,3),'LineWidth',2);
                end
            end
        end
    end
end

function g = g2(x,t)
    g = 2*(x-t).^2 + sin(2*pi*(x-t)).^2;
end

function g = g4(x,t)
    g = sqrt((x-t).^2)+sin(2*pi*(x-t)).^2;
end

function PS = GetPS(D,np,S)
    PS = zeros(np,D);
    for i = 1 : np
        PS(i,(i-1)*S+1:i*S) = 1;
    end
end


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Globally Optimal Solutions,
%           Solvability/Observability Analysis and Uncertainty Description.
%           IEEE Transactions on Robotics.
%           https://doi.org/10.1109/TRO.2022.3155880
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com
%
%
% syms_pnp.m: Generating symbolic expressions for solving PnP problem

clear all
close all
clc

addpath('func_files');
addpath('utils');

syms b1 b2 real
syms r1 r2 r3 real

image_pt = [b1, b2];
world_pt = [r1, r2, r3];

syms fx fy cx cy real
K = [fx, 0, 0;
      0, fy, 0;
     cx, cy, 1];

syms q0 q1 q2 q3 real
syms t1 t2 t3 lambda scale real
q = [q0; q1; q2; q3];
tt = [t1; t2; t3];
RR = q2R(q);
XX = [
    RR, tt;
    zeros(1, 3), 1];
J_pure = J_pnp_loss(image_pt, world_pt, K, RR, tt) * scale;
[coef_J_pure, mon_J_pure] = coeffs(J_pure, [q; tt]);
generateFuncFile(coef_J_pure, fullfile('func_files', 'coef_J_pure_pnp_func_new.m'), {image_pt, world_pt, [fx, fy, cx, cy], scale});
generateFuncFile(mon_J_pure, fullfile('func_files', 'mon_J_pure_pnp_func_new.m'), {q, tt});
J = J_pure - 0.5 * lambda * (q.' * q - 1);
x = [q; tt; lambda];
Jacob = jacobian(J, x);
Jacob = expand(Jacob.');

coef_len = length(coef_J_pure);
str = sprintf('coef_J = sym(''coef_J'', [1, %d]);', coef_len);
eval(str);
showSyms(symvar(coef_J));
J = coef_J * mon_J_pure.' - 0.5 * lambda * (q.' * q - 1);
x = [q; tt; lambda];
Jacob = jacobian(J, x);
Jacob = expand(Jacob.');

ts = solveSyms(Jacob(5 : 7), tt);
ss = Jacob(1 : 4);
ss = subs(ss, q0^3, (q0 * (1 - q1^2 - q2^2 - q3^2)));
ss = subs(ss, q0^2, (1 - q1^2 - q2^2 - q3^2));
Jacob(1 : 4) = ss;

ss = subs(ss, t1, ts.t1);
ss = subs(ss, t2, ts.t2);
ss = subs(ss, t3, ts.t3);

[coeft1, mont1] = coeffs(Jacob(5), tt);
g1_ = coeft1(1 : 3).';
g1 = vpa(zeros(3, 1));
for i = 1 : 3
    idx = sscanf(char(mont1(i)), 't%d');
    if(~isempty(idx))
        g1(idx) = g1_(i);
    end
end
[coeft2, mont2] = coeffs(Jacob(6), tt);
g2_ = coeft2(1 : 3).';
g2 = vpa(zeros(3, 1));
for i = 1 : 3
    idx = sscanf(char(mont2(i)), 't%d');
    if(~isempty(idx))
        g2(idx) = g2_(i);
    end
end
[coeft3, mont3] = coeffs(Jacob(7), tt);
g3_ = coeft3(1 : 3).';
g3 = vpa(zeros(3, 1));
for i = 1 : 3
    idx = sscanf(char(mont3(i)), 't%d');
    if(~isempty(idx))
        g3(idx) = g3_(i);
    end
end

[coeftq1, montq1] = coeffs(expand(Jacob(5) - g1.' * tt ), q);
generateFuncFile(coeftq1, fullfile('func_files', 'coeftq1_pnp_func_new.m'), {coef_J});
[coeftq2, montq2] = coeffs(expand(Jacob(6) - g2.' * tt ), q);
generateFuncFile(coeftq2, fullfile('func_files', 'coeftq2_pnp_func_new.m'), {coef_J});
[coeftq3, montq3] = coeffs(expand(Jacob(7) - g3.' * tt ), q);
generateFuncFile(coeftq3, fullfile('func_files', 'coeftq3_pnp_func_new.m'), {coef_J});
G = - [g1.'; g2.'; g3.'];
generateFuncFile(G, fullfile('func_files', 'G_pnp_func_new.m'), {coef_J});

pinvG = sym('pinvG', [3, 3]);
showSyms(symvar(pinvG));
coefs_tq = sym('coefs_tq', [3, 10]);
showSyms(symvar(coefs_tq));
ts = pinvG * coefs_tq * montq1.';
t1 = ts(1);
t2 = ts(2);
t3 = ts(3);
generateFuncFile(t1, fullfile('func_files', 't1_pnp_func_new.m'), {pinvG, coefs_tq, q});
generateFuncFile(t2, fullfile('func_files', 't2_pnp_func_new.m'), {pinvG, coefs_tq, q});
generateFuncFile(t3, fullfile('func_files', 't3_pnp_func_new.m'), {pinvG, coefs_tq, q});

[coef_Jacob1_qt, mon_Jacob1_qt] = coeffs(Jacob(1) + lambda * q0, [q; tt]);
generateFuncFile(coef_Jacob1_qt, fullfile('func_files', 'coef_Jacob1_qt_pnp_func_new.m'), {coef_J});
[coef_Jacob2_qt, mon_Jacob2_qt] = coeffs(Jacob(2) + lambda * q1, [q; tt]);
generateFuncFile(coef_Jacob2_qt, fullfile('func_files', 'coef_Jacob2_qt_pnp_func_new.m'), {coef_J});
[coef_Jacob3_qt, mon_Jacob3_qt] = coeffs(Jacob(3) + lambda * q2, [q; tt]);
generateFuncFile(coef_Jacob3_qt, fullfile('func_files', 'coef_Jacob3_qt_pnp_func_new.m'), {coef_J});
[coef_Jacob4_qt, mon_Jacob4_qt] = coeffs(Jacob(4) + lambda * q3, [q; tt]);
generateFuncFile(coef_Jacob4_qt, fullfile('func_files', 'coef_Jacob4_qt_pnp_func_new.m'), {coef_J});

coef_Jacob1_qt_syms = sym('coef_Jacob1_qt_syms', [1, 32]);
coef_Jacob2_qt_syms = sym('coef_Jacob2_qt_syms', [1, 32]);
coef_Jacob3_qt_syms = sym('coef_Jacob3_qt_syms', [1, 32]);
coef_Jacob4_qt_syms = sym('coef_Jacob4_qt_syms', [1, 32]);
showSyms(symvar(coef_Jacob1_qt_syms));
showSyms(symvar(coef_Jacob2_qt_syms));
showSyms(symvar(coef_Jacob3_qt_syms));
showSyms(symvar(coef_Jacob4_qt_syms));
coef_Jacob_qt_syms = [
    coef_Jacob1_qt_syms;
    coef_Jacob2_qt_syms;
    coef_Jacob3_qt_syms;
    coef_Jacob4_qt_syms;
    ];
Jacob_ = coef_Jacob_qt_syms * mon_Jacob1_qt.';

eqs = expand(eval([Jacob_; Jacob(8)]));

eqs_ = eqs;
eqs_(1 : 4) = eval(subs(eqs_(1 : 4), q0^2, (1 - q3 * q3 - q1 * q1 - q2 * q2)));
eqs_(1 : 4) = eval(subs(eqs_(1 : 4), q0^3, q0 * (1 - q3 * q3 - q1 * q1 - q2 * q2)));

f0 = eqs(1);
f1 = eqs(2);
f2 = eqs(3);
f3 = eqs(4);
[coef_f0_q, mon_f0_q] = coeffs(f0, q);
[coef_f1_q, mon_f1_q] = coeffs(f1, q);
[coef_f2_q, mon_f2_q] = coeffs(f2, q);
[coef_f3_q, mon_f3_q] = coeffs(f3, q);
coef_f0_q_sym = sym('coef_f0_q_sym', [1, 24]);
coef_f1_q_sym = sym('coef_f1_q_sym', [1, 24]);
coef_f2_q_sym = sym('coef_f2_q_sym', [1, 24]);
coef_f3_q_sym = sym('coef_f3_q_sym', [1, 24]);
showSyms(symvar(coef_f0_q_sym));
showSyms(symvar(coef_f1_q_sym));
showSyms(symvar(coef_f2_q_sym));
showSyms(symvar(coef_f3_q_sym));
f0 = coef_f0_q_sym * mon_f0_q.';
f1 = coef_f1_q_sym * mon_f1_q.';
f2 = coef_f2_q_sym * mon_f2_q.';
f3 = coef_f3_q_sym * mon_f3_q.';
coef_f_q_sym = [
    coef_f0_q;
    coef_f1_q;
    coef_f2_q;
    coef_f3_q;
    ];
generateFuncFile(coef_f_q_sym, fullfile('func_files', 'coef_f_q_sym_pnp_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});

eq = expand([
     f0 * q1 - f1 * q0;
     f0 * q2 - f2 * q0;
     f0 * q3 - f3 * q0;
     q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 - 1;
     ]);
generateFuncFile(eq, fullfile('func_files', 'eq_pnp_func_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym, q});

eq_ = eq;
[coefeq1, moneq1] = coeffs(eq_(1), q);
[coefeq2, moneq2] = coeffs(eq_(2), q);
[coefeq3, moneq3] = coeffs(eq_(3), q);

v1 = moneq1.';
generateFuncFile(v1, fullfile('func_files', 'v1_func_pnp_new.m'), {q});
v2 = moneq2.';
generateFuncFile(v2, fullfile('func_files', 'v2_func_pnp_new.m'), {q});
v3 = moneq3.';
generateFuncFile(v3, fullfile('func_files', 'v3_func_pnp_new.m'), {q});

D = [
    coefeq1;
    coefeq2;
    coefeq3;
    ];
generateFuncFile(D, fullfile('func_files', 'D_func_pnp_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym});

HH = jacobian(eq, q);
generateFuncFile(HH, fullfile('func_files', 'Jacob_pnp_func_new.m'), {coef_f0_q_sym, coef_f1_q_sym, coef_f2_q_sym, coef_f3_q_sym, q});
gradient = expand(HH.' * eq);

[coef1, mon1] = coeffs(eqs_(1), x);
[coef2, mon2] = coeffs(eqs_(2), x);
[coef3, mon3] = coeffs(eqs_(3), x);
[coef4, mon4] = coeffs(eqs_(4), x);

Q_sym = [
    coef1(7), coef1(14), coef1(18), coef1(20);
    coef2(7), coef2(14), coef2(18), coef2(20);
    coef3(7), coef3(14), coef3(18), coef3(20);
    coef4(7), coef4(14), coef4(18), coef4(20);
    ];
generateFuncFile(Q_sym, fullfile('func_files', 'Q_pnp_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});

res = expand(eqs_(1 : 4) - Q_sym * q);
H = jacobian(res, q);
h1 = H(:, 1);
h2 = H(:, 2);
h3 = H(:, 3);
h4 = H(:, 4);
P1 = jacobian(h1, q);
P2 = jacobian(h2, q);
P3 = jacobian(h3, q);
P4 = jacobian(h4, q);

for i = 1 : 4
    for j = 1 : 4
        str = sprintf('W%d%d = [jacobian(P%d(1, :), q%d).''; jacobian(P%d(2, :), q%d).''; jacobian(P%d(3, :), q%d).''; jacobian(P%d(4, :), q%d).''];', ...
                        i, j, i, j - 1, i, j - 1, i, j - 1, i, j - 1);
        eval(str);
    end
end

W1 = [W11, W12, W13, W14];
W2 = [W21, W22, W23, W24];
W3 = [W31, W32, W33, W34];
W4 = [W41, W42, W43, W44];
v = kron(q, q);
u = kron(v, q);
W = - [W1, W2, W3, W4] / 6;
generateFuncFile(W, fullfile('func_files', 'W_pnp_func_new.m'), {pinvG, coefs_tq, coef_Jacob_qt_syms});


%DEMO_REGRESSION1  Regression problem demonstration for 2-input
%                  function with Gaussian process
%
%  Description
%    The regression problem consist of a data with two input
%    variables and one output variable with Gaussian noise. The
%    model constructed is following:
%
%    The observations y are assumed to satisfy
%
%         y = f + e,    where e ~ N(0, s^2)
%
%    where f is an underlying function, which we are interested in.
%    We place a zero mean Gaussian process prior for f, which
%    implies that at the observed input locations latent values
%    have prior
%
%         f ~ N(0, K),
%
%    where K is the covariance matrix, whose elements are given as
%    K_ij = k(x_i, x_j | th). The function k(x_i, x_j | th) is
%    covariance function and th its parameters.
%
%    Since both likelihood and prior are Gaussian, we obtain a
%    Gaussian marginal likelihood
%
%        p(y|th) = N(0, K + I*s^2).
%
%   By placing a prior for parameters, p(th), we can find
%   the maximum a posterior (MAP) estimate for them by maximizing
%
%       argmax   log p(y|th) + log p(th).
%         th
%
%   An approximation for the posterior of the parameters, can be
%   found using Markov chain Monte Carlo (MCMC) methods. We can
%   integrate over the parameters also with other integration
%   approximations such as grid integration.
%
%   After finding MAP estimate or posterior samples of
%   parameters, we can use them to make predictions for f_new:
%
%       p(f_new | y, th) = N(m, S),
%
%          m = K_nt*(K + I*s^2)^(-1)*y
%          S = K_new - K_nt*(K + I*s^2)^(-1)*K_tn
%
%   where K_new is the covariance matrix of new f, and K_nt between
%   new f and training f.
%
%   For more detailed discussion of Gaussian process regression see,
%   for example, Rasmussen and Williams (2006) or Vanhatalo and
%   Vehtari (2008)
%
%   The demo is organised in three parts:
%     1) data analysis with MAP estimate for the parameters
%     2) data analysis with grid integration over the parameters
%     3) data analysis with MCMC integration over the parameters
%
%  See also DEMO_*
%
%  References:
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.
%
%    Vanhatalo, J. and Vehtari, A. (2008). Modelling local and global
%    phenomena with sparse Gaussian processes. Proceedings of the 24th
%    Conference on Uncertainty in Artificial Intelligence,

% Copyright (c) 2008-2010 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%========================================================
% PART 1 data analysis with full GP model
%========================================================
disp('GP with Gaussian noise model')

% Load the data
S = which('demo_regression1');
L = strrep(S,'demo_regression1.m','demodata/dat.1');
data=load(L);
x = [data(:,1) data(:,2)];
y = data(:,3);
[n, nin] = size(x);

% Now 'x' consist of the inputs and 'y' of the output.
% 'n' and 'nin' are the number of data points and the
% dimensionality of 'x' (the number of inputs).

% ---------------------------
% --- Construct the model ---
%
% First create structures for Gaussian likelihood and squared
% exponential covariance function with ARD
lik = lik_gaussian('sigma2', 0.2^2);
gpcf = gpcf_sexp('lengthScale', [1.1 1.2], 'magnSigma2', 0.2^2)

% Set some priors
pn = prior_logunif();
lik = lik_gaussian(lik,'sigma2_prior', pn);
pl = prior_unif();
pm = prior_sqrtunif();
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Following lines do the same since the default type is FULL
%gp = gp_set('type','FULL','lik',lik,'cf',gpcf);
gp = gp_set('lik', lik, 'cf', gpcf);

% Demostrate how to evaluate covariance matrices.
% K contains the covariance matrix without noise variance
%  at the diagonal (the prior covariance)
% C contains the covariance matrix with noise variance at
% the diagonal (the posterior covariance)
example_x = [-1 -1 ; 0 0 ; 1 1];
[K, C] = gp_trcov(gp, example_x)

% What has happend this far is the following
% - we created structures 'gpcf' and 'lik', which describe
%   the properties of the covariance function and Gaussian likelihood (see
%   gpcf_sexp and lik_gaussian for more details)
% - we created structures that describe the prior of the length-scale
%   and magnitude of the squared exponential covariance function and
%   the prior of the noise variance. These structures were set into
%   'gpcf' and 'lik' (see prior_* for more details)
% - we created a GP structure 'gp', which has among others 'gpcf'
%   and 'lik' structures.  (see gp_set for more details)

% -----------------------------
% --- Conduct the inference ---
%
% We will make the inference first by finding a maximum a posterior
% estimate for the parameters via gradient based optimization.
% After this we will use grid integration and Markov chain Monte
% Carlo sampling to integrate over the parameters.

% --- MAP estimate  ---
disp(' MAP estimate for the parameters')
% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% get optimized parameter values for display
[w,s]=gp_pak(gp);
% display exp(w) and labels
disp(s), disp(exp(w))

% For last, make predictions of the underlying function on a dense
% grid and plot it. Below Eft_map is the predictive mean and
% Varf_map the predictive variance.
[xt1,xt2]=meshgrid(-1.8:0.1:1.8,-1.8:0.1:1.8);
xt=[xt1(:) xt2(:)];
[Eft_map, Varft_map] = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure(1)
clf
mesh(xt1, xt2, reshape(Eft_map,37,37));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title('The predicted underlying function and the data points (MAP solution)');

% --- Grid integration ---
disp(' Grid integration over the parameters')
% Perform the grid integration and make predictions
% We can also get predictions for marginals of f, which are not
% Gaussian due to integration over the hyperparameters
[gp_array, P_TH, th, Eft_ia, Varft_ia, fx_ia, x_ia] = ...
    gp_ia(gp, x, y, xt, 'int_method', 'grid');

% Plot the predictions for two input locations
figure(2)
clf
subplot(2,1,1)
plot(x_ia(100,:), fx_ia(100,:))
title('p(f|D) at input location (-1.6, 0.7)');
subplot(2,1,2)
plot(x_ia(400,:), fx_ia(400,:))
title('p(f|D) at input location (-0.8, 1.1)');

% --- MCMC ---
disp(' MCMC integration over the parameters')
[gp_rec,g,opt] = gp_mc(gp, x, y, 'nsamples', 220,'display',20);

% After sampling we delete the burn-in and thin the sample chain
gp_rec = thin(gp_rec, 21, 2);

% Make the predictions
[Eft_mc, Varft_mc] = gp_pred(gp_rec, x, y, xt);

figure(1)
clf
subplot(1,2,1)
mesh(xt1, xt2, reshape(Eft_map,37,37));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title(['The predicted underlying function ';
       'and the data points (MAP solution)']);
subplot(1,2,2)
mesh(xt1, xt2, reshape(Eft_mc,37,37));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title(['The predicted underlying function  ';
       'and the data points (MCMC solution)']);
set(gcf,'pos',[93 511 1098 420])

% We can compare the posterior samples of the parameters to the MAP
% estimate that we got from optimization
figure(3)
clf
subplot(1,2,1)
plot(gp_rec.cf{1}.lengthScale)
title('The sample chain of length-scales')
subplot(1,2,2)
plot(gp_rec.cf{1}.magnSigma2)
title('The sample chain of magnitude')
set(gcf,'pos',[93 511 1098 420])

figure(4)
clf
subplot(1,4,1)
hist(gp_rec.cf{1}.lengthScale(:,1))
hold on
plot(gp.cf{1}.lengthScale(1), 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('Length-scale 1')
subplot(1,4,2)
hist(gp_rec.cf{1}.lengthScale(:,2))
hold on
plot(gp.cf{1}.lengthScale(2), 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('Length-scale 2')
subplot(1,4,3)
hist(gp_rec.cf{1}.magnSigma2)
hold on
plot(gp.cf{1}.magnSigma2, 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('magnitude')
subplot(1,4,4)
hist(gp_rec.lik.sigma2)
hold on
plot(gp.lik.sigma2, 0, 'rx', 'MarkerSize', 11, 'LineWidth', 2)
title('Noise variance')
legend('MCMC samples', 'MAP estimate')
set(gcf,'pos',[93 511 1098 420])

% Sample from two posterior marginals and plot them alongside
% with the MAP and grid integration results
% gpmc_preds returns the predictive mean of the latent function
% with every sampled parameter value.
[Eft_mcs, Varft_mcs] = gpmc_preds(gp_rec, x, y, xt);
sf = normrnd(Eft_mcs(100,:), sqrt(Varft_mcs(100,:)));
sf2 = normrnd(Eft_mcs(400,:), sqrt(Varft_mcs(400,:)));

figure(2)
subplot(1,2,1)
[N,X] = hist(sf);
hist(sf)
hold on
plot(x_ia(100,:), max(N)/max(fx_ia(100,:))*fx_ia(100,:), 'k')
ff = norm_pdf(x_ia(100,:)', Eft_map(100), sqrt(Varft_map(100)));
plot(x_ia(100,:), max(N)/max(ff)*ff, 'r', 'lineWidth', 2)
set(gca, 'Ytick', [])
title('p(f|D) at input location (-1.6, 0.7)');
xlim([0 1])

subplot(1,2,2)
[N,X] = hist(sf2);
hist(sf2)
hold on
plot(x_ia(400,:), max(N)/max(fx_ia(400,:))*fx_ia(400,:), 'k')
ff = norm_pdf(x_ia(400,:)', Eft_map(400), sqrt(Varft_map(400)));
plot(x_ia(400,:), max(N)/max(ff)*ff, 'r', 'lineWidth', 2)
set(gca, 'Ytick', [])
title('p(f|D) at input location (-0.8, 1.1)');
xlim([-1.2 -0.6])

disp('Done')

% ========================
% Print figures for manual
% ========================
% $$$ sf = normrnd(Eft_mc(100,:), sqrt(Varft_mc(100,:)));
% $$$ sf2 = normrnd(Eft_mc(400,:), sqrt(Varft_mc(400,:)));
% $$$
% $$$ figure
% $$$ subplot(1,2,1)
% $$$ [N,X] = hist(sf);
% $$$ hist(sf)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(x_ia(100,:), max(N)/max(fx_ia(100,:))*fx_ia(100,:), 'k')
% $$$ ff = norm_pdf(x_ia(100,:)', Eft_map(100), sqrt(Varft_map(100)));
% $$$ plot(x_ia(100,:), max(N)/max(ff)*ff, 'k', 'lineWidth', 2)
% $$$ set(gca, 'Ytick', [])
% $$$ xlim([0 1])
% $$$ ylim([0 110])
% $$$
% $$$ subplot(1,2,2)
% $$$ [N,X] = hist(sf2);
% $$$ hist(sf2)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(x_ia(400,:), max(N)/max(fx_ia(400,:))*fx_ia(400,:), 'k')
% $$$ ff = norm_pdf(x_ia(400,:)', Eft_map(400), sqrt(Varft_map(400)));
% $$$ plot(x_ia(400,:), max(N)/max(ff)*ff, 'k', 'lineWidth', 2)
% $$$ set(gca, 'Ytick', [])
% $$$ xlim([-1.2 -0.5])
% $$$
% $$$
% $$$ set(gcf,'units','centimeters');
% $$$ set(gcf,'pos',[15 14 7 5])
% $$$ set(gcf,'paperunits',get(gcf,'units'))
% $$$ set(gcf,'paperpos',get(gcf,'pos'))
% $$$ print -depsc2 /proj/bayes/jpvanhat/software/doc/GPstuffDoc/pics/demo_regression1_fig3.eps
% $$$
% $$$
% $$$ figure(4)
% $$$ clf, subplot(1,4,1)
% $$$ hist(gp_rec.cf{1}.lengthScale(:,1))
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.cf{1}.lengthScale(1), 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('Length-s 1')
% $$$ xlim([0.3 1.6])
% $$$
% $$$ subplot(1,4,2)
% $$$ hist(gp_rec.cf{1}.lengthScale(:,2))
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.cf{1}.lengthScale(2), 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('Length-s 2')
% $$$ xlim([0.4 1.4])
% $$$
% $$$ subplot(1,4,3)
% $$$ hist(gp_rec.cf{1}.magnSigma2)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.cf{1}.magnSigma2, 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('magnitude')
% $$$ xlim([0.5 6])
% $$$
% $$$ subplot(1,4,4)
% $$$ hist(gp_rec.lik.sigma2)
% $$$ h = findobj(gca,'Type','patch');
% $$$ set(h,'FaceColor','w','EdgeColor','k')
% $$$ hold on
% $$$ plot(gp.lik.sigma2, 0, 'kx', 'MarkerSize', 11, 'LineWidth', 2)
% $$$ xlabel('Noise variance')
% $$$ xlim([0.03 0.06])
% $$$ set(gca, 'Xtick', [0.03 0.06])
% $$$
% $$$ set(gcf,'units','centimeters');
% $$$ set(gcf,'pos',[15 14 11 5])
% $$$ set(gcf,'paperunits',get(gcf,'units'))
% $$$ set(gcf,'paperpos',get(gcf,'pos'))
% $$$
% $$$ print -depsc2 /proj/bayes/jpvanhat/software/doc/GPstuffDoc/pics/demo_regression1_fig2.eps


function value = r8poly_value ( n, a, x )

%*****************************************************************************80
%
%% R8POLY_VALUE evaluates an R8 polynomial.
%
%  Discussion:
%
%    For sanity's sake, the value of N indicates the NUMBER of
%    coefficients, or more precisely, the ORDER of the polynomial,
%    rather than the DEGREE of the polynomial.  The two quantities
%    differ by 1, but cause a great deal of confusion.
%
%    Given N and A, the form of the polynomial is:
%
%      p(x) = a(1) + a(2) * x + ... + a(n-1) * x^(n-2) + a(n) * x^(n-1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the polynomial.
%
%    Input, real A(1:N), the coefficients of the polynomial.
%    A(1) is the constant term.
%
%    Input, real X, the point at which the polynomial is to be evaluated.
%
%    Output, real VALUE, the value of the polynomial at X.
%
  value = 0.0;
  for i = n : -1 : 1
    value = value * x + a(i);
  end

  return
end


%%	Author:  Tianli Yu	https://netfiles.uiuc.edu/tyu/www/tianli/
%%	Date:	Monday Aug. 01 2005  11:58 pm
%%
%%  SampleTest: This is a sample script to test the TriangulateSpherePoints.m
%%

	VertexM = rand(800, 3) - 0.5;
	VNormV = (sum(VertexM .^ 2, 2)) .^ .5;
	VertexM = VertexM ./ repmat(VNormV, 1, 3);
	FacetM = TriangulateSpherePoints(VertexM);
	figure;
	patch('vertices', VertexM, 'faces', FacetM, 'FaceColor', [1, 1, 0], 'EdgeColor', [1 .2 .2]);
	axis equal;


% Isoparametric Formulation Implementation
% clear memory
clear all
close all
clc
% E: modulus of elasticity
% A: area of cross section
% L: length of bar
E=2e5; A=12.5e-4; L=1.5;
s_exact=@(x)1/2*(80000*x)*x/(A);
hold on;
ezplot(s_exact,[0 1.5])
title('Exact solution v.s. FEM','interpreter','latex');
xlabel('x','interpreter','latex');
ylabel('Axial stress, Pa','interpreter','latex');

for i=1:4   %For NEL = 1, 2, 4, 8
NNOD=3;
fprintf( '\nNumber of elements:%d\n\n',2^(i-1) );
% numberElements: number of elements
numberElements=2^(i-1);
% numberNodes: number of nodes
numberNodes=2*numberElements+1;
% generation of coordinates and connectivities

nodeCoordinates=linspace(0,L,numberNodes);
%Generate element length vector
for i=1:numberElements
    Le(i)=L/numberElements;
    elementNodes(i,:)=[(i-1)*2+1 (i-1)*2+2 (i-1)*2+3];
end
% for structure:
   % displacements: displacement vector
   % force : force vector
   % stiffness: stiffness matrix
force=zeros(numberNodes,1);
stiffness=zeros(numberNodes,numberNodes);
% computation of the system stiffness matrix and force vector
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:) ;
  detJacobian=Le(e)/2;
  invJacobian=1/detJacobian;
  ngp = 3;
  [w,xi]=gauss1d(ngp);
  xc=0.5*(nodeCoordinates(elementDof(1))+nodeCoordinates(elementDof(3)));
  for ip=1:ngp;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      stiffness(elementDof,elementDof)=...
      stiffness(elementDof,elementDof)+ B'*B*w(ip)*detJacobian*E*A;
      force(elementDof)=force(elementDof)+...
          -80000*(xc+detJacobian*xi(ip))*shape'*detJacobian*w(ip);
  end
end

% boundary conditions and solution
% prescribed dofs
prescribedDof=[numberNodes];
% solution
GDof=numberNodes;
displacements=solution(GDof,prescribedDof,stiffness,force);
% output displacements/reactions
outputDisplacementsReactionsPretty(displacements,stiffness, ...
    numberNodes,prescribedDof,force)
fprintf('Axial stress\n')
fprintf('element\t\taxial stress\n')
%Stress and strain recovery
ngp = 2;

elementNodeCoor=zeros(numberElements*NNOD,1);
elementNodeStr=zeros(numberElements*NNOD,1);
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:);
  detJacobian=Le(e)/2;
  invJacobian=1/detJacobian;
  xi=[-1 0 1];
  for ip=1:NNOD;
      [shape,naturalDerivatives]=shapeFunctionL3(xi(ip));
      B=naturalDerivatives*invJacobian;
      elementNodeCoor(ip+(e-1)*NNOD,1)=nodeCoordinates(elementDof(ip));
      elementNodeStr(ip+(e-1)*NNOD,1)=E*B*displacements(elementDof,1);
      fprintf('%2.0f\t%2.0fth node\t%10.4e\n', e, ip,elementNodeStr(ip+(e-1)*NNOD,1))
  end
end

%post process
switch numberElements
    case 1
        str='rs--';
    case 2
        str='gs--';
    case 4
        str='ks--';
    case 8
        str='ms--';
end
switch numberElements
    case 1
        strg='r*';
    case 2
        strg='g*';
    case 4
        strg='k*';
    case 8
        strg='m*';
end
plot(elementNodeCoor,elementNodeStr,str)
hold on;
%plot(elementIpCoor,elementAxiStr,strg)
end
legend('Exact solution','NEL=1','NEL=2','NEL=4','NEL=8');
title('Stress','interpreter','latex','FontSize',18);
xlabel('$\it{x}$','interpreter','latex','FontSize',14);
ylabel({'Stress, $\it{\sigma}$'},'interpreter','latex','FontSize',14);
box on;


function b = r83_vxm ( n, a, x )

%*****************************************************************************80
%
%% R83_VXM multiplies a vector by an R83 matrix.
%
%  Discussion:
%
%    The r83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how a R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the linear system.
%
%    Input, real A(3,N), the R83 matrix.
%
%    Input, real X(N), the vector to be multiplied by A'.
%
%    Output, real B(N), the product A' * x.
%
  b(1:n)   =            a(2,1:n)   .* x(1:n);
  b(1:n-1) = b(1:n-1) + a(3,1:n-1) .* x(2:n);
  b(2:n)   = b(2:n)   + a(1,2:n)   .* x(1:n-1);

  return
end


function A = randherm(n,cnd)
%RANDHERM     Random Hermitian matrix
%
%   res = randherm(n,cnd)
%
% cnd, if specified, is the approximate condition number of the generated matrix
%

% written  11/05/12     S.M. Rump
% modified 12/06/12     S.M. Rump  lower case, typo
%

  if nargin==1
    A = randn(n) + sqrt(-1)*randn(n);
  else
    s = exp( [ 0 rand(1,n-2) 1 ] * log(cnd) ) / cnd;
    A = orth(randn(n)+sqrt(-1)*randn(n));
    A = A' * diag(s.*sign(randn(1,n))) * A;
  end

  % make sure A is Hermitian
  A = A - sqrt(-1)*diag(imag(diag(A)));   % make sure diagonal is real
  A = tril(A)+tril(A,-1)';


function ydot = lorenzeq(t,y)
%LORENZEQ Equation of the Lorenz chaotic attractor.
%   ydot = lorenzeq(t,y).
%   The differential equation is written in almost linear form.

SIGMA = 10.;
RHO = 28.;
BETA = 8./3.;

A = [ -BETA    0     y(2)
    0  -SIGMA   SIGMA
    -y(2)   RHO    -1  ];

ydot = A*y;