% Minimize transition bandwidth of a linear phase lowpass FIR filter
% "Filter design" lecture notes (EE364) by S. Boyd
% (figures are generated)
%
% Designs a linear phase FIR lowpass filter such that it:
% - minimizes the transition band width (i.e. minimize w_stop)
% - has a constraint on the maximum passband ripple
% - has a constraint on the maximum stopband attenuation
%
% This is a quasiconvex problem and is solved using a bisection.
%
%   minimize   w_stop
%       s.t.   1/delta <= H(w) <= delta     for w in the passband
%              |H(w)| <= atten_level        for w in the stopband
%
% where H is the frequency response function and variable is
% the filter impulse response h (and its order/length).
% Data is delta (max passband ripple) and atten_level (max stopband
% attenuation level).
%
% Written for CVX by Almir Mutapcic 02/02/06

%********************************************************************
% user's filter specifications
%********************************************************************
% starting point for the stopband (needs to be feasible)
wstop = 0.24*pi;        % stopband start freq (in radians)
TOL = 1e-3;             % precision to which we should run bisection

n = 10;                 % filter order (2n+1 is the full order)
wpass = 0.12*pi;        % passband cutoff freq (in radians)
delta = 1;              % max (+/-) passband ripple in dB
atten_level = -30;      % stopband attenuation level in dB

%********************************************************************
% create optimization parameters
%********************************************************************
m = 30*n; % freq samples (rule-of-thumb)
w = linspace(0,pi,m);

%*********************************************************************
% use bisection algorithm to solve the problem
%*********************************************************************

wstop_bot = wpass;
wstop_top  = wstop;

while( wstop_top - wstop_bot > TOL)
  % try to find a feasible design for given specs
  wstop_cur = (wstop_top + wstop_bot)/2;

  % create optimization matrices (matrix of cosines)
  A = [ones(m,1) 2*cos(kron(w',[1:n]))];

  % passband 0 <= w <= w_pass
  ind = find((0 <= w) & (w <= wpass));    % passband
  Ap  = A(ind,:);

  % transition band is not constrained (w_pass <= w <= w_stop)

  % stopband (w_stop <= w) (this is the changing constraint)
  ind = find((wstop_cur <= w) & (w <= pi));   % stopband
  As  = A(ind,:);

  % formulate and solve the feasibility linear-phase lp filter design
  cvx_begin quiet
    variable h_cur(n+1,1);
    % feasibility problem
    % passband bounds
    Ap*h_cur <= 10^(delta/20);
    Ap*h_cur >= 10^(-delta/20);
    % stopband bounds
    abs( As*h_cur ) <= 10^(atten_level/20);
  cvx_end

  % bisection
  if strfind(cvx_status,'Solved') % feasible
    fprintf(1,'Problem is feasible for stopband freq = %3.4f rads\n',wstop_cur);
    wstop_top = wstop_cur;
    % construct the full impulse response
    h = [flipud(h_cur(2:end)); h_cur];
  else % not feasible
    fprintf(1,'Problem is not feasible for stopband freq = %3.4f rads\n',wstop_cur);
    wstop_bot = wstop_cur;
  end
end

wstop = wstop_top;
fprintf(1,['\nOptimum stopband frequency for given specs is %3.4f*pi rads\n' ...
           'and the minimum transition width is %3.4f*pi radians.\n'],...
            wstop/pi, (wstop-wpass)/pi);

%********************************************************************
% plots
%********************************************************************
figure(1)
% FIR impulse response
plot([0:2*n],h','o',[0:2*n],h','b:')
xlabel('t'), ylabel('h(t)')

figure(2)
% frequency response
H = exp(-j*kron(w',[0:2*n]))*h;
% magnitude
subplot(2,1,1)
plot(w,20*log10(abs(H)),...
     [wstop pi],[atten_level atten_level],'r--',...
     [0 wpass],[delta delta],'r--',...
     [0 wpass],[-delta -delta],'r--');
axis([0,pi,-40,10])
xlabel('w'), ylabel('mag H(w) in dB')
% phase
subplot(2,1,2)
plot(w,angle(H))
axis([0,pi,-pi,pi])
xlabel('w'), ylabel('phase H(w)')


close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Number of measurements
M = 64;
% Signal space
N = 2 * M;
% Dirac-Fourier Dictionary
Phi = spx.dict.simple.dirac_fourier_dict(M);

% Number of signals
Ss = [1, 2, 4, 8, 16, 32];
% Sparsity levels
Ks = 2:2:50;
num_trials = 100;
num_ks = length(Ks);
num_ss = length(Ss);
bp_success_with_k = zeros(num_ss, num_ks);

snr_threshold = 100;

for ns=1:num_ss
    % Current sparsity level
    S = Ss(ns);
    for nk=1:num_ks
        K = Ks(nk);
        num_bp_successes = 0;
        for nt=1:num_trials
            X = model_3_data(N, K, S);
            % Measurement vectors
            Y = Phi * X;

            % BP MMV solver instance
            bp_solver = spx.pursuit.joint.BasisPursuit(Phi);
            % Solve the sparse recovery problem
            result = bp_solver.solve_l2_l1_complex(Y);
            % Solution vectors
            X_BP = result.Z;
            % Comparison
            cs = spx.commons.SparseSignalsComparison(X, X_BP, K);
            snr = cs.cum_signal_to_noise_ratio;
            bp_success = snr > snr_threshold;
            num_bp_successes = num_bp_successes + bp_success;
            fprintf('S: %d, K=%d, trial=%d, residual bp: %e, SNR: %f dB\n'...
                , S, K, nt, cs.cum_difference_norm, snr);
        end
        bp_success_with_k(ns, nk) = num_bp_successes / num_trials;
    end
end

save ('bin/figure_2_dirac_fourier_dict_model_3_bp_success_with_k.mat');


function pass = test_lu( pref )
% Test for LU decomposition of a chebfun2.

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

tol = 100*pref.cheb2Prefs.chebfun2eps;

% Decomposition on [-1,1,-1,1]:
x = chebpts(100);
f = chebfun2( @(x,y) cos( x.*y ) );
[L, U] = lu( f );
g = L * U;
pivPos = f.pivotLocations;

% Accurate decomposition:
pass(1) = norm( fevalm(f, x, x) - fevalm(g, x, x) ) < tol;
pass(2) = norm( diag( L( pivPos( :, 2 ) , :) ) - ones( length(f), 1) ) < tol;
% pass(j) = norm( triu( L( pivPos( :, 2 ) , :) , 1) ) < sqrt(tol); j = j + 1;
pass(3) = norm( tril( U( :, pivPos( :, 1 ) ) , -1) ) < sqrt(tol);

% Try the same thing on different domain:
x = chebpts(100, [-2.1, 4.3]);
y = chebpts(100, [-1, 2.7]);

f = chebfun2( @(x,y) cos( x.*y ), [-2.1, 4.3, -1, 2.7]);
[L, U] = lu( f );
g = L * U;
pivPos = f.pivotLocations;

% Accurate decomposition:
pass(4) = norm( fevalm(f, x, y) - fevalm(g, x, y) ) < 2*tol;
pass(5) = norm( diag( L( pivPos( :, 2 ) , :) ) - ones( length(f), 1) ) < tol;
% pass(j) = norm( triu( L( pivPos( :, 2 ) , :) , 1) ) < sqrt(tol); j = j + 1;
pass(6) = norm( tril( U( :, pivPos( :, 1 ) ) , -1) ) < sqrt(tol);

end


function J=vl_imwhiten(I,alpha,cutoff)
% VL_IMWHITEN  Whiten an image
%   J = VL_IMWHITEN(I,ALPHA) approximatively whitens the power spectrum
%   of the natural image I. The algorithm assumes that the modulus of
%   the spectrum decays as 1/f^ALPHA (f is the frequency).
%
%   VL_IMWHITEN(I) uses ALPHA=1 (a typical value for natural images).
%
%   VL_IMWHITEN(I,ALPHA,CUTOFF) also applies a low-pass filter with
%   cutoff frequency equal to CUTOFF x FN, where FN is the Nyquist
%   frequency (half of the sampling frequency).
%
%   See also: VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if ~exist('alpha','var'),  alpha = 1 ; end
if ~exist('cutoff','var'), cutoff = [] ; end

[M,N]=size(I) ;

% Frequency domain
fn = 0.5 ; % Nyquist freq (=1/2T, T=1)
fx_range=linspace(-fn, fn, N) ;
fy_range=linspace(-fn, fn, M) ;
[fx fy]=meshgrid(fx_range, fy_range) ;

% Whitening filter
rho=sqrt(fx.*fx+fy.*fy);
filt=rho.^alpha ;

% Low-pass filter
if ~isempty(cutoff)
  fcut = cutoff * fn ;
  filt = filt .* exp(-(rho/fcut).^4);
  %filt = filt .* exp( - 0.5 * (rho / fcut) .^ 2);
end

% Apply filter
J = real(ifft2(fft2(I).*fftshift(filt))) ;


function [tuples,gain]=kCard2DAssign(C,k,maximize)
%%KCARD2DASSIGN Solve the two-dimensional k-cardinality assignment problem
%          with a rectangular cost matrix C. The problem being solved can
%          be formulated as minimize (or maximize)
%          \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}C_{i,j}*x_{i,j}
%          subject to
%          \sum_{j=1}^{numCol}x_{i,j}<=1 for all i
%          \sum_{i=1}^{numRow}x_{i,j}<=1 for all j
%          \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}x_{i,j}=k
%          x_{i,j}=0 or 1.
%
%INPUTS: C A numRowXnumCol cost matrix that does not contain any NaNs and
%          where the largest finite element minus the smallest element is a
%          finite quantity (does not overflow) when performing minimization
%          and where the smallest finite element minus the largest
%          element is finite when performing maximization. Forbidden
%          assignments can be given costs of +Inf for minimization and -Inf
%          for maximization.
%        k The integer number of assignments to make.
%          k<=min(numRow,numCol).
% maximize If true, the minimization problem is transformed into a
%          maximization problem. The default if this parameter is omitted
%          or an empty matrix is passed is false.
%
%OUTPUTS: tuples A 2XnumRow set of assignment values. This is ordered
%                [rowIndex;columnIndex]. If no feasible solution exists,
%                then an empty matrix will be returned.
%          gain This is the value of the cost. This is the sum of the
%               values in C corresponding to the tuples.
%
%As noted in [1], the k-cardinality 2D assignment problem can be
%transformed into a standard rectangular 2D assignment problem. This
%function implements that transformation and calls assign2D.
%
%EXAMPLE:
% C=[7,   51,  52,  87;
%    50,  12,   0,  64;
%    27,  77,   0,  18;
%    62,   0,   3,   8];
% k=4;
% [tuples4,gain4]=kCard2DAssign(C,4)
% [tuples3,gain3]=kCard2DAssign(C,3)
% [tuples2,gain2]=kCard2DAssign(C,2)
% [tuples1,gain1]=kCard2DAssign(C,1)
%One will see optimal gains of 25, 7, 0, and 0.
%
%REFERENCES:
%[1] A. Volgenant, "Solving the k-cardinality assignment problem by
%    transformation," European Journal of Operational Research, vol. 157,
%    no. 2, pp. 322-331, 1 Sep. 2004.
%
%June 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<3||isempty(maximize))
        maximize=false;
    end

    numRow=size(C,1);
    numCol=size(C,2);

    if(k==0)
       tuples=[];
       gain=0;
       return;
    end

    if(k<0||k~=fix(k))
        error('k is invalid.')
    end

    if(k>numRow||k>numCol)
        %The problem is not feasible.
        tuples=[];
        gain=-1;
        return
    end

    COrig=C;

    if(maximize==true)
        CDelta=max(C(:));

        %If C is all negative, do not shift.
        if(CDelta<0)
            CDelta=0;
        end

        C=-C+CDelta;
    else
        CDelta=min(C(:));

        %If C is all positive, do not shift.
        if(CDelta>0)
            CDelta=0;
        end

        C=C-CDelta;
    end

    %The minimum value in C is now 0 or something positive. We shall shift
    %the values to guarantee that 0 is less than every value in C.
    %Find the maximum finite value in C.
    CMax=max(max(C(isfinite(C))));

    %We shall now offset everything by a small fraction of the maximum
    %value. This is so that we guarantee that values will be assigned
    %to the zero padded rows that shall be added to C.
    COffset=CMax*1e-10;
    C=C+COffset;

    %Perform 2D assignment on the augmented matrix. An algorithm that does
    %not explicitely construct that assignment matrix is also possible.
    [~,row4Col]=assign2D([C;zeros(numCol-k,numCol)],false);

    if(isempty(row4Col))
        %If the problem is infeasible.
        tuples=[];
        gain=-1;
        return
    end

    tuples=zeros(2,k);
    curTuple=1;
    gain=0;
    for curCol=1:numCol

        curRow=row4Col(curCol);

        if(curRow<=numRow)
            gain=gain+COrig(curRow,curCol);
            tuples(:,curTuple)=[curRow;curCol];
            curTuple=curTuple+1;

           if(curTuple>k)
               break;
           end
        end
    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 b = int(a,x,L,U)
% function B = int(A,X,L,U)
%
% DESCRIPTION
%   Element-by-element integration of a polynomial with respect
%   to a single variable.
%
% INPUTS
%   A: polynomial
%   X: Scalar polynomial variable [Optional with default X = A.varname{1}]
%   L: Lower limit of definite integral
%   U: Upper limit of definite integral
%
% OUTPUTS
%   B: polynomial
%
% SYNTAX
%   B = int(A,X)
%     Indefinite integral of the polynomial, A, with respect to X.
%     X should be a polynomial variable or string.   Integration is done
%     element-by-element if A is a matrix.
%   B = int(A,X,L,U)
%     Definite integral of A with respect to X from lower limit L to
%     upper limit U.
%   B = int(A,X,[L U]);
%     Equivalent to B = diff(A,X,L,U)
%
% EXAMPLE
%   pvar x y z;
%   a = 2*x^3 - 2*x*z^2 + 5*y*z;
%   b = int(a,x)
%   diff(b,x)-a
%   c = int(a,[0 1])
%
% See also: diff, jacobian

% 12/6/2010 PJS  Initial Coding

% Process Inputs/ Error Checking
if nargin==1
    x = a.varname{1};
    L = []; U = [];
elseif nargin==2
    if isa(x,'double')
        % B = int(A,[L,U])
        L = x(1);
        U = x(2);
        x = a.varname{1};
    else
        % B = int(A,X)
        L = []; U = [];
    end
elseif nargin==3
    if isa(x,'double')
        % B = int(A,L,U)
        U = L;
        L = x;
        x = a.varname{1};
    else
        % B = int(A,X,[L U])
        U = L(2);
        L = L(1);
    end
elseif nargin~=4
    error(['Invalid syntax for the "int" command. ' ...
        'Type "help int" for more information.'])
end

if ispvar(x) && length(x)==1
    x = x.varname{1};
elseif ~ischar(x)
    error('X must be a single polynomial variable or a string');
end

% Get polynomial info about A
acoef = a.coefficient;
adeg = a.degmat;
avar = a.varname;
sza = a.matdim;
Naterms = size(acoef,1);

% Find variable we are differentiating with respect to.
varnumb = find( strcmp(avar,x) );

% Perform indefinite integral
szb = sza;
if isempty(varnumb)
    % int a(y) dx = a(y)*x
    bcoef = acoef;
    bdeg = [adeg ones(Naterms,1)];
    bvar = [avar; x];
else
    % int a(y)*x^n dx = a(y)*x^(n+1) / (n+1)
    bdeg = adeg;
    nplus1 = bdeg(:,varnumb)+1;
    bdeg(:,varnumb) = nplus1;

    bcoef = acoef;
    bcoef = lrscale( bcoef, 1./nplus1 , []);

    bvar = avar;
end

chkval = 0; % skip validity check
b = polynomial(bcoef,bdeg,bvar,szb,chkval);

% Evaluate definite integral
if ~isempty(L)
    bU = subs(b,{x},U);
    bL = subs(b,{x},L);
    b = bU-bL;
end

% Combine any common terms
b = combine(b);


function ray = createRay(varargin)
%CREATERAY Create a ray (half-line), from various inputs
%
%   RAY = createRay(POINT, ANGLE)
%   POINT is a N*2 array giving starting point of the ray, and ANGLE is the
%   orientation of the ray.
%
%   RAY = createRay(X0, Y0, ANGLE)
%   Specify ray origin with 2 input arguments.
%
%   RAY = createRay(P1, P2)
%   Create a ray starting from point P1 and going in the direction of point
%   P2.
%
%   Ray is represented in a parametric form: [x0 y0 dx dy]
%   x = x0 + t*dx
%   y = y0 + t*dy;
%   for all t>0
%
%   Example
%   origin  = [3 4];
%   theta   = pi/6;
%   ray = createRay(origin, theta);
%   figure(1); clf; hold on;
%   axis([0 10 0 10]);
%   drawRay(ray);
%
%   See also:
%   rays2d, createLine, points2d
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2007-10-18
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

if length(varargin)==2
    p0 = varargin{1};
    arg = varargin{2};
    if size(arg, 2)==1
        % second input is the ray angle
        ray = [p0 cos(arg) sin(arg)];
    else
        % second input is another point
        ray = [p0 arg-p0];
    end

elseif length(varargin)==3
    x = varargin{1};
    y = varargin{2};
    theta = varargin{3};
    ray = [x y cos(theta) sin(theta)];

else
    error('Wrong number of arguments in ''createRay'' ');
end


function a = r8vec_normalize_l1 ( n, a )

%*****************************************************************************80
%
%% R8VEC_NORMALIZE_L1 normalizes an R8VEC to have unit sum.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A(N), the vector to be normalized.
%
%    Output, real A(N), the entries of A should have unit sum.  However,
%    if the input vector has zero sum, the routine halts.
%
  a_sum = sum ( a(1:n) );

  if ( a_sum == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_NORMALIZE_L1 - Fatal error!\n' );
    fprintf ( 1, '  The vector entries sum to 0.\n' );
    error ( 'R8VEC_NORMALIZE_L1 - Fatal error!' );
  end

  a(1:n) = a(1:n) / a_sum;

  return
end


function h = r8mat_house_post ( n, a, row, col )

%*****************************************************************************80
%
%% R8MAT_HOUSE_POST computes a Householder post-multiplier matrix.
%
%  Discussion:
%
%    H(ROW,COL) has the property that the ROW-th column of
%    A*H(ROW,COL) is zero from entry COL+1 to the end.
%
%    In the most common case, where a QR factorization is being computed,
%    ROW = COL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrices.
%
%    Input, real A(N,N), the matrix whose Householder matrix
%    is to be computed.
%
%    Input, integer ROW, COL, specify the location of the
%    entry of the matrix A which is to be preserved.  The entries in
%    the same row, but higher column, will be zeroed out if
%    A is postmultiplied by H.
%
%    Output, real H(N,N), the Householder matrix.
%

%
%  Set up the vector V.
%
  a_row(1,1:col-1) = 0.0;
  a_row(1,col:n) = a(row,col:n);

  a_row = a_row';

  v = r8vec_house_column ( n, a_row, col );
%
%  Form the matrix H(V).
%
  h = r8mat_house_form ( n, v );

  return
end


function [label, model, llh] = mixLinReg(X, y, k, lambda)
% Mixture of linear regression
% input:
%   X: d x n data matrix
%   y: 1 x n responding vector
%   k: number of mixture component
%   lambda: regularization parameter
% output:
%   label: 1 x n cluster label
%   model: trained model structure
%   llh: loglikelihood
% Written by Mo Chen (sth4nth@gmail.com).
if nargin < 4
    lambda = 1;
end
n = size(X,2);
X = [X;ones(1,n)]; % adding the bias term
d = size(X,1);
label = ceil(k*rand(1,n));  % random initialization
R = full(sparse(label,1:n,1,k,n,n));
tol = 1e-6;
maxiter = 500;
llh = -inf(1,maxiter);
Lambda = lambda*eye(d);
W = zeros(d,k);
Xy = bsxfun(@times,X,y);
beta = 1;
for iter = 2:maxiter
    % maximization
    nk = sum(R,2);
    alpha = nk/n;
    for j = 1:k
        Xw = bsxfun(@times,X,sqrt(R(j,:)));
        U = chol(Xw*Xw'+Lambda);
        W(:,j) = U\(U'\(Xy*R(j,:)'));  % 3.15 & 3.28
    end
    D = bsxfun(@minus,W'*X,y).^2;
    % expectation
    logRho = (-0.5)*beta*D;
    logRho = bsxfun(@plus,logRho,log(alpha));
    T = logsumexp(logRho,1);
    logR = bsxfun(@minus,logRho,T);
    R = exp(logR);
    llh(iter) = sum(T)/n;
    if abs(llh(iter)-llh(iter-1)) < tol*abs(llh(iter)); break; end
end
llh = llh(2:iter);
model.alpha = alpha; % mixing coefficient
model.beta = beta; % mixture component precision
model.W = W;  % linear model coefficent
[~,label] = max(R,[],1);
model.label = label;


function [ triangle_index, alpha, beta, gamma, edge, step_num ] = ...
  triangulation_search_delaunay ( node_num, node_xy, triangle_order, ...
  triangle_num, triangle_node, triangle_neighbor, p )

%*****************************************************************************80
%
%% TRIANGULATION_SEARCH_DELAUNAY searches a Delaunay triangulation for a point.
%
%  Purpose:
%
%    The algorithm "walks" from one triangle to its neighboring triangle,
%    and so on, until a triangle is found containing point P, or P is found
%    to be outside the convex hull.
%
%    The algorithm computes the barycentric coordinates of the point with
%    respect to the current triangle.  If all three quantities are positive,
%    the point is contained in the triangle.  If the I-th coordinate is
%    negative, then P lies on the far side of edge I, which is opposite
%    from vertex I.  This gives a hint as to where to search next.
%
%    For a Delaunay triangulation, the search is guaranteed to terminate.
%    For other triangulations, a cycle may occur.
%
%    Note the surprising fact that, even for a Delaunay triangulation of
%    a set of points, the nearest point to P need not be one of the
%    vertices of the triangle containing P.
%
%    The code can be called for triangulations of any order, but only
%    the first three nodes in each triangle are considered.  Thus, if
%    higher order triangles are used, and the extra nodes are intended
%    to give the triangle a polygonal shape, these will have no effect,
%    and the results obtained here might be misleading.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2012
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the vertices.
%
%    Input, integer TRIANGLE_ORDER, the order of the triangles.
%
%    Input, integer TRIANGLE_NUM, the number of triangles in the triangulation.
%
%    Input, integer TRIANGLE_NODE(TRIANGLE_ORDER,TRIANGLE_NUM),
%    the nodes that make up each triangle.
%
%    Input, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the triangle
%    neighbor list.
%
%    Input, real P(2), the coordinates of a point.
%
%    Output, integer TRIANGLE_INDEX, the index of the triangle where the
%    search ended.  If a cycle occurred, then TRIANGLE_INDEX = -1.
%
%    Output, real ALPHA, BETA, GAMMA, the barycentric
%    coordinates of the point relative to triangle TRIANGLE_INDEX.
%
%    Output, integer EDGE, indicates the position of the point P in
%    triangle TRIANGLE_INDEX:
%    0, the interior or boundary of the triangle;
%    -1, outside the convex hull of the triangulation, past edge 1;
%    -2, outside the convex hull of the triangulation, past edge 2;
%    -3, outside the convex hull of the triangulation, past edge 3.
%
%    Output, integer STEP_NUM, the number of steps.
%
  persistent triangle_index_save;

  dim_num = 2;

  step_num = - 1;
  edge = 0;

  if ( length ( triangle_index_save ) == 0 )
    triangle_index_save = -1;
  end

  if ( triangle_index_save < 1 | triangle_num < triangle_index_save )
    triangle_index = floor ( ( triangle_num + 1 ) / 2 );
  else
    triangle_index = triangle_index_save;
  end

  while ( 1 )

    step_num = step_num + 1;

    if ( triangle_num < step_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TRIANGULATION_SEARCH_DELAUNAY - Fatal error!\n' );
      fprintf ( 1, '  The algorithm seems to be cycling.\n' );
      triangle_index = -1;
      alpha = -1.0;
      beta = -1.0;
      gamma = -1.0;
      edge = -1;
      return
    end
%
%  Get the vertices of triangle TRIANGLE_INDEX.
%
    a = triangle_node(1,triangle_index);
    b = triangle_node(2,triangle_index);
    c = triangle_node(3,triangle_index);
%
%  Using vertex C as a base, compute the distances to vertices A and B,
%  and the point P.
%
    dxa = node_xy(1,a) - node_xy(1,c);
    dya = node_xy(2,a) - node_xy(2,c);

    dxb = node_xy(1,b) - node_xy(1,c);
    dyb = node_xy(2,b) - node_xy(2,c);

    dxp = p(1)         - node_xy(1,c);
    dyp = p(2)         - node_xy(2,c);

    det = dxa * dyb - dya * dxb;
%
%  Compute the barycentric coordinates of the point P with respect
%  to this triangle.
%
    alpha = ( dxp * dyb - dyp * dxb ) / det;
    beta =  ( dxa * dyp - dya * dxp ) / det;
    gamma = 1.0 - alpha - beta;
%
%  If the barycentric coordinates are all positive, then the point
%  is inside the triangle and we're done.
%
    if ( 0.0 <= alpha && 0.0 <= beta && 0.0 <= gamma )
      break
    end
%
%  At least one barycentric coordinate is negative.
%
%  If there is a negative barycentric coordinate for which there exists
%  an opposing triangle neighbor closer to the point, move to that triangle.
%
%  (Two coordinates could be negative, in which case we could go for the
%  most negative one, or the most negative one normalized by the actual
%  distance it represents).
%
    if ( alpha < 0.0 && 0 < triangle_neighbor(2,triangle_index) )
      triangle_index = triangle_neighbor(2,triangle_index);
      continue;
    elseif ( beta < 0.0 && 0 < triangle_neighbor(3,triangle_index) )
      triangle_index = triangle_neighbor(3,triangle_index);
      continue;
    elseif ( gamma < 0.0 && 0 < triangle_neighbor(1,triangle_index) )
      triangle_index = triangle_neighbor(1,triangle_index);
      continue;
    end
%
%  All negative barycentric coordinates correspond to vertices opposite
%  sides on the convex hull.
%
%  Note the edge and exit.
%
    if ( alpha < 0.0 )
      edge = -2;
      break
    elseif ( beta < 0.0 )
      edge = -3;
      break
    elseif ( gamma < 0.0 )
      edge = -1;
      break
    end

  end

  triangle_index_save = triangle_index;

  return
end


function [r,spherCent]=osculatingSpher4LatLon(latLon,a,f,is2D)
%%OSCULATINGSPHER4LATLON Given the latitude and longitude of a point on an
%           ellipsoid, determine the radius and center location of an
%           osculating sphere going through that point. The osculating
%           sphere is such that the azimuth equals the longitude and the
%           elevation equals the latitude of the point on the ellipsoid.
%           Additionally, the local tangent planes at the point are the
%           same. The radius of curvature of the sphere at that point
%           should nominally equal the radius of curvature of the ellipsoid
%           at that point. However, since the ellipsoid has two radii of
%           curvature, the geometric mean is used (the mean radius of
%           curvature). On the other hand, if is2D is true, then just the
%           radius of curvature in the meridian will be used, which is what
%           perfectly matches a cut of the ellipsoid through the origin and
%           pole (A 2D ellipse in the x-z plane).
%
%INPUTS: latLon The 2X1 point of the format [latitude;longitude] in radians
%               where the osculating sphere should touch the ellipsoid.
%             a The semi-major axis length of the ellipsoid. If this
%               argument is omitted or an empty matrix is passed, the value
%               in Constants.WGS84SemiMajorAxis is used.
%             f The flattening factor of the ellipsoid. If this argument is
%               omitted or an empty matrix is passed, the value in
%               Constants.WGS84Flattening is used.
%          is2D Indicates whether one just cares about a 2D cut of the
%               ellipsoid. If so, then the radius used will be the radius
%               of curvature in the meridian. The default if omitted or an
%               empty matrix is passed is false.
%
%OUTPUTS: r The radius of the osculating sphere.
% spherCent The location of the center of the osculating sphere with
%           respect to the center of the ellipsoid.
%
%The formulae in Section 3 of [1] are used.
%
%EXAMPLE:
%Here, we consider a 2D slice of an ellipsoid that goes through
%longitude=0, we choose a point. We then find the osculating sphere. The
%ellipse cut of the ellipsoid and the circle cut of the sphere are then
%plotted with the point. We ALSO plot the circle cut obtained with
%is2D=true. Since we are only considering a 2D cut, one will see that that
%circle is actually better aligned with the curvature of the ellipse at
%that point. However, if one were to take a 2D cut in other directions, it
%would appear to be a worse fit.
% a=20;%Semi-major axis.
% f=0.5;%Flattening factor.
% b=a*(1-f);%The semi-minor axis of the ellipsoid.
%
% figure(1)
% clf
% hold on
% %Draw a 2D cut of the ellipsoid.
% A=inv([a^2,0;
%        0,b^2]);
% drawEllipse([0;0],A,1,'linewidth',4)
% latLon=[50;0]*(pi/180);
% oscPt=ellips2Cart([latLon;0],a,f);
% [r,spherCent]=osculatingSpher4LatLon(latLon,a,f,false);
% [r2D,spherCent2D]=osculatingSpher4LatLon(latLon,a,f,true);%is2D=true
% %Draw the both 2D cuts of the sphere:
% A=inv([r^2,0;
%        0,r^2]);
% drawEllipse([spherCent(1);spherCent(3)],A,1,'-.','linewidth',4)
% A=inv([r2D^2,0;
%        0,r2D^2]);
% drawEllipse([spherCent2D(1);spherCent2D(3)],A,1,'--','linewidth',2)
%
% %Note that spherCent(2)==0.
% scatter(oscPt(1),oscPt(3),100,'filled')
% legend('Ellipsoid Cut','Osculating Sphere','Osculating Sphere 2D','location','southwest')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] P. Williams and D. Last, "On Loran-C time-difference to co-ordinate
%    converters," in Proceedings of the 32nd Annual Convention & Technical
%    Symposium of the International Loran Association, Boulder, CO, 3-7
%    Nov. 2003.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(is2D))
    is2D=false;
end

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

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

%The squared eccentricity of the ellipsoid.
e2=f*(2-f);

lat=latLon(1);%Latitude.
lon=latLon(2);

Phi=lat;

denomTerm=sqrt(1-e2*sin(Phi)^2);
%Equation 3.17 in [1]. The radius of curvature in the meridian.
M0=a*(1-e2)/denomTerm^3;

if(is2D)
    r=M0;
else
    %Equation 3.18 in [1]. The radius of curvature in the prime vertical.
    N0=a/denomTerm;

    %Equation 3.16 in [1].
    r=sqrt(M0*N0);
end

xCartEllips=ellips2Cart([latLon(1:2);0],a,f);
xCartSpher=spher2Cart([r;lon;lat]);

spherCent=-(xCartSpher-xCartEllips);

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.


% =========================
%  Robust Bayesian allocation in the stock market, as described in
%  Meucci, A., (2005) "Robust Bayesian Allocation"
%
%  Most recent version of article and code available at http://symmys.com/node/102
%  =========================

clear; clc; close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% inputs
p_m=.1; % robustness parameter location
p_s=.1; % robustness parameter scatter
load SectorsSnP500

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute weekly returns
Ps=P(1:5:end,:);
R=Ps(2:end,:)./Ps(1:end-1,:)-1;
Dates_P=DP(1:5:end);
Dates_R=Dates_P(2:end);
[Ttot,N]=size(R);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimation
W=52; % rolling estimation period
NumPortf=10;
Ret_hat=[];
Ret_rB=[];
Dates=[];
for t=W+1:Ttot-1
    Ttot-t+2
    Rets=R(t-W:t,:);

    % sample estimate
    m_hat=mean(Rets)';
    S_hat=cov(Rets);
    [de_hat,ds_hat,w_hat] = EfficientFrontier(NumPortf, S_hat, m_hat);

    % Bayesian prior
    S0=diag(diag(S_hat));
    m0=.5*S0*ones(N,1)/N;
    T=size(Rets,1);
    T0=2*T;
    nu0=2*T;

    % Bayesian posterior parameters
    T1=T+T0;
    m1=1/T1*(m_hat*T+m0*T0);
    nu1=T+nu0;
    S1=1/nu1*( S_hat*T + S0*nu0 + (m_hat-m0)*(m_hat-m0)'/(1/T+1/T0)  );
    [d,d,w1] = EfficientFrontier(NumPortf, S1, m1);

    % robustness parameters
    q_m2=chi2inv(p_m,N);
    g_m=sqrt(q_m2/T1*nu1/(nu1-2));
    q_s2=chi2inv(p_s,N*(N+1)/2);
    PickVol=round(.8*NumPortf);
    v=(ds_hat(PickVol))^2;
    g_s=v/(  nu1/(nu1+N+1)+sqrt( 2*nu1*nu1*q_s2/((nu1+N+1)^3)));

    Target=[];

    wu=w_hat(PickVol,:)';
    Ret_hat=[Ret_hat R(t+1,:)*wu];

    for k=1:NumPortf-1
        NewTarget=-(10^10);
        if wu'*S1*wu <= g_s
            NewTarget = m1'*wu-g_m*sqrt(wu'*S1*wu);
        end
        Target=[Target NewTarget];
    end

    [Best,k]=max(Target);
    wu=w1(k,:)';
    Ret_rB=[Ret_rB R(t+1,:)*wu];

    Dates=[Dates Dates_R(t+1)];
end

NAV_hat=cumprod(1+Ret_hat);
NAV_rB=cumprod(1+Ret_rB);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plots

figure
subplot(2,1,1)
h=plot(Dates,Ret_hat);
xlim([Dates(1) Dates(end)])
YLim=get(gca,'ylim');
Datetick('x','mmmyy','keeplimits','keepticks');
grid on
subplot(2,1,2)
h=plot(Dates,Ret_rB);
set(gca,'ylim',YLim,'xlim',[Dates(1) Dates(end)]);
Datetick('x','mmmyy','keeplimits','keepticks');
grid on

figure
subplot(2,1,1)
h=plot(Dates,NAV_hat);
xlim([Dates(1) Dates(end)])
YLim=get(gca,'ylim');
Datetick('x','mmmyy','keeplimits','keepticks');
grid on
subplot(2,1,2)
h=plot(Dates,NAV_rB);
set(gca,'ylim',YLim,'xlim',[Dates(1) Dates(end)]);
Datetick('x','mmmyy','keeplimits','keepticks');
grid on


function Test_quadraticFunction

% Define a linear space
m = 5;
n = 2;
manifold = euclideanfactory(m,n);

% random symmetric square matrix
Q = multisym(randn(m));

% Create the problem structure.
problem.M = manifold;

% Define the problem cost function, Euclidean gradient and Hessian.
problem.cost = @cost;
function f = cost(X)
  f = 0.5*trace(X'*Q*X);
end

problem.egrad = @egrad;
function G = egrad(X)
  G = Q*X;
end

problem.ehess = @ehess;
function H = ehess(X, Xdot)
  H = Q*Xdot;
end

% Check gradient and Hessian correctness
% This is a quadratic function in a linear manifold,
% so the quadratic approximation used in `checkhessian` should be exact
% Other similar cases are those where the unknowns belong to a tangent
% space (also a linear manifold).
checkgradient( problem ); pause
checkhessian(  problem ); pause

end


function b = r83p_vxm ( n, a, x )

%*****************************************************************************80
%
%% R83P_VXM multiplies a vector by a R83P matrix.
%
%  Discussion:
%
%    The R83P storage format stores a periodic tridiagonal matrix as
%    a 3 by N array, in which each row corresponds to a diagonal, and
%    column locations are preserved.  The matrix value
%    A(1,N) is stored as the array entry A(3,N), and the matrix value
%    A(N,1) is stored as the array entry A(1,1).
%
%  Example:
%
%    Here is how a R83P matrix of order 5 would be stored:
%
%      A51 A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54 A15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 3.
%
%    Input, real A(3,N), the R83P matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product X * A.
%
  b(1) =   a(1,1)   * x(n)   + a(2,1) * x(1) + a(3,1)   * x(2);

  for i = 2 : n-1
    b(i) = a(1,i)   * x(i-1) + a(2,i) * x(i) + a(3,i)   * x(i+1);
  end

  b(n) =   a(1,n)   * x(n-1) + a(2,n) * x(n) + a(3,n)   * x(1);

  return
end


function proj2d = compute_one_projection_method_2(xs,ys,zs,data3d,psrc,pcdet,su,sv,nu,nv)
%
%	proj2d = compute_one_projection_method_2(xs,ys,zs,data3d,psrc,pcdet,su,sv,nu,nv)
%
%	Method 2: compute the project based on the stack of planes perpendicular to the src-detector line
%	This method should be faster than the line-integral method
%
%   Deshan Yang, PhD
%	Department of radiation oncology
%	Washington University in Saint Louis
%   01/16/2011, Saint Louis, MO, USA
%

corner_points = [...
	xs(1) ys(1) zs(1);...
	xs(1) ys(1) zs(end);...
	xs(1) ys(end) zs(1);...
	xs(1) ys(end) zs(end);...
	xs(end) ys(1) zs(1);...
	xs(end) ys(1) zs(end);...
	xs(end) ys(end) zs(1);...
	xs(end) ys(end) zs(end)...
	];

corner_points_t = zeros(8,1);
for k =1:8
	p = corner_points(k,:);
	[q,corner_points_t(k)] = project_1_point_to_a_line(psrc,pcdet,p);
end

t_min = min(corner_points_t);
t_max = max(corner_points_t);

dist = norm(psrc-pcdet);

N = ceil(dist*(t_max-t_min));	% N planes
dt = (t_max-t_min)/N;
ts = t_min:dt:t_max;

proj2d = zeros(nv,nu,'single');
vecu = [0 0 1];

vecn = psrc-pcdet;
vecv = cross(vecn,vecu);
vecv = vecv/norm(vecv);

us = ((-nu/2+0.5):1:(nu/2-0.5))*su/nu;
vs = ((-nv/2+0.5):1:(nv/2-0.5))*sv/nv;
[uu,vv] = meshgrid(us,vs);

for T = 1:N
	vecu_t = vecu*ts(T);
	vecv_t = vecv*ts(T);
	pcdet_t = psrc+ts(T)*(pcdet-psrc);

	vdet_t_xs = pcdet_t(1) + vecu_t(1)*uu + vecv_t(1)*vv;
	vdet_t_ys = pcdet_t(2) + vecu_t(2)*uu + vecv_t(2)*vv;
	vdet_t_zs = pcdet_t(3) + vecu_t(3)*uu + vecv_t(3)*vv;

	proj2d = proj2d + interp3(xs,ys,zs,data3d,vdet_t_xs,vdet_t_ys,vdet_t_zs,'linear',0)*dt*dist;
end

% adjust the value according to the projection path length
vdet_t_xs = pcdet(1) + vecu(1)*uu + vecv(1)*vv;
vdet_t_ys = pcdet(2) + vecu(2)*uu + vecv(2)*vv;
vdet_t_zs = pcdet(3) + vecu(3)*uu + vecv(3)*vv;

dists = sqrt((vdet_t_xs-psrc(1)).^2+(vdet_t_ys-psrc(2)).^2+(vdet_t_zs-psrc(3)).^2);
dists = dists / min(dists(:));
proj2d = proj2d .* dists;


function d = p01_fd ( p )

%*****************************************************************************80
%
%% P01_FD is a signed distance function for problem 1.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    06 February 2006
%
%  Parameters:
%
%    Input, real P, one or more points.
%
%    Output, real D, the signed distance of each point to the boundary of the region.
%
  d = sqrt ( sum ( p.^2, 2 ) ) - 1.0;

  return
end


function y=polyValVec(P,x)
%%POLYVALVEC Evaluate a set of polynomials at a given scalar point or set
%            of points. Evaluating a collection of polynomials produces a
%            vector estimate. This could, for example, be used to evaluate
%            polynomials to interpolate each component of a vector quantity
%            to a desired point given the ointerpolating polynomials for
%            each dimension.
%
%INPUTS: P A numDimX(numDeg+1) matrix of numDim polynomial coefficients for
%          each of the numDim components. numDeg is the degree of the
%          polynomials. The coefficients for each row are arranged for
%          exponents in the order [x^numDeg, x^(numDeg-1),...,1,0].
%        x A scalar value of x or a linear vector of numX points at which
%          the polynomials should be evaluated.
%
%OUTPUTS: y The numDim X numX set of polynomials evaluated at each of the
%           numX points.
%
%The algorithm just calls Matlab's polyval function multiple times in a
%loop.
%
%February 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(P,1);
numX=length(x);

%Make sure that it is a row vector.
x=reshape(x,1,numX);

y=zeros(numDim,numX);
for curDim=1:numDim
    y(curDim,:)=polyval(P(curDim,:),x);
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 deg = rad2deg(rad)

% RAD2DEG  Radians to degrees conversion.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

deg = rad/pi*180;

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


 function pgd_step_test
%function pgd_step_test
% test the PGD algorithm using 2D LS cost function

% cost function terms
kap = 4;
A = [1 0; 0 sqrt(kap)];
W = eye(2);
M = eye(2);
yy = 0;

f.niter = 10;
x = [-kap; 1];

% run PSD
xpsd = qpwls_psd(x, A, W, yy, 0, 'precon', 1, 'niter', f.niter, 'isave', 'all');

% run PGD
data = {yy, A, W};
f.step = 3.3; % best step is 0.4, so illustrate suboptimal step
[xpgd1 steps] = pgd_step(x, data, @costgrad, 'precon', M, ...
		'step_method', 'user', 'step', f.step, ...
		'niter', f.niter, 'isave', 'all', 'chat', 0);

%pr steps
[xpgd2 steps] = pgd_step(x, data, @costgrad, 'precon', M, ...
		'step_method', 'der2', ...
		'niter', f.niter, 'isave', 'all', 'chat', 0);
%pr steps

if im
	clf, subplot(211)
	plot(	xpsd(1,:), xpsd(2,:), 'y-o', ...
		xpgd1(1,:), xpgd1(2,:), 'b--+', ...
		xpgd2(1,:), xpgd2(2,:), 'm:.')
	xlabel x1, ylabel x2
	title 'QPWLS example'
	leg = {'PSD', sprintf('PGD:%g', f.step), 'PGD:Newton'};
	legend(leg{:}, 3)
end

% cost function map
if ~isvar('qq')
	x1 = linspace(-4.5,0.5,41)';
	x2 = linspace(-1,1.5,43)';
	[xx1 xx2] = ndgrid(x1,x2);
	qq = 0 * xx1;
	for i1=1:length(x1)
		for i2=1:length(x2)
			x = [x1(i1) x2(i2)]';
			qq(i1,i2) = norm(sqrtm(W) * (yy - A * x)).^2;
		end
	end
end

if im
	hold on
	contour(x1, x2, qq', 8)
	plot(0,0, 'rx')
	hold off
	axis equal
	axis([-4.5 0.5 -1 1.5])
	xtick(-4:0)
	ytick(-1:1)
	colormap(0.3+0.7*gray)
end

if im
	subplot(212)
	plot(	0:f.niter, sum(abs(xpsd), 1), 'y-o', ...
		0:f.niter, sum(abs(xpgd1)), 'b--x', ...
		0:f.niter, sum(abs(xpgd2)), 'm:.')
	xlabel 'iteration', ylabel '||x||_1'
	legend(leg{:}, 1)
end

% cost function and gradient for basic WLS
function [cost, grad] = costgrad(x, data)
y = data{1};
A = data{2};
W = data{3};

p = A * x;
cost = 0.5 * norm(sqrtm(W) * (y - p)).^2;
if nargout > 1
	grad = -A' * (W * (y - p));
end


function varargout = wfilters(wname,o)
%WFILTERS Wavelet filters.
%   [LO_D,HI_D,LO_R,HI_R] = WFILTERS('wname') computes four
%   filters associated with the orthogonal or biorthogonal
%   wavelet named in the string 'wname'.
%   The four output filters are:
%       LO_D, the decomposition low-pass filter
%       HI_D, the decomposition high-pass filter
%       LO_R, the reconstruction low-pass filter
%       HI_R, the reconstruction high-pass filter
%   Available wavelet names 'wname' are:
%   Daubechies: 'db1' or 'haar', 'db2', ... ,'db45'
%   Coiflets  : 'coif1', ... ,  'coif5'
%   Symlets   : 'sym2' , ... ,  'sym8', ... ,'sym45'
%   Discrete Meyer wavelet: 'dmey'
%   Biorthogonal:
%       'bior1.1', 'bior1.3' , 'bior1.5'
%       'bior2.2', 'bior2.4' , 'bior2.6', 'bior2.8'
%       'bior3.1', 'bior3.3' , 'bior3.5', 'bior3.7'
%       'bior3.9', 'bior4.4' , 'bior5.5', 'bior6.8'.
%   Reverse Biorthogonal:
%       'rbio1.1', 'rbio1.3' , 'rbio1.5'
%       'rbio2.2', 'rbio2.4' , 'rbio2.6', 'rbio2.8'
%       'rbio3.1', 'rbio3.3' , 'rbio3.5', 'rbio3.7'
%       'rbio3.9', 'rbio4.4' , 'rbio5.5', 'rbio6.8'.
%
%   [F1,F2] = WFILTERS('wname','type') returns the following
%   filters:
%   LO_D and HI_D if 'type' = 'd' (Decomposition filters)
%   LO_R and HI_R if 'type' = 'r' (Reconstruction filters)
%   LO_D and LO_R if 'type' = 'l' (Low-pass filters)
%   HI_D and HI_R if 'type' = 'h' (High-pass filters)
%
%   See also BIORFILT, ORTHFILT, WAVEINFO.

%   M. Misiti, Y. Misiti, G. Oppenheim, J.M. Poggi 12-Mar-96.
%   Last Revision: 05-Jul-1999.
%   Copyright 1995-2001 The MathWorks, Inc.
% $Revision: 1.10 $

% Check arguments.
if errargn(mfilename,nargin,[1 2],nargout,[0 1 2 4 8]), error('*'); end
if errargt(mfilename,wname,'str') , error('*'), end

wname         = deblankl(wname);
[wtype,fname] = wavemngr('fields',wname,'type','file');
mat_f         = findstr('.mat',fname);
if mat_f
   try
     load(fname,'-mat');
   catch
     msg = ['invalid wavelet file : ' fname];
     errargt(mfilename,msg,'msg');
     error('*');
   end
end

if wtype==1                % orth. wavelet
    if ~isempty(mat_f)
        F = eval(wname);
    else
        F = feval(fname,wname);
    end
    [Lo_D,Hi_D,Lo_R,Hi_R] = orthfilt(F);

elseif wtype==2            % biorth. wavelet
    if isempty(mat_f)
        [Rf,Df] = feval(fname,wname);
    else
        if exist('Rf')~=1 | exist('Df')~=1
            msg = ['invalid biorthogonal wavelet file : ' fname];
            errargt(mfilename,msg,'msg');
            error('*');
        end
    end
    [Lo_D,Hi_D1,Lo_R1,Hi_R,Lo_D2,Hi_D,Lo_R,Hi_R2] = biorfilt(Df,Rf,1);
    if (nargout>4) & (nargin<2)
        varargout(5:8) = {Lo_D2,Hi_D1,Lo_R1,Hi_R2};
    end

else
    msg = ['The wavelet ' wname ' is not valid!'];
    errargt(mfilename,msg,'msg');
    error('*');
    return;
end

if nargin==1
    varargout(1:4) = {Lo_D,Hi_D,Lo_R,Hi_R};
else
    o = lower(o(1));
    switch o
        case 'd' , varargout = {Lo_D,Hi_D};
        case 'r' , varargout = {Lo_R,Hi_R};
        case 'l' , varargout = {Lo_D,Lo_R};
        case 'h' , varargout = {Hi_D,Hi_R};
        otherwise
            errargt(mfilename,'invalid argument value','msg');
            error('*');
    end
end


function value = bst_prctile(vector, percentile)
% BST_PRCTILE: Returns the percentile value in vector
%
% USAGE: value = bst_prctile(vector, percentile)

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c)2000-2020 University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Martin Cousineau, 2020

% Try to use toolbox function
try
    value = prctile(vector, percentile);
    return;
catch
end

if ~isvector(vector)
    error('Only vectors supported.');
end

% Custom implementation
vector = sort(vector);
rank   = percentile / 100 * (length(vector) + 1);
lowerRank = floor(rank);
upperRank = ceil(rank);
fraction  = rank - lowerRank;

if fraction == 0
    value = vector(rank);
else
    value = fraction * (vector(upperRank) - vector(lowerRank)) + vector(lowerRank);
end


function mean = f_mean ( m, n )

%*****************************************************************************80
%
%% F_MEAN returns the mean of the F central PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the parameters of the PDF.
%    1 <= M,
%    1 <= N.
%    Note, however, that the mean is not defined unless 3 <= N.
%
%    Output, real MEAN, the mean of the PDF.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_MEAN - Fatal error!\n' );
    fprintf ( 1, '  The mean is not defined for N < 3.\n' );
    error ( 'F_MEAN - Fatal error!' );
  end

  mean = n / ( n - 2 );

  return
end


function [L,filters] = gaussSmooth( I, sigmas, shape, radius )
% Applies Gaussian smoothing to a (multidimensional) image.
%
% Smooths the n-dimensional array I with a n-dimensional gaussian with
% standard deviations specified by sigmas.  This operation in linearly
% seperable and is implemented as such.
%
% USAGE
%  [L,filters] = gaussSmooth( I, sigmas, [shape], [radius] )
%
% INPUTS
%  I       - input image
%  sigmas  - either n dimensional or 1 dimensional vector of standard devs
%            if sigmas(n)<=.3 then does not smooth along that dimension
%  shape   - ['full'] shape flag 'valid', 'full', 'same', or 'smooth'
%  radius  - [2.25] radius in units of standard deviation
%
% OUTPUTS
%  L       - smoothed image
%  filters - actual filters used, cell array of length n
%
% EXAMPLE
%  load trees; I=ind2gray(X,map);
%  I2 = gaussSmooth( I, 1, 'same' );
%  figure(1); im(I); figure(2); im(I2);
%
% See also FILTERGAUSS
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2008 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

nd = ndims(I);  if(length(sigmas)==1); sigmas=repmat(sigmas,[1,nd]); end
if( nd > length(sigmas)); error('Incorrect # of simgas specified'); end
sigmas = sigmas(1:nd);

if( isa( I, 'uint8' ) ); I = double(I); end
if( nargin<3 || isempty(shape) ); shape='full'; end
if( nargin<4 || isempty(radius) ); radius=2.25; end

% create and apply 1D gaussian masks along each dimension
L = I;  filters = cell(1,nd);
for i=1:nd
  if (sigmas(i)>.3)
    r = ceil( sigmas(i)*radius );
    f = filterGauss( 2*r+1, [], sigmas(i)^2 );
    f = permute( f, circshift(1:nd,[1,i-1]) );
    filters{i} = f;
    L = convnFast( L, f, shape );
  else
    filters{i} = 1;
  end
end


% Written by Ali A. Eftekhari
% Last checked: June 2021
clc % clean the command prompt
L = 0.1; % 10 cm length
W = 0.01; % 1 cm thickness
Nx = 50; % number of cell in x direction
Ny = 20; % number of cells in the y direction
m = createMesh2D(Nx, Ny, L, W); % creates a 2D Cartesian grid
% m = createMesh3D(Nx, Ny,Ny, L, W,W); % creates a 3D Cartesian grid, activate this line if you are curious
T_inf = 25+273.15; % [K] ambient temperature
T_base = 100+273.15; % [K] temperature at the base of the fin
k_val = 237; % W/(m.K) thermal conductivity
h_val = 10; % W/(m^2.K) heat transfer coefficient
k = createCellVariable(m, k_val); % assign thermal cond. value to all cells
k_face = geometricMean(k); % geometric average of the thermal conductivity values on the cell faces
BC = createBC(m); % creates a BC structure for the domain m; all Neumann boundaries
BC.left.a(:)=0; BC.left.b(:)=1; BC.left.c(:)=T_base; % convert the left boundary to constant temperature
BC.right.a(:)=k_val/h_val; BC.right.b(:)=1; BC.right.c(:)=T_inf; % right boundary to Robin
BC.top.a(:)=k_val/h_val; BC.top.b(:)=1; BC.top.c(:)=T_inf; % top boundary to Robin
BC.bottom.a(:)=-k_val/h_val; BC.bottom.b(:)=1; BC.bottom.c(:)=T_inf; % bottom boundary to Robin (don't forget the normal vector sign)
M_cond = diffusionTerm(k_face); % matrix of coefficients for the heat diffusion
[M_bc, RHS_bc] = boundaryCondition(BC); % matrix of coefficients and RHS vector for the boundary conditions
T = solvePDE(m, M_cond+M_bc, RHS_bc); % solve the linear system of discretized PDE
visualizeCells(T); % visualize the results