%%                   testPhaseLift.m

% This test file implements the Phaselift solver. The code builds
% a synthetic formulation of the Phase Retrieval problem, b = |Ax| and
% computes an estimate to x. The code finally plots a convergence curve
% and also makes a scatter plot of the true vs recovered solution.

% PAPER TITLE:
%             PhaseLift: Exact and Stable Signal Recovery from Magnitude
%             Measurements via Convex Programming.

% ARXIV LINK:
%              https://arxiv.org/abs/1109.4499

% 1) Each test script starts out by defining the length of the unknown
% signal, n and the number of measurements, m. These mesurements can be
% made complex by setting the isComplex flag to be true.
%
% 2) We then build the test problem by invoking the function
% 'buildTestProblem' which generates random gaussian measurements according
% to the user's choices in step(1). The function returns the measurement
% matrix 'A', the true signal 'xt' and the measurements 'b0'.
%
% 3) We set the options for the PR solver. For example, the maximum
% number of iterations, the tolerance value, the algorithm and initializer
% of choice. These options are controlled by setting the corresponding
% entries in the 'opts' struct.  Please see the user guide for a complete
% list of options.
%
% 4) We solve the phase retrieval problem by running the following line
% of code:
%   >>  [x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts)
% This solves the problem using the algorithm and initialization scheme
% specified by the user in the struct 'opts'.
%
% 5) Determine the optimal phase rotation so that the recovered solution
% matches the true solution as well as possible.
%
% 6) Report the relative reconstruction error. Plot residuals (a measure
% of error) against the number of iterations and plot the real part of the
% recovered signal against the real part of the original signal.

% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START-----------------------------------

clc;
clear;
close all;

n = 50;          % Dimension of unknown vector
m = 8 * n;        % Number of measurements
isComplex = true; % If the signal and measurements are complex

%%  Build a random test problem
fprintf('Building test problem...\n');
[A, xt, b0] = buildTestProblem(m, n, isComplex);

% Options
opts = struct;
opts.initMethod = 'custom';
opts.customx0 = randn(n,1); % random initial guess
opts.algorithm = 'PhaseLift';
opts.isComplex = isComplex;
opts.tol = 1e-6;
opts.maxIters = 10000;
opts.verbose = 2;

%% Try to recover x
fprintf('Running algorithm...\n');
[x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts);

%% Determine the optimal phase rotation so that the recovered solution
%  matches the true solution as well as possible.
alpha = (x'*xt)/(x'*x);
x = alpha * x;

%% Determine the relative reconstruction error.  If the true signal was
%  recovered, the error should be very small - on the order of the numerical
%  accuracy of the solver.
reconError = norm(xt-x)/norm(xt);
fprintf('relative recon error = %d\n', reconError);

% Plot a graph of error(definition depends on if opts.xt is provided) versus
% the number of iterations.
plotErrorConvergence(outs, opts)

% Plot a graph of the recovered signal x against the true signal xt.
plotRecoveredVSOriginal(x,xt);


% Fig. 6.15   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

w=logspace(-2,2,100);
k=10;
num=k;
den=conv([1 0],[1 2 1]);
[m10,p10]=bode(num,den,w);
k=2;
num=k;
den=conv([1 0],[1 2 1]);
[m2,p2]=bode(num,den,w);
k=0.1;
num=k;
den=conv([1 0],[1 2 1]);
[mp1,pp1]=bode(num,den,w);
figure(1)
loglog(w,m10,w,m2,w,mp1,w,ones(size(w)));
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.15 Frequency Response (a) magnitude');
text(1,20,'K=10');
text(0.2,3,'K=2');
text(0.9,0.01,'K=0.1');
bodegrid;
%pause;
figure(2)
semilogx(w,p10,w,p2,w,pp1,w,-180*ones(size(w)),'-');
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)')
title('Fig. 6.15 (b) phase');
bodegrid;


%CONTENTS GRAPHS Simple Toolbox for manipulating Geometric Graphs.
% Version 0.6 04-Sep-2017 .
%
%   The aim of this package is to provide functions to easily create,
%   modify and display geometric graphs (geometric in a sense position
%   nodes are associated to geometric position in 2D or 3D).
%
%   Graph are represented by a structure with at least two arrays:
%   * NODES, which contains coordinates of each vertex
%   * EDGES, which contains indices of start and end vertex.
%   Some graph functions consider adjacency list, as a cell array where
%   each cell contains the indices of the neighbor vertices.
%
%   Others arrays may sometimes be used:
%   * FACES, which contains indices of vertices of each face (either a
%       double array, or a cell array)
%   * CELLS, which contains indices of faces of each cell.
%
%   An alternative representation is to use a structure, with fields:
%   * 'nodes'
%   * 'edges'
%   corresponding to the data described above.
%
%   Note that topological description of 2D graph is entirely contained in
%   EDGES array, and that NODES array is used only to display the graph.
%
%   Caution: this type of data structure is easy to create and to manage,
%   but may be very inefficient for some algorithms.
%
%   Graphs are usually considered as non-oriented in this package.
%
%
% Graph creation
%   delaunayGraph              - Graph associated to Delaunay triangulation of input points.
%   euclideanMST               - Build euclidean minimal spanning tree of a set of points.
%   prim_mst                   - Minimal spanning tree by Prim's algorithm.
%   knnGraph                   - Create the k-nearest neighbors graph of a set of points.
%   relativeNeighborhoodGraph  - Relative Neighborhood Graph of a set of points.
%   gabrielGraph               - Gabriel Graph of a set of points.
%
% Create graph from images
%   imageGraph                 - Create equivalent graph of a binary image.
%   imageBoundaryGraph         - Convert boundary of a 2D/3D binary image into a graph or mesh.
%
% Voronoi Graphs
%   voronoi2d                  - Compute a voronoi diagram as a graph structure.
%   boundedVoronoi2d           - Comptues a bounded voronoi diagram as a graph structure.
%   centroidalVoronoi2d        - Centroidal Voronoi tesselation within a polygon.
%   centroidalVoronoi2d_MC     - Centroidal Voronoi tesselation by Monte-Carlo.
%   boundedCentroidalVoronoi2d - Create a 2D Centroidal Voronoi Tesselation in a box.
%   cvtUpdate                  - Update germs of a CVT with given points.
%   cvtIterate                 - Update germs of a CVT using random points with given density.
%   meshEnergy                 - Computes the energy of a tesselation, as the sum of second area moments.
%
% Geodesic and shortest path operations
%   grShortestPath             - Find a shortest path between two nodes in the graph.
%   grPropagateDistance        - Propagates distances from a vertex to other vertices.
%   grVertexEccentricity       - Eccentricity of vertices in the graph.
%   graphDiameter              - Diameter of a graph.
%   graphPeripheralVertices    - Peripheral vertices of a graph.
%   graphCenter                - Center of a graph.
%   graphRadius                - Radius of a graph.
%   grFindGeodesicPath         - Find a geodesic path between two nodes in the graph.
%   grFindMaximalLengthPath    - Find a path that maximizes sum of edge weights.
%
% Graph processing (general applications)
%   adjacencyListToEdges       - Convert an adjacency list to an edge array.
%   pruneGraph                 - Remove all edges with a terminal vertex.
%   mergeGraphs                - Merge two graphs, by adding nodes, edges and faces lists.
%   grMergeNodes               - Merge two (or more) nodes in a graph.
%   grMergeMultipleNodes       - Simplify a graph by merging multiple nodes.
%   grMergeMultipleEdges       - Remove all edges sharing the same extremities.
%   grSimplifyBranches         - Replace branches of a graph by single edges.
%
% Filtering operations on Graph
%   grMean                     - Compute mean value from neighbour nodes.
%   grMedian                   - Compute median value from neighbour nodes.
%   grDilate                   - Morphological dilation on graph.
%   grErode                    - Morphological erosion on graph.
%   grClose                    - Morphological closing on graph.
%   grOpen                     - Morphological opening on graph.
%
% Operations for geometric graphs
%   grEdgeLengths              - Compute length of edges in a geometric graph.
%   grMergeNodeClusters        - Merge cluster of connected nodes in a graph.
%   grMergeNodesMedian         - Replace several nodes by their median coordinate.
%   clipGraph                  - Clip a graph with a rectangular area.
%   clipGraphPolygon           - Clip a graph with a polygon.
%   clipMesh2dPolygon          - Clip a planar mesh with a polygon.
%   addSquareFace              - Add a (square) face defined from its vertices to a graph.
%   grFaceToPolygon            - Compute the polygon corresponding to a graph face.
%   graph2Contours             - Convert a graph to a set of contour curves.
%
% Graph information
%   grNodeDegree               - Degree of a node in a (undirected) graph.
%   grNodeInnerDegree          - Inner degree of a node in a graph.
%   grNodeOuterDegree          - Outer degree of a node in a graph.
%   grAdjacentNodes            - Find list of nodes adjacent to a given node.
%   grAdjacentEdges            - Find list of edges adjacent to a given node.
%   grOppositeNode             - Return opposite node in an edge.
%   grLabel                    - Associate a label to each connected component of the graph.
%
% Graph management (low level operations)
%   grRemoveNode               - Remove a node in a graph.
%   grRemoveNodes              - Remove several nodes in a graph.
%   grRemoveEdge               - Remove an edge in a graph.
%   grRemoveEdges              - Remove several edges from a graph.
%
% Graph display
%   drawGraph                  - Draw a graph, given as a set of vertices and edges.
%   drawGraphEdges             - Draw edges of a graph.
%   fillGraphFaces             - Fill faces of a graph with specified color.
%   drawDigraph                - Draw a directed graph, given as a set of vertices and edges.
%   drawDirectedEdges          - Draw edges with arrow indicating direction.
%   drawEdgeLabels             - Draw values associated to graph edges.
%   drawNodeLabels             - Draw values associated to graph nodes.
%   drawSquareMesh             - Draw a 3D square mesh given as a graph.
%   patchGraph                 - Transform 3D graph (mesh) into a patch handle.
%
% Input/Output
%   readGraph                  - Read a graph from a text file.
%   writeGraph                 - Write a graph to an ascii file.

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2005-11-07
% Copyright 2005-2022 INRA - Cepia Software Platform

help(mfilename);

% Deprecated functions
%   grSimplifyBranches_old     - Replace branches of a graph by single edges.
%   grRemoveMultiplePoints     - Remove groups of close nodes in a graph.
%   boundaryGraph              - Get boundary of image as a graph.
%   gcontour2d                 - Creates contour graph of a 2D binary image.
%   gcontour3d                 - Create contour graph of a 3D binary image.

% Functions that requires further development
%   quiverToGraph              - Converts quiver data to quad mesh.

% Other functions


function M = stiefelgeneralizedfactory(n, p, B)
% Returns a manifold structure of "scaled" orthonormal matrices.
%
% function M = stiefelgeneralizedfactory(n, p)
% function M = stiefelgeneralizedfactory(n, p, B)
%
% The generalized Stiefel manifold is the set of "scaled" orthonormal
% nxp matrices X such that X'*B*X is identity. B must be positive definite.
% If B is identity, then this is the standard Stiefel manifold.
%
% The generalized Stiefel manifold is endowed with a scaled metric
% by making it a Riemannian submanifold of the Euclidean space,
% again endowed with the scaled inner product.
%
% Some notions (not all) are from Section 4.5 of the paper
% "The geometry of algorithms with orthogonality constraints",
% A. Edelman, T. A. Arias, S. T. Smith, SIMAX, 1998.
%
% Paper link: http://arxiv.org/abs/physics/9806030.
%
% Note: egrad2rgrad and ehess2rhess involve solving linear systems in B. If
% this is a bottleneck for a specific application, then a way forward is to
% create a modified version of this file which preprocesses B to speed this
% up (typically, by computing a Cholesky factorization of it, then calling
% an appropriate solver; or by exploiting sparsity.)
%
% See also: stiefelfactory  grassmannfactory  grassmanngeneralizedfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, June 30, 2015.
% Contributors: Hiroyuki Sato and Kensuke Aihara, Dec. 27, 2018.
%
% Change log:
%
%    Sep.  6, 2018 (NB):
%        Removed M.exp() as it was not implemented.
%
%    Dec. 27, 2018 (NB):
%        Added retraction M.retr_qr (based on QR factorization) as an
%        alternative to the previous retraction, which is now accessible as
%        M.retr_polar. The new retraction should be more efficient: it is
%        now the default, as M.retr = M.retr_qr. This new retraction is a
%        contribution of H. Sato and K. Aihara, as per their paper:
%        'Cholesky QR-based retraction on the generalized Stiefel manifold'
%        https://link.springer.com/article/10.1007%2Fs10589-018-0046-7

    if ~exist('B', 'var') || isempty(B)
        B = speye(n); % Standard Stiefel manifold.
    end

    M.name = @() sprintf('Generalized Stiefel manifold St(%d, %d)', n, p);

    M.dim = @() (n*p - .5*p*(p+1));

    M.inner = @(X, eta, zeta) trace(eta'*(B*zeta)); % Scaled metric.

    M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

    M.dist = @(X, Y) error('stiefelgeneralizedfactory.dist not implemented yet.');

    M.typicaldist = @() sqrt(p);

    % Orthogonal projection of an ambient vector U to the tangent space
    % at X.
    M.proj = @projection;
    function Up = projection(X, U)
        BX = B*X;

        % Projection onto the tangent space
        Up = U - X*symm(BX'*U);
    end

    M.tangent = M.proj;

    M.egrad2rgrad = @egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad)

        % First, scale egrad according the to the scaled metric in the
        % Euclidean space. Ideally, B should be preprocessed to ease
        % solving linear systems, e.g., via Cholesky factorization.
        egrad_scaled = B\egrad;

        % Second, project onto the tangent space.
        % rgrad = egrad_scaled - X*symm((B*X)'*egrad_scaled);
        %
        % Verify that symm(BX'*egrad_scaled) = symm(X'*egrad).

        rgrad = egrad_scaled - X*symm(X'*egrad);
    end

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, H)
        egraddot = ehess;
        Xdot = H;

        % Directional derivative of the Riemannian gradient.
        egrad_scaleddot = B\egraddot;
        rgraddot = egrad_scaleddot - Xdot*symm(X'*egrad) ...
                                   - X*symm(Xdot'*egrad) ...
                                   - X*symm(X'*egraddot);

        % Project onto the tangent space.
        rhess = M.proj(X, rgraddot);
    end

    M.retr_polar = @retraction_polar;
    function Y = retraction_polar(X, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = guf(X + t*U); % Ensures Y'*B*Y is identity.
    end

    M.retr_qr = @retraction_qr;
    function Y = retraction_qr(X, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = gqf(X + t*U); % Ensures Y'*B*Y is identity.
    end

    % By default, we use the QR retraction
    M.retr = M.retr_qr;

    M.hash = @(X) ['z' hashmd5(X(:))];

    M.rand = @random;
    function X = random()
        X = guf(randn(n, p)); % Ensures X'*B*X is identity;
        % TODO: test if this can be replaced by gqf.
    end

    M.randvec = @randomvec;
    function U = randomvec(X)
        U = projection(X, randn(n, p));
        U = U / norm(U(:));
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(X) zeros(n, p);

    % This transport is compatible with the generalized polar retraction.
    M.transp = @(X1, X2, d) projection(X2, d);

    M.vec = @(X, u_mat) u_mat(:);
    M.mat = @(X, u_vec) reshape(u_vec, [n, p]);
    M.vecmatareisometries = @() false;

    % Some auxiliary functions
    symm = @(D) (D + D')/2;

    function X = gqf(Y)
        % Generalized QR decomposition of an n-by-p matrix Y.
        % X'*B*X is identity. See Sato and Aihara 2018,
        % Cholesky QR-based retraction on the generalized Stiefel manifold,
        % https://link.springer.com/article/10.1007%2Fs10589-018-0046-7
        %
        % Comment by Nicolas Boumal, Dec. 27, 2018, following discussions
        % with Hiroyuki Sato, Kensuke Aihara and Bamdev Mishra:
        %
        % In principle, to orthonormalize the columns of Y against the
        % inner product defined by B, it would be better to implement (for
        % example) a modified Gram-Schmidt algorithm, possibly run twice.
        % Indeed, the latter would be affected by the condition number of
        % sqrtm(B)*Y. In contrast, the method below first squares the data,
        % hence is affected by the condition number of Y'*B*Y, which is the
        % square of that of sqrtm(B)*Y. Fortunately, it is easily shown
        % that, when Y = X + U for a tangent vector U at X, the condition
        % number of sqrtm(B)*Y is upper bounded by the square root of
        % 1 + ||U||_X^2. In Riemannian optimization, it is seldom necessary
        % to retract very large vectors, hence it is expected that the
        % condition numbers encountered in practice will be reasonable. As
        % a result, the implementation below which calls directly upon
        % low-level routines (Cholesky and a small triangular system solve)
        % is expected to run faster in practice than a home-made
        % implementation of modified Gram-Schmidt, which would involve a
        % loop, and still be sufficiently accurate.
        R = chol(symm(Y'*(B*Y)));
        X = Y / R;
    end

    function X = guf(Y)
        % Generalized polar decomposition of an n-by-p matrix Y.
        % X'*B*X is identity.

        % Method 1
        [u, ~, v] = svd(Y, 0);

        % Instead of the following three steps, an equivalent, but an
        % expensive way is to do X = u*(sqrtm(u'*(B*u))\(v')).
        [q, ssquare] = eig(u'*(B*u));
        qsinv = q/sparse(diag(sqrt(diag(ssquare))));
        X = u*((qsinv*q')*v'); % X'*B*X is identity.

        % Another computation using restricted_svd
        % [u, ~, v] = restricted_svd(Y);
        % X = u*v'; % X'*B*X is identity.

    end

    function [u, s, v] = restricted_svd(Y) %#ok<DEFNU>
        % We compute a thin svd-like decomposition of an n-by-p matrix Y
        % into matrices u, s, and v such that u is an n-by-p matrix
        % with u'*B*u being identity, s is a p-by-p diagonal matrix
        % with positive entries, and v is a p-by-p orthogonal matrix.
        % Y = u*s*v'.
        [v, ssquare] = eig(symm(Y'*(B*Y))); % Y*B*Y is positive definite
        ssquarevec = diag(ssquare);

        s = sparse(diag(abs(sqrt(ssquarevec))));
        u = Y*(v/s); % u'*B*u is identity.
    end

end


function [err,elemErr] = getL2errorRT0(node,elem,exactSigma,sigmah,d)
%% GETL2ERRORRT0 L2 norm of RT0 element.
%
%  The input exactSigma can be a function bundle or vector array.
%  When exactSigma is a vector array, it should be size NT*2, and
%  exactSigma = \nabla u (or exactSigma= K\nabla u), vector array of size
%  NT by 2.
%
%  [err,elemErr] = getL2errorRT0(node,elem,exactSigma,sigmah).
%  err gives the L2 norm between exact flux and approximate one from RT0,
%  elemErr, an NT by 1 array, gives the square of L2 norm between exact
%  flux and approximate one from RT0 on each element.
%
%  [err,elemErr] = getL2errorRT0(node,elem,exactSigma,sigmah,d).
%  If the coefficient 'd' is inlcuded, then the input should be in the way
%  sigmah     = K\nabla u_h  (including K),
%  exactSigma =  \nabla u    (function bundle, don't including K),    or
%  exactSigma = K\nabla u    (vector array, including K).
%  err=||K exactSigma - sigma_h||_{K^(-1)} = |||u-u_h||| is the energy norm.
%  elemErr, an NT by 1 array, gives the square of L2 norm between exact
%  flux and approximate one from RT0 on each element
%
% Example
%
%     maxIt = 5;
%     node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%     elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%     bdEdge = setboundary(node,elem,'Dirichlet');
%     pde = mixBCdata;
%     err = zeros(maxIt,1); N = zeros(maxIt,1);
%     for i =1:maxIt
%         [node,elem,bdEdge] = uniformrefine(node,elem,bdEdge);
%         [u,sigma,NULL] = PoissonRT0(node,elem,pde,bdEdge);
%         err(i) = getL2errorRT0(node,elem,pde.Du,sigma);
%         N(i) = size(u,1);
%     end
%     r1 = showrate(N,err,2);
%     legend('||\sigma - \sigma_h||',['N^{' num2str(r1) '}'],...
%            'LOCATION','Best');
%
% See also getHdiverrorRT0, getL2error3RT0.
%
% Created by Ming Wang at Jan 17, 2011, M-lint modified at May 15, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin >=5 && ~isempty(d)
    if isreal(d)
        K = d;                  % d is an array
    else
    center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
              node(elem(:,3),:))/3;
    K = d(center);              % d is a function
    end
else
    K = [];
end

%% Construct Data Structure
[elem2dof,~,elem2edgeSign] = dofedge(elem);
NT = size(elem,1);% Ndof = max(elem2dof(:)); %N = size(node,1);
[Dlambda,area] = gradbasis(node,elem);
locEdge = [2 3; 3 1; 1 2];

%% Compute square of the L2 error element-wise
[lambda,w] = quadpts(3); % quadrature order is 3
nQuad = size(lambda,1);
err = zeros(NT,1);
rotMat = [0 -1; 1 0]; % rotation matrix for computing rotLambda.
for p = 1:nQuad
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:);
    if ~isnumeric(exactSigma)
        sigmap = exactSigma(pxy);
        if(nargin >=5)&& ~isempty(K) % multiply coeff. if fun bundle flux.
            sigmap = repmat(K,1,2).*sigmap;
        end
    else
        sigmap = exactSigma;         %  K*\nabla u_h.
    end
    sigmahp = zeros(NT,2);
    for k = 1:3 % for each basis
        i = locEdge(k,1); j = locEdge(k,2);
        % phi_k = lambda_iRot_j - lambda_jRot_i;
        sigmahp = sigmahp + repmat(elem2edgeSign(:,k).*sigmah(elem2dof(:,k)),1,2).*...
                   (lambda(p,i)*Dlambda(:,:,j)*rotMat-lambda(p,j)*Dlambda(:,:,i)*rotMat);
    end
    err = err + w(p)*sum((sigmap - sigmahp).^2,2);
end
err = err.*area;               % ||sigma - K\nabla u_h||^2
if(nargin >=5)&& ~isempty(K)   % ||sigma - K\nabla u_h||^2_{K^(-1)}
    err = err./K;
end
elemErr = err;           % ||sigma - K\nabla u_h||^2_{K^(-1)}
% modify the error
err(isnan(err)) = 0;
err = sqrt(sum(err));


function W = constructW(fea,options)
%	Usage:
%	W = constructW(fea,options)
%
%	fea: Rows of vectors of data points. Each row is x_i
%   options: Struct value in Matlab. The fields in options that can be set:
%
%           NeighborMode -  Indicates how to construct the graph. Choices
%                           are: [Default 'KNN']
%                'KNN'            -  k = 0
%                                       Complete graph
%                                    k > 0
%                                      Put an edge between two nodes if and
%                                      only if they are among the k nearst
%                                      neighbors of each other. You are
%                                      required to provide the parameter k in
%                                      the options. Default k=5.
%               'Supervised'      -  k = 0
%                                       Put an edge between two nodes if and
%                                       only if they belong to same class.
%                                    k > 0
%                                       Put an edge between two nodes if
%                                       they belong to same class and they
%                                       are among the k nearst neighbors of
%                                       each other.
%                                    Default: k=0
%                                   You are required to provide the label
%                                   information gnd in the options.
%
%           WeightMode   -  Indicates how to assign weights for each edge
%                           in the graph. Choices are:
%               'Binary'       - 0-1 weighting. Every edge receiveds weight
%                                of 1.
%               'HeatKernel'   - If nodes i and j are connected, put weight
%                                W_ij = exp(-norm(x_i - x_j)/2t^2). You are
%                                required to provide the parameter t. [Default One]
%               'Cosine'       - If nodes i and j are connected, put weight
%                                cosine(x_i,x_j).
%
%            k         -   The parameter needed under 'KNN' NeighborMode.
%                          Default will be 5.
%            gnd       -   The parameter needed under 'Supervised'
%                          NeighborMode.  Colunm vector of the label
%                          information for each data point.
%            bLDA      -   0 or 1. Only effective under 'Supervised'
%                          NeighborMode. If 1, the graph will be constructed
%                          to make LPP exactly same as LDA. Default will be
%                          0.
%            t         -   The parameter needed under 'HeatKernel'
%                          WeightMode. Default will be 1
%         bNormalized  -   0 or 1. Only effective under 'Cosine' WeightMode.
%                          Indicates whether the fea are already be
%                          normalized to 1. Default will be 0
%      bSelfConnected  -   0 or 1. Indicates whether W(i,i) == 1. Default 0
%                          if 'Supervised' NeighborMode & bLDA == 1,
%                          bSelfConnected will always be 1. Default 0.
%            bTrueKNN  -   0 or 1. If 1, will construct a truly kNN graph
%                          (Not symmetric!). Default will be 0. Only valid
%                          for 'KNN' NeighborMode
%
%
%    Examples:
%
%       fea = rand(50,15);
%       options = [];
%       options.NeighborMode = 'KNN';
%       options.k = 5;
%       options.WeightMode = 'HeatKernel';
%       options.t = 1;
%       W = constructW(fea,options);
%
%
%       fea = rand(50,15);
%       gnd = [ones(10,1);ones(15,1)*2;ones(10,1)*3;ones(15,1)*4];
%       options = [];
%       options.NeighborMode = 'Supervised';
%       options.gnd = gnd;
%       options.WeightMode = 'HeatKernel';
%       options.t = 1;
%       W = constructW(fea,options);
%
%
%       fea = rand(50,15);
%       gnd = [ones(10,1);ones(15,1)*2;ones(10,1)*3;ones(15,1)*4];
%       options = [];
%       options.NeighborMode = 'Supervised';
%       options.gnd = gnd;
%       options.bLDA = 1;
%       W = constructW(fea,options);
%
%
%    For more details about the different ways to construct the W, please
%    refer:
%       Deng Cai, Xiaofei He and Jiawei Han, "Document Clustering Using
%       Locality Preserving Indexing" IEEE TKDE, Dec. 2005.
%
%
%    Written by Deng Cai (dengcai2 AT cs.uiuc.edu), April/2004, Feb/2006,
%                                             May/2007
%

bSpeed  = 1;

if (~exist('options','var'))
   options = [];
end

if isfield(options,'Metric')
    warning('This function has been changed and the Metric is no longer be supported');
end

if ~isfield(options,'bNormalized')
    options.bNormalized = 0;
end

%=================================================
if ~isfield(options,'NeighborMode')
    options.NeighborMode = 'KNN';
end

switch lower(options.NeighborMode)
    case {lower('KNN')}  %For simplicity, we include the data point itself in the kNN
        if ~isfield(options,'k')
            options.k = 5;
        end
    case {lower('Supervised')}
        if ~isfield(options,'bLDA')
            options.bLDA = 0;
        end
        if options.bLDA
            options.bSelfConnected = 1;
        end
        if ~isfield(options,'k')
            options.k = 0;
        end
        if ~isfield(options,'gnd')
            error('Label(gnd) should be provided under ''Supervised'' NeighborMode!');
        end
        if ~isempty(fea) && length(options.gnd) ~= size(fea,1)
            error('gnd doesn''t match with fea!');
        end
    otherwise
        error('NeighborMode does not exist!');
end

%=================================================

if ~isfield(options,'WeightMode')
    options.WeightMode = 'HeatKernel';
end

bBinary = 0;
bCosine = 0;
switch lower(options.WeightMode)
    case {lower('Binary')}
        bBinary = 1;
    case {lower('HeatKernel')}
        if ~isfield(options,'t')
            nSmp = size(fea,1);
            if nSmp > 3000
                D = EuDist2(fea(randsample(nSmp,3000),:));
            else
                D = EuDist2(fea);
            end
            options.t = mean(mean(D));
        end
    case {lower('Cosine')}
        bCosine = 1;
    otherwise
        error('WeightMode does not exist!');
end

%=================================================

if ~isfield(options,'bSelfConnected')
    options.bSelfConnected = 0;
end

%=================================================

if isfield(options,'gnd')
    nSmp = length(options.gnd);
else
    nSmp = size(fea,1);
end
maxM = 62500000; %500M
BlockSize = floor(maxM/(nSmp*3));

if strcmpi(options.NeighborMode,'Supervised')
    Label = unique(options.gnd);
    nLabel = length(Label);
    if options.bLDA
        G = zeros(nSmp,nSmp);
        for idx=1:nLabel
            classIdx = options.gnd==Label(idx);
            G(classIdx,classIdx) = 1/sum(classIdx);
        end
        W = sparse(G);
        return;
    end

    switch lower(options.WeightMode)
        case {lower('Binary')}
            if options.k > 0
                G = zeros(nSmp*(options.k+1),3);
                idNow = 0;
                for i=1:nLabel
                    classIdx = find(options.gnd==Label(i));
                    D = EuDist2(fea(classIdx,:),[],0);
                    [dump idx] = sort(D,2); % sort each row
                    clear D dump;
                    idx = idx(:,1:options.k+1);

                    nSmpClass = length(classIdx)*(options.k+1);
                    G(idNow+1:nSmpClass+idNow,1) = repmat(classIdx,[options.k+1,1]);
                    G(idNow+1:nSmpClass+idNow,2) = classIdx(idx(:));
                    G(idNow+1:nSmpClass+idNow,3) = 1;
                    idNow = idNow+nSmpClass;
                    clear idx
                end
                G = sparse(G(:,1),G(:,2),G(:,3),nSmp,nSmp);
                G = max(G,G');
            else
                G = zeros(nSmp,nSmp);
                for i=1:nLabel
                    classIdx = find(options.gnd==Label(i));
                    G(classIdx,classIdx) = 1;
                end
            end

            if ~options.bSelfConnected
                for i=1:size(G,1)
                    G(i,i) = 0;
                end
            end

            W = sparse(G);
        case {lower('HeatKernel')}
            if options.k > 0
                G = zeros(nSmp*(options.k+1),3);
                idNow = 0;
                for i=1:nLabel
                    classIdx = find(options.gnd==Label(i));
                    D = EuDist2(fea(classIdx,:),[],0);
                    [dump idx] = sort(D,2); % sort each row
                    clear D;
                    idx = idx(:,1:options.k+1);
                    dump = dump(:,1:options.k+1);
                    dump = exp(-dump/(2*options.t^2));

                    nSmpClass = length(classIdx)*(options.k+1);
                    G(idNow+1:nSmpClass+idNow,1) = repmat(classIdx,[options.k+1,1]);
                    G(idNow+1:nSmpClass+idNow,2) = classIdx(idx(:));
                    G(idNow+1:nSmpClass+idNow,3) = dump(:);
                    idNow = idNow+nSmpClass;
                    clear dump idx
                end
                G = sparse(G(:,1),G(:,2),G(:,3),nSmp,nSmp);
            else
                G = zeros(nSmp,nSmp);
                for i=1:nLabel
                    classIdx = find(options.gnd==Label(i));
                    D = EuDist2(fea(classIdx,:),[],0);
                    D = exp(-D/(2*options.t^2));
                    G(classIdx,classIdx) = D;
                end
            end

            if ~options.bSelfConnected
                for i=1:size(G,1)
                    G(i,i) = 0;
                end
            end

            W = sparse(max(G,G'));
        case {lower('Cosine')}
            if ~options.bNormalized
                fea = NormalizeFea(fea);
            end

            if options.k > 0
                G = zeros(nSmp*(options.k+1),3);
                idNow = 0;
                for i=1:nLabel
                    classIdx = find(options.gnd==Label(i));
                    D = fea(classIdx,:)*fea(classIdx,:)';
                    [dump idx] = sort(-D,2); % sort each row
                    clear D;
                    idx = idx(:,1:options.k+1);
                    dump = -dump(:,1:options.k+1);

                    nSmpClass = length(classIdx)*(options.k+1);
                    G(idNow+1:nSmpClass+idNow,1) = repmat(classIdx,[options.k+1,1]);
                    G(idNow+1:nSmpClass+idNow,2) = classIdx(idx(:));
                    G(idNow+1:nSmpClass+idNow,3) = dump(:);
                    idNow = idNow+nSmpClass;
                    clear dump idx
                end
                G = sparse(G(:,1),G(:,2),G(:,3),nSmp,nSmp);
            else
                G = zeros(nSmp,nSmp);
                for i=1:nLabel
                    classIdx = find(options.gnd==Label(i));
                    G(classIdx,classIdx) = fea(classIdx,:)*fea(classIdx,:)';
                end
            end

            if ~options.bSelfConnected
                for i=1:size(G,1)
                    G(i,i) = 0;
                end
            end

            W = sparse(max(G,G'));
        otherwise
            error('WeightMode does not exist!');
    end
    return;
end

if bCosine && ~options.bNormalized
    Normfea = NormalizeFea(fea);
end

if strcmpi(options.NeighborMode,'KNN') && (options.k > 0)
    if ~(bCosine && options.bNormalized)
        G = zeros(nSmp*(options.k+1),3);
        for i = 1:ceil(nSmp/BlockSize)
            if i == ceil(nSmp/BlockSize)
                smpIdx = (i-1)*BlockSize+1:nSmp;
                dist = EuDist2(fea(smpIdx,:),fea,0);

                if bSpeed
                    nSmpNow = length(smpIdx);
                    dump = zeros(nSmpNow,options.k+1);
                    idx = dump;
                    for j = 1:options.k+1
                        [dump(:,j),idx(:,j)] = min(dist,[],2);
                        temp = (idx(:,j)-1)*nSmpNow+[1:nSmpNow]';
                        dist(temp) = 1e100;
                    end
                else
                    [dump idx] = sort(dist,2); % sort each row
                    idx = idx(:,1:options.k+1);
                    dump = dump(:,1:options.k+1);
                end

                if ~bBinary
                    if bCosine
                        dist = Normfea(smpIdx,:)*Normfea';
                        dist = full(dist);
                        linidx = [1:size(idx,1)]';
                        dump = dist(sub2ind(size(dist),linidx(:,ones(1,size(idx,2))),idx));
                    else
                        dump = exp(-dump/(2*options.t^2));
                    end
                end

                G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),1) = repmat(smpIdx',[options.k+1,1]);
                G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),2) = idx(:);
                if ~bBinary
                    G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),3) = dump(:);
                else
                    G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),3) = 1;
                end
            else
                smpIdx = (i-1)*BlockSize+1:i*BlockSize;

                dist = EuDist2(fea(smpIdx,:),fea,0);

                if bSpeed
                    nSmpNow = length(smpIdx);
                    dump = zeros(nSmpNow,options.k+1);
                    idx = dump;
                    for j = 1:options.k+1
                        [dump(:,j),idx(:,j)] = min(dist,[],2);
                        temp = (idx(:,j)-1)*nSmpNow+[1:nSmpNow]';
                        dist(temp) = 1e100;
                    end
                else
                    [dump idx] = sort(dist,2); % sort each row
                    idx = idx(:,1:options.k+1);
                    dump = dump(:,1:options.k+1);
                end

                if ~bBinary
                    if bCosine
                        dist = Normfea(smpIdx,:)*Normfea';
                        dist = full(dist);
                        linidx = [1:size(idx,1)]';
                        dump = dist(sub2ind(size(dist),linidx(:,ones(1,size(idx,2))),idx));
                    else
                        dump = exp(-dump/(2*options.t^2));
                    end
                end

                G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),1) = repmat(smpIdx',[options.k+1,1]);
                G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),2) = idx(:);
                if ~bBinary
                    G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),3) = dump(:);
                else
                    G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),3) = 1;
                end
            end
        end

        W = sparse(G(:,1),G(:,2),G(:,3),nSmp,nSmp);
    else
        G = zeros(nSmp*(options.k+1),3);
        for i = 1:ceil(nSmp/BlockSize)
            if i == ceil(nSmp/BlockSize)
                smpIdx = (i-1)*BlockSize+1:nSmp;
                dist = fea(smpIdx,:)*fea';
                dist = full(dist);

                if bSpeed
                    nSmpNow = length(smpIdx);
                    dump = zeros(nSmpNow,options.k+1);
                    idx = dump;
                    for j = 1:options.k+1
                        [dump(:,j),idx(:,j)] = max(dist,[],2);
                        temp = (idx(:,j)-1)*nSmpNow+[1:nSmpNow]';
                        dist(temp) = 0;
                    end
                else
                    [dump idx] = sort(-dist,2); % sort each row
                    idx = idx(:,1:options.k+1);
                    dump = -dump(:,1:options.k+1);
                end

                G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),1) = repmat(smpIdx',[options.k+1,1]);
                G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),2) = idx(:);
                G((i-1)*BlockSize*(options.k+1)+1:nSmp*(options.k+1),3) = dump(:);
            else
                smpIdx = (i-1)*BlockSize+1:i*BlockSize;
                dist = fea(smpIdx,:)*fea';
                dist = full(dist);

                if bSpeed
                    nSmpNow = length(smpIdx);
                    dump = zeros(nSmpNow,options.k+1);
                    idx = dump;
                    for j = 1:options.k+1
                        [dump(:,j),idx(:,j)] = max(dist,[],2);
                        temp = (idx(:,j)-1)*nSmpNow+[1:nSmpNow]';
                        dist(temp) = 0;
                    end
                else
                    [dump idx] = sort(-dist,2); % sort each row
                    idx = idx(:,1:options.k+1);
                    dump = -dump(:,1:options.k+1);
                end

                G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),1) = repmat(smpIdx',[options.k+1,1]);
                G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),2) = idx(:);
                G((i-1)*BlockSize*(options.k+1)+1:i*BlockSize*(options.k+1),3) = dump(:);
            end
        end

        W = sparse(G(:,1),G(:,2),G(:,3),nSmp,nSmp);
    end

    if bBinary
        W(logical(W)) = 1;
    end

    if isfield(options,'bSemiSupervised') && options.bSemiSupervised
        tmpgnd = options.gnd(options.semiSplit);

        Label = unique(tmpgnd);
        nLabel = length(Label);
        G = zeros(sum(options.semiSplit),sum(options.semiSplit));
        for idx=1:nLabel
            classIdx = tmpgnd==Label(idx);
            G(classIdx,classIdx) = 1;
        end
        Wsup = sparse(G);
        if ~isfield(options,'SameCategoryWeight')
            options.SameCategoryWeight = 1;
        end
        W(options.semiSplit,options.semiSplit) = (Wsup>0)*options.SameCategoryWeight;
    end

    if ~options.bSelfConnected
        W = W - diag(diag(W));
    end

    if isfield(options,'bTrueKNN') && options.bTrueKNN

    else
        W = max(W,W');
    end

    return;
end

% strcmpi(options.NeighborMode,'KNN') & (options.k == 0)
% Complete Graph

switch lower(options.WeightMode)
    case {lower('Binary')}
        error('Binary weight can not be used for complete graph!');
    case {lower('HeatKernel')}
        W = EuDist2(fea,[],0);
        W = exp(-W/(2*options.t^2));
    case {lower('Cosine')}
        W = full(Normfea*Normfea');
    otherwise
        error('WeightMode does not exist!');
end

if ~options.bSelfConnected
    for i=1:size(W,1)
        W(i,i) = 0;
    end
end

W = max(W,W');


function bool = inside_contour(pos, contour)

npos = size(pos,1);
ncnt = size(contour,1);
x = pos(:,1);
y = pos(:,2);

minx = min(x);
miny = min(y);
maxx = max(x);
maxy = max(y);

bool = true(npos,1);
bool(x<minx) = false;
bool(y<miny) = false;
bool(x>maxx) = false;
bool(y>maxy) = false;

% the summed angle over the contour is zero if the point is outside, and 2*pi if the point is inside the contour
% leave some room for inaccurate f
critval = 0.1;

% the remaining points have to be investigated with more attention
sel = find(bool);
for i=1:length(sel)
  contourx = contour(:,1) - pos(sel(i),1);
  contoury = contour(:,2) - pos(sel(i),2);
  angle = atan2(contoury, contourx);
  % angle = unwrap(angle);
  angle = my_unwrap(angle);
  total = sum(diff(angle));
  bool(sel(i)) = (abs(total)>critval);
end

function x = my_unwrap(x)
% this is a faster implementation of the MATLAB unwrap function
% with hopefully the same functionality
d    = diff(x);
indx = find(abs(d)>pi);
for i=indx(:)'
  if d(i)>0
    x((i+1):end) = x((i+1):end) - 2*pi;
  else
    x((i+1):end) = x((i+1):end) + 2*pi;
  end
end


function [ ux, uy, uz ] = HS3D( image1, image2, alpha, iterations, ...
    uxInitial, uyInitial, uzInitial)
%This function estimates deformations between two subsequent 3-D images
%using Horn-Schunck optical flow method.
%
%   Description :
%
%   -image1, image2 :   two subsequent images or frames
%   -alpha :    smoothness parameter, default value is 1.
%   -iterations :   number of iterations, default value is 10.
%   -uxInitial, uyInitial, uzInitial : initial flow vectors, default value
%                                     is 0;
%
%   Reference :
%   B. K. Horn and B. G. Schunck, Determining optical ow, Cambridge, MA,
%   USA, Tech. Rep., 1980.
%
%   Author : Mohammad Mustafa
%   By courtesy of The University of Nottingham and Mirada Medical Limited,
%   Oxford, UK
%
%   Published under a Creative Commons Attribution-Non-Commercial-Share Alike
%   3.0 Unported Licence http://creativecommons.org/licenses/by-nc-sa/3.0/
%
%   June 2012

ux=zeros(size(image1)); uy=ux; uz=ux;

if nargin==2
    alpha=1; iterations=10;
    ux=zeros(size(image1)); uy=ux; uz=ux;
elseif nargin==3
    iterations=10;
    ux=zeros(size(image1)); uy=ux; uz=ux;
elseif nargin==6
    ux=uxInitial; uy=uyInitial; uz=uzInitial;
end

[Ix,Iy,Iz,It]=imageDerivatives3D(image1,image2);

laplacian = zeros(3,3,3);
laplacian(:,:,1) = [0 3 0;3 10 3;0 3 0];
laplacian(:,:,3) = laplacian(:,:,1);
laplacian(:,:,2) = [3 10 3;10 -96 10;3 10 3];
laplacian=laplacian./96;

for i=1:iterations
    uxAvg=convn(ux,laplacian,'same');
    uyAvg=convn(uy,laplacian,'same');
    uzAvg=convn(uy,laplacian,'same');
    ux=uxAvg - ( Ix.*( (Ix.*uxAvg) + (Iy.*uxAvg) + (Iz.*uzAvg) + It))...
        ./ ( alpha.^2 + Ix.^2 + Iy.^ 2 + Iz.^ 2);
    uy=uyAvg - ( Iy.*( (Ix.*uxAvg) + (Iy.*uxAvg) + (Iz.*uzAvg) + It))...
        ./ ( alpha.^2 + Ix.^2 + Iy.^ 2 + Iz.^ 2);
    uz=uzAvg - ( Iz.*( (Ix.*uxAvg) + (Iy.*uxAvg) + (Iz.*uzAvg) + It))...
        ./ ( alpha.^2 + Ix.^2 + Iy.^ 2 + Iz.^ 2);
end

ux(isnan(ux))=0;
uy(isnan(uy))=0;
uz(isnan(uz))=0;
end


function erfx = r8_erf ( x )

%*****************************************************************************80
%
%% R8_ERF evaluates the error function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2007
%
%  Author:
%
%    W J Cody,
%    Mathematics and Computer Science Division,
%    Argonne National Laboratory,
%    Argonne, Illinois, 60439.
%
%  Reference:
%
%    W J Cody,
%    "Rational Chebyshev approximations for the error function",
%    Mathematics of Computation,
%    1969, pages 631-638.
%
%  Parameters:
%
%    Input, real X, the argument of the error function.
%
%    Output, real ERFX, the value of the error function.
%
  a = [ ...
    3.16112374387056560E+00, ...
    1.13864154151050156E+02, ...
    3.77485237685302021E+02, ...
    3.20937758913846947E+03, ...
    1.85777706184603153E-01 ];
  b = [ ...
    2.36012909523441209E+01, ...
    2.44024637934444173E+02, ...
    1.28261652607737228E+03, ...
    2.84423683343917062E+03 ];
  c = [ ...
    5.64188496988670089E-01, ...
    8.88314979438837594E+00, ...
    6.61191906371416295E+01, ...
    2.98635138197400131E+02, ...
    8.81952221241769090E+02, ...
    1.71204761263407058E+03, ...
    2.05107837782607147E+03, ...
    1.23033935479799725E+03, ...
    2.15311535474403846E-08 ];
  d = [ ...
    1.57449261107098347E+01, ...
    1.17693950891312499E+02, ...
    5.37181101862009858E+02, ...
    1.62138957456669019E+03, ...
    3.29079923573345963E+03, ...
    4.36261909014324716E+03, ...
    3.43936767414372164E+03, ...
    1.23033935480374942E+03 ];
  p = [ ...
    3.05326634961232344E-01, ...
    3.60344899949804439E-01, ...
    1.25781726111229246E-01, ...
    1.60837851487422766E-02, ...
    6.58749161529837803E-04, ...
    1.63153871373020978E-02 ];
  q = [ ...
    2.56852019228982242E+00, ...
    1.87295284992346047E+00, ...
    5.27905102951428412E-01, ...
    6.05183413124413191E-02, ...
    2.33520497626869185E-03 ];
  sqrpi = 0.56418958354775628695E+00;
  thresh = 0.46875E+00;
  xbig = 26.543E+00;
  xsmall = 1.11E-16;
%
  xabs = abs ( x );
%
%  Evaluate ERF(X) for |X| <= 0.46875.
%
  if ( xabs <= thresh )

    if ( xsmall < xabs )
      xsq = xabs * xabs;
    else
      xsq = 0.0;
    end

    xnum = a(5) * xsq;
    xden = xsq;
    for i = 1 : 3
      xnum = ( xnum + a(i) ) * xsq;
      xden = ( xden + b(i) ) * xsq;
    end

    erfx = x * ( xnum + a(4) ) / ( xden + b(4) );
%
%  Evaluate ERFC(X) for 0.46875 <= |X| <= 4.0.
%
  elseif ( xabs <= 4.0 )

    xnum = c(9) * xabs;
    xden = xabs;
    for i = 1 : 7
      xnum = ( xnum + c(i) ) * xabs;
      xden = ( xden + d(i) ) * xabs;
    end

    erfx = ( xnum + c(8) ) / ( xden + d(8) );
    xsq = floor ( xabs * 16.0 ) / 16.0;
    del = ( xabs - xsq ) * ( xabs + xsq );
    erfx = exp ( - xsq * xsq ) * exp ( - del ) * erfx;

    erfx = ( 0.5 - erfx ) + 0.5;

    if ( x < 0.0 )
      erfx = -erfx;
    end
%
%  Evaluate ERFC(X) for 4.0 < |X|.
%
  else

    if ( xbig <= xabs )

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

    else

      xsq = 1.0 / ( xabs * xabs );

      xnum = p(6) * xsq;
      xden = xsq;
      for i = 1 : 4
        xnum = ( xnum + p(i) ) * xsq;
        xden = ( xden + q(i) ) * xsq;
      end

      erfx = xsq * ( xnum + p(5) ) / ( xden + q(5) );
      erfx = ( sqrpi -  erfx ) / xabs;
      xsq = floor ( xabs * 16.0 ) / 16.0;
      del = ( xabs - xsq ) * ( xabs + xsq );
      erfx = exp ( - xsq * xsq ) * exp ( - del ) * erfx;

      erfx = ( 0.5 - erfx ) + 0.5;
      if ( x < 0.0 )
        erfx = -erfx;
      end

    end

  end

  return
end


% This file is part of libDAI - http://www.libdai.org/
%
% Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
%
% Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.

% This example program illustrates how to construct a factorgraph
% by means of the sprinkler network example discussed at
% http://www.cs.ubc.ca/~murphyk/Bayes/bnintro.html
% using the SWIG octave wrapper of libDAI

dai

C = dai.Var(0, 2);  % Define binary variable Cloudy (with label 0)
S = dai.Var(1, 2);  % Define binary variable Sprinkler (with label 1)
R = dai.Var(2, 2);  % Define binary variable Rain (with label 2)
W = dai.Var(3, 2);  % Define binary variable Wetgrass (with label 3)

% Define probability distribution for C
P_C = dai.Factor(C);
P_C(0) = 0.5;            % C = 0
P_C(1) = 0.5;            % C = 1

% Define conditional probability of S given C
P_S_given_C = dai.Factor(dai.VarSet(S,C));
P_S_given_C(0) = 0.5;    % C = 0, S = 0
P_S_given_C(1) = 0.9;    % C = 1, S = 0
P_S_given_C(2) = 0.5;    % C = 0, S = 1
P_S_given_C(3) = 0.1;    % C = 1, S = 1

% Define conditional probability of R given C
P_R_given_C = dai.Factor(dai.VarSet(R,C));
P_R_given_C(0) = 0.8;    % C = 0, R = 0
P_R_given_C(1) = 0.2;    % C = 1, R = 0
P_R_given_C(2) = 0.2;    % C = 0, R = 1
P_R_given_C(3) = 0.8;    % C = 1, R = 1

% Define conditional probability of W given S and R
SRW = dai.VarSet(S,R);
SRW.append(W);
P_W_given_S_R = dai.Factor(SRW);
P_W_given_S_R(0) = 1.0;  % S = 0, R = 0, W = 0
P_W_given_S_R(1) = 0.1;  % S = 1, R = 0, W = 0
P_W_given_S_R(2) = 0.1;  % S = 0, R = 1, W = 0
P_W_given_S_R(3) = 0.01; % S = 1, R = 1, W = 0
P_W_given_S_R(4) = 0.0;  % S = 0, R = 0, W = 1
P_W_given_S_R(5) = 0.9;  % S = 1, R = 0, W = 1
P_W_given_S_R(6) = 0.9;  % S = 0, R = 1, W = 1
P_W_given_S_R(7) = 0.99; % S = 1, R = 1, W = 1

% Build factor graph consisting of those four factors
SprinklerFactors = dai.VecFactor();
SprinklerFactors.append(P_C);
SprinklerFactors.append(P_R_given_C);
SprinklerFactors.append(P_S_given_C);
SprinklerFactors.append(P_W_given_S_R);
SprinklerNetwork = dai.FactorGraph(SprinklerFactors);

% Write factorgraph to a file
SprinklerNetwork.WriteToFile('sprinkler.fg');
fprintf('Sprinkler network written to sprinkler.fg\n');

% Output some information about the factorgraph
fprintf('%d variables\n', SprinklerNetwork.nrVars());
fprintf('%d factors\n', SprinklerNetwork.nrFactors());

% Calculate joint probability of all four variables
P = dai.Factor();
for I = 0:(SprinklerNetwork.nrFactors()-1)
    P *= SprinklerNetwork.factor(I);
end
P.normalize(); % Not necessary: a Bayesian network is already normalized by definition

% Calculate some probabilities
denom = P.marginal(dai.VarSet(W))(1);
fprintf('P(W=1) = %f\n', denom);
fprintf('P(S=1 | W=1) = %f\n', P.marginal(dai.VarSet(S,W))(3) / denom);
fprintf('P(R=1 | W=1) = %f\n', P.marginal(dai.VarSet(R,W))(3) / denom);


clear all
close all

disp('Random Effects demo with');
disp('two-region Neural Mass Models');
disp(' ');
disp('Initial conditions are known');
disp('Flow parameters are treated as random effects');

% Backward connection ?
back=1;

% Observation noise SD
sd=0.01;

% Number of subjects
N=3;

% Number of parameters
Np=2;

m=[1,1]';
C=0.1^2*eye(2);
w_true = spm_normrnd(m,C,N);
for n=1:N,
    [M{n},U{n}] = mci_nmm_struct(back,sd,Np);
    P=w_true(:,n);
    Y{n}.y = mci_nmm_gen(M{n},U{n},P);
end

% Assign init, flow and output parameters
assign.init_par='known';
assign.flow_par='random';
assign.out_par='known';
MCI.pout0=[]; % there are no 'output' parameters
MCI.assign=assign;

MCI.M=M; MCI.U=U; MCI.Y=Y;
MCI.verbose=1;
MCI.total_its=16;
MCI.rinit=0.25;

S.prior.P=Np;
S.prior.a=Np/2;
S.prior.B=M{1}.pC;
S.prior.beta=1;
S.prior.m=zeros(Np,1);
S.N=N;
MCI.S=S;

tic;
MCI = spm_mci_mfx_dynamic (MCI);
toc

disp('Mean true params:');
mwt=mean(w_true,2)

disp('MCI second level posterior:');
MCI.sm_mean

Np=M{1}.Npflow;
for j=1:Np,
    h=figure;
    set(h,'Name',sprintf('w(%d)',j));

    min_w=min(w_true(j,:));
    max_w=max(w_true(j,:));

    plot(w_true(j,:),MCI.sw_mean(j,:),'kx','MarkerSize',10);
    hold on
    plot([min_w max_w],[min_w max_w],'k-','LineWidth',2);
    set(gca,'FontSize',18);
    xlabel('True');
    ylabel('Estimated');
    grid on
end

e=MCI.sw_mean-w_true;
sse2=trace(e'*e);
disp(sprintf('Final first level SSE=%1.2f',sse2));

figure;
plot(MCI.sm');
grid on
xlabel('RFX iteration');
ylabel('Population Level Parameters');

h=figure;
set(h,'Name',sprintf('Subject Level'));
for j=1:Np,
    subplot(Np,1,j);
    plot(squeeze(MCI.sw(j,:,:))');
    grid on
    xlabel('RFX iteration');
    ylabel(sprintf('w(%d)',j));
end


function [CQ] = calculate_CQ(Seconds, Throttle, RPM, L, gramsMeas, varargin)
% This function calculates the parameter for a zero intercept linear fit
% between RPM^2 and torque of the form:
%   Torque = (RPM^2)*CQ
% Output is given in the form of returned CQ value and plots.
%
% Argument definitions:
%   L = length of torque rig arm from center of motor to scale contact
%   point (MUST BE IN INCHES)
%   gramsMeas = user measured values direct from scale, MUST BE IN GRAMS!
%   These values must correspond to ALL unique throttle values, even those
%   outside of "LowCutoff" and "HighCutoff."
% (Note: values of gramsMeas corresponding to throttle setting outside
% "LowCutoff" and "HighCutoff" are not used in calculation, therefore if
% measurements are only available for the range of "LowCutoff" to
% "HighCutoff", the vector can be padded with zeros in the appropriate
% locations.
%
% See also calculate_CR_B, calculate_CT.

% Assign variable length input arguments
if ((nargin-5)==0) % if no input arguments are given for cutoff range:
LowCutoff = min(Throttle); % Use full range of input throttle data
HighCutoff = max(Throttle);
else
LowCutoff = varargin{1}; % Ignore data below this throttle value
HighCutoff = varargin{2}; % Ignore data above this throttle value
end

% Find unique Throttle Setting values (Not counting 0)
ThrottleU = unique(Throttle(Throttle~=0));

% Check length of inputs match
lengthGM = length(gramsMeas);
lengthTU = length(ThrottleU);
if lengthGM ~= lengthTU
    msgbox({['Length of measured grams values vector (' num2str(lengthGM) ')'];
        [' and number of unique Throttle values (' num2str(lengthTU) ')']; ...
       ' do not match! Check input data!'},'Argument Error','warn');
    return
end

% Find unique/filtered Throttle Setting values between low/high cutoff limits
ThrottleUF = ThrottleU(ThrottleU>=LowCutoff & ThrottleU<=HighCutoff);

% Find coresponding grams measurments
gramsMeasU = gramsMeas(ThrottleU>=LowCutoff & ThrottleU<=HighCutoff); % Unique scale values

% Find an average RPM value for each unique/filtered Throttle Setting
RPMAverages100 = zeros(length(ThrottleUF),1); % preallocate
for i = 1:length(ThrottleUF) % For all of the unique and filtered throttle settings:
    RPMUF = RPM(Throttle==ThrottleUF(i));  % get RPM values cooresponding to unique/filtered Throttle value
    if length(RPMUF)>=120 % check to make sure there are enough data points for filtering
        RPMfilt100 = RPMUF(100:end); % RPMUF minus the first 100 data points (to remove transient RPM fluctuation as steady state value is reached)
    else RPMfilt100 = RPMUF((fix(length(RPMUF)/2)):end); % If there's a limited number of points, just take the second half of the values as steady state
    end
    RPMAverages100(i,1) = mean(RPMfilt100); % Take the average RPM value from these filtered data points
end
RPMsq = RPMAverages100.^2;

g = 9.81; % Gravity (m/s^2)
newtonMetersTorque = g*L*gramsMeasU/(1000*39.3701); % Torque in N*m (Conversion: (g-->kg and in-->m))
CQ = RPMsq\newtonMetersTorque; % This is a technique for forcing a zero intercept using matrix division techniques
CQp = [CQ;0];

% Plot the linear relationship and display calculated CQ value
figure
plot(RPMsq, newtonMetersTorque, '*', RPMsq, polyval(CQp, RPMsq))
xlabel('RPM^2 (rev^2/min^2)','FontSize',12)
ylabel('Torque (N*m)','FontSize',12)
title({['Linear Fit of Torque vs. RPM^2'];...
    [' ']; ['CQ: ' num2str(CQ) ' (N*m/RPM^2)']},'FontSize',14)
xlims = get(gca,'xlim');
ylims = get(gca,'ylim');
grid on

% Plot Residuals for assesing general quality of fit
predictedTorque = polyval(CQp, RPMsq); % predict thrust for each throttle setting of interest
residuals = newtonMetersTorque - predictedTorque; % find absolute error between prediction and actual
figure
plot(predictedTorque,residuals,'r*',[min(predictedTorque) max(predictedTorque)],[0 0])
title('CQ Residuals Plot','FontSize',14)
xlabel('Predicted Thrust (N)','FontSize',12)
ylabel('Prediction Error (N)','FontSize',12)
grid on
axis tight

github-repos/MATLAB/dch33-Quad-Sim/Quad-Sim-961bc69d4939c8d0661eeb9820de668994262f65/Quadcopter Dynamic Modeling and Simulation/Data Acquisition & Analysis/MATLAB Data Analysis/calculate_CQ.m


function [ n_data, x, fx ] = arcsin_values ( n_data )

%*****************************************************************************80
%
%% ARCSIN_VALUES returns some values of the arc sine function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      ArcSin[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  fx_vec = [ ...
    -0.10016742116155979635, ...
     0.00000000000000000000, ...
     0.10016742116155979635, ...
     0.20135792079033079146, ...
     0.30469265401539750797, ...
     0.41151684606748801938, ...
     0.52359877559829887308, ...
     0.64350110879328438680, ...
     0.77539749661075306374, ...
     0.92729521800161223243, ...
     1.1197695149986341867, ...
     1.5707963267948966192 ];

  x_vec = [ ...
    -0.1, ...
     0.0, ...
     0.1, ...
     0.2, ...
     0.3, ...
     0.4, ...
     0.5, ...
     0.6, ...
     0.7, ...
     0.8, ...
     0.9, ...
     1.0 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function a_sorted = r8vec_sort_heap_a ( n, a )

%*****************************************************************************80
%
%% R8VEC_SORT_HEAP_A ascending sorts an R8VEC using heap sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 April 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array to be sorted;
%
%    Output, real A_SORTED(N), the sorted array.
%
  if ( n < 1 )
    a_sorted = [];
    return
  end

  if ( n == 1 )
    a_sorted(1) = a(1);
    return
  end
%
%  1: Put A into descending heap form.
%
  a_sorted = r8vec_heap_d ( n, a );
%
%  2: Sort A.
%
%  The largest object in the heap is in A(1).
%  Move it to position A(N).
%
  temp = a_sorted(1);
  a_sorted(1) = a_sorted(n);
  a_sorted(n) = temp;
%
%  Consider the diminished heap of size N1.
%
  for n1 = n-1 : -1 : 2
%
%  Restore the heap structure of A(1) through A(N1).
%
    a_sorted(1:n1) = r8vec_heap_d ( n1, a_sorted );
%
%  Take the largest object from A(1) and move it to A(N1).
%
    temp = a_sorted(1);
    a_sorted(1) = a_sorted(n1);
    a_sorted(n1) = temp;

  end

  return
end


function [ cluster, cluster_center, cluster_population, cluster_energy, ...
  it_num ] = kmeans_02 ( dim_num, point_num, cluster_num, it_max, point, ...
  cluster_center )

%*****************************************************************************80
%
%% KMEANS_02 applies the K-Means algorithm.
%
%  Discussion:
%
%    The routine attempts to divide POINT_NUM points in
%    DIM_NUM-dimensional space into CLUSTER_NUM clusters so that the within
%    cluster sum of squares is minimized.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2009
%
%  Author:
%
%    Original FORTRAN77 by John Hartigan, Manchek Wong.
%    FORTRAN90 version by John Burkardt.
%
%  Reference:
%
%    John Hartigan, Manchek Wong,
%    Algorithm AS 136:
%    A K-Means Clustering Algorithm,
%    Applied Statistics,
%    Volume 28, Number 1, 1979, pages 100-108.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of spatial dimensions.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer CLUSTER_NUM, the number of clusters.
%
%    Input, integer IT_MAX, the maximum number of iterations.
%
%    Input, real POINT(DIM_NUM,POINT_NUM), the coordinates
%    of the points.
%
%    Input, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the cluster centers.
%
%    Output, integer CLUSTER(POINT_NUM), the cluster each
%    point belongs to.
%
%    Output, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the cluster centers.
%
%    Output, integer CLUSTER_POPULATION(CLUSTER_NUM), the number
%    of points in each cluster.
%
%    Output, real CLUSTER_ENERGY(CLUSTER_NUM), the
%    within-cluster sum of squares.
%
%    Output, integer IT_NUM, the number of iterations taken.
%
  it_num = 0;

  if ( cluster_num < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KMEANS_02 - Fatal error!\n' );
    fprintf ( 1, '  CLUSTER_NUM < 1.\n' );
    error ( 'KMEANS_02 - Fatal error!' )
  end

  if ( point_num <= cluster_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KMEANS_02 - Fatal error!\n' );
    fprintf ( 1, '  POINT_NUM <= CLUSTER_NUM.\n' );
    error ( 'KMEANS_02 - Fatal error!' )
  end

  if ( dim_num < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KMEANS_02 - Fatal error!\n' );
    fprintf ( 1, '  DIM_NUM < 1.\n' );
    error ( 'KMEANS_02 - Fatal error!' )
  end

  if ( point_num < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KMEANS_02 - Fatal error!\n' );
    fprintf ( 1, '  POINT_NUM < 1.\n' );
    error ( 'KMEANS_02 - Fatal error!' )
  end
%
%  For each point I, find its two closest centers, CLUSTER(I) and CLUSTER2(I).
%  Assign it to CLUSTER(I).
%
  for i = 1 : point_num

    cluster(i) = 1;
    cluster2(i) = 2;

    for il = 1 : 2
      dt(il) = sum ( ...
        ( point(1:dim_num,i) - cluster_center(1:dim_num,il) ).^2 );
    end

    if ( dt(2) < dt(1) )
      cluster(i) = 2;
      cluster2(i) = 1;
      temp = dt(1);
      dt(1) = dt(2);
      dt(2) = temp;
    end

    for l = 3 : cluster_num

      db = sum ( ( point(1:dim_num,i) - cluster_center(1:dim_num,l) ).^2 );

      if ( db < dt(1) )
        dt(2) = dt(1);
        cluster2(i) = cluster(i);
        dt(1) = db;
        cluster(i) = l;
      elseif ( db < dt(2) )
        dt(2) = db;
        cluster2(i) = l;
      end

    end

  end
%
%  Update cluster centers to be the average of points contained within them.
%
  cluster_population(1:cluster_num) = 0;
  cluster_center(1:dim_num,1:cluster_num) = 0.0;

  for i = 1 : point_num
    l = cluster(i);
    cluster_population(l) = cluster_population(l) + 1;
    cluster_center(1:dim_num,l) = cluster_center(1:dim_num,l) ...
      + point(1:dim_num,i);
  end
%
%  Check to see if there is any empty cluster.
%
  for l = 1 : cluster_num

    if ( cluster_population(l) == 0 )
      j = 1 + floor ( point_num * rand ( ) );
      cluster_center(1:dim_num) = point(1:dim_num,j)
    else
      cluster_center(1:dim_num,l) = cluster_center(1:dim_num,l) ...
        / cluster_population(l);
    end
%
%  Initialize AN1, AN2, ITRAN and NCP
%  AN1(L) = CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) - 1)
%  AN2(L) = CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) + 1)
%  ITRAN(L) = 1 if cluster L is updated in the quick-transfer stage,
%           = 0 otherwise
%  In the optimal-transfer stage, NCP(L) stores the step at which
%  cluster L is last updated.
%  In the quick-transfer stage, NCP(L) stores the step at which
%  cluster L is last updated plus POINT_NUM.
%
    an2(l) = cluster_population(l) / ( cluster_population(l) + 1 );

    if ( 1 < cluster_population(l) )
      an1(l) = cluster_population(l) / ( cluster_population(l) - 1 );
    else
      an1(l) = Inf;
    end

    itran(l) = 1;
    ncp(l) = -1;

  end

  indx = 0;
  ifault = 2;
  it_num = 0;

  d = zeros(point_num);
  live = zeros(cluster_num);

  while ( it_num < it_max )

    it_num = it_num + 1;
%
%  In this stage, there is only one pass through the data.   Each
%  point is re-allocated, if necessary, to the cluster that will
%  induce the maximum reduction in within-cluster sum of squares.
%
    [ cluster_center, cluster, cluster2, cluster_population, an1, an2, ...
      ncp, d, itran, live, indx ] = kmeans_02_optra ( dim_num, point_num, ...
      cluster_num, point, cluster_center, cluster, cluster2, cluster_population, ...
      an1, an2, ncp, d, itran, live, indx );
%
%  Stop if no transfer took place in the last POINT_NUM optimal transfer steps.
%
    if ( indx == point_num )
      ifault = 0;
      break
    end
%
%  Each point is tested in turn to see if it should be re-allocated
%  to the cluster to which it is most likely to be transferred,
%  CLUSTER2(I), from its present cluster, CLUSTER(I).   Loop through the
%  data until no further change is to take place.
%
    [ cluster_center, cluster, cluster2, cluster_population, an1, ...
      an2, ncp, d, itran, indx ] = kmeans_02_qtran ( dim_num, point_num, ...
      cluster_num, point, cluster_center, cluster, cluster2, cluster_population, ...
      an1, an2, ncp, d, itran, indx );
%
%  If there are only two clusters, there is no need to re-enter the
%  optimal transfer stage.
%
    if ( cluster_num == 2 )
      ifault = 0;
      break
    end
%
%  NCP has to be set to 0 before entering OPTRA.
%
    ncp(1:cluster_num) = 0;

  end

  if ( ifault == 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KMEANS_02 - Warning!\n' );
    fprintf ( 1, '  Maximum number of iterations reached\n' );
    fprintf ( 1, '  without convergence.\n' );
  end
%
%  Compute the within-cluster sum of squares for each cluster.
%
  cluster_center(1:dim_num,1:cluster_num) = 0.0;

  for i = 1 : point_num
    cluster_center(1:dim_num,cluster(i)) = ...
      cluster_center(1:dim_num,cluster(i)) + point(1:dim_num,i);
  end

  for j = 1 : dim_num
    cluster_center(j,1:cluster_num) = cluster_center(j,1:cluster_num) ...
      ./ cluster_population(1:cluster_num);
  end
%
%  Compute the cluster energies.
%
  cluster_energy(1:cluster_num) = 0.0;

  for i = 1 : point_num

    j = cluster(i);

    cluster_energy(j) = cluster_energy(j) + sum ( ...
      ( point(1:dim_num,i) - cluster_center(1:dim_num,j) ).^2 );

  end

  return
end
function [ cluster_center, cluster, cluster2, cluster_population, an1, an2, ...
  ncp, d, itran, live, indx ] = kmeans_02_optra ( dim_num, point_num, ...
  cluster_num, point, cluster_center, cluster, cluster2, cluster_population, ...
  an1, an2, ncp, d, itran, live, indx )

%*****************************************************************************80
%
%% KMEANS_02_OPTRA carries out the optimal transfer stage.
%
%  Discussion:
%
%    Each point is re-allocated, if necessary, to the cluster that
%    will induce a maximum reduction in the within-cluster sum of
%    squares.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2009
%
%  Author:
%
%    Original FORTRAN77 by John Hartigan, Manchek Wong.
%    FORTRAN90 version by John Burkardt.
%
%  Reference:
%
%    John Hartigan, Manchek Wong,
%    Algorithm AS 136:
%    A K-Means Clustering Algorithm,
%    Applied Statistics,
%    Volume 28, Number 1, 1979, pages 100-108.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of spatial dimensions.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer CLUSTER_NUM, the number of clusters.
%
%    Input, real POINT(DIM_NUM,POINT_NUM), the coordinates of
%    the points.
%
%    Input, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the cluster centers.
%
%    Input, integer CLUSTER(POINT_NUM), the cluster
%    each point belongs to.
%
%    Input, integer CLUSTER2(POINT_NUM), the cluster
%    to which each point is most likely to be transferred to.
%
%    Input, integer CLUSTER_POPULATION(CLUSTER_NUM),
%    the number of points in each cluster.
%
%    Input, real AN1(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) - 1)
%
%    Input, real AN2(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) + 1)
%
%    Input, integer NCP(CLUSTER_NUM), ?
%
%    Input, real D(POINT_NUM), ?
%
%    Input, integer ITRAN(CLUSTER_NUM),
%    1 if cluster L is updated in the quick-transfer stage,
%    0 otherwise.  Reset to 0 on output.
%
%    Input, integer LIVE(CLUSTER_NUM), ?
%
%    Input, integer INDX, ?
%
%    Output, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the cluster centers.
%
%    Output, integer CLUSTER(POINT_NUM), the cluster
%    each point belongs to.
%
%    Output, integer CLUSTER2(POINT_NUM), the cluster
%    to which each point is most likely to be transferred to.
%
%    Output, integer CLUSTER_POPULATION(CLUSTER_NUM),
%    the number of points in each cluster.
%
%    Output, real AN1(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) - 1)
%
%    Output, real AN2(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) + 1)
%
%    Output, integer NCP(CLUSTER_NUM), ?
%
%    Output, real D(POINT_NUM), ?
%
%    Output, integer ITRAN(CLUSTER_NUM),
%    1 if cluster L is updated in the quick-transfer stage,
%    0 otherwise.  Reset to 0 on output.
%
%    Output, integer LIVE(CLUSTER_NUM), ?
%
%    Output, integer INDX, ?
%

%
%  If cluster L is updated in the last quick-transfer stage, it
%  belongs to the live set throughout this stage.   Otherwise, at
%  each step, it is not in the live set if it has not been updated
%  in the last POINT_NUM optimal transfer steps.
%
  for l = 1 : cluster_num
    if ( itran(l) == 1 )
      live(l) = point_num + 1;
    end
  end

  for i = 1 : point_num

    indx = indx + 1;
    l1 = cluster(i);
    l2 = cluster2(i);
    ll = l2;
%
%  If point I is the only member of cluster L1, no transfer.
%
    if ( 1 < cluster_population(l1) )
%
%  If L1 has been updated in this stage, re-compute D(I).
%
      if ( ncp(l1) ~= 0 )
        d(i) = an1(l1) * sum ( ...
          ( point(1:dim_num,i) - cluster_center(1:dim_num,l1) ).^2 );
      end
%
%  Find the cluster with minimum R2.
%
      r2 = an2(l2) * sum ( ...
        ( point(1:dim_num,i) - cluster_center(1:dim_num,l2) ).^2 );

      for l = 1 : cluster_num
%
%  If LIVE(L1) <= I, then L1 is not in the live set.   If this is
%  true, we only need to consider clusters that are in the live set
%  for possible transfer of point I.
%
%  Otherwise, we need to consider all possible clusters.
%
        if ( ( i < live(l1) | i < live(l) ) & ...
             l ~= l1 & ...
             l ~= ll )

          rr = r2 / an2(l);

          dc = sum ( ( point(1:dim_num,i) - cluster_center(1:dim_num,l) ).^2 );

          if ( dc < rr )
            r2 = dc * an2(l);
            l2 = l;
          end

        end

      end
%
%  If no transfer is necessary, L2 is the new CLUSTER2(I).
%
      if ( d(i) <= r2 )

        cluster2(i) = l2;

      else
%
%  Update cluster centers, LIVE, NCP, AN1 and AN2 for clusters L1 and
%  L2, and update CLUSTER(I) and CLUSTER2(I).
%
        indx = 0;
        live(l1) = point_num + i;
        live(l2) = point_num + i;
        ncp(l1) = i;
        ncp(l2) = i;
        al1 = cluster_population(l1);
        alw = al1 - 1.0;
        al2 = cluster_population(l2);
        alt = al2 + 1.0;

        cluster_center(1:dim_num,l1) = ( cluster_center(1:dim_num,l1) * al1 ...
          - point(1:dim_num,i) ) / alw;

        cluster_center(1:dim_num,l2) = ( cluster_center(1:dim_num,l2) * al2 ...
          + point(1:dim_num,i) ) / alt;

        cluster_population(l1) = cluster_population(l1) - 1;
        cluster_population(l2) = cluster_population(l2) + 1;
        an2(l1) = alw / al1;

        if ( 1.0 < alw )
          an1(l1) = alw / ( alw - 1.0 );
        else
          an1(l1) = Inf;
        end

        an1(l2) = alt / al2;
        an2(l2) = alt / ( alt + 1.0 );
        cluster(i) = l2;
        cluster2(i) = l1;

      end

    end

    if ( indx == point_num )
      return
    end

  end
%
%  ITRAN(L) = 0 before entering QTRAN.
%
  itran(1:cluster_num) = 0;
%
%  LIVE(L) has to be decreased by POINT_NUM before re-entering OPTRA.
%
  live(1:cluster_num) = live(1:cluster_num) - point_num;

  return
end
function [ cluster_center, cluster, cluster2, cluster_population, an1, ...
  an2, ncp, d, itran, indx ] = kmeans_02_qtran ( dim_num, point_num, ...
  cluster_num, point, cluster_center, cluster, cluster2, cluster_population, ...
  an1, an2, ncp, d, itran, indx )

%*****************************************************************************80
%
%% KMEANS_02_QTRAN carries out the quick transfer stage.
%
%  Discussion:
%
%    For each point I, CLUSTER(I) and CLUSTER2(I) are switched, if necessary,
%    to reduce within-cluster sum of squares.  The cluster centers are
%    updated after each step.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2009
%
%  Author:
%
%    Original FORTRAN77 by John Hartigan, Manchek Wong.
%    FORTRAN90 version by John Burkardt.
%
%  Reference:
%
%    John Hartigan, Manchek Wong,
%    Algorithm AS 136:
%    A K-Means Clustering Algorithm,
%    Applied Statistics,
%    Volume 28, Number 1, 1979, pages 100-108.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of spatial dimensions.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer CLUSTER_NUM, the number of clusters.
%
%    Input, real POINT(DIM_NUM,POINT_NUM), the coordinates
%    of the points.
%
%    Input, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the cluster centers.
%
%    Input, integer CLUSTER(POINT_NUM), the cluster
%    each point belongs to.
%
%    Input, integer CLUSTER2(POINT_NUM), the cluster to which
%    each point is most likely to be transferred to.
%
%    Input, integer CLUSTER_POPULATION(CLUSTER_NUM),
%    the number of points in each cluster.
%
%    Input, real AN1(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) - 1)
%
%    Input, real AN2(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) + 1)
%
%    Input, integer NCP(CLUSTER_NUM), ?
%
%    Input, real D(POINT_NUM), ?
%
%    Input, integer ITRAN(CLUSTER_NUM),
%    1 if cluster L is updated in the quick-transfer stage,
%    0 otherwise.
%
%    Input, integer INDX, is set to 0 if any updating occurs.
%
%    Output, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the cluster centers.
%
%    Output, integer CLUSTER(POINT_NUM), the cluster
%    each point belongs to.
%
%    Output, integer CLUSTER2(POINT_NUM), the cluster to which
%    each point is most likely to be transferred to.
%
%    Output, integer CLUSTER_POPULATION(CLUSTER_NUM),
%    the number of points in each cluster.
%
%    Output, real AN1(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) - 1)
%
%    Output, real AN2(CLUSTER_NUM),
%    CLUSTER_POPULATION(L) / (CLUSTER_POPULATION(L) + 1)
%
%    Output, integer NCP(CLUSTER_NUM), ?
%
%    Output, real D(POINT_NUM), ?
%
%    Output, integer ITRAN(CLUSTER_NUM),
%    1 if cluster L is updated in the quick-transfer stage,
%    0 otherwise.
%
%    Output, integer INDX, is set to 0 if any updating occurs.
%

%
%  In the optimal transfer stage, NCP(L) indicates the step at which
%  cluster L is last updated.   In the quick transfer stage, NCP(L)
%  is equal to the step at which cluster L is last updated plus POINT_NUM.
%
  count = 0;
  step = 0;

  while ( 1 )

    for i = 1 : point_num

      count = count + 1;
      step = step + 1;
      l1 = cluster(i);
      l2 = cluster2(i);
%
%  If point I is the only member of cluster L1, no transfer.
%
      if ( 1 < cluster_population(l1) )
%
%  If NCP(L1) < STEP, no need to re-compute distance from point I to
%  cluster L1.   Note that if cluster L1 is last updated exactly POINT_NUM
%  steps ago, we still need to compute the distance from point I to
%  cluster L1.
%
        if ( step <= ncp(l1) )
          d(i) = an1(l1) * sum ( ...
            ( point(1:dim_num,i) - cluster_center(1:dim_num,l1) ).^2 );
        end
%
%  If STEP >= both NCP(L1) and NCP(L2) there will be no transfer of
%  point I at this step.
%
        if ( step < ncp(l1) | step < ncp(l2) )

          r2 = d(i) / an2(l2);

          dd = sum ( ( point(1:dim_num,i) - cluster_center(1:dim_num,l2) ).^2 );
%
%  Update cluster centers, NCP, CLUSTER_POPULATION, ITRAN, AN1 and AN2
%  for clusters L1 and L2.   Also update CLUSTER(I) and CLUSTER2(I).
%
%  Note that if any updating occurs in this stage, INDX is set back to 0.
%
          if ( dd < r2 )

            count = 0;
            indx = 0;
            itran(l1) = 1;
            itran(l2) = 1;
            ncp(l1) = step + point_num;
            ncp(l2) = step + point_num;
            al1 = cluster_population(l1);
            alw = al1 - 1.0;
            al2 = cluster_population(l2);
            alt = al2 + 1.0;

            cluster_center(1:dim_num,l1) = ...
              ( cluster_center(1:dim_num,l1) * al1 ...
              - point(1:dim_num,i) ) / alw;

            cluster_center(1:dim_num,l2) = ...
              ( cluster_center(1:dim_num,l2) * al2 ...
              + point(1:dim_num,i) ) / alt;

            cluster_population(l1) = cluster_population(l1) - 1;
            cluster_population(l2) = cluster_population(l2) + 1;
            an2(l1) = alw / al1;

            if ( 1.0 < alw )
              an1(l1) = alw / ( alw - 1.0 );
            else
              an1(l1) = Inf;
            end

            an1(l2) = alt / al2;
            an2(l2) = alt / ( alt + 1.0 );
            cluster(i) = l2;
            cluster2(i) = l1;

          end

        end

      end
%
%  If no re-allocation took place in the last POINT_NUM steps, return.
%
      if ( count == point_num )
        return
      end

    end

  end

  return
end


function [varargout] = binotest_dependent(X, Po)
% This function runs several different types of tests on dependent binomial data.
%
% :Usage:
% ::
%
%     [varargout] = binotest_dependent(X, Po)
%
% Overall, it tests the number of "hits" for each subject (row in X) against a null-hypothesis
% proportion p, across all subjects using a Z-test (two-tailed).
% The second level null hypothesis should be approximated by a normal
% distribution with a mean of p.  This approach assumes that each subject
% has an equal number of independent Bernoulli trials (columns in X) and
% that the number of subjects exceeds n=20 the test will be more accurate as n -> infinity.
% Also, calculates tests for each separate trial (e.g., subject columns),
% and the difference between proportions (two proportion z-test).
%
% :Inputs:
%
%   **X:**
%        X is a matrix of "hits" and "misses", coded as 1s and 0s.
%        where rows = subjects and columns = observations within
%        subject
%
%   **Po:**
%        Po is the null hypothesis proportion of "hits", e.g., often p = 0.5
%
% :Outputs:
%
%   **RES [1:5]:**
%        a structure containing the output of the stats for the
%        z-test, includes the number of subject (N), number of overall
%        hits (hits), the overall proportion of hits (prop), the standard
%        deviation (SE), z-statistic (Z), and the two tailed p-value (pval)
%        trial across all subjects.  Assumes independence
%
%   **RES1:**
%        Independent Single Interval Test for Column 1 (Column 1 only against Po)
%
%   **RES2:**
%        Independent Single Interval Test for Column 2 (Column 2 only against Po)
%
%   **RES3:**
%        Two proportion dependent difference z-test (Column 1 minus Column 2 against 0)
%
%   **RES4:**
%        Dependent single-interval test (Mean of Column 1 and Column 2 against Po)
%
%   **RES5:**
%        Two proportion dependent addition z-test (Column 1 plus Column 2 against 2 * Po)
%              (Similar to mean, not sure what this will be used for)
%
% :Examples:
% ::
%
%    [RES1, RES2, RES3, RES4, RES5] = binotest_dependent([1,1,1,1,0; 1,0,1,0,1]',.5)
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2014  Luke Chang & Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..

% ..
%    Process Data
% ..

X = double(X); % just in case
[N, k] = size(X);  % number of subjects x dependent obs per person
A = X(:, 1);
B = X(:, 2);

% -------------------------------------------------------------------------
% Test if Data is in Correct Format
% -------------------------------------------------------------------------
if k > 2, error('NOT IMPLEMENTED FOR MORE THAN 2 DEP OBSERVATIONS PER UNIT'); end

u = unique(X(:));
if any(u ~= 1 & u ~= 0), error('X MUST HAVE VALUES OF 1 OR 0 ONLY.'); end

if N < 20
    warning(['Running dependent binomial test requires many subjects to approximate normal distribution.  You are only using ' num2str(length(unique(subject_id))) ' subjects.  Interpret results with caution.'])
end

% -------------------------------------------------------------------------
% Calculate Variance and Covariance
% -------------------------------------------------------------------------
% First, calculate variance of the quantity we want for a single trial,
% using Pa and Pb as estimates for the binomial parameters
% (This is not the SD of the SAMPLING distribution, but the SD of the
% distribution of interest (e.g., variance of sum, difference)
% Later, we will use this to get the SD for the SAMPLING distribution,
% which is used to construct confidence intervals and tests.

%Calculate Probabilities of each Subject Trial
Pa = sum(A) ./ N;  % P-hat for A
Pb = sum(B) ./ N;  % P-hat for B

% Calculate Variance for each Subject Trial
Va = Pa * (1 - Pa); % ./ N;
Vb = Pb * (1 - Pb); % ./ N;

% Calculate Covariance between Trials
% C = ( (sum(A .* B) ./ N) * (sum(~A .* ~B) ./ N) ) - ( (sum(A .* ~B) ./ N) * (sum(B .* ~A) ./ N) ); % % Agresti, p. 410 SAME as below
C = ( (sum(A .* B) ./ N) - (Pa * Pb) );   % Covariance - expected prop under independence

% Calculate Variance for Test Statistics adjusting for covariance
Vaplusb = Va + Vb - (2 * C);
Vameanb = ( Va + Vb - (2 * C) ) / 4;  % / 4 because V(aX) = a^2Var(X)
Vaminusb = Va + Vb + (2 * C);

% -------------------------------------------------------------------------
% Define Functions
% -------------------------------------------------------------------------

sdfunction = @(V, N) (V ./ N) .^ .5;  % Calculate Standard Deviation
zfunc = @(Px, Po, SD) (Px - Po) ./ SD; % Calculate Z-Value
pfunction_gauss = @(z) 2 .* min(normcdf(z, 0, 1), normcdf(z, 0, 1, 'upper')); %Calculate p-value for z-distribution

% pfunction_bino = @(N, Po, hits) 2 * min(binocdf(hits, N, Po), (1 - binocdf(hits - 1, N, Po))); %Calculate p-value for binomial distribution

% -------------------------------------------------------------------------
% Define Various Confidence Intervals - TO DO - See Agresti Book
% -------------------------------------------------------------------------

% switch  cimethod
%     case 'Wald'
%
%         cifunc = @(z)
%
%     case 'scoring'  % default
%
%     otherwise
%         error('Unknown CI method. Check inputs.');
% end

% Now, we will use this to get the SD for the SAMPLING distribution,
% which is used to construct confidence intervals and tests.
% e.g., SDa = (Va ./ N) .^ .5;

% -------------------------------------------------------------------------
% Calculate test statistics
% -------------------------------------------------------------------------
% Use z-test (large-sample approximation; ok for N = 20+)

names = {'P1' 'P2' 'P1minusP2' 'PavgP1P2' 'P1plusP2' };
est = [Pa Pb Pa-Pb mean([Pa Pb]) Pa+Pb];
Vs = [Va Vb Vaminusb Vameanb Vaplusb];
Pnull = [Po Po 0 Po 2*Po];

for i = 1:length(names)
    SDs(i) = sdfunction(Vs(i), N);
    Z(i) = zfunc(est(i), Pnull(i), SDs(i));
    pval(i) = pfunction_gauss(Z(i));
    %ci(i) = add confidence intervals

    RES{i} = struct('n', N, 'hits', est(i).*N, 'prop', est(i), 'SE', SDs(i), 'Z', Z(i), 'p_val', pval(i));

    varargout{i} = RES{i};
end

end % function


function [mg,nu,sig,info] = spm_rice_mixture(h,x,K)
% Fit a mixture of Ricians to a histogram
% FORMAT [mg,nu,sig] = spm_rice_mixture(h,x,K)
% h    - histogram counts
% x    - bin positions (plot(x,h) to see the histogram)
% K    - number of Ricians
% mg   - integral under each Rician
% nu   - "mean" parameter of each Rician
% sig  - "standard deviation" parameter of each Rician
% info - This struct can be used for plotting the fit as:
%            plot(info.x(:),info.p,'--',info.x(:), ...
%                 info.h/sum(info.h)/info.md,'b.', ...
%                 info.x(:),info.sp,'r');
%
% An EM algorithm is used, which involves alternating between computing
% belonging probabilities, and then the parameters of the Ricians.
% The Koay inversion technique is used to compute the Rician parameters
% from the sample means and standard deviations. This is described at
% https://en.wikipedia.org/wiki/Rician_distribution
%__________________________________________________________________________
% Copyright (C) 2012-2019 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_rice_mixture.m 7595 2019-05-23 13:48:53Z mikael $

mg  = ones(K,1)/K;
nu  = (0:(K-1))'*max(x)/(K+1);
sig = ones(K,1)*max(x)/K/10;
lam = (sum(x.*h)/sum(h)/K).^2;

m0 = zeros(K,1);
m1 = zeros(K,1);
m2 = zeros(K,1);
ll = -Inf;
for iter=1:10000
    p  = zeros(numel(x),K);
    for k=1:K
        % Product Rule
        % p(class=k, x | mg, nu, sig) = p(class=k|mg) p(x | nu, sig, class=k)
        p(:,k) = mg(k)*ricepdf(x(:),nu(k),sig(k)^2) + eps;
    end

    % Sum Rule
    % p(x | mg, nu, sig) = \sum_k p(class=k, x | mg, nu, sig)
    sp  = sum(p,2);
    oll = ll;
    ll  = sum(log(sp).*h(:)); % Log-likelihood
    if ll-oll<1e-8*sum(h), break; end

    %fprintf('%g\n',ll);
    %md = mean(diff(x));
    %plot(x(:),p,'--',x(:),h/sum(h)/md,'b.',x(:),sp,'r'); drawnow

    % Bayes Rule
    % p(class=k | x, mg, nu, sig) = p(class=k, x | mg, nu, sig) / p(x | mg, nu, sig)
    p = bsxfun(@rdivide,p,sp);

    % Compute moments from the histograms, weighted by the responsibilities (p).
    for k=1:K
        m0(k) = sum(p(:,k).*h(:));              % Number of voxels in class k
        m1(k) = sum(p(:,k).*h(:).*x(:));        % Sum of the intensities in class k
        m2(k) = sum(p(:,k).*h(:).*x(:).*x(:));  % Sum of squares of intensities in class k
    end

    mg = m0/sum(m0); % Mixing proportions
    for k=1:K
        mu1 = m1(k)./m0(k);                                % Mean
        mu2 = (m2(k)-m1(k)*m1(k)/m0(k)+lam*1e-3)/(m0(k)+1e-3); % Variance

        % Compute nu & sig from mean and variance
        [nu(k),sig(k)] = moments2param(mu1,mu2);
    end
    %disp([nu'; sig'])
end

if nargout >= 4
    % This info can be used for plotting the fit
    info    = struct;
    info.x  = x;
    info.h  = h;
    info.p  = p;
    info.sp = sp;
    info.md = mean(diff(x));
end
%__________________________________________________________________________

%__________________________________________________________________________
function [nu,sig] = moments2param(mu1,mu2)
% Rician parameter estimation (nu & sig) from mean (mu1) and variance
% (mu2) via the Koay inversion technique.
% This follows the scheme at
% https://en.wikipedia.org/wiki/Rice_distribution#Parameter_estimation_.28the_Koay_inversion_technique.29
% This Wikipedia description is based on:
% Koay, C.G. and Basser, P. J., Analytically exact correction scheme
% for signal extraction from noisy magnitude MR signals,
% Journal of Magnetic Resonance, Volume 179, Issue = 2, p. 317–322, (2006)

r     = mu1/sqrt(mu2);
theta = sqrt(pi/(4-pi));
if r>theta
    for i=1:256
        xi    = 2+theta^2-pi/8*exp(-theta^2/2)*((2+theta^2)*besseli(0,theta^2/4)+theta^2*besseli(1,theta^2/4))^2;
        g     = sqrt(xi*(1+r^2)-2);
        if abs(theta-g)<1e-6, break; end
        theta = g;
    end
    if ~isfinite(xi), xi = 1; end
    sig = sqrt(mu2)/sqrt(xi);
    nu  = sqrt(mu1^2+(xi-2)*sig^2);
else
    nu  = 0;
    sig = (2^(1/2)*(mu1^2 + mu2)^(1/2))/2;
end
%__________________________________________________________________________

%__________________________________________________________________________
function p = ricepdf(x,nu,sig2)
% Rician PDF
% p = ricepdf(x,nu,sig2)
% https://en.wikipedia.org/wiki/Rice_distribution#Characterization
p       = zeros(size(x));
tmp     = -(x.^2+nu.^2)./(2*sig2);
msk     = (tmp > -95) & (x*(nu/sig2) < 85) ; % Identify where Rice probability can be computed
p(msk)  = (x(msk)./sig2).*exp(tmp(msk)).*besseli(0,x(msk)*(nu/sig2)); % Use Rician distribution
p(~msk) = (1./sqrt(2*pi*sig2))*exp((-0.5/sig2)*(x(~msk)-nu).^2);      % Use Gaussian distribution


function x = r83_cr_sl ( n, a_cr, b )

%*****************************************************************************80
%
%% R83_CR_SL solves a real linear system factored by R83_CR_FA.
%
%  Discussion:
%
%    The matrix A must be tridiagonal.  R83_CR_FA is called to compute the
%    LU factors of A.  It does so using a form of cyclic reduction.  If
%    the factors computed by R83_CR_FA are passed to R83_CR_SL, then one or many
%    linear systems involving the matrix A may be solved.
%
%    Note that R83_CR_FA does not perform pivoting, and so the solution
%    produced by R83_CR_SL may be less accurate than a solution produced
%    by a standard Gauss algorithm.  However, such problems can be
%    guaranteed not to occur if the matrix A is strictly diagonally
%    dominant, that is, if the absolute value of the diagonal coefficient
%    is greater than the sum of the absolute values of the two off diagonal
%    coefficients, for each row of the matrix.
%
%  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:
%
%    22 March 2004
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Hockney,
%    A fast direct solution of Poisson's equation using Fourier Analysis,
%    Journal of the ACM,
%    Volume 12, Number 1, pages 95-113, January 1965.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A_CR(3,2*N+1), factorization information computed by R83_CR_FA.
%
%    Input, real B(N), the right hand side vector.
%
%    Output, real X(N), the solution of the linear system.
%
  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R83_CR_SL - Fatal error!\n' );
    fprintf ( 1, '  Nonpositive N = %d\n', n );
    return
  end

  if ( n == 1 )
    x(1) = a_cr(2,2) * b(1);
    return
  end
%
%  Set up RHS.
%
  rhs(1) = 0.0;
  rhs(2:n+1) = b(1:n);
  rhs(n+2:2*n+1) = 0.0;

  il = n;
  ndiv = 1;
  ipntp = 0;

  while ( 1 < il )

    ipnt = ipntp;
    ipntp = ipntp + il;
    il = floor ( il / 2 );
    ndiv = ndiv * 2;
    ihaf = ipntp;

    for iful = ipnt+2 : 2 : ipntp
      ihaf = ihaf + 1;
      rhs(ihaf+1) = rhs(iful+1) - a_cr(3,iful) * rhs(iful) ...
        - a_cr(1,iful+1) * rhs(iful+2);
    end

  end

  rhs(ihaf+1) = rhs(ihaf+1) * a_cr(2,ihaf+1);
  ipnt = ipntp;

  while ( 0 < ipnt )

    ipntp = ipnt;
    ndiv = floor ( ndiv / 2 );
    il = floor ( n / ndiv );
    ipnt = ipnt - il;
    ihaf = ipntp;

    for ifulm = ipnt+1 : 2 : ipntp
      iful = ifulm + 1;
      ihaf = ihaf + 1;
      rhs(iful+1) = rhs(ihaf+1);
      rhs(ifulm+1) = a_cr(2,ifulm+1) * ( rhs(ifulm+1) - a_cr(3,ifulm) * rhs(ifulm) ...
        - a_cr(1,ifulm+1) * rhs(iful+1) );
    end

  end

  x(1:n) = rhs(2:n+1);

  return
end


function [err, errK] = getL2error3CR(node,elem,uvecexact,uh,quadOrder)
%% GETL2ERROR3CR L2 norm of the approximation error for vectorial CR element
%    need Matlab ver > 2016b for the vectorization to work
%
%    Reference: Error analysis of a decoupled finite element method for quad-curl problems
%               https://arxiv.org/abs/2102.03396
%
%    see alos: getL2error3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('quadOrder','var'); quadOrder = 3; end
elem2face = dof3face(elem);

%%
NT = size(elem,1);
err = zeros(NT,1);
[lambda,weight] = quadpts3(quadOrder);
phi = 1-3*lambda;

nQuad = size(lambda,1);
for p = 1:nQuad
    uhp = uh(elem2face(:,1),:)*phi(p,1) + ...
        uh(elem2face(:,2),:)*phi(p,2) + ...
        uh(elem2face(:,3),:)*phi(p,3) + ...
        uh(elem2face(:,4),:)*phi(p,4);
    % quadrature points in the x-y coordinate
    pxyz = lambda(p,1)*node(elem(:,1),:) ...
         + lambda(p,2)*node(elem(:,2),:) ...
         + lambda(p,3)*node(elem(:,3),:) ...
         + lambda(p,4)*node(elem(:,4),:);
    err = err + weight(p)*sum((uvecexact(pxyz) - uhp).^2,2);
end

%%
d12 = node(elem(:,2),:)-node(elem(:,1),:);
d13 = node(elem(:,3),:)-node(elem(:,1),:);
d14 = node(elem(:,4),:)-node(elem(:,1),:);
volume = abs(dot(mycross(d12,d13,2),d14,2)/6);
err(isnan(err)) = 0; % singular point is excluded
err = volume.*err;
if nargout > 1; errK = sqrt(err); end
err = sqrt(sum(err));

end