function value = r8_gmit ( a, x, algap1, sgngam, alx )

%*****************************************************************************80
%
%% R8_GMIT: Tricomi's incomplete gamma function for small X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real A, the parameter.
%
%    Input, real X, the argument.
%
%    Input, real ALGAP1, the logarithm of Gamma ( A + 1 ).
%
%    Input, real SGNGAM, the sign of Gamma ( A + 1 ).
%
%    Input, real ALX, the logarithm of X.
%
%    Output, real VALUE, the Tricomi incomplete gamma function.
%
  persistent bot
  persistent eps

  if ( isempty ( eps ) )
    eps = 0.5 * r8_mach ( 3 );
    bot = log ( r8_mach ( 1 ) );
  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_GMIT - Fatal error!\n' );
    fprintf ( 1, '  X <= 0.\n' );
    error ( 'R8_GMIT - Fatal error!' )
  end

  if ( a < 0.0 )
    ma = r8_aint ( a - 0.5 );
  else
    ma = r8_aint ( a + 0.5 );
  end

  aeps = a - ma;

  if ( a < - 0.5 )
    ae = aeps;
  else
    ae = a;
  end

  t = 1.0;
  te = ae;
  s = t;
  converged = 0;
  for k = 1 : 200
    fk = k;
    te = - x * te / fk;
    t = te / ( ae + fk );
    s = s + t;
    if ( abs ( t ) < eps * abs ( s ) )
      converged = 1;
      break
    end
  end

  if ( ~ converged )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_GMIT - Fatal error!\n' );
    fprintf ( 1, '  No convergence in 200 iterations.\n' );
    error ( 'R8_GMIT - Fatal error!' )
  end

  if ( - 0.5 <= a )
    algs = - algap1 + log ( s );
    value = exp ( algs );
    return
  end

  algs = - r8_lngam ( 1.0 + aeps ) + log ( s );
  s = 1.0;
  m = - ma - 1;
  t = 1.0;
  for k = 1 : m
    t = x * t / ( aeps - ( m + 1 - k ) );
    s = s + t;
    if ( abs ( t ) < eps * abs ( s ) )
      break
    end
  end

  value = 0.0;
  algs = - ma * log ( x ) + algs;

  if ( s == 0.0 || aeps == 0.0 )
    value = exp ( algs );
    return
  end

  sgng2 = sgngam * r8_sign ( s );
  alg2 = - x - algap1 + log ( abs ( s ) );

  if ( bot < alg2 )
    value = sgng2 * exp ( alg2 );
  end

  if ( bot < algs )
    value = value + exp ( algs );
  end

  return
end


function pass = test_composition_operators( pref )
% Check that composition operations are working.

if ( nargin == 0 )
    pref = chebfunpref;
end

tol = 1e3*pref.cheb2Prefs.chebfun2eps;

f = diskfun(@(x,y) cos(x.*y) + sin(x.*(y)) );

% Multiplication
x = diskfun(@(x,y) x);
y = diskfun(@(x,y) y);
exact = @(x,y) (cos(x.*y) + sin(x.*y)).*sin((x-.1).*(y+.4));
g = diskfun(@(x,y) exact(x,y));
pass(1) = ( norm( g - f.*sin((x-.1).*(y+.4)) ) < tol );

% Cosine
exact = @(x,y) cos(cos(x.*y) + sin(x.*y));
g = diskfun(@(x,y) exact(x,y));
pass(2) = ( norm( g - cos(f) ) < tol );

% Cosh
exact = @(x,y) cosh( cos(x.*y) + sin(x.*y) );
g = diskfun(@(x,y) exact(x,y));
pass(3) = ( norm( g - cosh(f) ) < tol );

% Sine
exact = @(x,y)  sin(cos(x.*y) + sin(x.*y));
g = diskfun(@(x,y,z) exact(x,y));
pass(4) = ( norm( g - sin(f) ) < tol );

% Sinh
exact = @(x,y) sinh( cos(x.*y) + sin(x.*y));
g = diskfun(@(x,y) exact(x,y));
pass(5) = ( norm( g - sinh(f) ) < tol );

% Multiple operations:
f = diskfun(@(x,y)  sin(pi*x.*y));
pass(6) = (norm(f+f+f-3*f) < 100*tol);
pass(7) = (norm(f.*f-f.^2) < tol);

% Composition with a CHEBFUN with one column:
f = diskfun(@(x,y) x + y);
g = chebfun(@(t) t.^2, [-2, 2]);
h = compose(f, g);
h_true = diskfun(@(x,y) (x + y).^2);
pass(8) = ( norm(h - h_true) < tol );

% ... and a CHEBFUN with two columns:
G = chebfun(@(t) [t.^2, exp(t)], [-2, 2]);
H = compose(f, G);
H_true = diskfunv(@(x,y) (x + y).^2, @(x,y) exp(x + y));
pass(9) = ( norm(H - H_true) < tol );

% Composition with a CHEBFUN2 and a CHEBFUN2V:
f = diskfun(@(x,y) x + y);
g = chebfun2(@(x,y) x.^2 + y.^2, [-2, 2, -2, 2]);
h = compose(f, g);
h_true = diskfun(@(x,y) (x + y).^2);
pass(10) = ( norm(h - h_true) < tol );

H = compose(f, [g; g]);
H_true = [h_true; h_true];
pass(11) = ( norm(H - H_true) < tol );

end


function [VAR, VARopt] = VARmodel(ENDO,nlag,const,EXOG,nlag_ex)
% =======================================================================
% Perform vector autogressive (VAR) estimation with OLS
% =======================================================================
% [VAR, VARopt] = VARmodel(ENDO,nlag,const,EXOG,nlag_ex)
% -----------------------------------------------------------------------
% INPUT
%	- ENDO: an (nobs x nvar) matrix of y-vectors
%	- nlag: lag length
% -----------------------------------------------------------------------
% OPTIONAL INPUT
%	- const: 0 no constant; 1 constant; 2 constant and trend; 3 constant,
%       trend, and trend^2 [dflt = 0]
%	- EXOG: optional matrix of variables (nobs x nvar_ex)
%	- nlag_ex: number of lags for exogeonus variables [dflt = 0]
% -----------------------------------------------------------------------
% OUTPUT
%   - VAR: structure including VAR estimation results
%   - VARopt: structure including VAR options (see VARoption)
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

% Note: this code is a modified version of of the vare.m function of James
% P. LeSage

% Representation -->  Y = Y(-1)*F' + u

% Note: compared to Eviews, there is a difference in the estimation of the
% constant when lag is > 2. This is because Eviews initialize the trend
% with the number of lags (i.e., when lag=2, the trend is [2 3 ...T]),
% while VARmakexy.m initialize the trend always with 1.

% I thank Jan Capek for spotting and addressing a compatibility issue with
% Matlab R2014a

%% Check inputs
%===============================================
[nobs, nvar] = size(ENDO);

% Create VARopt and update it
VARopt = VARoption;
VAR.ENDO = ENDO;
VAR.nlag = nlag;

% Check if ther are constant, trend, both, or none
if ~exist('const','var')
    const = 1;
end
VAR.const = const;

% Check if there are exogenous variables
if exist('EXOG','var')
    [nobs2, nvar_ex] = size(EXOG);
    % Check that ENDO and EXOG are conformable
    if (nobs2 ~= nobs)
        error('var: nobs in EXOG-matrix not the same as y-matrix');
    end
    clear nobs2
    % Check if there is lag order of EXOG, otherwise set it to 0
    if ~exist('nlag_ex','var')
        nlag_ex = 0;
    end
    VAR.EXOG = EXOG;
else
    nvar_ex = 0;
    nlag_ex = 0;
    VAR.EXOG = [];
end

%% Save some parameters and create data matrices
%===============================================
    nobse         = nobs - max(nlag,nlag_ex);
    VAR.nobs      = nobse;
    VAR.nvar      = nvar;
    VAR.nvar_ex   = nvar_ex;
    VAR.nlag      = nlag;
    VAR.nlag_ex   = nlag_ex;
    ncoeff        = nvar*nlag;
    VAR.ncoeff    = ncoeff;
    ncoeff_ex     = nvar_ex*(nlag_ex+1);
    ntotcoeff     = ncoeff + ncoeff_ex + const;
    VAR.ntotcoeff = ntotcoeff;
    VAR.const     = const;

% Create independent vector and lagged dependent matrix
[Y, X] = VARmakexy(ENDO,nlag,const);

% Create (lagged) exogeanous matrix
if nvar_ex>0
    X_EX  = VARmakelags(EXOG,nlag_ex);
    if nlag == nlag_ex
        X = [X X_EX];
    elseif nlag > nlag_ex
        diff = nlag - nlag_ex;
        X_EX = X_EX(diff+1:end,:);
        X = [X X_EX];
    elseif nlag < nlag_ex
        diff = nlag_ex - nlag;
        Y = Y(diff+1:end,:);
        X = [X(diff+1:end,:) X_EX];
    end
end

%% OLS estimation equation by equation
%===============================================

for j=1:nvar;
    Yvec = Y(:,j);
    OLSout = OLSmodel(Yvec,X,0);
    aux = ['eq' num2str(j)];
    eval( ['VAR.' aux '.beta  = OLSout.beta;'] );  % bhats
    eval( ['VAR.' aux '.tstat = OLSout.tstat;'] ); % t-stats
    % compute t-probs
    tstat = zeros(ncoeff,1);
    tstat = OLSout.tstat;
    tout = tdis_prb(tstat,nobse-ncoeff);
    eval( ['VAR.' aux '.tprob = tout;'] );        % t-probs
    eval( ['VAR.' aux '.resid = OLSout.resid;'] );% resids
    eval( ['VAR.' aux '.yhat  = OLSout.yhat;'] ); % yhats
    eval( ['VAR.' aux '.y     = Yvec;'] );        % actual y
    eval( ['VAR.' aux '.rsqr  = OLSout.rsqr;'] ); % r-squared
    eval( ['VAR.' aux '.rbar  = OLSout.rbar;'] ); % r-adjusted
    eval( ['VAR.' aux '.sige  = OLSout.sige;'] ); % standard error
end

%% Compute the matrix of coefficients & VCV
%===============================================
Ft = (X'*X)\(X'*Y);
VAR.Ft = Ft;
SIGMA = (1/(nobse-ntotcoeff))*(Y-X*Ft)'*(Y-X*Ft); % adjusted for # of estimated coeff per equation
VAR.sigma = SIGMA;
VAR.residuals = Y - X*Ft;
VAR.X = X;
VAR.Y = Y;
if nvar_ex > 0
    VAR.X_EX = X_EX;
end

%% Companion matrix of Ft' and max eigenvalue
%===============================================
F = Ft';
Fcomp = [F(:,1+const:nvar*nlag+const); eye(nvar*(nlag-1)) zeros(nvar*(nlag-1),nvar)];
VAR.Fcomp = Fcomp;
VAR.maxEig = max(abs(eig(Fcomp)));

%% Initialize other results
%===============================================
VAR.invA = [];  % inverse of teh A matrix (need identification: see VARir/VARfevd)
VAR.S    = [];  % Orthonormal matrix (need identification: see SR)


classdef KalmanPredictorX < PredictorX
% KalmanPredictorX class
%
% Summary of KalmanPredictorX:
% This is a class implementation of a standard Kalman Predictor.
%
% KalmanPredictorX Methods:
%   + KalmanPredictorX  - Constructor method
%   + predict - Performs full KF prediction step (both state and measurement)
%   + predictState - Performs KF state prediction step
%   + predictMeasurement - Preforms KF measurement prediction step
%
% (+) denotes puplic properties/methods
%
% See also DynamicModelX, ObservationModelX and ControlModelX template classes

    methods (Static)

        function [xPred, PPred, yPred, S, Pxy] = predict(x,P,F,Q,H,R,u,B,O)
        % predict Perform the discrete-time KF state and measurement
        % prediction steps, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % x: column vector
        %   The (xDim x 1) state estimate at the previous time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the previous
        %   time-step.
        % F: matrix
        %   An (xDim x xDim) state transition matrix.
        % Q: matrix
        %   The (xDim x xDim) process noise covariance matrix.
        % H: matrix
        %   A (xDim x yDim) measurement matrix.
        % R: matrix
        %   The (yDim x yDim) measurement noise covariance matrix.
        % u: column vector, optional
        %   A optional (xDim x 1) control input.
        %   If omitted, no control input is used.
        % B: matrix, optional
        %   An optional (xDim x xDim) control gain matrix.
        %   If omitted, B is assumed to be 1.
        % O: matrix, optional
        %   An optional (xDim x xDim) control noise covariance
        %   matrix. If omitted, Q is assumed to be 0.
        %
        % Returns
        % -------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % Pxy: matrix
        %   The (xDim x yDim) cross-covariance matrix.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            switch(nargin)
                case(6)
                    u = 0;
                    B = 0;
                    O = 0;
                case(7)
                    B = 1;
                    O = 0;
                case(8)
                    O = 0;
            end

           [xPred, PPred] = KalmanPredictorX.predictState(x,P,F,Q,u,B,O);
           [yPred, S, Pxy] = KalmanPredictorX.predictMeasurement(xPred,PPred,H,R);
        end

        function [xPred, PPred] = predictState(x,P,F,Q,u,B,Qu)
        % predictState Perform the discrete-time KF state prediction
        % step, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % x: column vector
        %   The (xDim x 1) state estimate at the previous time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the previous
        %   time-step.
        % F: matrix
        %   An (xDim x xDim) state transition matrix.
        % Q: matrix
        %   The (xDim x xDim) process noise covariance matrix.
        % u: column vector, optional
        %   An optional (xDim x 1) control input.
        %   If omitted, no control input is used.
        % B: matrix, optional
        %   An optional (xDim x xDim) control gain matrix.
        %   If omitted, B is assumed to be 1.
        % O: matrix, optional
        %   An optional (xDim x xDim) control noise covariance
        %   matrix. If omitted, Q is assumed to be 0.
        %
        % Returns
        % -------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            switch(nargin)
                case(4)
                    u  = 0;
                    B  = 0;
                    Qu = 0;
                case(5)
                    B  = 1;
                    Qu = 0;
                case(6)
                    Qu = 0;
            end

            % Compute predicted state mean and covariance
            xPred = F*x + B*u;
            PPred =F*P*F' + Q + B*Qu*B';
        end

        function [yPred, S, Pxy] = predictMeasurement(xPred,PPred,H,R)
        % predictMeasurement Perform the discrete-time KF observation prediction
        % step, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate at the current
        %   time-step.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix at
        %   the current time-step.
        % H: matrix
        %   An (xDim x yDim) measurement matrix.
        % R: matrix
        %   The (yDim x yDim) measurement noise covariance matrix.
        %
        % Returns
        % -------
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % Pxy: matrix
        %   The (xDim x yDim) cross-covariance matrix.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Compute predicted measurement mean and covariance
            yPred   = H*xPred;
            Pxy     = PPred*H';
            S       = H*PPred*H' + R;
        end
    end
end


close all; clear all;

%% Setting of the problem
global s
s = 0.3;
maxIt = 4;
pde = fonedata;
pde.s = s;
pde.L = 1;
option.solver = 'direct';
% option.solver = 'amg';
option.gNquadorder = 4;
[node,elem] = squaremesh([-1,1,-1,1],2);
% [node,elem] = delmesh(node,elem,'x>0 & y<0');

%% Finite element approximation
N = zeros(maxIt,1);
energy = zeros(maxIt,1);
for k = 1:maxIt
    [node,elem] = uniformrefine(node,elem);
    tic;
    [uh,eqn] = fracLapP1P1(node,elem,pde,option);
    Nv = size(node,1);
    figure(1); showresult(node,elem,uh(1:Nv));
    pause(0.1)
    N(k) = length(uh);
    energy(k) = (uh'*eqn.A*uh)/2 - sum(eqn.b(1:Nv).*uh(1:Nv));
    fprintf('#Dof %d   ',N(k));
    toc;
end
[node,elem] = uniformrefine(node,elem);
tic;
[uh,eqn,info] = fracLapP1P1(node,elem,pde,option);
toc;
display(length(uh));
fineEnergy = (uh'*eqn.A*uh)/2 - sum(eqn.b.*uh);

%% Plot convergence rates
energyError = sqrt(2*(energy(1:k)-fineEnergy));
figure(2)
showrate(N(1:k),energyError(1:k),1,'r-*','Energy Error');

%% Display error
colname = {' #Dof   sqrt(2*(E(uh)-E(u)))'};
disptable(colname,N,[],energyError,'%0.5e')


function window_sz = get_search_window_test( target_sz, im_sz)
% GET_SEARCH_WINDOW

% if(target_sz(1)/target_sz(2) > 2)
%     % For objects with large height, we restrict the search window with padding.height
%     window_sz = floor(target_sz.*[1+padding.height, 1+padding.generic]);
%
% elseif(prod(target_sz)/prod(im_sz(1:2)) > 0.05)
%     % For objects with large height and width and accounting for at least 10 percent of the whole image,
%     % we only search 2x height and width
%     window_sz=floor(target_sz*(1+padding.large));
%
% else
%     %otherwise, we use the padding configuration
%     window_sz = floor(target_sz * (1 + padding.generic));

ratio=target_sz(1)/target_sz(2);
if ratio>1
    window_sz=round(target_sz.*[2,2*ratio]);
else
    window_sz=round(target_sz.*[2/ratio,2]);
end
%
% %window_sz=round(target_sz.*[5,9]);
window_sz=window_sz-mod(window_sz,2)+1;

end


function Js = JacobianSpace_sym(Slist, thetalist)
% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***
% Takes Slist: The joint screw axes in the space frame when the manipulator
%              is at the home position, in the format of a matrix with the
%              screw axes as the columns,
%       thetalist: A list of joint coordinates.
% Returns the corresponding space Jacobian (6xn real numbers).
% Example Input:
%
% clear; clc;
% Slist = [[0; 0; 1;   0; 0.2; 0.2], ...
%        [1; 0; 0;   2;   0;   3], ...
%        [0; 1; 0;   0;   2;   1], ...
%        [1; 0; 0; 0.2; 0.3; 0.4]];
% thetalist = [0.2; 1.1; 0.1; 1.2];
% Js = JacobianSpace(Slist, thetalist)
%
% Output:
% Js =
%         0    0.9801   -0.0901    0.9575
%         0    0.1987    0.4446    0.2849
%    1.0000         0    0.8912   -0.0453
%         0    1.9522   -2.2164   -0.5116
%    0.2000    0.4365   -2.4371    2.7754
%    0.2000    2.9603    3.2357    2.2251

Js = sym(Slist);
T = eye(4);
for i = 2: length(thetalist)
    T = T * simplify(MatrixExp6_sym(VecTose3(Slist(:, i - 1)),thetalist(i - 1)));
	Js(:, i) = Adjoint(T) * Slist(:, i);
end
end


function szmM = calcSZM(quantizedM, nL, szmType)
% function szmM = calcSZM(quantizedM, nL, szmType)
%
% This function calculates the Size-Zone matrix for the passed quantized
% image.
%
% INPUTS:
%       quantizedM: quantized 3d matrix obtained, for example, by
%       imquantize_cerr.m
%       nL: Number of gray levels.
%       szmType: flag, 1 or 2.
%                   1: 3D zones
%                   2: 2D zones
% OUTPUT:
%       szmM: size-zone matrix of size (nL x L)
%
% EXAMPLE:
%
% numRows = 10;
% numCols = 10;
% numSlcs = 1;
%
% % number of gray levels
% nL = 3;
%
% % create an image with random numbers
% imgM = randi(nL,numRows,numCols,numSlcs);
%
% % set option to add run lengths from all directions
% szmType = 1;
%
% % call the rlm calculator
% szmType = calcSZM(imgM, nL, szmType);
%
%
% APA, 03/30/2017

if szmType == 1
    numNeighbors = 26;
else
    numNeighbors = 8;
end

szmM = sparse(nL,numel(quantizedM));
maxSiz = 0;
for level = 1:nL
    connM = bwlabeln(quantizedM==level, numNeighbors);
    if any(connM(:))
        regiosSizV = accumarray(connM(connM > 0),1);
        if ~isempty(regiosSizV)
            maxSiz = max(maxSiz,max(regiosSizV));
        end
        szmM(level,:) = accumarray(regiosSizV,1,[size(szmM,2) 1])';
    end
end
szmM = szmM(:,1:maxSiz);


function [k, sk, n2] = rbfperiodicKernCompute(kern, x, x2)

% RBFPERIODICKERNCOMPUTE Compute the RBFPERIODIC kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the RBF derived periodic
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the RBF derived periodic
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : rbfperiodicKernParamInit, kernCompute, kernCreate, rbfperiodicKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2007, 2009

% KERN
if isfield(kern, 'period')
  factor = 2*pi/kern.period;
else
  factor = 1;
end
if nargin < 3
  n2 = sin(0.5*factor*(repmat(x, 1, size(x, 1)) - repmat(x', size(x, 1), 1)));
  n2 = n2.*n2;
  wi2 = (2 .* kern.inverseWidth);
  sk = exp(-n2*wi2);
else
  n2 = sin(0.5*factor*(repmat(x, 1, size(x2, 1)) - repmat(x2', size(x, 1), 1)));
  n2 = n2.*n2;
  wi2 = (2 .* kern.inverseWidth);
  sk = exp(-n2*wi2);
end
k = kern.variance*sk;


function fd1d_advection_ftcs ( )

%*****************************************************************************80
%
%% FD1D_ADVECTION_FTCS solves the advection equation using the FTCS method.
%
%  Discussion:
%
%    The FTCS method is unstable for the advection problem.
%
%    Given a smooth initial condition, successive FTCS approximations will
%    exhibit erroneous oscillations of increasing magnitude.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_ADVECTION_FTCS:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the constant-velocity advection equation in 1D,\n' );
  fprintf ( 1, '    du/dt = - c du/dx\n' );
  fprintf ( 1, '  over the interval:\n' );
  fprintf ( 1, '    0.0 <= x <= 1.0\n' );
  fprintf ( 1, '  with periodic boundary conditions, and\n' );
  fprintf ( 1, '  with a given initial condition\n' );
  fprintf ( 1, '    u(0,x) = (10x-4)^2 (6-10x)^2 for 0.4 <= x <= 0.6\n' );
  fprintf ( 1, '           = 0 elsewhere.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We use a method known as FTCS:\n' );
  fprintf ( 1, '   FT: Forward Time  : du/dt = (u(t+dt,x)-u(t,x))/dt\n' );
  fprintf ( 1, '   CS: Centered Space: du/dx = (u(t,x+dx)-u(t,x-dx))/2/dx\n' );

  nx = 101;
  dx = 1.0 / ( nx - 1 );
  x = linspace ( 0.0, 1.0, nx );
  nt = 1000;
  dt = 1.0 / nt;
  c = 1.0;

  u = zeros ( 1, nx );
  i = find ( 0.4 <= x & x <= 0.6 );
  u(i) = ( 10.0 * x(i) - 4.0 ).^2 .* ( 6.0 - 10.0 * x(i) ).^2;

  iplot = 1;
  uplot(iplot,:) = u(:)';
  tplot(iplot) = 0.0;
  plotstep = ceil ( nt / 50 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes NX = %d\n', nx );
  fprintf ( 1, '  Number of time steps NT = %d\n', nt );
  fprintf ( 1, '  Constant velocity C = %g\n', c );

  im1 = [ nx, 1:nx-2, nx-1 ];
  i   = [ 1,  2:nx-1, nx   ];
  ip1 = [ 2,  3:nx,   1    ];

  for j = 1 : nt

    unew(i) = u(i) - c * dt / dx / 2.0 * ( u(ip1) - u(im1) );
    u(i) = unew(i);

    if ( rem ( j, plotstep ) < 1 )
      iplot = iplot + 1;
      uplot(iplot,:) = u(:)';
      tplot(iplot) = j * dt;
    end

  end
%
%  Plot.
%
  mesh ( x, tplot, uplot );
  xlabel ( '<--X-->' );
  ylabel ( '<--T-->');
  title ( 'U(X,T)');

  filename = 'fd1d_advection_ftcs.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot as "%s".\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_ADVECTION_FTCS\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function [q num den]= get_q_RANSAC(N, N_I, k)

% [q num den] = get_q_RANSAC(N, N_I, k)
%
% DESC:
% calculates the probability q
%
% AUTHOR
% Marco Zuliani - marco.zuliani@gmail.com
%
% VERSION:
% 1.0
%
% INPUT:
% N                 = number of elements
% N_I               = number of inliers
% k                 = cardinality of the MSS
%
% OUTPUT:
% q                 = probability
% num, den          = q = num / den

if (k > N_I)
    error('k should be less or equal than N_I')
end;

if (N == N_I)
    q = 1;
    return;
end;

num = N_I:-1:N_I-k+1;
den = N:-1:N-k+1;
q = prod(num./den);

return


classdef ImplicitEquationSolver < handle

    properties (Access = public)
        xsol
    end

    properties (Access = private)
        x0
        functionToSolve
    end

    methods (Access = public)

        function obj =  ImplicitEquationSolver(cParams)
            obj.init(cParams)
        end

        function r = solve(obj)
            F = obj.functionToSolve;
            [rub,rlb] = obj.findRbounds(F);
            r = fzero(F,[rlb,rub]);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.x0 = cParams.x0;
            obj.functionToSolve = cParams.functionToSolve;
        end

        function [rub,rlb] = findRbounds(obj,F)
            r0 = 1;
            F0 = F(r0);
            eps = 10^(-6);
            if F0 >= 0
                r1 = 1 + eps;
                F1 = F(r1);
                while F1 >= 0
                    rnew = obj.newPointBySecant(r0,r1,F0,F1);
                    r0 = r1;
                    F0 = F1;
                    r1 = rnew;
                    F1 = F(r1);
                end
                rub = r1;
                rlb = r0;
            else
                r0 = 1;
                r1 = 1 - eps;
                F1 = F(r1);
                while F1 <= 0
                    rnew = obj.newPointBySecant(r0,r1,F0,F1);
                    r0 = r1;
                    F0 = F1;
                    r1 = max(1e-12,rnew);
                    F1 = F(r1);
                end
                rub = r0;
                rlb = r1;
            end
        end

    end

    methods (Access = private, Static)

        function x2 = newPointBySecant(x0,x1,f0,f1)
            x2 = x1 - (x1-x0)/(f1 - f0)*f1;
        end

    end

end


% Exact solution of Riemann problem

% Theory in Section 10.2 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program is called by Euler schemes

% Functions called: f

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

gammab = 1/(gamma - 1); gam1 = gamma-1;

% Assumed structure of exact solution
%
%    \         /      |con |       |s|
%     \   f   /       |tact|       |h|
% left \  a  /  state |disc| state |o| right
% state \ n /    2    |cont|   3   |c| state
%   1    \ /          |tinu|       |k|   4
%         |           |ity |       | |

PRL = pright/pleft;
cright = sqrt(gamma*pright/rhoright); cleft = sqrt(gamma*pleft/rholeft);
CRL = cright/cleft;
MACHLEFT = (uleft - uright)/cleft;

p34 = fzero('f',3);		% p34 = p3/p4
p3 = p34*pright; 	alpha = (gamma+1)/(gamma-1);
rho3 = rhoright*(1+alpha*p34)/(alpha+p34);
rho2 = rholeft*(p34*pright/pleft)^(1/gamma);
u2 = uleft-uright+(2/(gamma-1))*cleft*...
	(1-(p34*pright/pleft)^((gamma-1)/(2*gamma)));
c2 = sqrt(gamma*p3/rho2);
spos = 0.5 + ...		% Shock position
	tend*cright*sqrt((gamma-1)/(2*gamma) + (gamma+1)/(2*gamma)*p34)+...
	tend*uright;

conpos = 0.5 + u2*tend + tend*uright;	% Position of contact discontinuity
pos1 = 0.5 + (uleft - cleft)*tend;	% Start of expansion fan
pos2 = 0.5 + (u2+uright-c2)*tend;	% End of expansion fan
xx = 0:0.002:1;
pexact = zeros(size(xx)); uexact= zeros(size(xx)); rhoexact = zeros(size(xx));
machexact = zeros(size(xx));  cexact = zeros(size(xx));
for i = 1:length(xx)
  if xx(i) <= pos1
    pexact(i) = pleft;    rhoexact(i) = rholeft;
    uexact(i) = uleft;    cexact(i)   = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= pos2
    pexact(i) = pleft*(1+(pos1-xx(i))/(cleft*alpha*tend))^(2*gamma/(gamma-1));
    rhoexact(i) = rholeft*(1+(pos1-xx(i))/(cleft*alpha*tend))^(2/(gamma-1));
    uexact(i) = uleft + (2/(gamma+1))*(xx(i)-pos1)/tend;
    cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= conpos
    pexact(i) = p3;    	      rhoexact(i) = rho2;
    uexact(i) = u2+uright;    cexact(i)   = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= spos
    pexact(i) = p3;    rhoexact(i) = rho3;    uexact(i) = u2+uright;
    cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  else
    pexact(i) = pright;    rhoexact(i) = rhoright;
    uexact(i) = uright;    cexact(i)   = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  end
end
entroexact = log(pexact./rhoexact.^gamma);

subplot(2,3,1),     	plot(xx,rhoexact)
subplot(2,3,2),    	plot(xx,uexact)
subplot(2,3,3),    	plot(xx,pexact)
subplot(2,3,4),  	plot(xx,machexact)
subplot(2,3,5),     	plot(xx,entroexact)


% h = drawfly(trk,varargin)
% h = drawfly(x,y,theta,quartermaj,quartermin,varargin)
% draw triangle corresponding to fly position
% extra arguments will be fed to patch
function varargout = drawfly(x,y,varargin)

% draw an isosceles triangle with center (x,y)
% with rotation theta
% with height maj*4
% with base min*4

if isstruct(x),
  fly = x;
  t = y;
  x = fly.x(t);
  y = fly.y(t);
  theta = fly.theta(t);
  maj = fly.a(t);
  min = fly.b(t);
else
  if nargin < 5,
    error('not enough arguments; usage: drawflyo(x,y,theta,a,b,...)');
  end
  theta = varargin{1};
  maj = varargin{2};
  min = varargin{3};
  varargin = varargin(4:end);
end

if 0,

h = ellipsedraw(maj*2,min*2,x,y,theta);

else

% isosceles triangle not yet rotated or centered
pts = [-maj*2,-min*2,
       -maj*2,min*2,
       maj*2,0];

% rotate
costheta = cos(theta);
sintheta = sin(theta);
R = [costheta,sintheta;-sintheta,costheta];
pts = pts*R;

% translate
pts(:,1) = pts(:,1) + x;
pts(:,2) = pts(:,2) + y;

% plot
if isempty(varargin),
  h = patch(pts([1:3,1],1),pts([1:3,1],2),'b');
else
  h = patch(pts([1:3,1],1),pts([1:3,1],2),varargin{:});
end

end

if nargout > 0,
  varargout{1} = h;
end;


% At_fhp.m
%
% Adjoint of At_fhp (2D Fourier half plane measurements).
%
% Usage: x = At_fhp(b, OMEGA, n)
%
% b - K vector = [mean; real part(OMEGA); imag part(OMEGA)]
%
% OMEGA - K/2-1 vector denoting which Fourier coefficients to use
%         (the real and imag parts of each freq are kept).
%
% n - Image is nxn pixels
%
% x - N vector
%
% Written by: Justin Romberg, Caltech
% Created: October 2005
% Email: jrom@acm.caltech.edu
%

function x = At_fhp(y, OMEGA, n)

K = length(y);

fx = zeros(n,n);
fx(1,1) = y(1);
fx(OMEGA) = sqrt(2)*(y(2:(K+1)/2) + i*y((K+3)/2:K));
x = reshape(real(n*ifft2(fx)), n*n, 1);


function x = gpml_randn(seed,varargin)

% Generate pseudo-random numbers in a quick and dirty way.
% The function makes sure, we obtain the same random numbers using Octave and
% Matlab for the demo scripts.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch 2010-07-07.

if nargin==2
  sz = varargin{1};
else
  sz = zeros(1,nargin-1);
  for i=1:nargin-1
    sz(i) = varargin{i};
  end
end

n = prod(sz); N = ceil(n/2)*2;

% minimal uniform random number generator for uniform deviates from [0,1]
% by Park and Miller
a = 7^5; m = 2^31-1;
% using Schrage's algorithm
q = fix(m/a); r = mod(m,a); % m = a*q+r
u = zeros(N+1,1); u(1) = fix(seed*2^31);
for i=2:N+1
  % Schrage's algorithm for mod(a*u(i),m)
  u(i) = a*mod(u(i-1),q) - r*fix(u(i-1)/q);
  if u(i)<0, u(i) = u(i)+m; end
end
u = u(2:N+1)/2^31;

% Box-Muller transform: Numerical Recipies, 2nd Edition, $7.2.8
% http://en.wikipedia.org/wiki/Box-Muller_transform
w = sqrt(- 2*log(u(1:N/2)));                             % split into two groups
x = [w.*cos(2*pi*u(N/2+1:N)); w.*sin(2*pi*u(N/2+1:N))];
x = reshape(x(1:n),sz);


function m_demo(index)
% M_DEMO  Demonstration program showing various maps in M_Map package
%         Dig into this to look for examples of things you want to do.
%
%         M_DEMO runs all the demos.
%         M_DEMO(NUM) runs examples NUM (1<=NUM<=10), pausing between examples.
%
%         Some demos may require you to install GSHHS or TerrainBase datafiles
%         (see documentation)

% Rich Pawlowicz (rich@ocgy.ubc.ca) 7/May/1997
% (thanks to Art Newhall for putting these examples into an m-file,
% and the Chuck Denham for enhancing the interface)
%
% 27/July/98 - more examples.
% 17/Aug/98     "
% 15/Nov/98  - another example, better interface.
% 23/Dec/98  - another example.

%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.

global MAP_PROJECTION

N_EXAMPLES=15;

if nargin==0
 index=1:N_EXAMPLES;
end

for i=index

clf;
switch i

  case 1

    m_proj('ortho','lat',48','long',-123');
    m_coast('patch','r');
    m_grid('linestyle','-','xticklabels',[],'yticklabels',[],'ytick',[-80:40:80]);
    xlabel('Orthographic Projection','visible','on');

  case 2

    m_proj('lambert','long',[-160 -40],'lat',[30 80]);
    m_coast('patch',[1 .85 .7]);
    [CS,CH]=m_elev('contourf',[500:500:4000]);
 %   m_elev('pcolor');
    m_grid('box','fancy','tickdir','in');
    colormap(flipud(copper));
    xlabel('Conic Projection of North America with elevations','visible','on');
    m_contfbar([0 .3],.9,CS,CH);

  case 3

    m_proj('stereographic','lat',90,'long',30,'radius',25);
    m_elev('contour',[-3500:1000:-500],'linecolor','b');
    m_grid('xtick',12,'tickdir','out','ytick',[70 80],'linestyle','-');
    m_coast('patch',[.7 .7 .7],'edgecolor','r');
    xlabel('Polar Stereographic Projection with bathymetry','visible','on');

  case 4

    subplot(211);
    Slongs=[-100 0;-75 25;0  45; 25 145;45 100;145 295;100 295];
    Slats= [  8 80;-80  8; 8 80;-80   8; 8  80;-80   0;  0  80];
    for l=1:7
     m_proj('sinusoidal','long',Slongs(l,:),'lat',Slats(l,:));
   %  colormap(m_colmap('blues'));caxis([-6000 0]);
   %  m_elev('shadedrelief');
     m_grid('fontsize',6,'xticklabels',[],'xtick',[-180:30:360],...
            'ytick',[-80:20:80],'yticklabels',[],'linestyle','-','color',[.9 .9 .9]);
     m_coast('patch','g');
    end
    xlabel('Interrupted Sinusoidal Projection of World Oceans');
    % In order to see all the maps we must undo the axis limits set by m_grid calls:
    set(gca,'xlimmode','auto','ylimmode','auto');

    subplot(212);
    Slongs=[-100 43;-75 20; 20 145;43 100;145 295;100 295];
    Slats= [  0  90;-90  0;-90   0; 0  90;-90   0;  0  90];
    for l=1:6
     m_proj('mollweide','long',Slongs(l,:),'lat',Slats(l,:));
   %  colormap(m_colmap('blues'));caxis([-6000 0]);
   %  m_elev('shadedrelief');
     m_grid('fontsize',6,'xticklabels',[],'xtick',[-180:30:360],...
            'ytick',[-80:20:80],'yticklabels',[],'linestyle','-','color','k');

     m_coast('patch',[.6 .6 .6]);
    end
    xlabel('Interrupted Mollweide Projection of World Oceans');
    set(gca,'xlimmode','auto','ylimmode','auto');

  case 5

    %% Nice looking data
    [lon,lat]=meshgrid([-136:2:-114],[36:2:54]);
    u=sin(lat/6);
    v=sin(lon/6);

    m_proj('oblique','lat',[56 30],'lon',[-132 -120],'aspect',.8);

    subplot(121);
    m_coast('patch',[.9 .9 .9],'edgecolor','none');
    m_grid('tickdir','out','yaxislocation','right',...
	   'xaxislocation','top','xlabeldir','end','ticklength',.02);
    hold on;
    m_quiver(lon,lat,u,v);
    xlabel('Simulated surface winds');

    subplot(122);
    m_coast('patch',[.9 .9 .9],'edgecolor','none');
    m_grid('tickdir','out','yticklabels',[],...
	   'xticklabels',[],'linestyle','none','ticklength',.02);
    hold on;
    [cs,h]=m_contour(lon,lat,sqrt(u.*u+v.*v));
    clabel(cs,h,'fontsize',8);
    xlabel('Simulated something else');

  case 6

    % Plot a circular orbit
    lon=[-180:180];
    lat=atan(tan(60*pi/180)*cos((lon-30)*pi/180))*180/pi;

    m_proj('miller','lat',82);
    m_coast('color',[0 .6 0]);
    m_line(lon,lat,'linewidth',3,'color','r');
    m_grid('linestyle','none','box','fancy','tickdir','out');

  case 7

    m_proj('lambert','lon',[-10 20],'lat',[33 48]);
    if MAP_PROJECTION.IsOctave
       [CS,CH]=m_etopo2('contourf',[-5000:500:0 250:250:3000],'linecolor','none');
    else
       [CS,CH]=m_etopo2('contourf',[-5000:500:0 250:250:3000],'edgecolor','none');
    end
    m_grid('linestyle','none','tickdir','out','linewidth',3);

    colormap([ m_colmap('blues',80); m_colmap('gland',48)]);
    brighten(.5);

    ax=m_contfbar(1,[.5 .8],CS,CH);
    title(ax,{'Level/m',''}); % Move up by inserting a blank line

  case 8

    m_vec;
    colormap(jet);

  case 9

    % Example showing the default coastline and all of the GSHHS coastlines.

    axes('position',[.35 .6 .37 .37]);
    m_proj('albers equal-area','lat',[40 60],'long',[-90 -50],'rect','on');
    m_coast('patch',[0 1 0]);
    m_grid('linestyle','none','linewidth',2,'tickdir','out','xaxisloc','top','yaxisloc','right','fontsize',6);
    m_text(-69,51,'Standard coastline','color','r','fontweight','bold');
    m_ruler([.5 .9],.8,3,'fontsize',8);
    drawnow;

    axes('position',[.09 .5 .37 .37]);
    m_proj('albers equal-area','lat',[40 54],'long',[-80 -55],'rect','on');
    m_gshhs_c('patch',[.2 .8 .2]);
    m_grid('linestyle','none','linewidth',2,'tickdir','out','xaxisloc','top','fontsize',6);
    m_text(-80,52.5,'GSHHS\_C (crude)','color','m','fontweight','bold');
    m_ruler([.5 .9],.8,2,'fontsize',8);
    drawnow;

    axes('position',[.13 .2 .37 .37]);
    m_proj('albers equal-area','lat',[43 48],'long',[-67 -58],'rect','on');
    m_gshhs_l('patch',[.4 .6 .4]);
    m_grid('linestyle','none','linewidth',2,'tickdir','out','fontsize',6);
    m_text(-66.5,43.5,'GSHHS\_L (low)','color','m','fontweight','bold');
    m_ruler([.5 .9],.8,3,'fontsize',8);
    drawnow;

    axes('position',[.35 .05 .37 .37]);
    m_proj('albers equal-area','lat',[45.8 47.2],'long',[-64.5 -62],'rect','on');
    m_gshhs_i('patch',[.5 .6 .5]);
    m_grid('linestyle','none','linewidth',2,'tickdir','out','yaxisloc','right','fontsize',6);
    m_text(-64.4,45.9,'GSHHS\_I (intermediate)   ','color','m','fontweight','bold','horizontalalignment','right');
    m_ruler([.5 .8],.1,3,'fontsize',8);
    drawnow;

    axes('position',[.5 .1 .37 .37]);
    m_proj('albers equal-area','lat',[46.375 46.6],'long',[-64.2 -63.7],'rect','on');
    m_gshhs_h('patch',[.6 .7 .6]);
    m_grid('linestyle','none','linewidth',2,'tickdir','out','xaxisloc','top','yaxisloc','right','fontsize',6);
    m_text(-64.18,46.4,'GSHHS\_H (high)','color','m','fontweight','bold');
    m_ruler([.5 .8],.2,3,'fontsize',8);
    drawnow;

    axes('position',[.55 .35 .37 .37]);
    m_proj('albers equal-area','lat',[46.55 46.65],'long',[-63.97 -63.77],'rect','on');
    m_gshhs_f('patch',[.7 .9 .7]);
    m_grid('linestyle','none','linewidth',2,'tickdir','out','xaxisloc','top','yaxisloc','right','fontsize',6);
    m_text(-63.95,46.56,'GSHHS\_F (full)','color','m','fontweight','bold');
    m_ruler([.5 .8],.2,3,'fontsize',8);

  case 10

    % Example showing a trackline plot

    clf
    m_proj('UTM','long',[-72 -68],'lat',[40 44]);
    m_gshhs_i('color','k');
    m_grid('box','fancy','tickdir','in');
    m_ruler(1.2,[.5 .8]);

    % fake up a trackline
    lons=[-71:.1:-67];
    lats=60*cos((lons+115)*pi/180);
    dates=datenum(1997,10,23,15,1:41,zeros(1,41));

    m_track(lons,lats,dates,'ticks',0,'times',4,'dates',8,...
           'clip','off','color','r','orient','upright');

  case 11

    % example showing range rings

    clf
    m_proj('hammer','clong',170);
    m_grid('xtick',[],'ytick',[],'linestyle','-');
    m_coast('patch','g');
    m_line(100.5,13.5,'marker','s','color','r');
    m_range_ring(100.5,13.5,[1000:1000:15000],'color','b','linewidth',2);
    xlabel('1000km range rings from Bangkok');

  case 12

    % Example showing speckle

    bndry_lon=[-128.8 -128.8 -128.3 -128 -126.8 -126.6 -128.8];
    bndry_lat=[49      50.33  50.33  50   49.5   49     49];

    clf;
    m_proj('lambert','long',[-130 -121.5],'lat',[47 51]);
    m_gshhs_i('color','k');
    m_gshhs_i('speckle','color','k');
    m_line(bndry_lon,bndry_lat,'linewidth',2,'color','k');     % Area outline ...
    m_hatch(bndry_lon,bndry_lat,'single',30,5,'color','k'); % ...with hatching added.

    m_grid('linewidth',2,'linestyle','none');
    title({'Speckled Boundaries','for nice B&W presentation','(best in postscript format)'});
    m_text(-128,48,{'Pacific','Ocean'},'fontsize',18);

  case 13

    % Colouring the ocean blue

    clf
    m_proj('miller','lat',[-77 77]);
 %   set(gca,'color',[.9 .99 1]);
    m_coast('patch',[.7 1 .7],'edgecolor','none');
    m_grid('box','fancy','linestyle','-','gridcolor','w','backcolor',[.2 .65 1]);

    cities={'Cairo','Washington','Buenos Aires'};
    lons=[ 30+2/60  -77-2/60   -58-22/60];
    lats=[ 31+21/60  38+53/60  -34-45/60];

    for k=1:3
      [range,ln,lt]=m_lldist([-123-6/60 lons(k)],[49+13/60  lats(k)],40);
      m_line(ln,lt,'color','r','linewidth',2);
      m_text(ln(end),lt(end),sprintf('%s - %d km',cities{k},round(range)));
    end

    % set(gcf,'color','w');  % To defeat the tendency of print to turn white into black

    title('Great Circle Routes','fontsize',12,'fontweight','bold');

    case 14
        clf
        m_proj('lambert','long',[-130 -122],'lat',[48 52.5],'rect','on');
        if MAP_PROJECTION.IsOctave
           [CS,CH]=m_elev('contourf',[-3000:500:-500 -200 -100 -50 -1 2 20 50 100 250:250:2000],'linecolor','none');
        else
           [CS,CH]=m_etopo2('contourf',[-3000:500:-500 -200 -100 -50 -1 2 20 50 100 250:250:2000],'edgecolor','none');
        end

        m_grid('linewi',2,'tickdir','out','yaxisloc','right');

        if MAP_PROJECTION.IsOctave
            ax=m_contfbar(-.03,[.5 .8],CS,CH,'linecolor','none');
        else
            ax=m_contfbar(-.03,[.5 .8],CS,CH,'edgecolor','none');
        end
        title(ax,{'meters',''}); % Move up by inserting a blank line

        colormap([m_colmap('blues',96);m_colmap('gland',64)]);
        caxis([-3000 2000]);

    case 15
        clf;
        m_proj('azimuthal equal-area','radius',156,'lat',-46,'long',-95,'rot',30);

        ax1=subplot(2,2,1,'align');
        m_coast('patch','r');
        m_grid('xticklabel',[],'yticklabel',[],'linestyle','-','ytick',[-60:30:60]);

        ax2=subplot(2,2,2,'align');
        if MAP_PROJECTION.IsOctave
            m_elev('contourf',[-7000:1000:0 500:500:3000],'linecolor','none');
        else
             m_elev('contourf',[-7000:1000:0 500:500:3000],'edgecolor','none');
        end
        colormap(ax2,[m_colmap('blues',70);m_colmap('gland',30)]);
        caxis(ax2,[-7000 3000]);
        m_grid('xticklabel',[],'yticklabel',[],'linestyle','-','ytick',[-60:30:60]);

        ax3=subplot(2,2,3,'align');
        colormap(ax3,[m_colmap('blues',70);m_colmap('gland',30)]);
        caxis(ax3,[-7000 3000]);
        m_elev('image');
        m_grid('xticklabel',[],'yticklabel',[],'linestyle','-','ytick',[-60:30:60]);

        ax4=subplot(2,2,4,'align');
        colormap(ax4,[m_colmap('blues')]);
        caxis(ax4,[-8000 000]);
        m_elev('shadedrelief','gradient',.5);
        m_coast('patch',[.7 .7 .7],'edgecolor','none');
        m_grid('xticklabel',[],'yticklabel',[],'linestyle','-','ytick',[-60:30:60]);

        ha = axes('Position',[0 0 1 1],'Xlim',[0 1],'Ylim',[0  1],'Box','off','Visible','off','Units','normalized', 'clipping' , 'off');
        text(0.5, 0.98,'This projection shows all oceans connected to each other','horiz','center','fontsize',20);

end

 if i<length(index)
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   disp('  hit return to continue');
   pause
   disp('        ...drawing');
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 end
end


function [ p2, dp2, p1 ] = gegenbauer_recur ( x, n, alpha, c )

%*****************************************************************************80
%
%% GEGENBAUER_RECUR finds the value and derivative of a Gegenbauer polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2008
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest,
%    MATLAB version by John Burkardt,
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, real X, the point at which polynomials are evaluated.
%
%    Input, integer N, the order of the polynomial to be computed.
%
%    Input, real ALPHA, the exponent of (1-X^2) in the quadrature rule.
%
%    Input, real C(N), the recursion coefficients.
%
%    Output, real P2, the value of J(N)(X).
%
%    Output, real DP2, the value of J'(N)(X).
%
%    Output, real P1, the value of J(N-1)(X).
%
  p1 = 1.0;
  dp1 = 0.0;

  p2 = x;
  dp2 = 1.0;

  for i = 2 : n

    p0 = p1;
    dp0 = dp1;

    p1 = p2;
    dp1 = dp2;

    p2 = x * p1 - c(i) * p0;
    dp2 = x * dp1 + p1 - c(i) * dp0;

  end

  return
end


function [ vX ] = ProjectL1BallDual( vY, ballRadius, vLowerBound, vUpperBound )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProjectL1Ball( vY, ballRadius, vLowerBound, vUpperBound )
%   Solving the Orthogonal Porjection Problem of the input vector onto the
%   L1 Ball with Box Constraints using Dual Function.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - ballRadius    -   Ball Radius.
%                       Sets the Radius of the L1 Ball. For Unit L1 Ball
%                       set to 1.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
%   - vLowerBound   -   Lower Bound Vector.
%                       Sets the lower bound values of the solution
%                       (Element wise).
%                       Structure: Vector.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vUpperBound   -   Upper Bound Vector.
%                       Sets the upper bound values of the solution
%                       (Element wise).
%                       Structure: Vector.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - vX            -   Output Vector.
%                       The projection of the Input Vector onto the L1
%                       Ball.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  https://math.stackexchange.com/a/2830242/33.
% Remarks:
%   1.  S
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     24/06/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

DEBUG_MODE = OFF;

paramLambda     = 0; %<! Initialization value

% Check feasibility of the problem
minSum = 0;
for ii = 1:size(vY, 1)
    if(sign(vLowerBound(ii)) == sign(vUpperBound(ii)))
        % Zero isn't within the boundary (Or both are zero)
        minSum = minSum + min(abs(vLowerBound(ii)), abs(vUpperBound(ii)));
    end
end

if(minSum > ballRadius)
    % The problem is infeasible
    vX = mean([vLowerBound, vUpperBound], 2);
    return;
end

% The dual objective function which should be maximized to find the optimal
% 'paramLambda'.
% The functios is negated as we want to maximize while using
% MATLAB's minimization function
hObjFun = @(paramLambda) -ObjectiveDualFunction(vY, ballRadius, vLowerBound, vUpperBound, paramLambda); %<! Objective function

if(DEBUG_MODE == ON)
    numSamples = 1000;

    vParamLambda    = linspace(0, 5, numSamples);
    vObjVal         = zeros([numSamples, 1]);

    for ii = 1:numSamples
        vObjVal(ii) = hObjFun(vParamLambda(ii));
    end

    figure();
    plot(vParamLambda, vObjVal);
end

paramLambda = fminsearch(hObjFun, paramLambda); %<! Objective Function isn't smooth hence can't use Newton Method
paramLambda = max(paramLambda, 0);

% Solution as derived for the updated forumaltion of the problem.
[~, vX] = ObjectiveDualFunction(vY, ballRadius, vLowerBound, vUpperBound, paramLambda);

end

function [ valObj, vX ] = ObjectiveDualFunction( vY, ballRadius, vL, vU, paramLambda )

vX      = zeros([size(vY, 1), 1]);

for ii = 1:size(vY, 1)

    if(sign(vL(ii)) == sign(vU(ii)))
        valX = ProjBoxFunction(vY(ii) - (paramLambda * sign(vL(ii))), vL(ii), vU(ii));
    elseif(sign(vY(ii)) == sign(vL(ii)))
        % Implictily vL(ii) <= 0, vU(ii) >= 0 and vY(ii) <= 0
        valX = ProjBoxFunction(vY(ii) + paramLambda, vL(ii), 0); %<! Making sure sign of valX is non positive
    elseif(sign(vY(ii)) == sign(vU(ii)))
        % Implictily vU(ii) >= 0, vL(ii) <= 0 and vY(ii) >= 0
        valX = ProjBoxFunction(vY(ii) - paramLambda, 0, vU(ii)); %<! Making sure sign of valX is non negative
    end

    vX(ii) = valX;

end

valObj = (0.5 * sum((vX - vY) .^ 2)) + (paramLambda * (sum(abs(vX)) - ballRadius));

end

function [ outVal ] = ProjBoxFunction( valIn, valL, valU )

outVal = min(max(valIn, valL), valU);

end


function [H_filt, h_filt] = arraySHTfiltersMeas_regLSHD(H_array, order_sht, grid_dirs_rad, w_grid, nFFT, amp_threshold)
%ARRAYSHTFILTERSMEAS_REGLSHD Generate SHT filters based on measured responses
%(regularized least-squares in the SHD)
%
%   Generate the filters to convert microphone signals from a spherical
%   microphone array to SH signals, based on a least-squares solution with
%   a constraint on noise amplification, using Tikhonov regularization. The
%   method formulates the LS problem in the spherical harmonic domain, by
%   first expanding the array response to SH coefficients, and then solving
%   the LS problem, similar to
%
%       Jin, C.T., Epain, N. and Parthy, A., 2014.
%       Design, optimization and evaluation of a dual-radius spherical microphone array.
%       IEEE/ACM Transactions on Audio, Speech, and Language Processing, 22(1), pp.193-204.
%
%   Inputs:
%       H_array:    nFFT x nMics x nMeasurement_dirs matrix of measured
%           responses, in the frequency domain with length nFFT
%       order_sht:  order of SH signals to generate
%       grid_dirs:  nMeasurement_dirs x 2 matrix of [azi elev] of the
%           measurement directions, in rads
%       w_grid:     nMeasurement_dirs x 1 vector of weights for
%           weighted-least square solution, based on the importance or area
%           around each measurement point (leave empty if not known, or
%           not important)
%       nFFT:       number of FFT points in the responses
%       amp_threshold:  max allowed amplification for filters, in dB
%
%   Outputs:
%       H_filt: nSH x nMics x (nFFT/2+1) returned filters in the frequency
%           domain (half spectrum up to Nyquist)
%       h_filt: nFFT x nMics x nSH impulse responses of the above filters
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% ARRAYSHTFILTERSMEAS_REGLSHD.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nGrid = size(H_array,3);
nMics = size(H_array,2);
if order_sht>sqrt(nMics)-1
    warning('Set order too high for the number of microphones, should be N<=sqrt(Q)-1')
    order_sht = floor( sqrt(nMics)-1 );
end
nBins = nFFT/2+1;
if isempty(w_grid)
    w_grid = ones(nGrid,1);
end

% SH matrix at grid directions
order_array = floor(sqrt(nGrid)/2-1);
aziElev2aziPolar = @(dirs) [dirs(:,1) pi/2-dirs(:,2)]; % function to convert from azimuth-inclination to azimuth-elevation
Y_grid = sqrt(4*pi) * getSH(order_array, aziElev2aziPolar(grid_dirs_rad), 'real')'; % SH matrix for grid directions

% compute inverse matrix
a_dB = amp_threshold;
alpha = 10^(a_dB/20);
beta = 1/(2*alpha);
W_grid = diag(w_grid);
H_filt = zeros((order_sht+1)^2, nMics, nBins);
% compute first the SHT of the array response
for kk=1:nBins
    tempH = squeeze(H_array(kk,:,:));
    H_nm(kk,:,:) = tempH * W_grid * Y_grid'* inv(Y_grid*W_grid*Y_grid');
end
% compute the inverse matrix in the SHD with regularization
for kk=1:nBins
    tempH_N = squeeze(H_nm(kk,:,:));
    tempH_N_trunc = tempH_N(:,1:(order_sht+1)^2);
    H_filt(:,:,kk) = tempH_N_trunc' * inv(tempH_N*tempH_N' + beta^2*eye(nMics));
end

if nargout>1
    % time domain filters
    h_filt = H_filt;
    h_filt(:,:,end) = abs(h_filt(:,:,end));
    h_filt = cat(3, h_filt, conj(h_filt(:,:,end-1:-1:2)));
    h_filt = real(ifft(h_filt, [], 3));
    h_filt = fftshift(h_filt, 3);
end


function XYZ = quat2cube(q)
%
% transforms unit quaternions (representing rotations) into cubochoric
%   representation
% the transformation is achieved by going from representation as unit
%   quaternions to homochoric representation (Lambert) and then further to
%   cubochoric representation (ball2cube)
%
% Input
%  q - @quaternion
%
% Output
%  XYZ - cubochoric coordinates

xyz = Lambert([q.a(:) q.b(:) q.c(:) q.d(:)]);
XYZ = ball2cube(xyz);

end


function welldrawdown
% Well drawdown - comparison for confined / half-confined / unconfined
% aquifers
%    using analytocal solutions
%
%   $Ekkehard Holzbecher  $Date: 2006/04/30 $
%--------------------------------------------------------------------------
K = 1.1e-5;            % hydraulic conductivity
H = 10;                % depth
T = K*H;               % transmissivity
Q = 1.e-4;             % pumping rate
r0 = 0.1;              % well radius
s0 = -1.;               % drawdown in well
c = 0.8e8;             % resistance of half-permeable layer
R = 35;                % maximum radius

r = linspace (r0,R,100);
hc = s0 + (Q/(2*pi*T))*log(r/r0);
hu = -H + s0 + sqrt(H*H + (Q/(pi*K))*log(r/r0));
hh = -(Q/(2*pi*T))*besselk(0,r/sqrt(T*c));

plot (r,hc,r,hh,r,hu);
legend ('confined','half-confined','unconfined')
xlabel ('distance [m]'); ylabel ('drawdown (neg) [m]');
title('Groundwater Drawdown due to Pumping')


function m=pesq2mos(p)
%PESQ2MOS convert PESQ speech quality scores to MOS m=(p)
%Inputs:    p  is a matrix of PESQ scores
%
%Outputs:   m  is a matrix, the same size as p, of MOS scores
%
% The PESQ measure is defined in [2]. The mapping function, defined in [3],
% converts raw PESQ scores (which lie in the range -0.5 to 4.5) onto the
% MOS-LQO (Mean Opinion Score - Listening Quality Objective [2]) scale in the
% range 1 to 5. The MOS scale is defined in [1] as
%           5=Excellent, 4=Good, 3=Fair, 2=Poor, 1=Bad.
%
% Refs: [1]	ITU-T. Methods for subjective determination of transmission quality.
%           Recommendation P.800, Aug. 1996.
%       [2]	ITU-T. Mean opinion score (MOS) terminology.
%           Recommendation P.800.1, July 2006.
%       [2]	ITU-T. Perceptual evaluation of speech quality (PESQ), an objective
%           method for end-to-end speech quality assessment of narrowband telephone
%           networks and speech codecs. Recommendation P.862, Feb. 2001.
%       [3]	ITU-T. Mapping function for transforming P.862 raw result scores to MOS-LQO.
%           Recommendation P.862.1, Nov. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: pesq2mos.m 3289 2013-08-01 13:56:02Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=0.999;
    b=4.999-a;
    c=-1.4945;
    d=4.6607;
end
if nargout>0
    m=a+b./(1+exp(c*p+d));
else
    if nargin<1 || isempty(p)
        pp=linspace(-0.5,4.5,100);
    else
        pp=p;
    end
    plot(pp,pesq2mos(pp));
    xlabel('PESQ (P.862)');
    ylabel('Mean Opimion Score (MOS)');
end


function [y, stopCondition, sigma, sigma0, t] ...
    = cons_smacof_pip(dx, y, isFree, bnd, w, con, smacof_opts, scip_opts)
% CONS_SMACOF_PIP  SMACOF algorithm with polynomial constraints (PIP file
% format).
%
% Scaling by MAjorizing a COnvex Function (SMACOF) is an iterative solution
% to the Multidimensional Scaling (MDS) problem (see de Leeuw and Mair [1]
% for an up to date review).
%
% The classic implementation of SMACOF uses an iterative algorithm that
% relies on the Guttman transform update. Our function here implements a
% different approach, where the Guttman transform update is replaced by
% solving a Quadratic Program (QP) (as proposed by Dwyer et al [2]) with
% polynomial constraints.
%
% In this implementation, we use the SCIP binary to solve the constrained
% QP. This binary can be downloaded for Linux, MacOS X and Windows from the
% Zuse Institute Berlin (ZIB) website
%
%   http://scip.zib.de/#download
%
% The objective function is
%
%   min_y 1/2 y' * H * y + f' * y
%
% subject to the constraints and bounds provided by the user.
%
% We use the PIP file format to formulate the constrained QP and pass it to
% SCIP.
%
%   http://polip.zib.de/pipformat.php
%
%
% [Y, STOPCONDITION, SIGMA, SIGMA0, T] = CONS_SMACOF_PIP(D, Y0, ISFREE, BND, [], CON)
%
%   D is an (N, N)-distance matrix, with distances between the points in an
%   N-point configuration. D can be full or sparse. D(i,j)=0 means that
%   vertices i and j are not directly connected.
%
%   Y0 is an initial guess of the solution, given as an (N, P)-matrix,
%   where P is the dimensionality of the output points. Currently, P must
%   be either 2 or 3.
%
%   BND is a cell array with the variable bounds in PIP format, and cannot
%   be empty (otherwise SCIP doesn't return a solution). E.g.
%
%      BND = {'Bounds', ' -1 <= x1 <= 4', ' -1 <= y1 <= 2.5', ...
%             ' -1 <= x2 <= 4', ' -1 <= y2 <= 2.5'};
%
%   CON is a cell array with the problem constraints in PIP format, and
%   cannot be empty (otherwise SCIP doesn't return a solution). E.g.
%
%      CON = {'Subject to', ...
%            ' c1: -0.5 x6 y7 +0.5 x3 y7 +0.5 x7 y6 -0.5 x3 y6 -0.5 x7 y3 +0.5 x6 y3 >= 0.1'};
%
%   Y is the best solution computed by the algorithm within all iterations.
%   Y is a point configuration with the same size as Y0. If the algorithm
%   cannot find any valid solution (e.g. because we set a too short time
%   limit, or because a valid solution doesn't exist, Y will be an array of
%   NaNs).
%
%   STOPCONDITION is a cell array with a string for each stop condition
%   that made the algorithm stop at the last iteration.
%
%   SIGMA is a vector with the stress value at each iteration. Weighted
%   stress is given as
%
%     SIGMA = \sum_{i<j} W_ij (D_ij - DY_ij)^2
%
%   where W is a weight matrix the same size as D. W_ij = 0 means that the
%   distance between points i and j does not affect the stress measure.
%
%   If the algorithm could not find any solution, SIGMA is a single scalar
%   with value = Inf. Note that even if the last SIGMA value is Inf, this
%   doesn't mean that the algorithm hasn't found a valid solution in a
%   previous iteration.
%
%   SIGMA0 is a scalar with the stress value of the initial guess Y0. Note
%   that we have no guarantee that Y0 is a valid solution, so be careful
%   when comparing SIGMA0 to SIGMA. In particular, SIGMA0 may be smaller
%   than any SIGMA, but may not be a valid solution according to the
%   constraints.
%
%   T is a vector with the time between the beginning of the algorithm and
%   each iteration. Units in seconds.
%
% Y = CONS_SMACOF_PIP(..., SMACOF_OPTS, SCIP_OPTS)
%
%   SMACOF_OPTS is a struct with parameters to tweak the SMACOF algorithm.
%
%     'MaxIter': (default = 0) Maximum number of majorization iterations we
%                allow the optimisation algorithm.
%
%     'Epsilon': (default = Inf) The algorithm will stop if
%                (SIGMA(I+1)-SIGMA(I))/SIGMA(I) < OPTS.Epsilon.
%
%     'Display': (default = 'off') Do not display any internal information.
%                'iter': display internal information at every iteration.
%
%     'TolFun':  (default = 1e-12) Termination tolerance of the stress
%                value.
%
%   SCIP_OPTS is a struct with parameters to tweak the SCIP algorithm.
%
%     'scipbin': (defaults = 'scip.linux.x86_64.gnu.opt.spx' (Linux),
%                            'scip.darwin.x86_64.gnu.opt.spx' (Mac),
%                            'scip.mingw.x86_64.intel.opt.spx.exe' (Win))
%                Name of the SCIP binary/executable. This binary should be
%                available in the system path. E.g. place it in
%                gerardus/programs. The binaries/executable can be
%                downloaded from http://scip.zib.de/#download. You may need
%                to rename them so that they fit the expected filenames.
%
%     'display_verblevel': (default 4) verbosity level of output (0: SCIP
%                quiet mode).
%
%     'display_freq': (default 100) frequency for displaying node
%                information lines.
%
%     'limits_absgap': (default 0.0) solving stops, if the absolute
%                gap = |primalbound - dualbound| is below the given value.
%
%     'limits_gap': (default 0.0) solving stops, if the relative
%                gap = |primal - dual|/MIN(|dual|,|primal|) is below the
%                given value.
%
%     'limits_time': (default 1e+20) maximal time in seconds to run.
%
%     'limits_solutions': (default -1) solving stops, if the given number
%                of solutions were found (-1: no limit).
%
%     'lp_threads': (default 0: automatic) number of threads used for
%                solving the LP.
%
%     'numerics_feastol': (default 1e-6) feasibility tolerance for
%                constraints in SCIP. If this value is changed from the
%                default, it needs to be passed to the function that
%                creates the constraints too, e.g.
%                tri_ccqp_smacof_nofold_sph_pip().
%
%
% [1] J de Leeuw, P Mair, "Multidimensional scaling using majorization:
% SMACOF in R", Journal of Statistical Software, 31(3), 2009.
%
% [2] T. Dwyer, Y. Koren, and K. Marriott, "Drawing directed graphs using
% quadratic programming," IEEE Transactions on Visualization and Computer
% Graphics, vol. 12, no. 4, pp. 536-548, 2006.
%
% See also: cmdscale, qcqp_smacof, tri_sphparam.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014, 2016 University of Oxford
% Version: 0.4.6
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

%% Input arguments

% check arguments
narginchk(6, 8);
nargoutchk(0, 5);

% start clock
tic

% number of points
N = size(dx, 1);

% dimensionality of the points
D = size(y, 2);
if ((D ~= 2) && (D ~= 3))
    error('Only implemented for 2D output')
end

% check inputs
if (N ~= size(dx, 2))
    error('D must be a square matrix')
end
if (N ~= size(y, 1))
    error('Y0 must have the same number of rows as D')
end

% defaults
if (nargin < 3 || isempty(isFree))
    isFree = true(N, 1);
end
Nfree = nnz(isFree);

if (isempty(w))
    % if the user doesn't provide a weight matrix, we simply assign 1 if
    % two vertices are connected, and 0 if not
    w = double(dx ~= 0);
    if (any(diag(w)) ~= 0)
        error('Assertion error: W matrix has diagonal elements that are non-zero')
    end
end
if (any(size(w) ~= [N N]))
    error('W must be a square matrix with the same size as D')
end

% if the user doesn't allow any vertices to be optimized, then we can exit
% the function
if (Nfree == 0)
    stopCondition = 'No free vertices to optimise';
    sigma = [];
    dy = dmatrix_con(dx, y);
    sigma0 = 0.5 * sum(sum(w .* (dx - dy).^2));
    t = [];
    return;
end

% SMACOF_OPTS defaults
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'MaxIter'))
    smacof_opts.MaxIter = 100;
end
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'Epsilon'))
    smacof_opts.Epsilon = 0;
end
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'Display'))
    smacof_opts.Display = 'off';
end
if (nargin < 7 || isempty(smacof_opts) || ~isfield(smacof_opts, 'TolFun'))
    smacof_opts.TolFun = 1e-12;
end

% SCIP_OPTS
if (nargin < 8 || isempty(scip_opts) || ~isfield(scip_opts, 'scipbin'))

    % default name of the SCIP binary depending on the architecture
    if (isunix)
        SCIPBIN = 'scip.linux.x86_64.gnu.opt.spx';
    elseif (ismac)
        SCIPBIN = 'scip.darwin.x86_64.gnu.opt.spx';
    elseif (ispc)
        SCIPBIN = 'scip.mingw.x86_64.intel.opt.spx.exe';
    else
        error('Operating system not recognized: I do not know what is the default name for the SCIP binary')
    end
end

scip_opts_comm = {};
if (nargin >= 8 && ~isempty(scip_opts))

    % SCIP binary
    if (isfield(scip_opts, 'scipbin'))
        SCIPBIN = scip_opts.scipbin;
    end

    % display options
    QUIETFLAG = [];
    OUTPUTREDIR = []; % output redirecton, e.g. "> /dev/null"
    if isfield(scip_opts, 'display_verblevel')
        if (scip_opts.display_verblevel == 0)
            QUIETFLAG = ' -q ';

            % bug workaround: there's a bug in
            % scip-3.1.0.linux.x86_64.gnu.opt.spx, which causes the
            % solution to be written as an empty file when the quiet flag
            % is used. As a workaround, we disable the quiet flag in that
            % case, and instead send the output to /dev/null, as suggested
            % by Stefan Vigerske
            if strcmp(SCIPBIN, 'scip-3.1.0.linux.x86_64.gnu.opt.spx')
                QUIETFLAG = [];
                OUTPUTREDIR = ' > /dev/null';
            end

        end
        scip_opts_comm{end+1} = [' -c "set display verblevel ' num2str(scip_opts.display_verblevel) '"'];
    end

    % frequency for displaying node information lines [100]
    if (isfield(scip_opts, 'display_freq'))
        scip_opts_comm{end+1} = [' -c "set display freq ' num2str(scip_opts.display_freq) '"'];
    end

    % limits options

    % solving stops, if the absolute gap = |primalbound - dualbound| is below the given value [0.0]
    if (isfield(scip_opts, 'limits_absgap'))
        scip_opts_comm{end+1} = [' -c "set limits absgap ' num2str(scip_opts.limits_absgap) '"'];
    end

    % solving stops, if the relative gap = |primal - dual|/MIN(|dual|,|primal|) is below the given value [0.0]
    if (isfield(scip_opts, 'limits_gap'))
        scip_opts_comm{end+1} = [' -c "set limits gap ' num2str(scip_opts.limits_gap) '"'];
    end

    % maximal time in seconds to run [1e+20]
    if (isfield(scip_opts, 'limits_time'))
        scip_opts_comm{end+1} = [' -c "set limits time ' num2str(scip_opts.limits_time) '"'];
    end

    % solving stops, if the given number of solutions were found (-1: no limit) [-1]
    if (isfield(scip_opts, 'limits_solutions'))
        scip_opts_comm{end+1} = [' -c "set limits solutions ' num2str(scip_opts.limits_solutions) '"'];
    end

    % lp options

    % number of threads used for solving the LP (0: automatic)
    if (isfield(scip_opts, 'lp_threads'))
        scip_opts_comm{end+1} = [' -c "set lp advanced threads ' num2str(scip_opts.lp_threads) '"'];
    end

    % numerics options

    % feasibility tolerance for constraints in SCIP [1e-6]
    if (isfield(scip_opts, 'numerics_feastol'))
        scip_opts_comm{end+1} = [' -c "set numerics feastol ' num2str(scip_opts.numerics_feastol) '"'];
    end

end

%% Objective function: 1/2 nu' * H * nu + f' * nu

% pre-compute the weighted Laplacian matrix
V = -w;
V(1:N+1:end) = sum(w, 2);

% quadratic terms of the objective function

% upper triangular matrix terms (we assume symmetric matrix)
dx = tril(dx);

% remove pairs where both vertices are fixed, as those only contribute a
% constant to the objective function and can be ignored in the optimization
dx(~isFree, ~isFree) = 0;

Nterms = nnz(dx);
objfunq = cell(1, Nterms + Nfree);
count = 1;
for idx = find(dx)'

    % decode dx matrix index into [I, J] vertices (we swap I,J so that we
    % get I as smaller index, and J larger index
    [J, I] = ind2sub(size(dx), idx);

    % upper triangular terms

    % 2D outputs
    if (D == 2)

        % xi (free), xj (free)
        if (isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d x%d + %.16g y%d y%d', ...
                2*full(V(I, J)), I, J, ...
                2*full(V(I, J)), I, J);
        % xi (free), xj (fixed)
        elseif (isFree(I) && ~isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d', ...
                2*full(V(I, J)) * y(J, 1), I, ...
                2*full(V(I, J)) * y(J, 2), I);
        % xi (fixed), xj (free)
        elseif (~isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d', ...
                2*full(V(I, J)) * y(I, 1), J, ...
                2*full(V(I, J)) * y(I, 2), J);
        end
        count = count + 1;

    % 3D outputs
    elseif (D == 3)

        % xi (free), xj (free)
        if (isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d x%d + %.16g y%d y%d + %.16g z%d z%d', ...
                2*full(V(I, J)), I, J, ...
                2*full(V(I, J)), I, J, ...
                2*full(V(I, J)), I, J);
        % xi (free), xj (fixed)
        elseif (isFree(I) && ~isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d + %.16g z%d', ...
                2*full(V(I, J)) * y(J, 1), I, ...
                2*full(V(I, J)) * y(J, 2), I, ...
                2*full(V(I, J)) * y(J, 3), I);
        % xi (fixed), xj (free)
        elseif (~isFree(I) && isFree(J))
            objfunq{count} = sprintf(...
                '+%.16g x%d + %.16g y%d + %.16g z%d', ...
                2*full(V(I, J)) * y(I, 1), J, ...
                2*full(V(I, J)) * y(I, 2), J, ...
                2*full(V(I, J)) * y(I, 3), J);
        end
        count = count + 1;

    else
        error('Assertion fail: Output dimension D is not 2 or 3')
    end
end

% main diagonal terms
for I = find(isFree)'

    % 2D outputs
    if (D == 2)

        % only free vertices contribute to the objective function
        objfunq{count} = sprintf(...
            '+%.16g x%d^2 + %.16g y%d^2', ...
            full(V(I, I)), I, ...
            full(V(I, I)), I);

    % 3D outputs
    elseif (D == 3)

        % only free vertices contribute to the objective function
        objfunq{count} = sprintf(...
            '+%.16g x%d^2 + %.16g y%d^2 + %.16g z%d^2', ...
            full(V(I, I)), I, ...
            full(V(I, I)), I, ...
            full(V(I, I)), I);

    else
        error('Assertion fail: Output dimension D is not 2 or 3')
    end
    count = count + 1;

end
objfunq{1} = [' obj: ' objfunq{1}];

% the linear term of the objective function (f) has to be computed at each
% iteration of the QPQC-SMACOF algorithm. Thus, it is not computed here

%% SMACOF algorithm

% file name and path to save PIP model, computed solution and initial guess
% (generate unique names so that it is possible to run several instances of
% this function in parallel)
[~, aux] = fileparts(tempname);
pipfilename = [tempdir 'model-' aux '.pip']; % PIP model
solfilename = [tempdir 'model-' aux '.sol']; % output solution
sol0filename = [tempdir 'model-' aux '.sol0']; % initial guess

% init stopCondition
stopCondition = [];

% Euclidean distances between vertices in the current solution
dy = dmatrix_con(dx, y);

% initial stress
sigma0 = 0.5 * sum(sum(w .* (dx - dy).^2));

% if dx, dy are sparse matrices, sigma0 will be a sparse scalar, and this
% gives an error with fprintf below
sigma0 = full(sigma0);

% vector of stress computed by the algorithm
sigma = zeros(1, smacof_opts.MaxIter);

% display algorithm's evolution
t = zeros(1, smacof_opts.MaxIter); % time past from 0th iteration
if (strcmp(smacof_opts.Display, 'iter'))
    fprintf('Iter\t\tSigma\t\t\tTime (sec)\n')
    fprintf('===================================================\n')
    fprintf('%d\t\t%.4e\t\t%.4e\n', 0, sigma0, 0.0)
end

% auxiliary intermediate result
mwdx = -w .* dx;

% initialize the storage of the best solution found by the algorithm. We
% start with Inf so that the initial guess will always be replaced by any
% valid solution. The reason is that we cannot be sure that the initial
% guess fulfills the constraints, but any valid solution from a later
% iteration does
sigmabest = Inf;
ybest = nan(size(y));

% majorization loop
for I = 1:smacof_opts.MaxIter

    % auxiliary matrix B: non-main-diagonal elements
    B = mwdx ./ dy;
    B(isnan(B)) = 0;

    % auxiliary matrix B: main diagonal elements
    B(1:N+1:end) = -sum(B, 2);

    % the linear term (f) of the quadratic objective function
    % 1/2 nu' * H * nu + f' * nu
    % has to be recomputed at every iteration
    f = -2 * B * y;

    % convert linear term to PIP format
    objfunl = cell(1, Nfree);
    J = find(isFree);
    for idx = 1:length(J)

        % 2D output
        if (D == 2)

            objfunl{idx} = sprintf(...
                '+%.16g x%d +%.16g y%d', ...
                f(J(idx), 1), J(idx), f(J(idx), 2), J(idx));

        % 3D output
        elseif (D == 3)

            objfunl{idx} = sprintf(...
                '+%.16g x%d +%.16g y%d +%.16g z%d', ...
                f(J(idx), 1), J(idx), f(J(idx), 2), J(idx), ...
                f(J(idx), 3), J(idx));

        else
            error('Assertion fail: Output dimension D is not 2 or 3')
        end

    end

    % create PIP file to describe problem
    fid = fopen(pipfilename, 'w');
    if (fid == -1)
        error(['Cannot open file ' pipfilename ' to save PIP model'])
    end
    fprintf(fid, '%s\n%s\n%s\n%s\n%s\n%s\n', ...
        'Minimize', objfunq{:}, objfunl{:}, bnd{:}, con{:}, 'End');
    if (fclose(fid) == -1)
        error(['Cannot close file ' pipfilename ' to save PIP model'])
    end

    % create file for the initial guess
    write_solution(sol0filename, y);

    % solve the quadratic problem
    system([...
        SCIPBIN...
        QUIETFLAG ...
        ' -c "read ' pipfilename '"'...
        strcat(scip_opts_comm{:}) ...
        ' -c "optimize"'...
        ' -c "write solution ' solfilename '"'...
        ' -c "quit"' ...
        OUTPUTREDIR]);

    % read solution
    [aux, status] = read_solution(solfilename, size(y));

    % delete temp files
    if (exist(pipfilename, 'file')), delete(pipfilename); end
    if (exist(solfilename, 'file')), delete(solfilename); end
    if (exist(sol0filename, 'file')), delete(sol0filename); end

    % if SCIP cannot find a valid solution (e.g. we set a too short time
    % limit or it doesn't exist), then we cannot update the current
    % solution. This also means that we need to stop the optimization,
    % because the problem matrices and vectors won't change, and basically
    % we would try to solve the same problem again, not reaching a solution
    % either. Note however that it's possible that we have been finding
    % valid solutions before, and it's only at a later iteration that we
    % get to this "not valid solution" situation. Thus, we cannot assume
    % that the stop condition below these lines means "error".
    if (isempty(aux))
        stopCondition{end+1} = ['SCIP: ' status];
        sigma(I) = NaN;
        t(I) = toc;
        break;
    end

    % we only have to update the positions of the free vertices. The values
    % for fixed vertices in aux are all 0, so they need to be ignored
    y(isFree, :) = aux(isFree, :);

    % check that no two vertices are overlapping. Duplicated vertices cause
    % errors in the SMACOF algorithm because they create dy=0 components,
    % that produce Inf values in B = mwdx ./ dy;. Ideally, we would like
    % to move one of them a bit so that they don't overlap, while
    % maintaining the positive orientation of the triangles they are
    % involved with. However, we don't have the triangulation in this
    % function, and leave that for future work. For the time being, we are
    % going to consider overlapping vertices as a failed solution, and exit
    if (size(unique(y, 'rows'), 1) ~= N)
        stopCondition{end+1} = 'Duplicated vertex/vertices';
        break
    end

    % recompute distances between vertices in the current solution
    dy = dmatrix_con(dx, y);

    % compute stress with the current solution
    sigma(I) = 0.5 * sum(sum(w .* (dx - dy).^2));

    % update best solution
    if (sigma(I) < sigmabest)
        sigmabest = sigma(I);
        ybest = y;
    end

    % display algorithm's evolution
    t(I) = toc;
    if (strcmp(smacof_opts.Display, 'iter'))
        fprintf('%d\t\t%.4e\t\t%.4e\n', I, sigma(I), t(I))
    end

    % check whether the stress is under the tolerance level requested by
    % the user
    if (sigma(I) < smacof_opts.TolFun)
        stopCondition{end+1} = 'TolFun';
    end

    % check whether the improvement in stress is below the user's request,
    % and it's positive (don't stop if the stress gets worse, because
    % stress can go up and down in the optimization)
    if (I > 1)
        if ((sigma(I-1)-sigma(I))/sigma(I-1) < smacof_opts.Epsilon ...
                && (sigma(I-1)-sigma(I))/sigma(I-1) >= 0)
            stopCondition{end+1} = 'Epsilon';
        end
    end

    % stop if any stop condition has been met
    if (~isempty(stopCondition))
        break;
    end

end

% return the best solution the algorithm has found in all iterations
y = ybest;

% check whether the "maximum number of iterations" stop condition has been
% met
if (I == smacof_opts.MaxIter)
    stopCondition{end+1} = 'MaxIter';
end

% prune stress and time vectors if convergence was reached before the
% maximum number of iterations
sigma(I+1:end) = [];
t(I+1:end) = [];

end

% read SCIP solution from a text file created by SCIP
%
% file: path and file name of the solution file.
%
% sz: size of the output matrix with the solution. This parameter makes the
%     code easier to write, and it also allows to detect missing variables
%     in the solution
%
% y: matrix with the solution as a point configuration (each row is a
%    point)
function [y, status] = read_solution(file, sz)

if ((sz(2) ~= 2) && (sz(2) ~= 3))
    error('We only know how to read solutions that are sets of 2D or 3D points')
end

fid = fopen(file, 'r');
if (fid == -1)
    error(['Cannot open file ' file ' to read solution'])
end

% read status of the solution
status = fgetl(fid);
if ((isnumeric(status) && status == -1) ...
        || ~strcmp(status(1:16), 'solution status:'))
    error(['Assertion fail: File with SCIP solution does not start with string ''solution status:''. File ' file])
end
status = status(18:end);

% unless we obtained a valid solution, we don't bother with the rest of the
% file (which should be empty anyway), we exit, and the main function
% should detect the empty solution y, and create a stopCondition with the
% status returned by SCIP
if (~strcmp(status, 'optimal solution found') ...
        && ~strcmp(status, 'solution limit reached'))
    fclose(fid);
    y = [];
    return;
end

% the second line in the file should be the value of the objective function
aux = fgetl(fid);
if (~strcmp(aux(1:16), 'objective value:'))
    error(['Assertion fail: Second line in file with SCIP solution does not start with string ''objective value:''. File ' file])
end

% read contents of the file. Example result:
%
% solution status: optimal solution found
% objective value:                     468.345678663118
% x1                                                 -4 	(obj:0)
% y1                                                  2 	(obj:0)
% x2                                  0.613516669331233 	(obj:0)
% y2                                                 -4 	(obj:0)
% x3                                   1.24777861008035 	(obj:0)
% y3                                  -3.43327058768579 	(obj:0)
% x4                                                 -4 	(obj:0)
% y4                                 -0.804343353251697 	(obj:0)
% x5                                                  2 	(obj:0)
% y5                                                 -4 	(obj:0)
% x6                                  -3.31069797707353 	(obj:0)
% y6                                   1.21192102496956 	(obj:0)
% x7                                     1.643412276563 	(obj:0)
% y7                                  -3.72674560989634 	(obj:0)
% quadobjvar                           468.345678663118 	(obj:1)
c = textscan(fid, '%s%f%s', 'Delimiter', ' ', 'MultipleDelimsAsOne', true);
fclose(fid);

if isempty(c{1})
    error('SCIP did not return any solution')
end

% if SCIP has found a solution, read it from the file into the output
% matrix. Note that if a variable = 0, SCIP will not put it in the output
% file
y = zeros(sz);
for I = 1:length(c{1})-1

    % variable name
    varname = c{1}{I};

    % if list of output variables has finished, we can exit the loop
    if (strcmp(varname, 'quadobjvar'))
        break;
    end

    % vertex index
    idx = str2double(varname(2:end));

    if (c{1}{I}(1) == 'x') % this is an x-coordinate
        y(idx, 1) = c{2}(I);
    elseif (c{1}{I}(1) == 'y') % this is a y-coordinate
        y(idx, 2) = c{2}(I);
    elseif (c{1}{I}(1) == 'z') % this is a z-coordinate
        y(idx, 3) = c{2}(I);
    end

end

end

% write SCIP initial solution to a text file that SCIP can understand
%
% y: matrix with the solution as a point configuration (each row is a
%    point), including the points that are not free points
%
% file: path and file name of the solution file.
function write_solution(file, y)

% size of full solution configuration
sz = size(y);

if ((sz(2) ~= 2) && (sz(2) ~= 3))
    error('We only know how to read solutions that are sets of 2D or 3D points')
end

fid = fopen(file, 'w');
if (fid == -1)
    error(['Cannot open file ' file ' to write solution'])
end

% write solution to file. Example result:
%
% x1                                                 -4 	(obj:0)
% y1                                                  2 	(obj:0)
% x2                                  0.613516669331233 	(obj:0)
% y2                                                 -4 	(obj:0)
% x3                                   1.24777861008035 	(obj:0)
% y3                                  -3.43327058768579 	(obj:0)
% x4                                                 -4 	(obj:0)
% y4                                 -0.804343353251697 	(obj:0)
% x5                                                  2 	(obj:0)
% y5                                                 -4 	(obj:0)
% x6                                  -3.31069797707353 	(obj:0)
% y6                                   1.21192102496956 	(obj:0)
% x7                                     1.643412276563 	(obj:0)
% y7                                  -3.72674560989634 	(obj:0)
for I = 1:sz(1)

    % write x-coordinate
    fprintf(fid, '%s%d\t%.16e\t%s\n', 'x', I, y(I, 1), '(obj:0)');

    % write y-coordinate
    fprintf(fid, '%s%d\t%.16e\t%s\n', 'y', I, y(I, 2), '(obj:0)');

    % write z-coordinate
    if (sz(2) == 3)
        fprintf(fid, '%s%d\t%.16e\t%s\n', 'z', I, y(I, 3), '(obj:0)');
    end

end

% close file
st = fclose(fid);
if (st == -1)
    error(['Cannot close file ' file ' after writing solution'])
end

end


function [ lines ] = findlines( imge,map,theta,rho,minLen,maxLineNum,maxPeakNum,resol,maxGap )
%FINDLINE Find line features in the image using Hough transform
%   LINES = FINDLINES(IMGE,MAP,THETA,RHO,MINLEN,MAXLINENUM,MAXPEAKNUM,RESOL,MAXGAP)
%	IMGE is the binary edge image;
%	MAP,THETA,RHO is the output of HOUGH;
%	MINLEN is the minimum length of lines that will be found;
%	No more MAXLINENUM lines will be returned;
%	MAXPEAKNUM is the maximum peaks	that will be detected on the map;
%	MAXGAP is the maximum gap length that will be merged;
%	RESOL is the resolution of the detected lines, the smaller, the lines
%	must be further away from each other. A suggest value is 100.
%	LINES is a struct array with members: theta,rho,point1([row,col]),point2.
%
%	Yan Ke @ THUEE, 20110123, xjed09@gmail.com

% imge = edge(img);
% [map theta rho] = hough1(imge,[]);
[pri pti] = findpeaks(map,minLen,maxPeakNum,resol);
peakNum = length(pri);
prho = rho(pri); % peak points
pthe = theta(pti)*pi/180;
rhoThres = abs(rho(2)-rho(1))/2;

fitPts = cell(1,peakNum);
fitPtsNum = zeros(1,peakNum);
for p = 1:peakNum
	 % store points which are on the lines
	fitPts{p} = zeros(2,ceil(map(pri(p),pti(p))*1.2));
end
[Y X] = find(imge);
X = X-1; % The origin is the bottom-left corner.
M = size(imge,1);
Y = M-Y;

% find the points that's on the selected lines
for p = 1:length(X)
	rho1 = X(p)*sin(pthe)+Y(p)*cos(pthe);
	isOnPeak = find(abs(rho1-prho)<=rhoThres);
	fitPtsNum(isOnPeak) = fitPtsNum(isOnPeak)+1;
	for q = isOnPeak
		fitPts{q}(:,fitPtsNum(q)) = [X(p);Y(p)];
	end
end

% track the points on each line, find the start points and the end points,
lines = [];
tempLine = struct;
for p = 1:peakNum
	t1 = [-cos(pthe(p)),sin(pthe(p))]; % the unit directional vector of the line
	dist1 = t1*fitPts{p}(:,1:fitPtsNum(p)); % dist along the line
	[ordDist ordIdx] = sort(dist1,'ascend');
	ordPts = fitPts{p}(:,ordIdx);

	% handle with the gaps
	dist2 = diff(ordDist); % dist with each other
	gapPos = [0 find(dist2>maxGap) length(ordDist)];
	lineLen = diff(gapPos);
	for q = find(lineLen>=minLen)
		tempLine.theta = pthe(p);
		tempLine.rho = prho(p);
		tempLine.point1 = [M-ordPts(2,gapPos(q)+1),ordPts(1,gapPos(q)+1)+1];
		tempLine.point2 = [M-ordPts(2,gapPos(q+1)),ordPts(1,gapPos(q+1))+1];
		lines = [lines,tempLine];
		if length(lines) == maxLineNum, break; end
	end
	if length(lines) == maxLineNum, break; end
end

end

function [rows cols] = findpeaks(map,minLen,maxPeakNum,resol)
%	Find the at most maxLinNum points that's no less than minLen in map, meanwhile not
%	8-adjacent to each other, in MAP. Rows and cols are returned.

if max(max(map)) < minLen, return; end
rows = zeros(1,maxPeakNum);
cols = rows;
sz = size(map);
sup = ceil(sz/resol);
for p = 1:maxPeakNum
	[V,I] = max(map(:));
	if V<minLen
		rows = rows(1:p-1);
		cols = cols(1:p-1);
		break;
	end
	[rows(p),cols(p)] = ind2sub(sz,I);

	% non-maximal suppression
	left = max(1,cols(p)-sup(2));
	lr = min(sup(2)*2+1,sz(2)-left);
	up = max(1,rows(p)-sup(1));
	ud = min(sup(1)*2+1,sz(1)-up);
	map(up:up+ud,left:left+lr) = 0;
end

% if max(max(map)) < minLen, return; end
% map(map<minLen) = 0;
% mapMax = ordfilt2(map,25,ones(5));
% mapPeak = (mapMax == map) & map; % non-maximal suppression
% pt = find(mapPeak);
% pv = map(pt);
% [pvo,idx] = sort(pv,'descend');
% pt = pt(idx);
% if length(pvo) > maxPeakNum, pt = pt(1:maxPeakNum); end
% [rows cols] = ind2sub(size(map),pt);

end


function line_num = sphere_llq_grid_line_count ( lat_num, long_num )

%*****************************************************************************80
%
%% SPHERE_LLQ_GRID_LINE_COUNT counts lines for an LLQ grid on a sphere.
%
%  Discussion:
%
%    A SPHERE LLQ grid imposes a grid of quadrilaterals on a sphere,
%    using latitude and longitude lines.
%
%    The number returned is the number of pairs of points to be connected
%    to form all the line segments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude and
%    longitude lines to draw.  The latitudes do not include the North and South
%    poles, which will be included automatically, so LAT_NUM = 5, for instance,
%    will result in points along 7 lines of latitude.
%
%    Output, integer LINE_NUM, the number of grid lines.
%
  line_num = long_num * ( lat_num + 1 ) ...
           + lat_num  * long_num;

  return
end


function value = index3_row ( i_min, i, i_max, j_min, j, j_max, ...
  k_min, k, k_max, index_min )

%*****************************************************************************80
%
%% INDEX3_ROW indexes a 3D array by rows.
%
%  Discussion:
%
%    When we say "by rows", we really just mean that entries of the array are
%    indexed starting at entry (I_MIN,J_MIN,K_MIN), and the increasing the LAST
%    index first, then the next-to-the-last, and so on.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I_MIN, I, I_MAX, for row indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer J_MIN, J, J_MAX, for column indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer K_MIN, K, K_MAX, for plane indices,
%    the minimum, the index, and the maximum.
%
%    Input, integer INDEX_MIN, the index of (I_MIN,J_MIN,K_MIN).
%    Typically, this is 0 or 1.
%
%    Output, integer VALUE, the index of element (I,J,K).
%
  value = index_min ...
             + ( k - k_min ) ...
             + ( j - j_min ) * ( k_max + 1 - k_min ) ...
             + ( i - i_min ) * ( j_max + 1 - j_min ) * ( k_max + 1 - k_min );

  return
end


function [ t, rank ] = subset_next ( n, t, rank )

%*****************************************************************************80
%
%% SUBSET_NEXT computes the subset lexicographic successor.
%
%  Discussion:
%
%    This is a lightly modified version of "subset_lex_successor()" from COMBO.
%
%  Example:
%
%    On initial call, N is 5 and the input value of RANK is -1.
%    Then here are the successive outputs from the program:
%
%   Rank   T1   T2   T3   T4   T5
%   ----   --   --   --   --   --
%      0    0    0    0    0    0
%      1    0    0    0    0    1
%      2    0    0    0    1    0
%      3    0    0    0    1    1
%     ..   ..   ..   ..   ..   ..
%     30    1    1    1    1    0
%     31    1    1    1    1    1
%     -1    0    0    0    0    0  <-- Reached end of cycle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the master set.
%    N must be positive.
%
%    Input/output, integer T(N), describes a subset.  T(I) is 0 if
%    the I-th element of the master set is not in the subset, and is
%    1 if the I-th element is part of the subset.
%    On input, T describes a subset.
%    On output, T describes the next subset in the ordering.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is -1.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    t = zeros ( n, 1 );
    rank = 0;
    return
  end

  for i = n : -1 : 1

    if ( t(i) == 0 )
      t(i) = 1;
      rank = rank + 1;
      return
    else
      t(i) = 0;
    end

  end

  rank = -1;

  return
end


function poly2 = densifyPolygon(poly, N)
%DENSIFYPOLYGON Add several points on each edge of the polygon.
%
%   POLY2 = densifyPolygon(POLY, N)
%   POLY is a NV-by-2 array containing polygon coordinates. The function
%   iterates on polygon edges, divides it into N subedges (by inserting N-1
%   new vertices on each edges), and return the resulting polygon.
%   The new polygon POLY has therefore N*NV vertices.
%
%   Example
%     % Densifies a simple polygon
%     poly = [0 0 ; 10 0;5 10;15 15;5 20;-5 10];
%     poly2 = densifyPolygon(poly, 10);
%     figure; drawPolygon(poly); axis equal
%     hold on; drawPoint(poly2);
%
%   See also
%     drawPolygon, edgeToPolyline
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-11-25, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

% number of vertices, and of edges
Nv = size(poly, 1);

% number of vertices in new polygon
N2 = N * Nv;
poly2 = zeros(N2, 2);

% iterate on polygon edges
for i = 1:Nv
    % extract current edge
    v1 = poly(i, :);
    v2 = poly(mod(i, Nv) + 1, :);

    % convert current edge to polyline
    newVertices = edgeToPolyline([v1 v2], N);

    % indices of current polyline to resulting polygon
    i1 = (i-1)*N + 1;
    i2 = i * N;

    % fill up polygon
    poly2(i1:i2, :) = newVertices(1:end-1, :);
end


% another simple 1D convection example with periodic BC
% diffusion coefficients are defined but not used
% It compares the results of upwind with TVD and shows how diffusive the upwind
% See how diffusive the upwind scheme can be althou it is so bad
% everywhere. Play with flux limiters, time steps, initial condition and time steps
% and see the difference between schemes in action
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
% define a 1D domain and mesh
W = 1;
Nx = 500;
mesh1 = createMesh1D(Nx, W);
x = mesh1.cellcenters.x;
% define the boundaries
BC = createBC(mesh1); % all Neumann
BC.left.periodic=1;
BC.right.periodic =1;
% Initial values
phi_old = createCellVariable(mesh1, 0.0, BC);
phi_old.value(20:120) = 1;
phi_old.value(180:400)= sin(x(180:400)*10*pi());
% initial guess for phi
phi = phi_old;
% initial values for upwind scheme
phiuw = phi;
phiuw_old=phi;
% keep the initial values for visualization
phiinit=phi_old;
% velocity field
u = 0.3;
uf = createFaceVariable(mesh1, u);
% diffusion field
D = 1e-2;
Df = createFaceVariable(mesh1, D);
% transient term coefficient
alfa = createCellVariable(mesh1,1.0);
% upwind convection term
Mconvuw = convectionUpwindTerm1D(uf);
% define the BC term
[Mbc, RHSbc] = boundaryCondition(BC);
% choose a flux limiter
FL = fluxLimiter('Superbee');
% solver
dt = 0.0005; % time step
final_t = W/u;
t = 0;
while t<final_t
    t = t+dt;
    % inner loop for TVD scheme
    [Mt, RHSt] = transientTerm(phi_old, dt, alfa);
    for j = 1:5
        [Mconv, RHSconv] = convectionTvdTerm1D(uf, phi, FL);
        M = Mconv+Mt+Mbc;
        RHS = RHSt+RHSbc+RHSconv;
        phi = solvePDE(mesh1, M, RHS);
    end
    [Mtuw, RHStuw] = transientTerm(phiuw_old, dt, alfa);
    Muw = Mconvuw+Mtuw+Mbc;
    RHSuw = RHStuw+RHSbc;
    phiuw = solvePDE(mesh1, Muw, RHSuw);
    phiuw_old = phiuw;
    phi_old = phi;
    figure(1);plot(x, phiinit.value(2:Nx+1), x, phi.value(2:Nx+1), '-o', x, ...
        phiuw.value(2:Nx+1)); drawnow;

end


%%*******************************************************
%% maxcut: MAXCUT problem.
%%
%% (P)    min  Tr C*X
%%        s.t.  diag(X) = e;
%%
%% C = -(diag(B*e)-B)/4.
%%
%% (dual problem)   max  e'*y
%%                  s.t. diag(y) + S = C.
%%-------------------------------------------------------
%%
%% [X,y,objval] = maxcut(B);
%%
%% B: weighted adjacency matrix of a graph.
%%
%% DSDP5.0
%% Copyright (c) 2004 by
%% S. Benson, Y. Ye
%% Last modified: Jan 2004
%%*******************************************************

 function [X,y,objval] = maxcut(B);

   if ~isreal(B); error('only real B allowed'); end;

   n = length(B); e = ones(n,1);
   CC = -(spdiags(B*e,0,n,n)-B)/4.0;
   b = e;

   AC = cell(1,3);
   AC{1,1}='SDP';
   AC{1,2}=n;
   nn=n*(n+1)/2;

   AAC=sparse(nn,n);
   for k = 1:n; AAC(:,k) = dsparse(k,k,1,n,n); end;
   AAC=[AAC sparse(dvec(CC))];
   AC{1,3}=AAC;
   y0 = -1.1*abs(CC)*e;
   OPTIONS=doptions;
   OPTIONS.gaptol=0.0001;
   OPTIONS.r0=0;
   OPTIONS.print=1;
   OPTIONS.rho=5;
   OPTIONS.zbar=norm(B,1);
   [STAT,y,Xv] = dsdp(b,AC,OPTIONS,y0);

   [objval,dobj,err] = derror(STAT,y,Xv,b,AC);
   X=dmat(Xv{1});
return;
%%=======================================================


% icp_demo.m
% run
% >> icp_demo
% in MATLAB

% size of model/data
n_model=2000;
n_data=1300;

% model points
model=4*randn(3,n_model)-2*ones(3,n_model);
bol=(model(1,:).^2+model(2,:).^2)<2^2;
while any(not(bol))
    model(:,not(bol))=4*randn(3,sum(not(bol)))-2*ones(3,sum(not(bol)));
    bol=(model(1,:).^2+model(2,:).^2)<2^2;
end
model(:,not(bol))=4*randn(3,sum(not(bol)))-2*ones(3,sum(not(bol)));
model(3,:)=0.5*(model(1,:).^2-model(2,:).^2);
bol=(model(1,:).^2+model(2,:).^2)<0.9^2;
model(3,bol)=3.5-3.5*(1/0.9)*sqrt(model(1,bol).^2+model(2,bol).^2);

% data points
data=model(:,1:n_data);

% weights
weights=ones(1,n_data);weights=rand(1,n_data);

% Transform points in data (move away from start position).
deg=15;
v1=2*(rand-0.5)*deg*(pi/180);
v2=2*(rand-0.5)*deg*(pi/180);
v3=2*(rand-0.5)*deg*(pi/180);
TR0=[cos(v1) sin(v1) 0;-sin(v1) cos(v1) 0;0 0 1];
TR0=TR0*[cos(v2) 0 sin(v2);0 1 0;-sin(v2) 0 cos(v2)];
TR0=TR0*[1 0 0;0 cos(v3) sin(v3);0 -sin(v3) cos(v3)];
TT0=3.0*(rand(3,1)-0.5);
data=TR0*data+repmat(TT0,1,n_data);

% randvec
rndvec=uint32(randperm(n_data)-1);

% sizerand, number of point-matchings in each iteration.
sizernd=ceil(1.45*n_data);

% Number of iterations.
iter=40;

% Compile the c++ files (not nessesacily if it is already done).
try
    make
end

% Create the kd-tree, TreeRoot is the pointer to the kd-tree
[tmp, tmp, TreeRoot] = kdtree( model', []);

% Run the ICP algorithm.
[R,T]=icpCpp(model,data,weights,rndvec,sizernd,TreeRoot,iter);

% Free allocated memory for the kd-tree.
kdtree([],[],TreeRoot);

figure(1),plot3(model(1,:),model(2,:),model(3,:),'r.',data(1,:),data(2,:),data(3,:),'c.'),hold off;

% Transform the data points.
data=R*data+repmat(T,1,n_data);

figure(2),plot3(model(1,:),model(2,:),model(3,:),'r.',data(1,:),data(2,:),data(3,:),'b.'),hold off;

clear functions