function [logml]=nwmlikgrid(X,n,k,phi0,S0,Sbar,alphabar)

% compute the first determinant part
% create the square root matrix of phi0
% because phi0 is diagonal, this is simply the square root of the diagonal terms of phi0
Fphi=spdiags(diag(phi0).^0.5,0,k,k);
% compute the product
product=Fphi'*X'*X*Fphi;
% compute the eigenvalues of the product
eigenvalues=eig(product);
% now compute the full determinant term
temp1=(-n/2)*log(prod(diag(eye(k)+diag(eigenvalues))));

% compute the second determinant part
% create the square root matrix of inv(S0)
invS0=spdiags(1./diag(S0),0,n,n);
Fs=spdiags(diag(invS0).^0.5,0,n,n);
% compute the summation
summ=Fs'*(Sbar-S0)*Fs;
% compute the eigenvalues of the summation
eigenvalues=eig(summ);
% now compute the full determinant term
temp2=(-alphabar/2)*log(prod(diag(eye(n)+diag(eigenvalues))));

% compute the marginal likelihood
logml=real(temp1+temp2);


function [bb] = cdtbal1(pp,ee)
%CDTBAL1 compute the circumballs associated with a 1-simplex
%triangulation embedded in R^2.
%   [BB] = TRIBAL1(PP,EE) returns the circumscribing balls
%   associated with the 1-simplexes in [PP,TT], such that BB
%   = [XC,YC,RC.^2].

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 24/03/2017

%---------------------------------------------- basic checks
    if (~isnumeric(pp) || ...
        ~isnumeric(ee) )
        error('cdtbal1:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end
%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ndims(ee) ~= +2)
        error('cdtbal1:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(pp,2)~= +2 || size(ee,2) < +2)
        error('cdtbal1:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    bb = zeros(size(ee,1),3);

    bb(:,1:2) = (pp(ee(:,1),:)+pp(ee(:,2),:))*.50 ;
    bb(:,  3) = ...
      sum((pp(ee(:,1),:)-pp(ee(:,2),:)).^2,2)*.25 ;

end


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

%*****************************************************************************80
%
%% GAMMA_LOG_VALUES returns some values of the Log Gamma function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Log[Gamma[x]]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
      0.1524063822430784E+01, ...
      0.7966778177017837E+00, ...
      0.3982338580692348E+00, ...
      0.1520596783998375E+00, ...
      0.0000000000000000E+00, ...
     -0.4987244125983972E-01, ...
     -0.8537409000331584E-01, ...
     -0.1081748095078604E+00, ...
     -0.1196129141723712E+00, ...
     -0.1207822376352452E+00, ...
     -0.1125917656967557E+00, ...
     -0.9580769740706586E-01, ...
     -0.7108387291437216E-01, ...
     -0.3898427592308333E-01, ...
     0.00000000000000000E+00, ...
     0.69314718055994530E+00, ...
     0.17917594692280550E+01, ...
     0.12801827480081469E+02, ...
     0.39339884187199494E+02, ...
     0.71257038967168009E+02 ];

  x_vec = [ ...
      0.20E+00, ...
      0.40E+00, ...
      0.60E+00, ...
      0.80E+00, ...
      1.00E+00, ...
      1.10E+00, ...
      1.20E+00, ...
      1.30E+00, ...
      1.40E+00, ...
      1.50E+00, ...
      1.60E+00, ...
      1.70E+00, ...
      1.80E+00, ...
      1.90E+00, ...
      2.00E+00, ...
      3.00E+00, ...
      4.00E+00, ...
     10.00E+00, ...
     20.00E+00, ...
     30.00E+00 ];

  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 M = fixedrankMNquotientfactory(m, n, k)
% Manifold of m-by-n matrices of rank k with two factor quotient geometry.
%
% function M = fixedrankMNquotientfactory(m, n, k)
%
% This follows the quotient geometry described in the following paper:
% P.-A. Absil, L. Amodei and G. Meyer,
% "Two Newton methods on the manifold of fixed-rank matrices endowed
%  with Riemannian quotient geometries", arXiv, 2012.
%
% Paper link: http://arxiv.org/abs/1209.0068
%
% A point X on the manifold is represented as a structure with two
% fields: M and N. The matrix M (mxk) is orthonormal, while the matrix N
% (nxk) is full-rank such that X = M*N';
%
% Tangent vectors are represented as a structure with two fields (M, N).
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{absil2014fixedrank,
%       Title   = {Two Newton methods on the manifold of fixed-rank matrices endowed with Riemannian quotient geometries},
%       Author  = {Absil, P.-A. and Amodei, L. and Meyer, G.},
%       Journal = {Computational Statistics},
%       Year    = {2014},
%       Number  = {3-4},
%       Pages   = {569--590},
%       Volume  = {29},
%       Doi     = {10.1007/s00180-013-0441-6}
%     }

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    M.name = @() sprintf('MN'' quotient manifold of %dx%d matrices of rank %d', m, n, k);

    M.dim = @() (m+n-k)*k;

    % Choice of the metric is motivated by the symmetry present in the
    % space.
    M.inner = @(X, eta, zeta) eta.M(:).'*zeta.M(:) + eta.N(:).'*zeta.N(:);

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

    M.dist = @(x, y) error('fixedrankMNquotientfactory.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    symm = @(X) .5*(X+X');
    stiefel_proj = @(M, H) H - M*symm(M'*H);

    M.egrad2rgrad = @egrad2rgrad;
    function eta = egrad2rgrad(X, eta)
        eta.M = stiefel_proj(X.M, eta.M);
    end

    M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)

        % Directional derivative of the Riemannian gradient.
        Hess.M = ehess.M - eta.M*symm(X.M'*egrad.M);
        Hess.M = stiefel_proj(X.M, Hess.M);

        Hess.N = ehess.N;

        % Projection onto the horizontal space.
        Hess = M.proj(X, Hess);
    end

    M.proj = @projection;
    function etaproj = projection(X, eta)

        % Start by projecting the vector from Rmp x Rnp to the tangent
        % space to the total space, that is, eta.M should be in the
        % tangent space to Stiefel at X.M and eta.N is arbitrary.
        eta.M = stiefel_proj(X.M, eta.M);

        % Now project from the tangent space to the horizontal space, that
        % is, take care of the quotient.

        % First solve a Sylvester equation (A symm., B skew-symm.)
        A = X.N'*X.N + eye(k);
        B = eta.M'*X.M + eta.N'*X.N;
        B = B-B';
        omega = lyap(A, -B);

        % And project along the vertical space to the horizontal space.
        etaproj.M = eta.M + X.M*omega;
        etaproj.N = eta.N + X.N*omega;

    end

    M.exp = @exponential;
    function Y = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        A = t*X.M'*eta.M;
        S = t^2*eta.M'*eta.M;
        Y.M = [X.M t*eta.M]*expm([A -S ; eye(k) A])*eye(2*k, k)*expm(-A);

        % re-orthonormalize (seems necessary from time to time).
        [Q R] = qr(Y.M, 0);
        Y.M = Q * diag(sign(diag(R)));

        Y.N = X.N + t*eta.N;

    end

    % Factor M lives on the Stiefel manifold, hence we will reuse its
    % random generator.
    stiefelm = stiefelfactory(m, k);

    M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        Y.M = uf(X.M + t*eta.M); % This is a valid retraction
        Y.N = X.N + t*eta.N;
    end

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

    M.rand = @random;
    function X = random()
        X.M = stiefelm.rand();
        X.N = randn(n, k);
    end

    M.randvec = @randomvec;
    function eta = randomvec(X)
        eta.M = randn(m, k);
        eta.N = randn(n, k);
        eta = projection(X, eta);
        nrm = M.norm(X, eta);
        eta.M = eta.M / nrm;
        eta.N = eta.N / nrm;
    end

    M.lincomb = @lincomb;

    M.zerovec = @(X) struct('M', zeros(m, k), 'N', zeros(n, k));

    M.transp = @(x1, x2, d) projection(x2, d);

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INMSL>

    if nargin == 3
        d.M = a1*d1.M;
        d.N = a1*d1.N;
    elseif nargin == 5
        d.M = a1*d1.M + a2*d2.M;
        d.N = a1*d1.N + a2*d2.N;
    else
        error('Bad use of fixedrankMNquotientfactory.lincomb.');
    end

end

function A = uf(A)
    [L, unused, R] = svd(A, 0);
    A = L*R';
end


function a = c8mat_identity ( n )

%*****************************************************************************80
%
%% C8MAT_IDENTITY sets the square matrix A to the identity.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, complex A(N,N), the matrix.
%
  a(1:n,1:n) = 0.0;

  for i = 1 : n
    a(i,i) = 1.0;
  end

  return
end


function submit()
  addpath('./lib');

  conf.assignmentSlug = 'anomaly-detection-and-recommender-systems';
  conf.itemName = 'Anomaly Detection and Recommender Systems';
  conf.partArrays = { ...
    { ...
      '1', ...
      { 'estimateGaussian.m' }, ...
      'Estimate Gaussian Parameters', ...
    }, ...
    { ...
      '2', ...
      { 'selectThreshold.m' }, ...
      'Select Threshold', ...
    }, ...
    { ...
      '3', ...
      { 'cofiCostFunc.m' }, ...
      'Collaborative Filtering Cost', ...
    }, ...
    { ...
      '4', ...
      { 'cofiCostFunc.m' }, ...
      'Collaborative Filtering Gradient', ...
    }, ...
    { ...
      '5', ...
      { 'cofiCostFunc.m' }, ...
      'Regularized Cost', ...
    }, ...
    { ...
      '6', ...
      { 'cofiCostFunc.m' }, ...
      'Regularized Gradient', ...
    }, ...
  };
  conf.output = @output;

  submitWithConfiguration(conf);
end

function out = output(partId, auxstring)
  % Random Test Cases
  n_u = 3; n_m = 4; n = 5;
  X = reshape(sin(1:n_m*n), n_m, n);
  Theta = reshape(cos(1:n_u*n), n_u, n);
  Y = reshape(sin(1:2:2*n_m*n_u), n_m, n_u);
  R = Y > 0.5;
  pval = [abs(Y(:)) ; 0.001; 1];
  Y = (Y .* double(R));  % set 'Y' values to 0 for movies not reviewed
  yval = [R(:) ; 1; 0];
  params = [X(:); Theta(:)];
  if partId == '1'
    [mu sigma2] = estimateGaussian(X);
    out = sprintf('%0.5f ', [mu(:); sigma2(:)]);
  elseif partId == '2'
    [bestEpsilon bestF1] = selectThreshold(yval, pval);
    out = sprintf('%0.5f ', [bestEpsilon(:); bestF1(:)]);
  elseif partId == '3'
    [J] = cofiCostFunc(params, Y, R, n_u, n_m, ...
                       n, 0);
    out = sprintf('%0.5f ', J(:));
  elseif partId == '4'
    [J, grad] = cofiCostFunc(params, Y, R, n_u, n_m, ...
                             n, 0);
    out = sprintf('%0.5f ', grad(:));
  elseif partId == '5'
    [J] = cofiCostFunc(params, Y, R, n_u, n_m, ...
                       n, 1.5);
    out = sprintf('%0.5f ', J(:));
  elseif partId == '6'
    [J, grad] = cofiCostFunc(params, Y, R, n_u, n_m, ...
                             n, 1.5);
    out = sprintf('%0.5f ', grad(:));
  end
end


%%                   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;


%rhsta simulates the examples of Sections 3.2.3 and 3.3.4
%    ____________________________________________________________________
%  /Program:  rhsta.m													   \
% / Description:  This program runs the simulation of the Rhors example     \
%|  	found in sections 3.2.3 and 3.3.4 of the text.  The program allows	 |
%|  	the user to choose between the example of feedforward around 		 |
%|	plant or feedforward in both the plant and model.  The program 	     |
%|	prompts the user to enter values for the variables that are adjusted |
%|	within the text.													 |
%|	This program requires the simulink diagram rhsim.m in order to run   |
% \ 		/
%  \_______________________________________________________________________/

%Copyright

% Programmed Summer 1996 by Kenyon Thayer, RPI, Troy, NY

a=exist('D');
if a==1
	D_old=D;
	tau_old=tau;
	Ti_old=Ti;
	Tp_old=Tp;
else
	D_old=0.1;
	tau_old=0.2;
	Ti_old=10;
	Tp_old=10;
end

disp('  **This programs simulates the text book examples 3.2.3 and 3.3.4.**');
disp('  ');
disp('Note: example 3.2.3 is feedforward in the plant only');
disp('      example 3.3.4 is feedforward in the plant and model');

str=input('Is this feedforward in both plant and model? Y/N: ','s');
if str=='Y' | str=='y'
	ff=1;
else
	ff=0;
end
% State space form of a Model
% **************************
num1=[3];den1=[1 3];
[Am,Bm,Cm,Dm]=tf2ss(num1,den1);

% Initial Conditions
% *************************************
Key0=0;
Kx0=0;
Ku0=0;
Ki0=[Key0 Kx0 Ku0];
xm0=0;

% State space form of a FF Compensator
% *************************************
D=input(sprintf('Enter a value for D (default value is %5.3f): ',D_old));
if isempty(D)
	D=D_old;
end

tau=input(sprintf('Enter a value for tau (default value is %5.3f): ',...
	tau_old));
if isempty(tau)
	tau=tau_old;
end

Ti=input(sprintf('Enter a value for Ti (default value is %d): ',...
	Ti_old));
if isempty(Ti)
	Ti=Ti_old;
end

Tp=input(sprintf('Enter a value for Tp (default value is %d): ',...
	Tp_old));
if isempty(Tp)
	Tp=Tp_old;
end

numc=D;
denc=[tau 1];
[Af,Bf,Cf,Df]=tf2ss(numc,denc);
xf0=[0];

% State space form of a Plant
%*****************************
a=30.0;
nump=[458];denp=[1 (a+1) (a+229) 229];
[Ap,Bp,Cp,Dp]=tf2ss(nump,denp);
xp0=[0 0 0]';

%************ Ti and Tp ************
Tie=Ti;Tix=Ti;Tiu=Ti;
Tpe=Tp;Tpx=Tp;Tpu=Tp;

%*********************************************
%[t,x,y]=gear('rhsim',40);
[t,x,y]=rk45('rhsim',40,[],[1e-6,1e-2,1]);
%[t,x,y]=linsim('rhsim',10,[],[1e-5,1e-2,1-2]);

plot(t,y(:,1),'--',t,y(:,2))
axis([0 40 -.4 .4])
text(.41, 1.05, 'Ym', 'color', [1 0 1], 'FontSize', 12, 'Units', 'normal');
text(.51, 1.05, 'Yp', 'color', [1 1 0], 'FontSize', 12, 'Units', 'normal');


function [ y, m, d, f ] = jed_to_ymdf_soor_san ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_SOOR_SAN converts a JED to a Soor San YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F,
%    the YMDF date.
%
  jed_epoch = epoch_to_jed_soor_san ( );

  j = floor ( jed - jed_epoch );
  f = ( jed - jed_epoch ) - j;

  d = 1 + j;
  m = 1;
  y = 1;
%
%  Account for the number of completed 4 year cycles of 1461 days.
%
  n = floor ( ( d - 1 ) / 1461 );
  y = y + 4 * n;
  d = d - n * 1461;
%
%  Account for the number of completed 365 day years.
%
  n = floor ( ( d - 1 ) / 365 );
  y = y + n;
  d = d - n * 365;
%
%  Account for the number of completed 30 day months.
%
  n = floor ( ( d - 1 ) / 30 );
  m = m + n;
  d = d - n * 30;

  return
end


function [m] = cellmsq(x, dim)

% [M] = CELLMSQ(X, DIM) computes the mean of squares, across all cells in x along
% the dimension dim.
%
% X should be an linear cell-array of matrices for which the size in at
% least one of the dimensions should be the same for all cells

nx = size(x);
if ~iscell(x) || length(nx)>2 || all(nx>1),
  error('incorrect input for cellmsq');
end

if nargin==1,
  scx1 = cellfun('size', x, 1);
  scx2 = cellfun('size', x, 2);
  if     all(scx2==scx2(1)), dim = 2; %let second dimension prevail
  elseif all(scx1==scx1(1)), dim = 1;
  else   error('no dimension to compute mean for');
  end
end

nx   = max(nx);
nsmp = cellfun('size', x, dim);
ssmp = cellfun(@sumsq,   x, repmat({dim},1,nx), 'UniformOutput', 0);
m    = sum(cell2mat(ssmp), dim)./sum(nsmp);

function [s] = sumsq(x, dim)

s = sum(x.^2, dim);


%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 [in2] = cm22in2(cm2)
% Convert area from square centimeters to square inches.
% Chad A. Greene 2012
in2 = cm2*0.1550003100006 ;


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


%SiftPointFeature.support Support region of feature
%
% OUT = F.support(IM, W) is an image of the support region of the
% feature F, extracted from the image IM in which the feature appears.
% The support region is scaled to WxW and rotated so that the feature's
% orientation axis is upward.
%
% OUT = F.support(IMAGES, W) as above but if the features were extracted
% from an image sequence IMAGES then the feature is extracted from the
% appropriate image in the same sequence.
%
% [OUT,T] = F.support(IMAGES, W) as above but returns the pose of the feature
% as a 3x3 homogeneous transform in SE(2) that comprises the feature position
% and orientation.
%
% F.support(IM, W) as above but the support region is displayed.
%
% See also SiftPointFeature.

function [out,TT] = support(sf, images, N)

    if nargin < 3
        N = 50;
    end

    im = images(:,:,sf.image_id_);

    d = 2*sf.scale_;

    [Uo,Vo] = imeshgrid(N, N);

    T = se2(sf.u_, sf.v_, sf.theta_) * diag([d/N,d/N,1]) * se2(-N/2, -N/2);

    UV = transformp(T, [Uo(:) Vo(:)]');
    U = reshape(UV(1,:), size(Uo));
    V = reshape(UV(2,:), size(Vo));

    [Ui,Vi] = imeshgrid(im);

    im2 = interp2(Ui, Vi, idouble(im), U, V);

    if nargout == 0
        idisp(im2)
    elseif nargout == 1
        out = im2;
    elseif nargout == 2
        out = im2;
        TT = T;
    end


% [Cs, Es] = curve_ext_multi(Tx, log2(fs), nc, lambda, clwin)
%
% Consecutively extract maximum energy / minimum curvature curves
% from a synchrosqueezing representation.  As curves are extracted,
% their associated energies are zeroed out in the synsq
% representation.  Curve extraction is then again performed on
% the remaining representaton.
%
% For more details, see help curve_ext.
%
%
%
% Input:
%  Tx, fs, lambda - same as input to curve_ext() (lambda default = 1e3)
%  nc - Number of curves to extract
%  clwin - frequency clearing window; after curve extraction, a window
%    of frequencies [Cs(:,i)-clwin:Cs(:,i)+clwin] is removed from the
%    original representation. (default = 2)
%
% Output:
%  Cs - [N x nc] matrix of curve indices (N==size(Tx,2))
%  Es - [nc x 1] vector of associated (logarithmic) energies
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function [Cs, Es] = curve_ext_multi(Tx, lfs, nc, lambda, clwin)
if nargin<5, clwin = 4; end
if nargin<4, lambda = 1e3; end

[na, N] = size(Tx);

Cs = zeros(N, nc);
Es = zeros(nc, 1);

for ni=1:nc
    [Cs(:,ni), Es(ni)] = curve_ext(Tx, lfs, lambda);

    % Remove this curve from the representation
    % Max/min frequencies for each time step
    for cli=0:clwin
        fb = max(1, Cs(:,ni)-cli);
        fe = min(na, Cs(:,ni)+cli);
        Tx([0:N-1]'*na+fb) = sqrt(eps);
        if cli>0, Tx([0:N-1]'*na+fe) = sqrt(eps); end
    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);


function [u, dataError, nSpikes, energy] = iSparsByPottsADMM(f, gamma, A, p, opts)
%iSparsPottsADMM Minimizes the sparsity problem using an Potts ADMM method
%
%Description:
% Minimizes the sparsity functional
%
%  \gamma \| u \|_0 + \| A u - f \|_p^p
%
% using the inverse Potts functional (see iPottsADMM)

% written by M. Storath
% $Date: 2013-04-05 12:22:32 +0200 (Fr, 05. Apr 2013) $	$Revision: 73 $

% create differentiation matrix
if isa(A, 'linop')
    D = linop( @(x) diff(x), @(x) conv(x, [-1 1], 'full') );
elseif isa(A, 'convop')
    d(numel(f),1) = -1; d(1) = 1;
    D = convop(fft(d));
else
    D = spdiffmatrix(size(A, 2)+1);
end

% create substitution matrix
B = A * D;

% parse parameters
if ~exist('opts', 'var')
    % standard parameters
    opts.muInit = gamma * 1e-6;
    %opts.muInit = 2 * gamma / norm(B' * f, 2)^2 ;
    opts.muStep = 1.05;
    opts.dispStatus = 1;
    opts.tol = 1e-6;
    opts.iMax = 1;
end

% call Potts problem with matrix A * D
v = iPottsADMM(f, gamma, B, p, opts);

% resubstitution
u = real(D * v);

% compute error
res = A * u - f;
dataError = sum(res(:).^p);

% count number of spikes
nSpikes = sum(u(:) ~= 0);

% total energy
energy = gamma * nSpikes + dataError;

end


function prob = my_mvnpdf(x,meanV,varV,diagCov)
if nargin < 3
    diagCov = 0;
end
if diagCov == 0
    prob = mvnpdf(x,meanV,varV);
else
    % meanV, varV are row vectors
    % each row of x is a sample
    [N_vector,Dim] = size(x);
    norm_term = ( (2*pi)^(Dim/2) ) * prod( sqrt(varV) );
    if 1
        tmp = bsxfun(@minus, x, meanV);
        tmp = tmp.*tmp;
        tmp = bsxfun(@times, tmp, 1./varV);
    end
    if 0
        tmp = (x - repmat(meanV,N_vector,1)).^2;
        tmp = tmp ./ repmat(varV,N_vector,1);
    end
    tmp = sum(tmp,2);
    prob = exp(-tmp/2) / norm_term;
end

end


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 r = coth(a)
%COTH         Taylor hyperbolic cotangent  coth(a)
%
%Thanks to George Corliss for providing the Taylor expansion
%

% written  06/03/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  K = getappdata(0,'INTLAB_TAYLOR_ORDER');

  r = a;
  ct = a.t;
  N = size(a.t,2);
  r.t(1,:) = coth(a.t(1,:));
  ct(1,:) = 1-r.t(1,:).^2;     % 1+a^2
  r.t(2,:) = ct(1,:) .* a.t(2,:);
  for i=2:K
    ct(i,:) = - sum( r.t(1:i,:).*r.t(i:-1:1,:) , 1 );
    r.t(i+1,:) = sum( ct(1:i,:).*a.t(i+1:-1:2,:).*repmat((i:-1:1)',1,N) , 1 )/i;
  end

  if rndold
    setround(rndold)
  end


function x = p20_start ( option, nvar )

%*****************************************************************************80
%
%% P20_START returns a starting point for problem 20.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Output, real X(NVAR), the starting point.
%
  hold_index = p20_i4_get ( 'hold_index' );
  hold_value = p20_r8_get ( 'hold_value' );

  if ( hold_index == 1 )
    l = hold_value;
    theta = pi / 8.0;
  elseif ( hold_index == 2 )
    l = 0.25;
    theta = hold_value;
  else
    l = 0.25;
    theta = pi / 8.0;
  end

  [ lambda, mu ] = p20_setup ( l, theta );

  x = [ l, theta, lambda, mu ]';

  return
end


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 b = multihconj(a, dim)
%MULTIHCONJ  Hermitian conjugating arrays of matrices.
%    B = MULTIHCONJ(A) is equivalent to B = MULTIHCONJ(A, DIM), where
%    DIM = 1.
%
%    B = MULTIHCONJ(A, DIM) is equivalent to
%    B = PERMUTE(A, [1:DIM-1, DIM+1, DIM, DIM+2:NDIMS(A)]), where A is an
%    array containing N P-by-Q matrices along its dimensions DIM and DIM+1,
%    and B is an array containing the Q-by-P Hermitian conjugate (') of
%    those N matrices along the same dimensions. N = NUMEL(A) / (P*Q), i.e.
%    N is equal to the number of elements in A divided by the number of
%    elements in each matrix.
%
%
%    Example:
%       A 5-by-9-by-3-by-2 array may be considered to be a block array
%       containing ten 9-by-3 matrices along dimensions 2 and 3. In this
%       case, its size is so indicated:  5-by-(9-by-3)-by-2 or 5x(9x3)x2.
%       If A is ................ a 5x(9x3)x2 array of 9x3 matrices,
%       C = MULTIHCONJ(A, 2) is a 5x(3x9)x2 array of 3x9 matrices.
%
%    See also MULTITRANSP MULTIHERM.

% This file is part of Manopt: www.manopt.org.
% Original author: Hiroyuki Sato, April 27, 2015.
% Contributors:
% Change log:

    % Setting DIM if not supplied.
    if nargin == 1, dim = 1; end

    % Transposing
    b = multitransp(a, dim);

    %Conjugating
    b = conj(b);

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