function res = ssc_W(Y,x,ta,sc,type,opC,rl)
% PURPOSE: Temporal disaggregation using the dynamic Chow-Lin method
%          proposed by Santos Silva-Cardoso (2001).
%          Without pretesting for intercept
% ------------------------------------------------------------
% SYNTAX: res = ssc_W(Y,x,ta,sc,type,opC,rl);
% ------------------------------------------------------------
% OUTPUT: res: a structure
%           res.meth    ='Santos Silva-Cardoso';
%           res.ta      = type of disaggregation
%           res.type    = method of estimation
%           res.opC     = option related to intercept
%           res.N       = nobs. of low frequency data
%           res.n       = nobs. of high-frequency data
%           res.pred    = number of extrapolations
%           res.sc      = frequency conversion between low and high freq.
%           res.p       = number of regressors (including intercept)
%           res.Y       = low frequency data
%           res.x       = high frequency indicators
%           res.y       = high frequency estimate
%           res.y_dt    = high frequency estimate: standard deviation
%           res.y_lo    = high frequency estimate: sd - sigma
%           res.y_up    = high frequency estimate: sd + sigma
%           res.u       = high frequency residuals
%           res.U       = low frequency residuals
%           res.gamma    = estimated model parameters (including y(0))
%           res.gamma_sd = estimated model parameters: standard deviation
%           res.gamma_t  = estimated model parameters: t ratios
%           res.phi      = dynamic parameter phi
%           res.beta     = estimated model parameters (excluding y(0))
%           res.beta_sd = estimated model parameters: standard deviation
%           res.beta_t  = estimated model parameters: t ratios
%           res.phi     = innovational parameter
%           res.aic     = Information criterion: AIC
%           res.bic     = Information criterion: BIC
%           res.val     = Objective function used by the estimation method
%           res.wls     = Weighted least squares as a function of phi
%           res.loglik  = Log likelihood as a function of phi
%           res.r       = grid of innovational parameters used by the estimation method
%           res.et       = elapsed time
% ------------------------------------------------------------
% INPUT: Y: Nx1 ---> vector of low frequency data
%        x: nxp ---> matrix of high frequency indicators (without intercept)
%        ta: type of disaggregation
%            ta=1 ---> sum (flow)
%            ta=2 ---> average (index)
%            ta=3 ---> last element (stock) ---> interpolation
%            ta=4 ---> first element (stock) ---> interpolation
%        sc: number of high frequency data points for each low frequency data points
%            sc= 4 ---> annual to quarterly
%            sc=12 ---> annual to monthly
%            sc= 3 ---> quarterly to monthly
%        type: estimation method:
%            type=0 ---> weighted least squares
%            type=1 ---> maximum likelihood
%        opC: 1x1 option related to intercept
%            opc = 0 : no intercept in hf model
%            opc = 1 : intercept in hf model
%        rl: innovational parameter
%            rl = []: 0x0 ---> rl=[0.05 0.99], 100 points grid
%            rl: 1x1 ---> fixed value of phi parameter
%            rl: 1x2 ---> [r_min r_max] search is performed
%                on this range, using a 200 points grid
% ------------------------------------------------------------
% LIBRARY: aggreg, criterion_SSC
% ------------------------------------------------------------
% SEE ALSO: chowlin, litterman, fernandez, td_plot, td_print
% ------------------------------------------------------------
% REFERENCE: Santos, J.M.C. and Cardoso, F.(2001) "The Chow-Lin method
% using dynamic models",Economic Modelling, vol. 18, p. 269-280.
% Di Fonzo, T. (2002) "Temporal disaggregation of economic time series:
% towards a dynamic extension", Dipartimento di Scienze Statistiche,
% Universita di Padova, Working Paper n. 2002-17.

% written by:
%  Enrique M. Quilis
%  Macroeconomic Research Department
%  Ministry of Economy and Competitiveness
%  <enrique.quilis@mineco.es>

% Version 2.1 [April 2012]

t0=clock;

% ------------------------------------------------------------
% Size of the problem

[N,M] = size(Y);    % Size of low-frequency input
[n,p] = size(x);    % Size of p high-frequency inputs (without intercept)

% ------------------------------------------------------------
% Initial checks

if ((opC ~= 0) & (opC ~= 1))
      error ('*** c has an invalid value ***');
end

% ------------------------------------------------------------
% Preparing the X matrix: including an intercept if opC==1

if (opC == 1)
   e=ones(n,1);
   x=[e x];       % Expanding the regressor matrix
   p=p+1;         % Number of p high-frequency inputs (plus intercept)
end

% ------------------------------------------------------------
% Generating the aggregation matrix

C = aggreg(ta,N,sc);

% -----------------------------------------------------------
% Expanding the aggregation matrix to perform
% extrapolation if needed.

if (n > sc * N)
   pred = n - sc*N;           % Number of required extrapolations
   C = [C zeros(N,pred)];
else
   pred = 0;
end

% -----------------------------------------------------------
% Temporal aggregation of the indicators

X = C*x;

% -----------------------------------------------------------
% Optimization of objective funcion (Lik. or WLS)

nrl = length(rl);

switch nrl
    case 0 %Default
        rl = [0.05 0.99];
        rex = criterion_SSC(Y,x,ta,type,opC,rl,X,C,N,n,p);
        phi = rex.phi;
        r   = rex.r;
    case 1 %Fixed innovational parameter
        phi = rl;
        type = 4;
    case 2
        rex = criterion_SSC(Y,x,ta,type,opC,rl,X,C,N,n,p);
        phi = rex.phi;
        r   = rex.r;
    otherwise
        error('*** Grid search on phi is improperly defined. Check rl ***');
end

% -----------------------------------------------------------
% Final estimation with optimal phi
% -----------------------------------------------------------

% Auxiliary matrix useful to simplify computations

I=eye(n); w=I;
LL = diag(-ones(n-1,1),-1);

% -----------------------------------------------------------
% Final estimation with optimal phi

% -----------------------------------------------------------
% Generation of difference matrix D_phi

D_phi = I + phi*LL;
D_phi(1,1)=sqrt(1-phi^2);
iD_phi = inv(D_phi);

% -----------------------------------------------------------
% Truncation remainder: q parameter

q=zeros(n,1);
q(1)=phi;

% -----------------------------------------------------------
% Expanded set of regressors: high and low frequency

z = [x q];
z_phi = iD_phi * z;
Z_phi = C * z_phi;

% -----------------------------------------------------------
% GLS estimator of gamma

w = inv(D_phi' * D_phi);
W = C * w * C';
iW = inv(W);

gamma = (Z_phi' * iW * Z_phi) \ (Z_phi' * iW * Y); % gamma GLS

U = Y - Z_phi * gamma;           % Low frequency residuals
scp = U' * iW * U;               % Weighted least squares
sigma_a = scp/(N-p-1);           % sigma_a estimator (p+1 due to lagged endogenous)
L = w * C' * iW;                 % Filtering matrix
u = L * U;                       % High frequency residuals

% -----------------------------------------------------------
% Temporally disaggregated time series

y = z_phi * gamma + u;

% -----------------------------------------------------------
% Information criteria
% p is expanded to include lagged endogenous

aic=log(sigma_a)+2*(p+1)/N;
bic=log(sigma_a)+log(N)*(p+1)/N;

% -----------------------------------------------------------
% VCV matrix of high frequency estimates

sigma_gamma = sigma_a * inv(Z_phi' * iW * Z_phi);

VCV_y = sigma_a * (eye(n)-L*C) * w + (z_phi-L*Z_phi) * sigma_gamma * (z_phi-L*Z_phi)';

d_y=sqrt((diag(VCV_y)));   % Std. dev. of high frequency estimates
y_li=y-d_y;                % Lower lim. of high frequency estimates
y_ls=y+d_y;                % Upper lim. of high frequency estimates

% -----------------------------------------------------------
% -----------------------------------------------------------
% Loading the structure

res.meth='Santos Silva-Cardoso';

% -----------------------------------------------------------
% Basic parameters

res.ta        = ta;
res.type      = type;
res.N         = N;
res.n         = n;
res.pred      = pred;
res.sc        = sc;
res.p         = p;
res.opC       = opC;
res.rl        = rl;

% -----------------------------------------------------------
% Series

res.Y         = Y;
res.x         = x;
res.y         = y;
res.y_dt      = d_y;
res.y_lo      = y_li;
res.y_up      = y_ls;

% -----------------------------------------------------------
% Residuals

res.u         = u;
res.U         = U;

% -----------------------------------------------------------
% Parameters

res.gamma     = gamma;
res.gamma_sd  = sqrt(diag(sigma_gamma));
res.gamma_t   = gamma./sqrt(diag(sigma_gamma));
res.phi       = phi;

res.beta      = gamma(1:end-1);
res.beta_sd   = res.gamma_sd(1:end-1);
res.beta_t    = res.gamma_t(1:end-1);
res.sigma_a   = sigma_a;

% -----------------------------------------------------------
% Information criteria

res.aic       = aic;
res.bic       = bic;

% -----------------------------------------------------------
% Objective function

if (nrl == 1)
   res.val      = [];
   res.wls      = [];
   res.loglik   = [];
   res.r        = [];
else
   res.val       = rex.val;
   res.wls  	 = rex.wls;
   res.loglik    = rex.loglik;
   res.r         = r;
end

% -----------------------------------------------------------
% Elapsed time

res.et  = etime(clock,t0);


function latLonEnd=directRhumbProbGen(latLonStart,azimuth,dist,height,useHeightApprox,a,f,numSteps4Circ)
%%DIRECTRHUMBLINEPROBGEN Given a starting location near the surface of the
%                    reference ellipsoid as well as a constant heading a
%                    distance to travel, determine the location of a vessel
%                    traveling at the constant heading after traveling the
%                    distance. A constant heading trajectory is a rhumb
%                    line (loxodrome) and is usually not the shortest path
%                    between two points. This function is general in that
%                    it will also solve the direct rhumb problem at a fixed
%                    height above the reference ellipsoid, if desired.
%
%INPUTS: latLonStart A 2X1 vector of the starting ellipsoidal latitude
%                    (North) and longitude (East) in radians.
%            azimuth The constant heading that is to be traveled in
%                    radians East of North.
%               dist The distance that is to be traveled on a constant
%                    heading course.
%             height The heights above the reference ellipsoid at which the
%                    trajectory should be determined. If this parameter
%                    is omitted, then the default value of 0 is used.
%    useHeightApprox If true, and the height is not zero, then a distance
%                    scaling height approximation is used. An equatorial
%                    trajectory traveling a distance dist at a height h
%                    above the reference ellipsoid will only travel
%                    a/(a+h)*dist over the surface. If useHeightApprox is
%                    true, then this scaling applied to any trajectry
%                    (including non-equatorial). If useHeightApprox is
%                    false, a slow iterative optimization used. The default
%                    value if omitted or an empty matrix is passed is
%                    false. This parameter is ignored if height=0.
%                  a The semi-major axis of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84SemiMajorAxis is used.
%                  f The flattening factor of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84Flattening is used.
%      numSteps4Circ If height!=0, and useHeightApprox=false, then an
%                    algorithm propagating a state in ECEF coordinates
%                    around the curved Earth is used. This parameter
%                    determines the number of steps that would be needed
%                    for a target that circumnavigates the globe around the
%                    equator. The default value if this parameter is not
%                    provided is 2000. A value of 6000 appears to be about
%                    the best number for overall precision. Reducing the
%                    number of steps will speed up the function. This
%                    parameter is not used if height=0.
%
%OUTPUTS: latLonEnd The ellipsoidal latitude and longitude that one will be
%                    after starting at latLonStart and traveling a distance
%                    of dist at a constant heading on azimuth.
%
%For zero-altitude, the directRhumbProb function is called. The solution
%for a non-zero height is based on [1]. The function is significantly
%faster if a zero hight is used or if useHeightApprox=true, in which case
%the approximation described in [2] is used.
%
%The algorithm is generally the opposite of indirectRhumbProblem, except
%when a starting or ending point is at a geographic pole, as discussed in
%the comments to the indirectRhumbProblem function.
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%[2] D. F. Crouse, "Worldwide Ground Target State Propagation," in
%    Proceedings of the 23rd International Conference on Information
%    Fusion, 6-9 Jul. 2020.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<8||isempty(numSteps4Circ))
%This is the number of steps that would be taken to travel a distance of
%2*pi*a (circumnavigate the globe the long way). This is a design parameter
%that should be large enough to ensure that the results are accurate.
%However, if it is too large, finite precision errors will accumulate.
    numSteps4Circ=2000;
end

if(nargin<7||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<6||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<5||isempty(useHeightApprox))
    useHeightApprox=false;
end

if(nargin<4||isempty(height))
    height=0;
end

if(useHeightApprox)
    %Scale the distance based on the height and then solve the rumb problem
    %with the scaled distance.
    dist=a/(a+height)*dist;
    height=0;
end

if(height==0)
    latLonEnd=directRhumbProblem(latLonStart,azimuth,dist,0,a,f);
    return
else
    %If the height is not zero, then use the algorithm for propagating a
    %level, non-maneuvering dynamic model along a rhumb line forward in
    %time.

    %This is the number of Runge-Kutta steps on a curved Earth that will be
    %performed.
    numSteps=ceil(numSteps4Circ*(dist/(2*pi*a)));
    stepSize=1/numSteps;

    %We have to calculate the initial location and heading in CARTESIAN
    %ECEF coordinates.
    xyzInit=ellips2Cart([latLonStart;height],a,f);

    %The initial heading in the LOCAL East-North-Up tangent-plane
    %coordinate system for level flight.
    uh=[sin(azimuth);cos(azimuth);0];

    %The speed of the target is such that it travels a distance of dist in
    %1 unit of time.
    speed=dist;

    %The initial target state with the position in the GLOBAL ECEF
    %coordinate system and the velocity in a LOCAL flat-Earth coordinate
    %system.
    xInit=[xyzInit;speed*uh];

    aDyn=@(x,t)aPoly(x,t,3);%A constant velocity model.

    %When traveling along a rhumb-line, the local coordinate system is
    %known at all times: it is the local ENU coordinate system. The local
    %basis vectors are deterministially known at all places on the Earth.
    uDet=@(x,t)getENUAxes(Cart2Ellipse(x(1:3),[],a,f));
    xList=RungeKCurvedAtTimes(xInit,[],[0;1],aDyn,uDet,stepSize,4);
    plhEnd=Cart2Ellipse(xList(1:3,end),[],a,f);
    latLonEnd=plhEnd(1:2);%The Cartesian location at the end of the
                          %trajectory.
end
end

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


function [W, stat] = gsp_learn_graph_log_degrees(Z, a, b, params)
%GSP_LEARN_GRAPH_LOG_DEGREES Learn graph from pairwise distances using negative log prior on nodes degrees
%   Usage:  [W, stat] = gsp_learn_graph_log_degrees(Z, a, b)
%           [W, stat] = gsp_learn_graph_log_degrees(Z, a, b, params)
%
%   Inputs:
%         Z         : Matrix with (squared) pairwise distances of nodes
%         a         : Log prior constant  (bigger a -> bigger weights in W)
%         b         : ||W||_F^2 prior constant  (bigger b -> more dense W)
%         params    : Optional parameters
%
%   Outputs:
%         W         : Weighted adjacency matrix
%         stat      : Optional output statistics (adds small overhead)
%
%
%   'W = gsp_learn_graph_log_degrees(Z, a, b, params)' computes a weighted
%   adjacency matrix $W$ from squared pairwise distances in $Z$, using the
%   smoothness assumption that $\text{trace}(X^TLX)$ is small, where $X$ is
%   the data (columns) changing smoothly from node to node on the graph and
%   $L = D-W$ is the combinatorial graph Laplacian. See the paper of the
%   references for the theory behind the algorithm.
%
%   Alternatively, Z can contain other types of distances and use the
%   smoothness assumption that
%
%   .. sum(sum(W .* Z))
%
%   .. math:: \sum_i\sum_j W_{ij}Z_{ij}
%
%   is small.
%
%   The minimization problem solved is
%
%   .. minimize_W sum(sum(W .* Z)) - a * sum(log(sum(W))) + b * ||W||_F^2/2 + c * ||W-W_0||_F^2/2
%
%   .. math:: \min_W \sum_i\sum_j W_{ij}Z_{ij} -\alpha\sum_i\log(\sum_jW_{ij}) + \frac{\beta}{2} \|W\|_F^2 + c/2 ||W - W0||_F^2
%
%   subject to $W$ being a valid weighted adjacency matrix (non-negative,
%   symmetric, with zero diagonal).
%
%   The algorithm used is forward-backward-forward (FBF) based primal dual
%   optimization (see references).
%
%   Example:::
%
%         G = gsp_sensor(256);
%         f1 = @(x,y) sin((2-x-y).^2);
%         f2 = @(x,y) cos((x+y).^2);
%         f3 = @(x,y) (x-.5).^2 + (y-.5).^3 + x - y;
%         f4 = @(x,y) sin(3*((x-.5).^2+(y-.5).^2));
%         X = [f1(G.coords(:,1), G.coords(:,2)), f2(G.coords(:,1), G.coords(:,2)), f3(G.coords(:,1), G.coords(:,2)), f4(G.coords(:,1), G.coords(:,2))];
%         figure; subplot(2,2,1); gsp_plot_signal(G, X(:,1)); title('1st smooth signal');
%         subplot(2,2,2); gsp_plot_signal(G, X(:,2)); title('2nd smooth signal');
%         subplot(2,2,3); gsp_plot_signal(G, X(:,3)); title('3rd smooth signal');
%         subplot(2,2,4); gsp_plot_signal(G, X(:,4)); title('4th smooth signal');
%         Z = gsp_distanz(X').^2;
%         % we can multiply the pairwise distances with a number to control sparsity
%         [W] = gsp_learn_graph_log_degrees(Z*25, 1, 1);
%         % clean up zeros
%         W(W<1e-5) = 0;
%         G2 = gsp_update_weights(G, W);
%         figure; gsp_plot_graph(G2); title('Graph with edges learned from above 4 signals');
%
%
%   Additional parameters
%   ---------------------
%
%   * *params.W_init*   : Initialization point. default: zeros(size(Z))
%   * *verbosity*       : Default = 1. Above 1 adds a small overhead
%   * *maxit*           : Maximum number of iterations. Default: 1000
%   * *tol*             : Tolerance for stopping criterion. Defaul: 1e-5
%   * *step_size*       : Step size from the interval (0,1). Default: 0.5
%   * *max_w*           : Maximum weight allowed for each edge (or inf)
%   * *w_0*             : Vector for adding prior c/2*||w - w_0||^2
%   * *c*               : multiplier for prior c/2*||w - w_0||^2 if w_0 given
%   * *fix_zeros*       : Fix a set of edges to zero (true/false)
%   * *edge_mask*       : Mask indicating the non zero edges if "fix_zeros"
%
%   If fix_zeros is set, an edge_mask is needed. Only the edges
%   corresponding to the non-zero values in edge_mask will be learnt. This
%   has two applications: (1) for large scale applications it is cheaper to
%   learn a subset of edges. (2) for some applications we don't want some
%   connections to be allowed, for example for locality on images.
%
%   The cost of each iteration is linear to the number of edges to be
%   learned, or the square of the number of nodes (numel(Z)) if fix_zeros
%   is not set.
%
%   The function is using the UNLocBoX functions sum_squareform and
%   squareform_sp.
%   The stopping criterion is whether both relative primal and dual
%   distance between two iterations are below a given tolerance.
%
%   To set the step size use the following rule of thumb: Set it so that
%   relative change of primal and dual converge with similar rates (use
%   verbosity > 1).
%
%   See also: gsp_learn_graph_l2_degrees gsp_distanz gsp_update_weights
%       squareform_sp sum_squareform gsp_compute_graph_learning_theta
%
%   References: kalofolias2016learn komodakis2015playing kalofolias2017large
%

% Author: Vassilis Kalofolias
% Testing: gsp_test_learn_graph
% Date: June 2015

%% Default parameters
if nargin < 4
    params = struct;
end

if not(isfield(params, 'verbosity')),   params.verbosity = 1;   end
if not(isfield(params, 'maxit')),       params.maxit = 1000;      end
if not(isfield(params, 'tol')),         params.tol = 1e-5;      end
if not(isfield(params, 'step_size')),   params.step_size = .5;      end     % from (0, 1)
if not(isfield(params, 'fix_zeros')),   params.fix_zeros = false;      end
if not(isfield(params, 'max_w')),       params.max_w = inf;         end

%% Fix parameter size and initialize
if isvector(Z)
    z = Z;  % lazy copying of matlab doesn't allocate new memory for z
else
    z = squareform_sp(Z);
end
% clear Z   % for large scale computation

z = z(:);
l = length(z);                      % number of edges
% n(n-1)/2 = l => n = (1 + sqrt(1+8*l))/ 2
n = round((1 + sqrt(1+8*l))/ 2);    % number of nodes

if isfield(params, 'w_0')
    if not(isfield(params, 'c'))
        error('When params.w_0 is specified, params.c should also be specified');
    else
        c = params.c;
    end
    if isvector(params.w_0)
        w_0 = params.w_0;
    else
        w_0 = squareform_sp(params.w_0);
    end
    w_0 = w_0(:);
else
    w_0 = 0;
end

% if sparsity pattern is fixed we optimize with respect to a smaller number
% of variables, all included in w
if params.fix_zeros
    if not(isvector(params.edge_mask))
        params.edge_mask = squareform_sp(params.edge_mask);
    end
    % use only the non-zero elements to optimize
    ind = find(params.edge_mask(:));
    z = full(z(ind));
    if not(isscalar(w_0))
        w_0 = full(w_0(ind));
    end
else
    z = full(z);
    w_0 = full(w_0);
end

w = zeros(size(z));

%% Needed operators
% S*w = sum(W)
if params.fix_zeros
    [S, St] = sum_squareform(n, params.edge_mask(:));
else
    [S, St] = sum_squareform(n);
end

% S: edges -> nodes
K_op = @(w) S*w;

% S': nodes -> edges
Kt_op = @(z) St*z;

if params.fix_zeros
    norm_K = normest(S);
    % approximation:
    % sqrt(2*(n-1)) * sqrt(nnz(params.edge_mask) / (n*(n+1)/2)) /sqrt(2)
else
    % the next is an upper bound if params.fix_zeros
    norm_K = sqrt(2*(n-1));
end

%% TODO: Rescaling??
% we want    h.beta == norm_K   (see definition of mu)
% we can multiply all terms by s = norm_K/2*b so new h.beta==2*b*s==norm_K

%% Learn the graph
% min_{W>=0}     tr(X'*L*X) - gc * sum(log(sum(W))) + gp * norm(W-W0,'fro')^2, where L = diag(sum(W))-W
% min_W       I{W>=0} + W(:)'*Dx(:)  - gc * sum(log(sum(W))) + gp * norm(W-W0,'fro')^2
% min_W                f(W)          +       g(L_op(W))      +   h(W)

% put proximal of trace plus positivity together
f.eval = @(w) 2*w'*z;    % half should be counted
%f.eval = @(W) 0;
f.prox = @(w, c) min(params.max_w, max(0, w - 2*c*z));  % all change the same

param_prox_log.verbose = params.verbosity - 3;
g.eval = @(z) -a * sum(log(z));
g.prox = @(z, c) prox_sum_log(z, c*a, param_prox_log);
% proximal of conjugate of g: z-c*g.prox(z/c, 1/c)
g_star_prox = @(z, c) z - c*a * prox_sum_log(z/(c*a), 1/(c*a), param_prox_log);

if w_0 == 0
    % "if" not needed, for c = 0 both are the same but gains speed
    h.eval = @(w) b * norm(w)^2;
    h.grad = @(w) 2 * b * w;
    h.beta = 2 * b;
else
    h.eval = @(w) b * norm(w)^2 + c * norm(w - w_0,'fro')^2;
    h.grad = @(w) 2 * ((b+c) * w - c * w_0);
    h.beta = 2 * (b+c);
end

%% My custom FBF based primal dual (see [1] = [Komodakis, Pesquet])
% parameters mu, epsilon for convergence (see [1])
mu = h.beta + norm_K;     %TODO: is it squared or not??
epsilon = lin_map(0.0, [0, 1/(1+mu)], [0,1]);   % in (0, 1/(1+mu) )

% INITIALIZATION
% primal variable ALREADY INITIALIZED
%w = params.w_init;
% dual variable
v_n = K_op(w);
if nargout > 1 || params.verbosity > 1
    stat.f_eval = nan(params.maxit, 1);
    stat.g_eval = nan(params.maxit, 1);
    stat.h_eval = nan(params.maxit, 1);
    stat.fgh_eval = nan(params.maxit, 1);
    stat.pos_violation = nan(params.maxit, 1);
end
if params.verbosity > 1
    fprintf('Relative change of primal, dual variables, and objective fun\n');
end

tic
gn = lin_map(params.step_size, [epsilon, (1-epsilon)/mu], [0,1]);              % in [epsilon, (1-epsilon)/mu]
for i = 1:params.maxit
    Y_n = w - gn * (h.grad(w) + Kt_op(v_n));
    y_n = v_n + gn * (K_op(w));
    P_n = f.prox(Y_n, gn);
    p_n = g_star_prox(y_n, gn); % = y_n - gn*g_prox(y_n/gn, 1/gn)
    Q_n = P_n - gn * (h.grad(P_n) + Kt_op(p_n));
    q_n = p_n + gn * (K_op(P_n));

    if nargout > 1 || params.verbosity > 2
        stat.f_eval(i) = f.eval(w);
        stat.g_eval(i) = g.eval(K_op(w));
        stat.h_eval(i) = h.eval(w);
        stat.fgh_eval(i) = stat.f_eval(i) + stat.g_eval(i) + stat.h_eval(i);
        stat.pos_violation(i) = -sum(min(0,w));
    end
    rel_norm_primal = norm(- Y_n + Q_n, 'fro')/norm(w, 'fro');
    rel_norm_dual = norm(- y_n + q_n)/norm(v_n);

    if params.verbosity > 3
        fprintf('iter %4d: %6.4e   %6.4e   %6.3e', i, rel_norm_primal, rel_norm_dual, stat.fgh_eval(i));
    elseif params.verbosity > 2
        fprintf('iter %4d: %6.4e   %6.4e   %6.3e\n', i, rel_norm_primal, rel_norm_dual, stat.fgh_eval(i));
    elseif params.verbosity > 1
        fprintf('iter %4d: %6.4e   %6.4e\n', i, rel_norm_primal, rel_norm_dual);
    end

    w = w - Y_n + Q_n;
    v_n = v_n - y_n + q_n;

    if rel_norm_primal < params.tol && rel_norm_dual < params.tol
        break
    end
end
stat.time = toc;
if params.verbosity > 0
    fprintf('# iters: %4d. Rel primal: %6.4e Rel dual: %6.4e  OBJ %6.3e\n', i, rel_norm_primal, rel_norm_dual, f.eval(w) + g.eval(K_op(w)) + h.eval(w));
    fprintf('Time needed is %f seconds\n', stat.time);
end

% use the following for testing:
% g.L = K_op;
% g.Lt = Kt_op;
% g.norm_L = norm_K;
% [w, info] = fbf_primal_dual(w, f, g, h, params);
% %[w, info] = fb_based_primal_dual(w, f, g, h, params);

%%

if params.verbosity > 3
    figure; plot(real([stat.f_eval, stat.g_eval, stat.h_eval])); hold all; plot(real(stat.fgh_eval), '.'); legend('f', 'g', 'h', 'f+g+h');
    figure; plot(stat.pos_violation); title('sum of negative (invalid) values per iteration')
    figure; semilogy(max(0,-diff(real(stat.fgh_eval'))),'b.-'); hold on; semilogy(max(0,diff(real(stat.fgh_eval'))),'ro-'); title('|f(i)-f(i-1)|'); legend('going down','going up');
end

if params.fix_zeros
    w = sparse(ind, ones(size(ind)), w, l, 1);
end

if isvector(Z)
    W = w;
else
    W = squareform_sp(w);
end


function f = niederreiter_mccurley4 ( x )

%*****************************************************************************80
%
%% NIEDERREITER_MCCURLEY4 evaluates the Niederreiter-McCurley function 4.
%
%  Discussion:
%
%    The spatial dimension is taken as 4.
%
%    The maximum is sought within the unit hypercube.
%
%    The reported maximum value occurs at approximately
%
%      X = [ 1, 1, 1, 1 ]
%
%    with
%
%      F(X) = 9.196986029286059
%
%    Note that FMINCON is designed to minimize functions, so we must negate
%    the output value here in order to satisfy FMINCON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, real X(N), the argument of the objection function.
%
%    Output, real F, the value of the objective function.
%
  f = - 100 * prod ( x ) * exp ( - x(4) ) ./ ( 1 + x(1) * x(2) * x(3) ).^2;

  return
end


function [out] = GLCM_Features4(glcmin,pairs)
%
%
% This is an update of GLCM_Features2 (vectorized) without ismember()
%
% GLCM_Features2 helps to calculate the features from the different GLCMs
% that are input to the function. The GLCMs are stored in a i x j x n
% matrix, where n is the number of GLCMs calculated usually due to the
% different orientation and displacements used in the algorithm. Usually
% the values i and j are equal to 'NumLevels' parameter of the GLCM
% computing function graycomatrix(). Note that matlab quantization values
% belong to the set {1,..., NumLevels} and not from {0,...,(NumLevels-1)}
% as provided in some references
% http://www.mathworks.com/access/helpdesk/help/toolbox/images/graycomatrix
% .html
%
% This vectorized version of GLCM_FEatures1.m reduces the 19 'for' loops
% used in the earlier code to 5 'for' loops
% http://blogs.mathworks.com/loren/2006/07/12/what-are-you-really-measuring
% /
% Using tic toc and cputime as in above discussion
%
% Although there is a function graycoprops() in Matlab Image Processing
% Toolbox that computes four parameters Contrast, Correlation, Energy,
% and Homogeneity. The paper by Haralick suggests a few more parameters
% that are also computed here. The code is not fully vectorized and hence
% is not an efficient implementation but it is easy to add new features
% based on the GLCM using this code. Takes care of 3 dimensional glcms
% (multiple glcms in a single 3D array)
%
% If you find that the values obtained are different from what you expect
% or if you think there is a different formula that needs to be used
% from the ones used in this code please let me know.
% A few questions which I have are listed in the link
% http://www.mathworks.com/matlabcentral/newsreader/view_thread/239608
%
%
%
% Features computed
% Autocorrelation: [2]                      (out.autoc)
% Contrast: matlab/[1,2]                    (out.contr)
% Correlation: matlab                       (out.corrm)
% Correlation: [1,2]                        (out.corrp)
% Cluster Prominence: [2]                   (out.cprom)
% Cluster Shade: [2]                        (out.cshad)
% Dissimilarity: [2]                        (out.dissi)
% Energy: matlab / [1,2]                    (out.energ)
% Entropy: [2]                              (out.entro)
% Homogeneity: matlab                       (out.homom)
% Homogeneity: [2]                          (out.homop)
% Maximum probability: [2]                  (out.maxpr)
% Sum of squares: Variance [1]              (out.sosvh)
% Sum average [1]                           (out.savgh)
% Sum variance [1]                          (out.svarh)
% Sum entropy [1]                           (out.senth)
% Difference variance [1]                   (out.dvarh)
% Difference entropy [1]                    (out.denth)
% Information measure of correlation1 [1]   (out.inf1h)
% Informaiton measure of correlation2 [1]   (out.inf2h)
% Inverse difference (INV) is homom [3]     (out.homom)
% Inverse difference normalized (INN) [3]   (out.indnc)
% Inverse difference moment normalized [3]  (out.idmnc)
%
% The maximal correlation coefficient was not calculated due to
% computational instability
% http://murphylab.web.cmu.edu/publications/boland/boland_node26.html
%
% Formulae from MATLAB site (some look different from
% the paper by Haralick but are equivalent and give same results)
% Example formulae:
% Contrast = sum_i(sum_j(  (i-j)^2 * p(i,j) ) ) (same in matlab/paper)
% Correlation = sum_i( sum_j( (i - u_i)(j - u_j)p(i,j)/(s_i.s_j) ) ) (m)
% Correlation = sum_i( sum_j( ((ij)p(i,j) - u_x.u_y) / (s_x.s_y) ) ) (p[2])
% Energy = sum_i( sum_j( p(i,j)^2 ) )           (same in matlab/paper)
% Homogeneity = sum_i( sum_j( p(i,j) / (1 + |i-j|) ) ) (as in matlab)
% Homogeneity = sum_i( sum_j( p(i,j) / (1 + (i-j)^2) ) ) (as in paper)
%
% Where:
% u_i = u_x = sum_i( sum_j( i.p(i,j) ) ) (in paper [2])
% u_j = u_y = sum_i( sum_j( j.p(i,j) ) ) (in paper [2])
% s_i = s_x = sum_i( sum_j( (i - u_x)^2.p(i,j) ) ) (in paper [2])
% s_j = s_y = sum_i( sum_j( (j - u_y)^2.p(i,j) ) ) (in paper [2])
%
%
% Normalize the glcm:
% Compute the sum of all the values in each glcm in the array and divide
% each element by it sum
%
% Haralick uses 'Symmetric' = true in computing the glcm
% There is no Symmetric flag in the Matlab version I use hence
% I add the diagonally opposite pairs to obtain the Haralick glcm
% Here it is assumed that the diagonally opposite orientations are paired
% one after the other in the matrix
% If the above assumption is true with respect to the input glcm then
% setting the flag 'pairs' to 1 will compute the final glcms that would result
% by setting 'Symmetric' to true. If your glcm is computed using the
% Matlab version with 'Symmetric' flag you can set the flag 'pairs' to 0
%
% References:
% 1. R. M. Haralick, K. Shanmugam, and I. Dinstein, Textural Features of
% Image Classification, IEEE Transactions on Systems, Man and Cybernetics,
% vol. SMC-3, no. 6, Nov. 1973
% 2. L. Soh and C. Tsatsoulis, Texture Analysis of SAR Sea Ice Imagery
% Using Gray Level Co-Occurrence Matrices, IEEE Transactions on Geoscience
% and Remote Sensing, vol. 37, no. 2, March 1999.
% 3. D A. Clausi, An analysis of co-occurrence texture statistics as a
% function of grey level quantization, Can. J. Remote Sensing, vol. 28, no.
% 1, pp. 45-62, 2002
% 4. http://murphylab.web.cmu.edu/publications/boland/boland_node26.html
%
%
% Example:
%
% Usage is similar to graycoprops() but needs extra parameter 'pairs' apart
% from the GLCM as input
% I = imread('circuit.tif');
% GLCM2 = graycomatrix(I,'Offset',[2 0;0 2]);
% stats = GLCM_features4(GLCM2,0)
% The output is a structure containing all the parameters for the different
% GLCMs
%
% [Avinash Uppuluri: avinash_uv@yahoo.com: Last modified: 04/05/2010]

% If 'pairs' not entered: set pairs to 0
if ((nargin > 2) || (nargin == 0))
   error('Too many or too few input arguments. Enter GLCM and pairs.');
elseif ( (nargin == 2) )
    if ((size(glcmin,1) <= 1) || (size(glcmin,2) <= 1))
       error('The GLCM should be a 2-D or 3-D matrix.');
    elseif ( size(glcmin,1) ~= size(glcmin,2) )
        error('Each GLCM should be square with NumLevels rows and NumLevels cols');
    end
elseif (nargin == 1) % only GLCM is entered
    pairs = 0; % default is numbers and input 1 for percentage
    if ((size(glcmin,1) <= 1) || (size(glcmin,2) <= 1))
       error('The GLCM should be a 2-D or 3-D matrix.');
    elseif ( size(glcmin,1) ~= size(glcmin,2) )
       error('Each GLCM should be square with NumLevels rows and NumLevels cols');
    end
end

format long e
if (pairs == 1)
    newn = 1;
    for nglcm = 1:2:size(glcmin,3)
        glcm(:,:,newn)  = glcmin(:,:,nglcm) + glcmin(:,:,nglcm+1);
        newn = newn + 1;
    end
elseif (pairs == 0)
    glcm = glcmin;
end

size_glcm_1 = size(glcm,1);
size_glcm_2 = size(glcm,2);
size_glcm_3 = size(glcm,3);

% checked
out.autoc = zeros(1,size_glcm_3); % Autocorrelation: [2]
out.contr = zeros(1,size_glcm_3); % Contrast: matlab/[1,2]
out.corrm = zeros(1,size_glcm_3); % Correlation: matlab
out.corrp = zeros(1,size_glcm_3); % Correlation: [1,2]
out.cprom = zeros(1,size_glcm_3); % Cluster Prominence: [2]
out.cshad = zeros(1,size_glcm_3); % Cluster Shade: [2]
out.dissi = zeros(1,size_glcm_3); % Dissimilarity: [2]
out.energ = zeros(1,size_glcm_3); % Energy: matlab / [1,2]
out.entro = zeros(1,size_glcm_3); % Entropy: [2]
out.homom = zeros(1,size_glcm_3); % Homogeneity: matlab
out.homop = zeros(1,size_glcm_3); % Homogeneity: [2]
out.maxpr = zeros(1,size_glcm_3); % Maximum probability: [2]

out.sosvh = zeros(1,size_glcm_3); % Sum of sqaures: Variance [1]
out.savgh = zeros(1,size_glcm_3); % Sum average [1]
out.svarh = zeros(1,size_glcm_3); % Sum variance [1]
out.senth = zeros(1,size_glcm_3); % Sum entropy [1]
out.dvarh = zeros(1,size_glcm_3); % Difference variance [4]
%out.dvarh2 = zeros(1,size_glcm_3); % Difference variance [1]
out.denth = zeros(1,size_glcm_3); % Difference entropy [1]
out.inf1h = zeros(1,size_glcm_3); % Information measure of correlation1 [1]
out.inf2h = zeros(1,size_glcm_3); % Informaiton measure of correlation2 [1]
%out.mxcch = zeros(1,size_glcm_3);% maximal correlation coefficient [1]
%out.invdc = zeros(1,size_glcm_3);% Inverse difference (INV) is homom [3]
out.indnc = zeros(1,size_glcm_3); % Inverse difference normalized (INN) [3]
out.idmnc = zeros(1,size_glcm_3); % Inverse difference moment normalized [3]

glcm_sum  = zeros(size_glcm_3,1);
glcm_mean = zeros(size_glcm_3,1);
glcm_var  = zeros(size_glcm_3,1);

% http://www.fp.ucalgary.ca/mhallbey/glcm_mean.htm confuses the range of
% i and j used in calculating the means and standard deviations.
% As of now I am not sure if the range of i and j should be [1:Ng] or
% [0:Ng-1]. I am working on obtaining the values of mean and std that get
% the values of correlation that are provided by matlab.
u_x = zeros(size_glcm_3,1);
u_y = zeros(size_glcm_3,1);
s_x = zeros(size_glcm_3,1);
s_y = zeros(size_glcm_3,1);

% checked p_x p_y p_xplusy p_xminusy
p_x = zeros(size_glcm_1,size_glcm_3); % Ng x #glcms[1]
p_y = zeros(size_glcm_2,size_glcm_3); % Ng x #glcms[1]
p_xplusy = zeros((size_glcm_1*2 - 1),size_glcm_3); %[1]
p_xminusy = zeros((size_glcm_1),size_glcm_3); %[1]
% checked hxy hxy1 hxy2 hx hy
hxy  = zeros(size_glcm_3,1);
hxy1 = zeros(size_glcm_3,1);
hx   = zeros(size_glcm_3,1);
hy   = zeros(size_glcm_3,1);
hxy2 = zeros(size_glcm_3,1);

corm = zeros(size_glcm_3,1);
corp = zeros(size_glcm_3,1);

for k = 1:size_glcm_3

    glcm_sum(k) = sum(sum(glcm(:,:,k)));
    glcm(:,:,k) = glcm(:,:,k)./glcm_sum(k); % Normalize each glcm
    glcm_mean(k) = mean2(glcm(:,:,k)); % compute mean after norm
    glcm_var(k)  = (std2(glcm(:,:,k)))^2;

    for i = 1:size_glcm_1

        for j = 1:size_glcm_2
            p_x(i,k) = p_x(i,k) + glcm(i,j,k);
            p_y(i,k) = p_y(i,k) + glcm(j,i,k); % taking i for j and j for i
            %if (ismember((i + j),[2:2*size_glcm_1]))
                p_xplusy((i+j)-1,k) = p_xplusy((i+j)-1,k) + glcm(i,j,k);
            %end
            %if (ismember(abs(i-j),[0:(size_glcm_1-1)]))
                p_xminusy((abs(i-j))+1,k) = p_xminusy((abs(i-j))+1,k) +...
                    glcm(i,j,k);
            %end
        end
    end

end

% marginal probabilities are now available [1]
% p_xminusy has +1 in index for matlab (no 0 index)
% computing sum average, sum variance and sum entropy:

%Q    = zeros(size(glcm));

i_matrix  = repmat([1:size_glcm_1]',1,size_glcm_2);
j_matrix  = repmat([1:size_glcm_2],size_glcm_1,1);
% i_index = [ 1 1 1 1 1 .... 2 2 2 2 2 ... ]
i_index   = j_matrix(:);
% j_index = [ 1 2 3 4 5 .... 1 2 3 4 5 ... ]
j_index   = i_matrix(:);
xplusy_index = [1:(2*(size_glcm_1)-1)]';
xminusy_index = [0:(size_glcm_1-1)]';
mul_contr = abs(i_matrix - j_matrix).^2;
mul_dissi = abs(i_matrix - j_matrix);
%div_homop = ( 1 + mul_contr); % used from the above two formulae
%div_homom = ( 1 + mul_dissi);

for k = 1:size_glcm_3 % number glcms

    out.contr(k) = sum(sum(mul_contr.*glcm(:,:,k)));
    out.dissi(k) = sum(sum(mul_dissi.*glcm(:,:,k)));
    out.energ(k) = sum(sum(glcm(:,:,k).^2));
    out.entro(k) = - sum(sum((glcm(:,:,k).*log(glcm(:,:,k) + eps))));
    out.homom(k) = sum(sum((glcm(:,:,k)./( 1 + mul_dissi))));
    out.homop(k) = sum(sum((glcm(:,:,k)./( 1 + mul_contr))));
    % [1] explains sum of squares variance with a mean value;
    % the exact definition for mean has not been provided in
    % the reference: I use the mean of the entire normalized glcm
    out.sosvh(k) = sum(sum(glcm(:,:,k).*((i_matrix - glcm_mean(k)).^2)));
    out.indnc(k) = sum(sum(glcm(:,:,k)./( 1 + (mul_dissi./size_glcm_1) )));
    out.idmnc(k) = sum(sum(glcm(:,:,k)./( 1 + (mul_contr./(size_glcm_1^2)))));
    out.maxpr(k) = max(max(glcm(:,:,k)));

    u_x(k)       = sum(sum(i_matrix.*glcm(:,:,k)));
    u_y(k)       = sum(sum(j_matrix.*glcm(:,:,k)));
    % using http://www.fp.ucalgary.ca/mhallbey/glcm_variance.htm for s_x
    % s_y : This solves the difference in value of correlation and might be
    % the right value of standard deviations required
    % According to this website there is a typo in [2] which provides
    % values of variance instead of the standard deviation hence a square
    % root is required as done below:
    s_x(k)  = (sum(sum( ((i_matrix - u_x(k)).^2).*glcm(:,:,k) )))^0.5;
    s_y(k)  = (sum(sum( ((j_matrix - u_y(k)).^2).*glcm(:,:,k) )))^0.5;

   corp(k) = sum(sum((i_matrix.*j_matrix.*glcm(:,:,k))));
   corm(k) = sum(sum(((i_matrix - u_x(k)).*(j_matrix - u_y(k)).*glcm(:,:,k))));

   out.autoc(k) = corp(k);
   out.corrp(k) = (corp(k) - u_x(k)*u_y(k))/(s_x(k)*s_y(k));
   out.corrm(k) = corm(k) / (s_x(k)*s_y(k));

   out.cprom(k) = sum(sum(((i_matrix + j_matrix - u_x(k) - u_y(k)).^4).*...
                glcm(:,:,k)));
   out.cshad(k) = sum(sum(((i_matrix + j_matrix - u_x(k) - u_y(k)).^3).*...
                glcm(:,:,k)));

   out.savgh(k) = sum((xplusy_index + 1).*p_xplusy(:,k));
   % the summation for savgh is for i from 2 to 2*Ng hence (i+1)
   out.senth(k) =  - sum(p_xplusy(:,k).*...
            log(p_xplusy(:,k) + eps));

    % compute sum variance with the help of sum entropy
    out.svarh(k) = sum((((xplusy_index + 1) - out.senth(k)).^2).*...
        p_xplusy(:,k));
        % the summation for savgh is for i from 2 to 2*Ng hence (i+1)

    % compute difference variance, difference entropy,
    % out.dvarh2(k) = var(p_xminusy(:,k));
    % but using the formula in
    % http://murphylab.web.cmu.edu/publications/boland/boland_node26.html
    % we have for dvarh
    out.denth(k) = - sum((p_xminusy(:,k)).*...
        log(p_xminusy(:,k) + eps));
    out.dvarh(k) = sum((xminusy_index.^2).*p_xminusy(:,k));

    % compute information measure of correlation(1,2) [1]
    hxy(k) = out.entro(k);
    glcmk  = glcm(:,:,k)';
    glcmkv = glcmk(:);

    hxy1(k) =  - sum(glcmkv.*log(p_x(i_index,k).*p_y(j_index,k) + eps));
    hxy2(k) =  - sum(p_x(i_index,k).*p_y(j_index,k).*...
        log(p_x(i_index,k).*p_y(j_index,k) + eps));
     hx(k) = - sum(p_x(:,k).*log(p_x(:,k) + eps));
     hy(k) = - sum(p_y(:,k).*log(p_y(:,k) + eps));

    out.inf1h(k) = ( hxy(k) - hxy1(k) ) / ( max([hx(k),hy(k)]) );
    out.inf2h(k) = ( 1 - exp( -2*( hxy2(k) - hxy(k) ) ) )^0.5;

    %     eig_Q(k,:)   = eig(Q(:,:,k));
    %     sort_eig(k,:)= sort(eig_Q(k,:),'descend');
    %     out.mxcch(k) = sort_eig(k,2)^0.5;
    % The maximal correlation coefficient was not calculated due to
    % computational instability
    % http://murphylab.web.cmu.edu/publications/boland/boland_node26.html

end

%       GLCM Features (Soh, 1999; Haralick, 1973; Clausi 2002)
%           f1. Uniformity / Energy / Angular Second Moment (done)
%           f2. Entropy (done)
%           f3. Dissimilarity (done)
%           f4. Contrast / Inertia (done)
%           f5. Inverse difference
%           f6. correlation
%           f7. Homogeneity / Inverse difference moment
%           f8. Autocorrelation
%           f9. Cluster Shade
%          f10. Cluster Prominence
%          f11. Maximum probability
%          f12. Sum of Squares
%          f13. Sum Average
%          f14. Sum Variance
%          f15. Sum Entropy
%          f16. Difference variance
%          f17. Difference entropy
%          f18. Information measures of correlation (1)
%          f19. Information measures of correlation (2)
%          f20. Maximal correlation coefficient
%          f21. Inverse difference normalized (INN)
%          f22. Inverse difference moment normalized (IDN)


function b=num2ibm(x)
% num2ibm : convert IEEE 754 doubles to IBM 32 bit floating point format
%    b=num2ibm(x)
% x is a matrix of doubles
% b is a corresponding matrix of uint32
%
% The representations for NaN and inf are arbitrary
%
% See also ibm2num

%
%    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 should have received a copy of the GNU General Public License
%    along with this program; if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
%
% (C) Brian Farrelly, 22 October 2001
%  mailto:Brian.Farrelly@nho.hydro.com          Norsk Hydro Research Centre
%  phone +47 55 99 68 74                 (((                  Postboks 7190
%  fax   +47 55 99 69 70                2oooS                 N-5020 Bergen
%  home  +47 55 13 78 49                HYDRO                        Norway
%

b=repmat(uint32(0),size(x));
err=zeros(size(x));

%format long

x(x> 7.236998675585915e+75)= inf;        % change big numbers to infinity
x(x<-7.236998675585915e+75)=-inf;        % 7.236998675585915e+75 is
                                         %    ibm2num(uint32(hex2dec('7fffffff')) or
                                         %    ibm2num(num2ibm(inf))

[F E]=log2(abs(x));

e=E/4;                         % exponent of base 16
ec=ceil(e);                    % adjust upwards to integer
p=ec+64;                       % offset exponent

f=F.*2.^(-4*(ec-e));           % correct mantissa for fractional part of exponent
f=round(f*2^24);               % convert to integer. Roundoff here can be as large as
                               % 0.5/2^20 when mantissa is close to 1/16 so that
                               % 3 bits of signifance are lost.

p(f==2^24)=p(f==2^24)+1;       % Roundoff can cause f to be 2^24 for numbers just under a
f(f==2^24)=2^20;               % power of 16, so correct for this

%format hex
psi=uint32(p*2^24);            % put exponent in first byte of psi.
phi=uint32(f);                 % put mantissa into last 3 bytes of phi

% make bit representation

b=bitor(psi,phi);                        % exponent and mantissa
b(x<0)=bitset(b(x<0),32);                % sign bit
%format long

% special cases

b(x==0)          =uint32(0)                  ;         %  bias is incorrect for zero
b(isnan(x))      =uint32(hex2dec('7fffffff'));         %  7.237005145973116e+75 in IBM format
b(isinf(x) & x>0)=uint32(hex2dec('7ffffff0'));         %  7.236998675585915e+75    ,,
b(isinf(x) & x<0)=uint32(hex2dec('fffffff0'));         % -7.236998675585915e+75    ,,
                                                       % Note that NaN > inf in IBM format

% check bit representation for normal cases

checkx=ibm2num(b);                      % note that use of base 16 in IBM format
z=x==0;                                 % can lead to a loss of 3 bits of precision
err(z)=0;                               % compared with an IEEE single.
q=(checkx(~z)-x(~z))./x(~z);
err(~z) = abs(q) > 5e-7;                % this is almost reached with numbers
                                        % of the form 16^n + 0.5*16^(n-5) where
                                        % the mantissa is 100001 hex. Roundoff
                                        % error is then 0.5/16^5=0.5/2^20=4.7684e-7

if any(err)
   warning('Conversion error in num2ibm for the following:')
   disp(x(logical(err)))
end


function mb = trimeshMeanBreadth(vertices, faces)
%TRIMESHMEANBREADTH Mean breadth of a triangular mesh.
%
%   MB = trimeshMeanBreadth(VERTICES, FACES)
%   Computes the mean breadth (proporitonal to the integral of mean
%   curvature) of a triangular mesh.
%
%   Example
%     [V, F] = createCube;
%     F2 = triangulateFaces(F);
%     MB = trimeshMeanBreadth(V, F2)
%     MB =
%         1.5000
%
%   See also
%   meshes3d, trimeshSurfaceArea, trimeshEdgeFaces, polyhedronMeanBreadth
%
%   References
%   Stoyan D., Kendall W.S., Mecke J. (1995) "Stochastic Geometry and its
%       Applications", John Wiley and Sons, p. 26
%   Ohser, J., Muescklich, F. (2000) "Statistical Analysis of
%       Microstructures in Materials Sciences", John Wiley and Sons, p.352

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-08-19,    using Matlab 8.5.0.197613 (R2015a)
% Copyright 2015 INRA - Cepia Software Platform.

%% Check input validity

if size(faces, 2) ~= 3
    error('meshes3d:trimeshMeanBreadth:NonTriangularMesh', ...
        'Requires a triangular mesh as input');
end

%% Compute edge and edgeFaces arrays
% Uses the same code as in trimeshEdgeFaces

% compute vertex indices of each edge (in increasing index order)
edges = sort([faces(:,[1 2]) ; faces(:,[2 3]) ; faces(:,[3 1])], 2);

% create an array to keep indices of faces "creating" each edge
nFaces = size(faces, 1);
edgeFaceInds = repmat( (1:nFaces)', 3, 1);

% sort edges, keeping indices
[edges, ia, ib] = unique(edges, 'rows'); %#ok<ASGLU>
nEdges = size(edges, 1);

% allocate memory for result
edgeFaces = zeros(nEdges, 2);

% iterate over edges, to identify incident faces
for iEdge = 1:nEdges
    inds = find(ib == iEdge);
    edgeFaces(iEdge, 1:length(inds)) = edgeFaceInds(inds);
end

%% Compute dihedral angle for each edge

% compute normal of each face
normals = meshFaceNormals(vertices, faces);

% allocate memory for resulting angles
alpha = zeros(nEdges, 1);

% iterate over edges
for iEdge = 1:nEdges
    % indices of adjacent faces
    indFace1 = edgeFaces(iEdge, 1);
    indFace2 = edgeFaces(iEdge, 2);

    % normal vector of adjacent faces
    normal1 = normals(indFace1, :);
    normal2 = normals(indFace2, :);

    % compute dihedral angle of two vectors
    alpha(iEdge) = vectorAngle3d(normal1, normal2);
end

%% Compute mean breadth
% integrate the dihedral angles weighted by the length of each edge

% compute length of each edge
lengths = meshEdgeLength(vertices, edges);

% compute product of length by angles
mb = sum(alpha .* lengths) / (4*pi);


function diff = getDiffuseness_TV(i_vecs)
%GETDIFFUSENESS_TV Compute diffuseness from temporal variation of intensity
%vectors
%
%   This routine estimates diffuseness using the temporal variation of
%   intensity vectors, as proposed in
%
%       Ahonen, J. and Pulkki, V., 2009.
%       Diffuseness estimation using temporal variation of intensity vectors.
%       In 2009 IEEE Workshop on Applications of Signal Processing to Audio and Acoustics (WASPAA).
%
%   also termed coefficient of variation in other works. It is based on the
%   relation sqrt( 1 - ||<Ia>||/<||Ia||> ), where Ia is the active intensity
%   vector. It is similar to the spherical variance estimator, but it takes
%   into account the magnitude of the DoA vectors, hence giving more weight
%   to strong sound estimates instead of low noisy ones. It is also flexible
%   in the sense that alternative DoA vectors with magnitude that relates
%   to the energy of the sound-field can be used instead of the intensity
%   vector.
%
%   Inputs:
%       i_vecs:  Kx3 matrix of DoA vectors, for K temporal observations
%
%   Outputs:
%       diff:       diffuseness value
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% GETDIFFUSENESS_TV.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% mean of active intensity vectors
mean_i_vecs = mean(i_vecs, 1);
% norm of mean vector
norm_mean_i_vecs = sqrt(sum(mean_i_vecs.^2, 2));

% norm of intensity vectors
norm_i_vecs = sqrt(sum(i_vecs.^2, 2));
% mean of norms
mean_norm_i_vecs = mean(norm_i_vecs);

% diffuseness
diff = sqrt( 1 - norm_mean_i_vecs/mean_norm_i_vecs );

end


function nu = PoissonRatio(S,x,y)
% Poisson ratio of an elasticity tensor
%
% Syntax
%   nu = PoissonRatio(S,x,y)
%   nu = PoissonRatio(S) % isotropic case
%
% Input
%  S - elastic @complianceTensor
%  x - @vector3d tension direction
%  y - @vector3d perpendicular direction
%
% Output
%  nu - Poisson ratio
%
% Description
%
% $$\nu = \frac{-S_{ijkl} x_i x_j y_k y_l}{S_{mnop} x_m x_n x_o x_p}$$
%

if nargin == 1  % isotrpoic theory

  S_iso_Voigt = mean(S,uniformODF(S.CS));
  nu = S_iso_Voigt.PoissonRatio(xvector,yvector);

  return
end

% generate a function if required
if nargin == 1 || isempty(x)

  nu = S2FunHarmonicSym.quadrature(@(v) PoissonRatio(S,v,y),'bandwidth',4,S.CS);

elseif nargin <= 2 || isempty(y)

  nu = S2FunHarmonicSym.quadrature(@(v) PoissonRatio(S,x,v),'bandwidth',4,S.CS);

else

  % compute tensor product
  nu = -EinsteinSum(S,[-1 -2 -3 -4],x,-1,x,-2,y,-3,y,-4) ./ ...
    EinsteinSum(S,[-1 -2 -3 -4],x,-1,x,-2,x,-3,x,-4);

end


function flux = EulerRoe2D(nx, ny, QM, QP, gamma)

% function flux = EulerRoe2D(nx, ny, QM, QP, gamma)
% Purpose: compute surface fluxes for Euler's equations using the approximate
%          Riemann solver based on Roe averages

% Rotate "-" trace momentum to face normal-tangent coordinates
rhouM = QM(:,:,2); rhovM = QM(:,:,3);
QM(:,:,2) = nx.*rhouM + ny.*rhovM; QM(:,:,3) =-ny.*rhouM + nx.*rhovM;

% Rotate "+" trace momentum to face normal-tangent coordinates
rhouP = QP(:,:,2); rhovP = QP(:,:,3);
QP(:,:,2) = nx.*rhouP + ny.*rhovP; QP(:,:,3) =-ny.*rhouP + nx.*rhovP;

% Compute fluxes and primitive variables in rotated coordinates
[fxQM,fyQM,rhoM,uM,vM,pM] = EulerFluxes2D(QM, gamma);
[fxQP,fyQP,rhoP,uP,vP,pP] = EulerFluxes2D(QP, gamma);
EnerM = QM(:,:,4); EnerP = QP(:,:,4);

Nfields = 4;
HM   = (EnerM+pM)./rhoM; cM = sqrt(gamma*pM./rhoM);
HP   = (EnerP+pP)./rhoP; cP = sqrt(gamma*pP./rhoP);

% Compute Roe average variables
rhoMs = sqrt(rhoM); rhoPs = sqrt(rhoP);

rho = rhoMs.*rhoPs;
u   = (rhoMs.*uM + rhoPs.*uP)./(rhoMs + rhoPs);
v   = (rhoMs.*vM + rhoPs.*vP)./(rhoMs + rhoPs);
H   = (rhoMs.*HM + rhoPs.*HP)./(rhoMs + rhoPs);

c2  = (gamma-1)*(H - 0.5*(u.^2 + v.^2)); c = sqrt(c2);

% Compute estimate of waves speeds
SL = min(uM-cM, u-c); SR = max(uP+cP, u+c);

% Compute HLL flux
t1 = (min(SR,0)-min(0,SL))./(SR-SL);
t2 = 1-t1;
t3 = (SR.*abs(SL)-SL.*abs(SR))./(2*(SR-SL));

for n=1:4
  fx(:,:,n) = t1.*fxQP(:,:,n) + t2.*fxQM(:,:,n) - t3.*(QP(:,:,n)-QM(:,:,n));
end

% rotate flux back into Cartesian coordinates
flux(:,:,1) = fx(:,:,1);
flux(:,:,2) = nx.*fx(:,:,2) - ny.*fx(:,:,3);
flux(:,:,3) = ny.*fx(:,:,2) + nx.*fx(:,:,3);
flux(:,:,4) = fx(:,:,4);;
return;


%                           solveWirtFlow.m
%
%  Implementation of the Wirtinger Flow (WF) algorithm proposed in the
%  paper.
%
%% I/O
%  Inputs:
%     A:    m x n matrix or a function handle to a method that
%           returns A*x.
%     At:   The adjoint (transpose) of 'A'. If 'A' is a function handle,
%           'At' must be provided.
%     b0:   m x 1 real,non-negative vector consists of all the measurements.
%     x0:   n x 1 vector. It is the initial guess of the unknown signal x.
%     opts: A struct consists of the options for the algorithm. For details,
%           see header in solvePhaseRetrieval.m or the User Guide.
%
%     Note: When a function handle is used, the
%     value of 'At' (a function handle for the adjoint of 'A') must be
%     supplied.
%
%  Outputs:
%     sol:  n x 1 vector. It is the estimated signal.
%     outs: A struct consists of the convergence info. For details,
%           see header in solvePhaseRetrieval.m or the User Guide.
%
%
%  See the script 'testWirtFlow.m' for an example of proper usage of this
%  function.
%
%% Notations
%  x is the estimation of the signal. y is the vector of measurements such
%  that yi = |<ai,x>|^2 for i = 1,...,m
%
%% Algorithm Description
%  WF successively refines the estimate via an update rule that bears a
%  strong resemblance to a gradient descent scheme. Specifically, at each
%  iteration, x = x + mu/m * gradient log-likelihood of x given y For the
%  detailed formulation of "gradient log-likelihood of x given y" and a
%  detailed explanation of the theory, see the WF paper referenced below.
%
%% References
%  Paper Title:   Phase Retrieval via Wirtinger Flow: Theory and Algorithms
%  Place:         Chapter 2.3
%  Authors:       Emmanuel Candes, Xiaodong Li, Mahdi Soltanolkotabi
%  arXiv Address: https://arxiv.org/abs/1407.1065
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

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

function [sol, outs] = solveWirtFlow(A, At, b0, x0, opts)
    innerOpts = struct;
    innerOpts.maxIters = opts.maxIters;
    innerOpts.maxTime = opts.maxTime;
    innerOpts.tol = opts.tol;
    innerOpts.verbose = opts.verbose;
    innerOpts.recordTimes = opts.recordTimes;
    innerOpts.recordResiduals = opts.recordResiduals;
    innerOpts.recordMeasurementErrors = opts.recordMeasurementErrors;
    innerOpts.recordReconErrors = opts.recordReconErrors;
    innerOpts.xt = opts.xt;

    innerOpts.searchMethod = opts.searchMethod;
    innerOpts.betaChoice = opts.betaChoice;

    [sol, outs] = gradientDescentSolver(A, At, x0, b0, @updateObjective, innerOpts);

    function [f, gradf] = updateObjective(~, ~)
        f = @(z) 0.5 * norm(abs(z).^2 - b0.^2)^2;
        gradf = @(z) (abs(z).^2 - b0.^2) .* z;
    end
end


function [J, grad] = linearRegCostFunction(X, y, theta, lambda)
%LINEARREGCOSTFUNCTION Compute cost and gradient for regularized linear
%regression with multiple variables
%   [J, grad] = LINEARREGCOSTFUNCTION(X, y, theta, lambda) computes the
%   cost of using theta as the parameter for linear regression to fit the
%   data points in X and y. Returns the cost in J and the gradient in grad

% Initialize some useful values
m = length(y); % number of training examples

% You need to return the following variables correctly
J = 0;
grad = zeros(size(theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost and gradient of regularized linear
%               regression for a particular choice of theta.
%
%               You should set J to the cost and grad to the gradient.
%

h = X*theta;
hy = h-y;
J = sum(hy.^2)/(2*m) + lambda*sum(theta(2:end).^2)/(2*m);
hyrep = repmat(hy,[1 size(X,2)]);
grad = sum(hyrep.*X,1)'/m;

grad(2:end) = grad(2:end) + lambda.*theta(2:end)/m;

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

grad = grad(:);

end


function cdf = beta_binomial_cdf ( x, a, b, c )

%*****************************************************************************80
%
%% BETA_BINOMIAL_CDF evaluates the Beta Binomial CDF.
%
%  Discussion:
%
%    A simple summing approach is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the argument of the CDF.
%
%    Input, real A, B, parameters of the PDF.
%    0.0D+00 < A,
%    0.0D+00 < B.
%
%    Input, integer C, a parameter of the PDF.
%    0 <= C.
%
%    Output, real CDF, the value of the CDF.
%
  if ( x < 0 )

    cdf = 0.0;

  elseif ( x < c )

    cdf = 0.0;
    for y = 0 : x
      pdf = beta ( a + y, b + c - y ) / ( ( c + 1 ) ...
        * beta ( y + 1,  c - y + 1 ) * beta ( a, b ) );
      cdf = cdf + pdf;
    end

  elseif ( c <= x )

    cdf = 1.0;

  end

  return
end


N = 25;
x = 0:N-1;
H = zeros(N,1);

for i = 1:N
    if i == (N+1)/2
        H(i) = 1/6;
    else
        H(i) = sin(pi*(i-(N+1)/2)/6)/(pi*(i-(N+1)/2));
    end;
end;
H = H.*blackman(N);
newN = 1000;
H = [H',zeros(1,newN-length(H))];
Y = fft(H);
Y = fftshift(Y);
W = calculateDiscreteFrequencyIndex(length(Y));
subplot(2,1,1);
%plot(W,abs(Y));
plot(W,10*log10(abs(Y)));
subplot(2,1,2);
plot(W,angle(Y));


function v=v_dlyapsq(a,b)
%V_DLYAPSQ Solves the discrete Lyapunov equation AV'VA' - V'V + BB' = 0
% V is upper triangular with real non-negative diagonal entries
% this is equivalent to v=chol(dlyap(a,b*b')) but better conditioned numerically

%      Copyright (C) Mike Brookes 2002
%      Version: $Id: v_dlyapsq.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[q,s]=schur(a');
[q,s]=rsf2csf(q,s);
[qd,r]=qr(b'*q,0);
% save r for testing
r0=r;
[m,n]=size(r);
u=zeros(n,n);
if m==1
   for i=1:n-1
      in=i+1:n;
      si=s(i,i);
      aa=sqrt(1-si*si');
      u(i,i)=r(1)/aa;
      u(i,in)=(u(i,i)*si'*s(i,in)+aa*r(2:end))/(eye(n-i)-si'*s(in,in));
      r=aa*(u(i,i)*s(i,in)+u(i,in)*s(in,in))-si*r(2:end);
   end
   u(n,n)=r/sqrt(1-s(n,n)*s(n,n)');

else
   w=zeros(m,1); w(m)=1;
   em=eye(m);
   for i=1:n-m
      in=i+1:n;
      si=s(i,i);
      aa=sqrt(1-si*si');
      u(i,i)=r(1,1)/aa;
      u(i,in)=(u(i,i)*si'*s(i,in)+aa*r(1,2:end))/(eye(n-i)-si'*s(in,in));
      vv=aa*(u(i,i)*s(i,in)+u(i,in)*s(in,in))-si*r(1,2:end);
      rr=zeros(m,n-i);
      rr(1:m-1,:)=r(2:end,2:end);
      [qq,r]=qrupdate(em,rr,w,vv');
   end
   for i=n-m+1:n-1
      in=i+1:n;
      si=s(i,i);
      aa=sqrt(1-si*si');
      u(i,i)=r(1,1)/aa;
      u(i,in)=(u(i,i)*si'*s(i,in)+aa*r(1,2:end))/(eye(n-i)-si'*s(in,in));
      vv=aa*(u(i,i)*s(i,in)+u(i,in)*s(in,in))-si*r(1,2:end);
      rr=zeros(n-i+1,n-i);
      rr(1:n-i,:)=r(2:end,2:end);
      [qq,rr]=qrupdate(eye(n-i+1),rr,w(m-n+i:end),vv');
      r=rr(1:n-i,:);
   end

   u(n,n)=r/sqrt(1-s(n,n)*s(n,n)');

end

v=triu(qr(u*q'));
dv=diag(v);
ix=dv~=0;
v(ix,:)=diag(abs(dv(ix))./dv(ix))*v(ix,:);
if isreal(a) & isreal(b)
   v=real(v);
end


function x = block_levinson(y, L)
%BLOCK_LEVINSON Block Levinson recursion for efficiently solving
%symmetric block Toeplitz matrix equations.
%   BLOCK_LEVINSON(Y, L) solves the matrix equation T * x = y, where T
%   is a symmetric matrix with block Toeplitz structure, and returns the
%   solution vector x. The matrix T is never stored in full (because it
%   is large and mostly redundant), so the input parameter L is actually
%   the leftmost "block column" of T (the leftmost d columns where d is
%   the block dimension).

%   Author: Keenan Pepper
%   Last modified: 2007-12-23

%   References:
%     [1] Akaike, Hirotugu (1973). "Block Toeplitz Matrix Inversion".
%     SIAM J. Appl. Math. 24 (2): 234-241

s = size(L);
d = s(2);                 % Block dimension
N = s(1) / d;             % Number of blocks

B = reshape(L, [d,N,d]);  % This is just to get the bottom block row B
B = permute(B, [1,3,2]);  % from the left block column L
B = flipdim(B, 3);
B = reshape(B, [d,N*d]);

f = L(1:d,:)^-1;          % "Forward" block vector
b = f;                    % "Backward" block vector
x = f * y(1:d);           % Solution vector

for n = 2:N
    ef = B(:,(N-n)*d+1:N*d) * [f;zeros(d)];
    eb = L(1:n*d,:)' * [zeros(d);b];
    ex = B(:,(N-n)*d+1:N*d) * [x;zeros(d,1)];
    A = [eye(d),eb;ef,eye(d)]^-1;
    fn = [[f;zeros(d)],[zeros(d);b]] * A(:,1:d);
    bn = [[f;zeros(d)],[zeros(d);b]] * A(:,d+1:end);
    f = fn;
    b = bn;
    x = [x;zeros(d,1)] + b * (y((n-1)*d+1:n*d) - ex);
end


%	nufft_tune_gauss
%	tune the FWHM for gaussian interpolation in 1D NUFFT
%	to minimize the worst-case error

if ~isvar('err_gauss')
	N = 2^8;
	K_N = 2;				% oversampling factor
	Jlist = [2:16];
	Plist = linspace(0.2, 0.4, 101);	% sig = parameter * sqrt(J)
	rlist = [0:20]'/40;			% fractions of gamma

	err_gauss.zn = zeros([length(rlist) length(Jlist) length(Plist)]);
	err_gauss.ft = zeros([length(rlist) length(Jlist) length(Plist)]);

	for jj=1:length(Jlist)
		J = Jlist(jj);	printf('J=%d', J)

		K = K_N * N;
		gam = 2*pi/K;
		om = gam * rlist;

		%
		%	gaussian with various FWHM
		%
		for ii=1:length(Plist)
			sig = Plist(ii) * sqrt(J);
			[Gfunc, Gfunc_ft] = nufft_gauss('inline', J, sig);

			%
			%	interpolator worst-case error
			%
		        err_gauss.ft(:,jj,ii) = nufft1_error(om, N, J, K, ...
				Gfunc, Gfunc_ft);
		        err_gauss.zn(:,jj,ii) = nufft1_error(om, N, J, K, ...
				Gfunc);
		end
	end
end

emax.zn = squeeze( max(err_gauss.zn, [], 1) );		% [M,J,F] -> [J,F]
[emax.zn, ibest.zn] = min(emax.zn, [], 2);		% [J,F] -> J
pbest.zn = Plist(ibest.zn);

emax.ft = squeeze( max(err_gauss.ft, [], 1) );		% [M,J,F] -> [J,F]
[emax.ft, ibest.ft] = min(emax.ft, [], 2);		% [J,F] -> J
pbest.ft = Plist(ibest.ft);

%	plot best sig vs J (on log scale!)
clf
if 1
	log_J = log(Jlist);
	log_sig.zn = log(pbest.zn .* sqrt(Jlist));
	log_sig.ft = log(pbest.ft .* sqrt(Jlist));

	subplot(131)
	plot(log_J, log_sig.zn, 'co', log_J, log_sig.ft, 'yx'), axis tight
	lsline, legend('zn', 'ft')
	xlabel log(J), ylabel 'log(sig best)'
	pol = polyfit(log_J, log_sig.zn, 1)
	printf('\\sigma \\approx %g * J^{%g}', exp(pol(2)), pol(1))
	pol = polyfit(log_J, log_sig.ft, 1)
	printf('\\sigma \\approx %g * J^{%g}', exp(pol(2)), pol(1))
end

%
%	plot emax vs J
%
if 1
	subplot(132)
	semilogy(Jlist, emax.zn, 'c-o', Jlist, emax.ft, 'y-x')
	xlabel J, ylabel E_{max}
        title(sprintf('Maximum error for K/N=%g', K/N))
	legend('zn', 'ft')

	subplot(133)
	plot(emax.zn ./ emax.ft), axis tight, title 'emax: zn / ft'
%	savefig c 'fig_?'
end

%
%	save both sets of tuned sigma's to "private" file
%
if 1
	Jgauss2 = Jlist;
	Sgauss2.zn = pbest.zn .* sqrt(Jlist);		% tuned sig zn
	Sgauss2.ft = pbest.ft .* sqrt(Jlist);		% tuned sig ft
	save nufft_gauss2 Jgauss2 Sgauss2
	disp('MOVE TO private!')
end


%% B-A Scale-Free Network Generation and Visualization
% *By Mathew Neil George*

%% Description and Cautions
% The *SFNG* m-file is used to simulate the B-A algorithm and returns scale-free
% networks of given node sizes. Understanding the B-A algorithm is key
% to using this code to its fullest. Due to Matlab resource limitations, it may not be
% possible to generate networks much larger than 15000 nodes, and increasing the
% *mlinks* variable increases processing time severely. This code was
% developed so that one could generate a network of small size, and then
% use that network as a seed to build a greater sized network, continuing
% this process until the actual desired network size is reached. This is for
% processor and time management purposes. However, realize that the initial
% seed does not have to have scale-free properties, while the later seeds
% may happen to have these properties. Therefore, it is prudent not to make the
% initial seed size much larger than a few nodes (most commonly 5
% interconnected nodes). In addition, the *mlinks* should be kept constant
% throughout the creation of the scale-free network.
%
% The *PLplot* m-file takes a scale-free network in adjacency matrix format
% and draws a best fit line to the frequency of degrees distribution of the
% nodes. Degree is the number of links that connect to and from a single node
% For scale-free networks, the frequency of degrees distribution forms a
% power-law curve, with an exponent usually between -2 and -3. This code is
% designed to allow only non-zero frequencies to be graphed in log-log format.
% The function returns the equation of the power-law fit in a cfit variable.
%
% The *CNet* m-file function creats a network graph using the *gplot*
% function with circular coordinates. It allows for a simple, yet
% intuitive, visualization of a given network.

%% Parameters
% *SFNG*
%
% * *Nodes* is the desired network size, including the seed network size
%   (i.e. Nodes minus seed network size equals the number of nodes to be
%   added).
%
% * *mlinks* controls the number of links a new node can make to the existing
%   network nodes.
%
% * *seed* is the original network to which the B-A algorithm links
%   additional nodes with a specific preferential attachment procedure.
%   This undirected adjacency matrix can be created manually, or one could
%   use the *Adjacency Matrix GUI*. Each node must have at least one link.
%   The *seed* variable can be replaced with a developed scale-free network
%   to generate a larger one. Make sure the new *Nodes* variable is greater
%   than the size of the *seed* network.
%
% *PLplot*
%
% * *Net* is the input network which is to be graphed.
%
% *CNet*
%
% * *Net* is the input network which is to be graphed.
%
% Note that variables *Nodes*, *mlinks*, and *size* must be whole numbers and
% variables *seed* and *Net* must be undirected adjacency matrices. The
% diagonol elements of any adjacency matrix used with these functions must
% all be zero.

%% Sample Output
% Here is a small example to demonstrate how to use the code. This code
% creates a seed network of 5 nodes, generates a scale-free network of 300 nodes from
% the seed network, and then performs the two graphing procedures.
seed =[0 1 0 0 1;1 0 0 1 0;0 0 0 1 0;0 1 1 0 0;1 0 0 0 0]
Net = SFNG(300, 1, seed);
PL_Equation = PLplot(Net)
CNet(Net)

%% References
% One explanation of the B-A Algorithm can be found on this PDF website
% http://arxiv.org/PS_cache/cond-mat/pdf/0107/0107420.pdf
%
% Undirected Adjecency Matrices are defined on Wikipedia.org
% http://en.wikipedia.org/wiki/Adjacency_matrix
%
% The *Adjacency Matrix GUI* file by Steve Chuang can be found on the Matlab File Exchange
% http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=6937&objectType=file

%% Acknowledgements
% Special thanks to Mark Ballerini with the Massapequa High School Science
% Research Program and Zoltan Dezso at the University of Notre Dame for
% their invaluable help in researching network theory as well as to my
% family for providing motivation and encouragement in pursuing science.


function s=norm(a,P)
%norm: Matrix or vector norm of vector/matrix of mp-type

%     For matrices...
%       NORM(X) is the largest singular value of X, max(svd(X)).
%       NORM(X,2) is the same as NORM(X).
%       NORM(X,1) is the 1-norm of X, the largest column sum,
%                       = max(sum(abs(X))).
%       NORM(X,inf) is the infinity norm of X, the largest row sum,
%                       = max(sum(abs(X'))).
%       NORM(X,'fro') is the Frobenius norm, sqrt(sum(diag(X'*X))).
%       NORM(X,P) is available for matrix X only if P is 1, 2, inf or 'fro'.
%
%     For vectors...
%       NORM(V,P) = sum(abs(V).^P)^(1/P).
%       NORM(V) = norm(V,2).
%       NORM(V,inf) = max(abs(V)).
%       NORM(V,-inf) = min(abs(V)).
%
%     See also COND, RCOND, CONDEST, NORMEST.

if nargin<2,P=2;end
if isa(P,'mp')
 error(['Second parameter should not be of mp-type'])
end
if min(size(a))==1
 %is a vector
 if isinf(P) | strcmp(lower(P),'inf')
  if P>0
   s=max(abs(a));
  else
   s=min(abs(a));
  end
  return
 end
 A=max(abs(a(:)));a=a/A;
 if P==2
  %naive implementation
  s=A*sqrt(sum(a.*a));
 else
  s=A*(sum(a.^P))^(1/P);
 end
else
 %is a matrix
 if isinf(P) | strcmp(lower(P),'inf')
  s=max(sum(abs(a')));
  return
 end
 if ischar(P)
  if strcmp(lower(P),'fro')
   s=sqrt(sum(diag(a'*a)));
  else
   error(['Second parameter is not recognized as an option: ' P])
  end
  return
 end
 switch P
  case 1
   s=max(sum(abs(a)));
  case 2
   s=max(svd(a));
  otherwise
   error(['NORM(X,P) is available for matrix X only if P is 1, 2, inf or ' setstr(39) 'fro' setstr(39) '.'])
 end
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logisitc regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell use
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

for c = 1:num_labels

% Set initial theta
initial_theta=zeros(n+1,1);
%initial_theta=zeros(num_labels, n + 1);

% Set options for fminunc
options = optimset('GradObj', 'on', 'MaxIter', 50);

% Run fmincg to obtain the optimal theta
% This function will return theta and the cost
[theta]=fmincg(@(t)(lrCostFunction(t,X, (y == c), lambda)), initial_theta, options);

% Updating theta to overall all_theta
if (c==1)
	history_theta=theta';
else
	history_theta=[history_theta; theta'];
end

end

all_theta=history_theta;

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

end


function triangle_symq_rule_test02 ( degree, numnodes, vert1, vert2, vert3 )

%*****************************************************************************80
%
%% TRIANGLE_SYMQ_RULE_TEST02 calls TRIASYMQ for a quadrature rule of given order and region.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    27 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.  0 <= DEGREE <= 50.
%
%    Input, integer NUMNODES, the number of nodes to be used by the rule.
%
%    Input, real VERT1(2), VERT2(2), VERT3(2), the
%    vertices of the triangle.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_SYMQ_RULE_TEST02\n' );
  fprintf ( 1, '  Symmetric quadrature rule for a triangle.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );

  area = triangle_area ( vert1, vert2, vert3 );
%
%  Retrieve and print a symmetric quadrature rule.
%
  [ rnodes, weights ] = triasymq ( degree, vert1, vert2, vert3, numnodes );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  NUMNODES = %d', numnodes );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     J      W               X               Y\n' );
  fprintf ( 1, '\n' );
  for j = 1 : numnodes
    fprintf ( 1, '  %4d  %14.6g  %14.6g  %14.6g\n', ...
      j, weights(j), rnodes(1,j), rnodes(2,j) );
  end

  d = sum ( weights(1:numnodes) );

  fprintf ( 1, '   Sum  %14.6g\n', d );
  fprintf ( 1, '  Area  %14.6g\n', area );

  return
end


function X = randnd(beta,varargin)
%% X = randnd(beta,varargin)
% This function is a "decorator" for the Matlab in-build 'randn' function.
% It takes an additional first argument (beta) which shapes the spectral
% characteristic of the data (in all dimensions) as f^beta. The output data
% is scaled to zero mean and unit standard deviation.
%
% For help on 'varargin' go to Matlab help for the 'randn' function.
%
% EXAMPLE 1 - Input robustness
% A = randnd(-1,[2^5+1 1]); % Handle odd dimensionality
% A = randnd((rand-0.5)*4,[134 12 1 1 1 26 1 9]); % Handle large number of dimentions
% A = randnd(-1,[1 0 3]); % Handle 0's
% A = randnd(2,[1 1 1 256]); % Handle leading singleton dimensions
%
% EXAMPLE 2 - Pink map
% A = randnd(-1,2^8); % Generate 256 x 256 grid with random 1/f (pink) noise
% contourf(A,'edgecolor','none'); axis square; % Display it as a contour plot
%
% EXAMPLE 3 - Brownian cube
% A = randnd(-2,[2^4 2^4 2^4]); % Generate 16 x 16 x 16 cube of f^-2 (Brownian) noise
% [X,Y,Z] = meshgrid(1:2^4); % Generate 3d meshgrid
% scatter3(X(:),Y(:),Z(:),100,A(:),'filled');
% axis vis3d; axis off;
%
% EXAMPLE 4 - Single blue noise
% p = single(magic(10)); % Single precision matrix of interest
% A = randnd(1,size(p),'like',p); % Generate f (blue) noise 'like' p
% isa(A,'single') % X is also a single precision random number
%
% EXAMPLE 5 - Proof that beta scales power spectral density correctly
% beta = -2;
% A = randnd(beta,[1 2^11]);
% Afft = abs(fft(A.^2)); % Get the magnitude of power (i.e. signal^2) spectrum
% Afft = Afft(2:length(Afft)/2); % Get the spectrum between DC and Nyquist
% k = 1:length(Afft); % Create wavenumber (frequency) vector
% P = polyfit(log(k),log(Afft),1); % Fit a straight line to the loglog plot
% loglog(k,Afft,'.',k,exp(P(2))*k.^P(1),'-'); grid on; % Plot on loglog axis
% xlim([min(k) max(k)]); title(['Beta = ',num2str(beta),'; Slope = ',num2str(P(1))]);
%
%
% Based on similar functions by Jon Yearsley and Hristo Zhivomirov
% Written by Marcin Konowalczyk
% Timmel Group @ Oxford University

%% Parse the input
narginchk(0,Inf); nargoutchk(0,1);

if nargin < 2 || isempty(beta); beta = 0; end % Default to white noise
assert(isnumeric(beta) && isequal(size(beta),[1 1]),'''beta'' must be a number');
assert(-2 <= beta && beta <= 2,'''beta'' out of range'); % Put on reasonable bounds

%% Generate N-dimensional white noise with 'randn'
X = randn(varargin{:});
if isempty(X); return; end; % Usually happens when size vector contains zeros

% Squeeze prevents an error if X has more than one leading singleton dimension
% This is a slight deviation from the pure functionality of 'randn'
X = squeeze(X);

% Return if white noise is requested
if beta == 0; return; end;

%% Generate corresponding N-dimensional matrix of multipliers
N = size(X);
% Create matrix of multipliers (M) of X in the frequency domain
M = [];
for j = 1:length(N)
    n = N(j);

    if (rem(n,2)~=0) % if n is odd
        % Nyquist frequency bin does not show up in odd-numbered fft
        k = ifftshift(-(n-1)/2:(n-1)/2);
    else
        k = ifftshift(-n/2:n/2-1);
    end

    % Spectral multipliers
    m = (k.^2)';

    if isempty(M);
        M = m;
    else
        % Create the permutation vector
        M_perm = circshift(1:length(size(M))+1,1,2);
        % Permute a singleton dimension to the beginning of M
        M = permute(M,M_perm);
        % Add m along the first dimension of M
        M = bsxfun(@plus,M,m);
    end
end
clear j M_perm k n m
% Reverse M to match X (since new dimensions were being added form the left)
M = permute(M,length(size(M)):-1:1);
assert(isequal(size(M),size(X)),'Bad programming error'); % This should never occur

% Shape the amplitude multipliers by (beta/2) which corresponds to shaping
% the power by beta
M = M.^(beta/2);

% Set the DC component to zero
M(1,1) = 0;

%% Multiply X by M in frequency domain
Xstd = std(X(:));
Xmean = mean(X(:));
X = real(ifftn(fftn(X).*M));

% Force zero mean unity standard deviation
X = X - mean(X(:));
X = X./std(X(:));

% Restore the standard deviation and mean from before the spectral shaping.
% This ensures the random sample from randn is truly random. After all, if
% the mean was always exactly zero it would not be all that random.
X = X + Xmean;
X = X.*Xstd;
end


function SR = getInitialSRImage(W, LRImages, photoParams)
% GETINITIALSRIMAGE Initial guess for super-resolved image
%   GETINITIALSRIMAGE computes a rough estimate for the super-resolved
%   image from given model parameters.
%
%   X = GETINITIALSRIMAGE(W, Y, PHOTOPARAMS) computes the "average image" X
%   as initial guess based on the system matrix W, the low-resolution data
%   Y and the photometric parameters PHOTOPARAMS.
%
%   See David P. Capel, Image Mosaicing and Super-resolution, PhD thesis,
%   2001 for details.

    if nargin > 2 && ~isempty(photoParams)
        numFrames = length(photoParams.mult);
        numLRPixel = length(LRImages) / numFrames;

        % Compose photometric parameters into additive and multiplicative term.
        bm = zeros(size(LRImages));
        ba = zeros(size(LRImages));
        for k = 1:numFrames
            bm( ((k-1)*numLRPixel + 1):(k*numLRPixel) ) = repmat(photoParams.mult(k), numLRPixel, 1);
            ba( ((k-1)*numLRPixel + 1):(k*numLRPixel) ) = repmat(photoParams.add(k), numLRPixel, 1);
        end
        bm_invmat = spdiags(1./bm(:), 0, length(bm), length(bm));

        % Compute "average image" from low-resolution observtions, system
        % matrix and phtometric parameters.
        SR =  (W' * bm_invmat * (LRImages - ba .* ones(size(LRImages)))) ./ (sum(W, 1)' + 1e-4);
    else
        % Compute "average image" without photometric parameters
        SR = (W' * LRImages) ./ (sum(W, 1)' + 1e-4);
    end


function [out] = baseflow_3(S,Smax)
%baseflow_3

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Empirical non-linear outflow from a reservoir
% Constraints:  None specified
% @(Inputs):    S    - current storage [mm]
%               Smax - maximum contributing storage [mm]

out = Smax^(-4)/(4)*(S^5);

end


function crackFVMP2
%% CRACK Problem
%
% crack solves the Poisson equation $-\Delta u =f$ in $\Omega$ and $u =
% g_D$ on $\partial \Omega$ in a crack domain
% $\Omega=\{|x|+|y|<1\}\backslash \{0<= x <=1, y=0\}$
%  using adaptive finite element method (AFEM). We choose f=1 and g_D such
%  that the exact solution is $u = r^{\beta}\sin(\beta\theta)-0.25r^2,
%  \beta = 1/2$ in the polar coordinate.
%
% EXAMPLE
%
%    crack
%
% See also  crack_performance, Lshape
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all;clc;clear;
%% Parameters
maxN = 2e3;     theta = 0.5;    maxIt = 50;
N = zeros(maxIt,1); err = zeros(maxIt,3);
%%  Generate an initial mesh
node = [1,0; 0,1; -1,0; 0,-1; 0,0; 1,0];        % nodes
elem = [5,1,2; 5,2,3; 5,3,4; 5,4,6];            % elements
elem = label(node,elem);                        % label the mesh
bdEdge = setboundary(node,elem,'Dirichlet');    % Dirichlet boundary condition
showmesh(node,elem);                            % plot mesh
findelem(node,elem);                            % plot element indices
findnode(node,1:5);                             % plot node indices
findedge(node,[1 5; 5 6],1);                    % plot the crack edge
text(node(6,1)+0.05,node(6,2)+0.075,int2str(6), ...
     'FontSize',14,'FontWeight','bold');
%%
% node 1 and node 6 are the same point (1,0)

%%  Get a fine mesh by uniform bisection
for k = 1:2
    [node,elem,bdEdge] = uniformbisect(node,elem,bdEdge);
end
clf; showmesh(node,elem);
%% Set up PDE data
pde.f = @f;
pde.exactu = @exactu;
pde.g_D = @exactu;
pde.Du=@Du;
pde.d=[];
%%  Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*
for k = 1:maxIt
    % Step 1: SOLVE
   [u,A,~,~,edge] = PoissonP2FVM(node,elem,[],@pde.f,@pde.g_D);
    % Plot mesh and solution
    figure(1);  showresult(node,elem,u(1:size(node,1)),[-7,12]);
    % Step 2: ESTIMATE
   % eta = estimaterecovery(node,elem,u); % recovery type % need eta^2 for mark
    eta = recoverFlux2(node,elem,pde,bdEdge,u);
    % Record error and number of vertices
   uI = pde.exactu([node; (node(edge(:,1),:)+node(edge(:,2),:))/2]);
   err(k,1) = sqrt((u-uI)'*A*(u-uI));
   err(k,2) = getL2error(node,elem,@exactu,u);
   err(k,3) = getH1error(node,elem,@pde.Du,u);
    N(k) = size(node,1);
    if (N(k)>maxN), break; end
    % Step 3: MARK
    markedElem = mark(elem,eta,theta);
    % Step 4: REFINE
    [node,elem,bdEdge] = bisect(node,elem,markedElem,bdEdge);
end

%% Plot convergence rates
N= N(1:k);
erruIuh= err(1:k,1); errL2 = err(1:k,2); errH1 = err(1:k,3);
figure(2);
r1 = showrate(N,erruIuh,5,'-*');
hold on;
r2 = showrate(N,errL2,5,'k-+');
r3 = showrate(N,errH1,10,'r-+');
legend('||Du_I-Du_h||',['cN^{' num2str(r1) '}'], ...
       '||u-u_h||',['cN^{' num2str(r2) '}'], ...
       '||Du-Du_h||',['cN^{' num2str(r3) '}'], ...
       'LOCATION','Best');
%%
% Using AFEM, we obtain optimal rate of convergence the error in the energy
% norm ($N^{-0.5}$) and in the $L^2$ norm ($N^{-1}$).
end % End of function CRACK

%%  Data of CRACK
function z = f(p)   % load data (right hand side function)
z = ones(size(p,1),1);
end

function z = exactu(p) % exact solution
r = sqrt(sum(p.^2,2));
z = sqrt(0.5*(r-p(:,1)))-0.25*r.^2;
end

function z = Du(p) % derivative of the exact solution
r = sqrt(sum(p.^2,2));
z(:,1) = (p(:,1)./r-1)./sqrt(8*(r-p(:,1)))-0.5*p(:,1); % u_x
z(:,2) = p(:,2)./r./sqrt(8*(r-p(:,1)))-0.5*p(:,2);     % u_y
end


function y = circle_characteristic ( m, n, x )

%*****************************************************************************80
%
%% CIRCLE_CHARACTERISTIC evaluates the characteristic function of a circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points to check.
%
%    Input, real X(M,N), the coordinates of points to be checked.
%
%    Output, real Y(N,1), is 1 if the point is inside, 0 otherwise.
%
  y = sqrt ( sum ( x.^2, 1 ) );

  y = y';

  y = ( y < 1.0 );

  return
end


function r8plu_inverse_test ( )

%*****************************************************************************80
%
%% R8PLU_INVERSE_TEST tests R8PLU_INVERSE;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8PLU_INVERSE_TEST\n' );
  fprintf ( 1, '  R8PLU_INVERSE determines the inverse of a matrix\n' );
  fprintf ( 1, '  from its compressed PLU factors.\n' );
  fprintf ( 1, '  Using initial random number seed = %d\n', seed );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  r8mat_print ( n, n, a, '  The matrix A:' );
%
%  Factor the matrix.
%
  [ pivot, lu, info ] = r8mat_to_r8plu ( n, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST0852 - Fatal error!\n' );
    fprintf ( 1, '  R8MAT_TO_R8PLU declares the matrix is singular!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end
%
%  Compute the inverse.
%
  b = r8plu_inverse ( n, pivot, lu );

  r8mat_print ( n, n, b, '  The inverse matrix B:' );
%
%  Compute the product C = A * B.
%
  c(1:n,1:n) = a(1:n,1:n) * b(1:n,1:n);

  r8mat_print ( n, n, c, '  The product matrix C = A * B:' );

  return
end


function g = year_to_golden_number ( y )

%*****************************************************************************80
%
%% YEAR_TO_GOLDEN_NUMBER returns the golden number of a Common year.
%
%  Discussion:
%
%    Nineteen solar years are very close to 235 lunations.  Calendars
%    that try to keep track of both the sun and moon often make use of
%    this fact, ascribed to the Greek astronomer Meton.
%
%    While trying to determine a formula for Easter, Dionysus Exiguus
%    symbolized the place of each year in its Metonic cycle by a
%    "golden number" between 1 and 19.  The numbering began with the
%    year 1 BC, assigned the golden number of 1.  The following year,
%    1 AD, got the golden number of 2, and after that it gets easier.
%
%    The same golden year calculation is done for years in the Julian
%    or Gregorian calendar.
%
%  Example:
%
%    Year  Golden Number
%
%      1 BC     1
%      1 AD     2
%      2 AD     3
%     18 AD    19
%     19 AD     1
%     20 AD     2
%   1066 AD     3
%   1900 AD     1
%   1919 AD     1
%   1938 AD     1
%   1957 AD     1
%   1976 AD     1
%   1995 AD     1
%   2014 AD     1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, integer G, the golden number, between 1 and 19.  This
%    records the position of the year in the 19 year Metonic cycle.
%
  if ( y == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YEAR_TO_GOLDEN_NUMBER - Fatal error!\n' );
    fprintf ( 1, '  Illegal input Y = 0.\n' );
    error ( 'YEAR_TO_GOLDEN_NUMBER - Fatal error!' );
  end
%
%  We assume that BC years come in as negative numbers, and that
%  the year before 1 AD is 1 BC.  So add 1 to any negative value
%  so that the arithmetic works.
%
  y2 = y_common_to_astronomical ( y );

  g = i4_wrap ( y2 + 1, 1, 19 );

  return
end


function R=return_Rt_matrix(alpha,beta,gamma,tx,ty,tz)

% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
% General Public License for more details.
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

R(1,1)=cos(alpha)*cos(gamma)-cos(beta)*sin(alpha)*sin(gamma);
R(2,1)=cos(gamma)*sin(alpha)+cos(alpha)*cos(beta)*sin(gamma);
R(3,1)=sin(beta)*sin(gamma);
R(4,1)=0;

R(1,2)=-cos(beta)*cos(gamma)*sin(alpha)-cos(alpha)*sin(gamma);
R(2,2)=cos(alpha)*cos(beta)*cos(gamma)-sin(alpha)*sin(gamma);
R(3,2)=cos(gamma)*sin(beta);
R(4,2)=0;

R(1,3)=sin(alpha)*sin(beta);
R(2,3)=-cos(alpha)*sin(beta);
R(3,3)=cos(beta);
R(4,3)=0;

R(:,4)=[tx,ty,tz,1]';


function [lx, r0, ux] = get_rate_grid_boundaries( model, modparam, t, gamma)
% Retrive lower and upper boundaries for variance grid, based on first two moments of variance process
% model : see below
% modparam : container with each of the model parameters required
% t: time to maturity used for the vairance grid
% gamma: grid mult param, rougly centered around +/- gamma standard deviations of the variance process

if model == 1 %CIR  (eta, theta, Rho, Sigmar, r0)
    eta = modparam.eta; theta = modparam.theta; Sigmar = modparam.Sigmar; r0 = modparam.r0;

    mu_H = exp(-eta*t)*r0 + theta*(1-exp(-eta*t));
    sig2_H = Sigmar^2/eta*r0*(exp(-eta*t)-exp(-2*eta*t)) +theta*Sigmar^2/(2*eta)*(1-2*exp(-eta*t)+exp(-2*eta*t));

elseif model == 2 %VASICEK ( in SV this is STEIN STEIN / same moments as SCOTT) ... (eta, theta, Rho, Sigmar, r0)
    eta = modparam.eta; theta = modparam.theta; Sigmar = modparam.Sigmar; r0 = modparam.r0;

    mu_H = exp(-eta*t)*r0 + theta*(1-exp(-eta*t));
    sig2_H = Sigmar^2/(2*eta)*(1 -exp(-2*eta*t));

elseif model == 3 % Merton
    lambda = modparam.lambda;  Sigmar = modparam.Sigmar; r0 = modparam.r0;

    mu_H = r0 + lambda*t;
    sig2_H = Sigmar^2*t;

elseif model == 4 % Dothan
    a = modparam.a;  Sigmar = modparam.Sigmar; r0 = modparam.r0;

    mu_H = r0*exp(a*t);
    sig2_H = r0^2*exp(2*a*t)*(exp(Sigmar^2*t) - 1);

end

ux = mu_H + gamma*sqrt(sig2_H);  %variance grid upper bound
lx = mu_H - gamma*sqrt(sig2_H);

if model == 1 || model == 4
    lx = max(0.00001,lx);  %variance grid lower bound
end

end


function [fa,md,rd,ad] = dtiComputeFA(eigVal)
% Computes fractional anisotropy (FA) from tensor eigenvalues
%
%   [fa,md,rd,ad] = dtiComputeFA(eigVal)
%
% eigVal  XxYxZx3xN (or nx3xN) array of tensor eigenvalues (N is No. of subjects)
% fa      XxYxZxN (or nxN) array of FA values
% md      XxYxZxN (or nxN) array of Mean Diffusivity values (10^-^6 mm^2/sec)
% rd      XxYxZxN (or nxN) array of Radial Diffusivity values
%
% HISTORY:
% 2003.12.04 ASH (armins@stanford.edu) wrote it.
% 2003.12.04 RFD (bob@white.stanford.edu) added check to avoid divide-by-zero.
% 2004.02.19 ASH added extra dimension for subjects
% 2005.05.02 RFD: added option to pass in a dt6 array.
% 2006.07.12 ASH added indexed format
% 2006.07.13 RFD: fixed the features that ASH so rudely clobbered yesterday.
% 2007.02.28 RFD: added rd return arg.

% Programming:  Since this returns MD as an option, how about renaming the
% function.  Or giving it a flag about which to return.  Or something.

% Check inputs
if (ndims(eigVal)==2)
    Ind = 1;    % eigVal in indexed nx3 format
    eigVal = shiftdim(eigVal, -2);
elseif(ndims(eigVal)==3),
    if(size(eigVal,2)==3 && size(eigVal,3)~=3)
        Ind = 1;    % Data in indexed nx3xN format
        eigVal = shiftdim(eigVal, -2);
    else
        Ind = 2;    % Data in XxYx3 format
        eigVal = shiftdim(eigVal, -1);
    end
else
    Ind = 0;    % eigVal in XxYxZx3xN format
end
if (ndims(eigVal)<4 || ndims(eigVal)>5),
    error('Wrong input format');
end

if(size(eigVal,4) ~= 3),
    if(size(eigVal,4)==6)
        [eigVec, eigVal] = dtiEig(eigVal);
        clear eigVec;
    else
    error('Wrong input format');
    end
end

% Main
epsilon = 1e-10;  % Avoid division by zero

% Mean diffusivity
md = sum(eigVal,4)/3;

%
stdevDiffusivity = sqrt(sum((eigVal-repmat(md,[1,1,1,3,1])).^2,4));
normDiffusivity = sqrt(sum(eigVal.^2,4));
nz = normDiffusivity > epsilon;

% Formula for fractional anisotropy
fa = repmat(NaN, size(md));
fa(nz) = sqrt(3/2).*(stdevDiffusivity(nz)./normDiffusivity(nz));
fa = squeeze(fa);

% Mean diffusivity
md = squeeze(md);

% If requested, return rd and ad
if(nargout>2)
    % Radial diffusivity is the average of the two smaller eigenvalues
    rd = squeeze((eigVal(:,:,:,2)+eigVal(:,:,:,3))/2);
end
if(nargout>3)
    % Axial diffusivity - the largest eigenvalue
    ad = squeeze(eigVal(:,:,:,1));
end

return


function flag = checkinterfacemesh3(node, elem, interfaceData)
%% CHECKINTERFACEMESH3 check the validity of the polyhedra mesh generated
%   by interfacemesh3.
%
%   Here we check  validity of the polyhedra mesh by the Euler formla:
%            F - E + V = 2
%   where F, E and V are the number of faces, edges and vertices of a simply
%   closed polyhedron.
%
%   Author: Huayi Wei <weihuayi@xtu.edu.cn>, based on discussion with Long Chen.
%
%   See also: interfacemesh3
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

face = interfaceData.face;
face2elem = interfaceData.face2elem;
N = size(node,1);
NF = size(face,1);

NP = max(face2elem);
isCutPoly = false(NP, 1);
isCutPoly(face2elem) = true;
cutPolyIdx = zeros(NP, 1);
NP = sum(isCutPoly);
cutPolyIdx(isCutPoly) = 1:NP;
face2elem = cutPolyIdx(face2elem);

isTriFace = face(:,4) == 0;
edge = 4*ones(NF,1);
edge(isTriFace) = 3;
E = accumarray(face2elem, edge)/2;
F = accumarray(face2elem, ones(NF,1));

poly2node = sparse(face2elem(isTriFace)*ones(1,3), face(isTriFace, 1:3), 1, NP,N)...
+ sparse(face2elem(~isTriFace)*ones(1,4), face(~isTriFace, 1:4), 1, NP,N);
poly2node = poly2node > 0;
NV = poly2node*ones(N,1);

isBdPoly = ((F - E + NV) ~= 2);

if ~isempty(find(isBdPoly,1))
    figure
    showmesh(node, face(isTriFace & isBdPoly(face2elem),1:3));
    sface = face(~isTriFace & isBdPoly(face2elem),1:4);
    hold on
    tsface = [sface(:, 1:3);sface(:,[3, 4,1])];
    showmesh(node, tsface, 'Facecolor','y');

%     bdPolyIdx = find(isBdPoly);
%     figure
%     for i = 1:size(bdPolyIdx)
%         sface = face(~isTriFace & face2elem == bdPolyIdx(i),1:4);
%         tsface = [sface(:, 1:3);sface(:,[3, 4,1])];
%         showmesh(node, [face(isTriFace & face2elem == bdPolyIdx(i),1:3);tsface]);
%     end
    flag = false;
else
    flag = true;
end