function WHVLoss = CalWHVLoss(PopObj,FrontNo,wz,AA,RA)
% Calculate the weighted hypervolume (WHV) loss of each solution

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Calculate the weight of each solution for WHV calculation
    Weight = ones(1,size(PopObj,1));
    if nargin > 2 && ~isempty(wz)
        for i = 1 : size(PopObj,1)
            if any(all(repmat(PopObj(i,:),size(AA,1),1)<=AA,2))
                % The solution belongs to the preferred region (Pr)
                Weight(i) = wz(3);
            elseif any(all(repmat(PopObj(i,:),size(RA,1),1)>RA,2))
                % The solution belongs to the dominated region (Do)
                Weight(i) = wz(1);
            else
                % The solution belongs to the no preference information
                % region (In)
                Weight(i) = wz(2);
            end
        end
    end

    %% Calculate the WHV loss of each solution front by front
    WHVLoss  = zeros(1,size(PopObj,1));
    RefPoint = max(PopObj,[],1) + 0.1;
    for f = setdiff(unique(FrontNo),inf)
        current  = find(FrontNo==f);
        totalWHV = CalWHV(PopObj(current,:),RefPoint,Weight(current));
        for i = 1 : length(current)
            drawnow('limitrate');
            currenti           = current([1:i-1,i+1:end]);
            WHVLoss(current(i))= totalWHV - CalWHV(PopObj(currenti,:),RefPoint,Weight(currenti));
        end
    end
end


function y=binocdf(x,n,p);

% BINOCDF binomial cumulative distribution function
%
% Y=BINOCDF(X,N,P) returns the binomial cumulative distribution
% function with parameters N and P at the values in X.
%
% See also BINOPDF and STATS (Matlab statistics toolbox)

% compute the cumulative probability for all values up to the maximum
c = cumsum(binopdf(0:max(x(:)),n,p));
y = c(x+1);

% fix rounding errors
y(y<0) = 0;
y(y>1) = 1;


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% SABR density script using different parameters

% base scenario
clear; clc;
t=0; T=1;
f = 0.03;
alpha = 0.075;
beta = 0.5;
nu = 0.1;
rho = 0;

sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);

lcoeff = 1; ucoeff = 1;
lowerbound = f-lcoeff*f;
upperbound = f + ucoeff*f;

xvals = lowerbound:.0005:upperbound;

zvals1d = density1d(xvals,sabrdensity,0,1);

% change rho
rho = -.9;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
rho = .9;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \rho')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\rho=-0.9','\rho=0','\rho=0.9')
hold off;
rho = 0.0;

% change nu
nu = 0.05;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
nu = .15;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \nu')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\nu=0.05','\nu=0.4','\nu=0.2')
hold off;
nu = 0.1;

% change beta
beta = 0.4;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
beta = 0.8;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \beta')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\beta=0.4','\beta=0.5','\beta=0.6')
hold off;
beta = 0.5;

% change alpha
alpha = 0.05;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
alpha = .1;
sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \alpha')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\alpha=0.05','\alpha=0.075','\alpha=0.1')
hold off;
alpha = 0.075;


function bsv_test03 ( )

%*****************************************************************************80
%
%% BSV_TEST03 varies the left boundary value ALPHA in the Burgers equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BSV_TEST03:\n' );
  fprintf ( 1, '  Solution of steady viscous Burgers equation.\n' );
  fprintf ( 1, '  Vary the left boundary condition ALPHA around the value +1.\n' );

  a = -1.0;
  b = +1.0;
  alpha_test = [ 0.96, 0.98, 0.99, 0.995, 1.0, 1.005, 1.010, 1.02, 1.04 ];
  beta = -1.0;
  nu = 0.1;
  test_num = length ( alpha_test );
  n = 161;
  output = 0;

  u = zeros(n,test_num);

  fprintf ( 1, '\n' );

  for test = 1 : test_num

    alpha = alpha_test(test);

    fprintf ( 1, '  Using ALPHA = %g\n', alpha );

    u(:,test) = bsv ( a, b, alpha, beta, nu, n, output );

  end

  x = linspace ( a, b, n );

  figure ( 3 )
  clf
  hold on
  plot ( x, u, 'b-', 'LineWidth', 3 );
  plot ( [a,b], [0.0,0.0], 'r-', 'LineWidth', 2 )
  grid on
  title ( 'ALPHA = 0.96, 0.98, 0.99, 0.995, 1, 1.005, 1.01, 1.02, 1.04' )
  xlabel ( '<--- X --->' )
  ylabel ( '<---U(X) --->' )
  axis ( [ a, b, -1.5, alpha ] )
  hold off
  filename = 'bsv_test03.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot to file "%s".\n', filename );

  return
end


function M = symmetricfactory(n, k)
% Returns a manifold struct to optimize over k symmetric matrices of size n
%
% function M = symmetricfactory(n)
% function M = symmetricfactory(n, k)
%
% Returns M, a structure describing the Euclidean space of n-by-n symmetric
% matrices equipped with the standard Frobenius distance and associated
% trace inner product, as a manifold for Manopt.
% By default, k = 1. If k > 1, points and vectors are stored in 3D matrices
% X of size nxnxk such that each slice X(:, :, i), for i = 1:k, is
% symmetric.

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Jan. 22, 2014.
% Contributors:
% Change log:

    if ~exist('k', 'var') || isempty(k)
        k = 1;
    end

    M.name = @() sprintf('(Symmetric matrices of size %d)^%d', n, k);

    M.dim = @() k*n*(n+1)/2;

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d(:), 'fro');

    M.dist = @(x, y) norm(x(:)-y(:), 'fro');

    M.typicaldist = @() sqrt(k)*n;

    M.proj = @(x, d) multisym(d);

    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @(x, eg, eh, d) M.proj(x, eh);

    M.tangent = @(x, d) d;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

	M.log = @(x, y) y-x;

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

    M.rand = @() multisym(randn(n, n, k));

    M.randvec = @randvec;
    function u = randvec(x) %#ok<INUSD>
        u = multisym(randn(n, n, k));
        u = u / norm(u(:), 'fro');
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(n, n, k);

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

    M.pairmean = @(x1, x2) .5*(x1+x2);

    % Elaborate list of indices of diagonal entries of an nxnxk matrix.
    single_diag_entries = (1:(n+1):n^2)';
    all_diag_entries = bsxfun(@plus, single_diag_entries, n^2*(0:(k-1)));
    all_diag_entries = all_diag_entries(:);

    % Likewise, elaborate list of indices of upper-triangular entries.
    single_upper_triangle = find(triu(ones(n), 1));
    all_upper_triangle = bsxfun(@plus, single_upper_triangle, n^2*(0:(k-1)));
    all_upper_triangle = all_upper_triangle(:);

    % To vectorize a matrix, we extract all diagonal entries, then all
    % upper-triangular entries, the latter being scaled by sqrt(2) to
    % ensure isometry, that is: given two tangent vectors U and V at a
    % point X, M.inner(X, U, V) is equal to u'*v, where u = M.vec(X, U) and
    % likewise for v. This construction has the advantage of providing a
    % vectorized representation of matrices that has the same length as the
    % intrinsic dimension of the space they live in.
    M.vec = @(x, u_mat) [u_mat(all_diag_entries) ; ...
                         sqrt(2)*u_mat(all_upper_triangle)];
    M.mat = @matricize;
    function u_mat = matricize(X, u_vec) %#ok<INUSL>
        u_mat = zeros(n, n, k);
        u_mat(all_upper_triangle) = u_vec((k*n+1):end) / sqrt(2);
        u_mat = u_mat + multitransp(u_mat);
        u_mat(all_diag_entries) = u_vec(1:(k*n));
    end
    M.vecmatareisometries = @() true;

end

% Former, easier versions for vec / mat. They had the disadvantage of
% giving vector representations of length k*n^2, instead of k*n*(n+1).
% M.vec = @(x, u_mat) u_mat(:);
% M.mat = @(x, u_vec) reshape(u_vec, [m, n]);
% M.vecmatareisometries = @() true;


function y=udb10(x);

% y=udb10(x);
%
% Convert from db values to linear power values.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

y=10.^(x/10);


function point_num = sparse_grid_cc_size ( dim_num, level_max )

%*****************************************************************************80
%
%% SPARSE_GRID_CC_SIZE sizes a sparse grid using Clenshaw Curtis rules.
%
%  Discussion:
%
%    The grid is defined as the sum of the product rules whose LEVEL
%    satisfies:
%
%      0 <= LEVEL <= LEVEL_MAX.
%
%    This calculation is much faster than a previous method.  It simply
%    computes the number of new points that are added at each level in the
%    1D rule, and then counts the new points at a given DIM_NUM dimensional
%    level vector as the product of the new points added in each dimension.
%
%    This approach will work for nested families, and may be extensible
%    to other families, and to mixed rules.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 December 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Output, integer POINT_NUM, the total number of unique
%    points in the grids.
%

%
%  Special case.
%
  if ( level_max < 0 )
    point_num = 0;
    return
  end

  if ( level_max == 0 )
    point_num = 1;
    return
  end
%
%  Construct the vector that counts the new points in the 1D rule.
%
  new_1d = zeros ( level_max+1, 1 );

  new_1d(0+1) = 1;
  new_1d(1+1) = 2;

  j = 1;
  for l = 2 : level_max
    j = j * 2;
    new_1d(l+1) = j;
  end
%
%  Count the number of points by counting the number of new points
%  associated with each level vector.
%
  level_1d = zeros ( dim_num, 1 );

  point_num = 0;

  for level = 0 : level_max

    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );

      point_num = point_num + prod ( new_1d(level_1d(1:dim_num)+1) );

      if ( ~more )
        break
      end

    end

  end

  return
end


function [u,s,U_s,U_pk,U_pd,U_idp]  = projIdpPntIntoPinHole(Sf, Spk, Spd, idp)

% PROJIDPPNTINTOPINHOLE Project Idp pnt into pinhole.
%    [U,S] = PROJIDPPNTINTOPINHOLE(RF, SF, SPK, SPD, L) projects 3D Inverse
%    Depth points into a pin-hole camera, providing also the non-measurable
%    depth. The input parameters are:
%       SF : pin-hole sensor frame
%       SPK: pin-hole intrinsic parameters [u0 v0 au av]'
%       SPD: radial distortion parameters [K2 K4 K6 ...]'
%       L  : 3D inverse depth point [x y z pitch yaw rho]'
%    The output parameters are:
%       U  : 2D pixel [u v]'
%       S  : non-measurable depth
%
%    The function accepts an idp points matrix L = [L1 ... Ln] as input.
%    In this case, it returns a pixels matrix U = [U1 ... Un] and a depths
%    row-vector S = [S1 ... Sn].
%
%    [U,S,U_S,U_K,U_D,U_L] = ... gives also the jacobians of the
%    observation U wrt all input parameters. Note that this only works for
%    single points.
%
%    See also PINHOLE, PY2VEC, PROJIDPPNTINTOPINHOLEONROB.

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

p0 = idp(1:3,:);   % origin
py = idp(4:5,:);   % pitch and roll
r  = idp(6,:);     % inverse depth

t = Sf.t; % frame things
q = Sf.q;

if nargout <= 2  % No Jacobians requested

    m = py2vec(py);    % vector from anchor
    v = m - (t-p0).*r;  % vector from sensor
    w = Rtp(q,v);
    u = pinHole(w, Spk, Spd); % pixel

    e = w./r;  % euclidean
    s = e(3,:);

else            % Jacobians requested

    if size(idp,2) == 1

        % function calls
        [m,M_py] = py2vec(py);    % vector from anchor
        v        = m - (t-p0)*r;  % vector from sensor
        V_m      = 1;
        V_p0     = r;  %  r*eye(3);
        V_t      = -r; % -r*eye(3);
        V_r      = p0-t;
        [w, W_q, W_v] = Rtp(q,v);
        [u, ~, U_w, U_pk, U_pd] = pinHole(w, Spk, Spd); % pixel

        e = w/r;   % euclidean
        s = e(3);  % depth of point

        % chain rule
        U_v  = U_w*W_v;
        U_p0 = U_v*V_p0;
        U_py = U_v*V_m*M_py;
        U_r  = U_v*V_r;

        U_t  = U_v*V_t;
        U_q  = U_w*W_q;

        U_idp = [U_p0 U_py U_r];
        U_s   = [U_t U_q];

    else
        error('??? Jacobians not available for multiple IDP points.')

    end

end

return

%% jac
syms x y z a b c d real
syms au av u0 v0 real
syms d1 d2 d3 real
syms x0 y0 z0 p y r real

Sf.x = [x;y;z;a;b;c;d];
Sf   = updateFrame(Sf);
Spk  = [u0;v0;au;av];
Spd  = [d1;d2;d3];
idp  = [x0;y0;z0;p;y;r];

[u,s,U_s,U_pk,U_pd,U_idp]  = projIdpPntIntoPinHole(Sf, Spk, Spd, idp);
u,s  = projIdpPntIntoPinHole(Sf, Spk, Spd, idp);

simplify(U_pk  - jacobian(u,Spk))
simplify(U_pd  - jacobian(u,Spd))
% simplify(U_idp - jacobian(u,idp))

% ========== 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 demmet1(plot_wait)
%DEMMET1 Demonstrate Markov Chain Monte Carlo sampling on a Gaussian.
%
%	Description
%	The problem consists of generating data from a Gaussian in two
%	dimensions using a Markov Chain Monte Carlo algorithm. The points are
%	plotted one after another to show the path taken by the chain.
%
%	DEMMET1(PLOTWAIT) allows the user to set the time (in a whole number
%	of seconds) between the plotting of points.  This is passed to PAUSE
%
%	See also
%	DEMHMC1, METROP, GMM, DEMPOT
%

%	Copyright (c) Ian T Nabney (1996-2001)

if nargin == 0 | plot_wait < 0
  plot_wait = 0; % No wait if not specified or incorrect
end
dim = 2;            	% Data dimension
ncentres = 1;		% Number of centres in mixture model

seed = 42;              % Seed for random weight initialization.
randn('state', seed);
rand('state', seed);

clc
disp('This demonstration illustrates the use of the Markov chain Monte Carlo')
disp('algorithm to sample from a Gaussian distribution.')
disp('The mean is at [0 0].')
disp(' ')
disp('First we set up the parameters of the mixture model we are sampling')
disp('from.')
disp(' ')
disp('Press any key to continue.')
pause

% Set up mixture model to sample from
mix = gmm(dim, ncentres, 'spherical');
mix.centres(1, :) = [0 0];
x = [0 4];  % Start vector

% Set up vector of options for hybrid Monte Carlo.

nsamples = 150;		% Number of retained samples.

options = foptions;     % Default options vector.
options(1) = 0;		% Switch off diagnostics.
options(14) = nsamples;	% Number of Monte Carlo samples returned.
options(18) = 0.1;

clc
disp('Next we take 150 samples from the distribution.')
disp('Sampling starts at the point [0 4].')
disp('The new state is accepted if the threshold value is greater than')
disp('a random number between 0 and 1.')
disp(' ')
disp('Press any key to continue.')
pause

[samples, energies] = metrop('dempot', x, options, '', mix);

clc
disp('The plot shows the samples generated by the MCMC function in order')
disp('as an animation to show the path taken by the Markov chain.')
disp('The different colours are used to show that the first few samples')
disp('should be discarded as they lie too far from the mean.')
disp(' ')
disp('Press any key to continue.')
pause
probs = exp(-energies);
fh1 = figure;
g1end = floor(nsamples/4);

for n = 1:nsamples

  if n < g1end
    Marker = 'k.';
    p1 = plot(samples(n,1), samples(n,2), Marker, ...
      'EraseMode', 'none', 'MarkerSize', 12);
    if n == 1
      axis([-3 5 -2 5])
    end
  else
    Marker = 'r.';
    p2 = plot(samples(n,1), samples(n,2), Marker, ...
      'EraseMode', 'none', 'MarkerSize', 12);
  end
  hold on
  drawnow;  % Force drawing immediately
  pause(plot_wait);
end
lstrings = char(['Samples 1-' int2str(g1end)], ...
  ['Samples ' int2str(g1end+1) '-' int2str(nsamples)]);
legend([p1 p2], lstrings, 1);

disp(' ')
disp('Press any key to exit.')
pause
close(fh1);
clear all;


function [s,f] = ltas(x,fs,varargin)
%LTAS calculate the long-term average spectrum of a signal
%
%   S = IOSR.DSP.LTAS(X,FS) calculates the long-term average spectrum
%   (LTAS) of signal X, sampled at FS Hz. The spectrum is calculated from
%   the average power spectral density (PSD) obtained from a series of
%   overlapping FFTs; the FFT length is 4096, and the hop size is 2048. The
%   segments of X are Hann-windowed. The average PSD is then
%   Gaussian-smoothed to 1/3-octave resolution.
%
%   X can be a vector, matrix, or multidimensional array; LTAS will operate
%   along the first non-signleton dimension, and return the LTAS for each
%   corresponding row/column/etc.
%
%   S = IOSR.DSP.LTAS(X,FS,'PARAMETER','VALUE') allows numerous parameters
%   to be specified. These parameters are:-
%       'dim'     : {find(size(X)>1,1,'first')} | scalar
%           Specifies the dimension of operation (defaults to the first
%           non-singleton dimension).
%       'graph'   : {false} | true
%           Choose whether to plot a graph of the LTAS.
%       'hop'     : {NFFT/2} | scalar
%           Specifies the step size through X used to calculate each
%           segment. NFFT is determined by the 'win' parameter.
%       'noct'    : {3} | scalar
%           Apply 1/noct-octave smoothing to the frequency spectrum.
%           Setting 'noct' to 0 results in no smoothing.
%       'scaling' : {'none'} | 'max0'
%           Specifies any scaling to apply to S. By default, no scaling is
%           applied. If scaling is set to 'max0', S will be scaled to have
%           a maximum value of 0dB.
%       'units'   : {dB} | 'none'
%           Specifies the output units. By default the PSD is calculated in
%           dB. Otherwise the PSD is returned directly.
%       'win'     : {4096} | scalar | vector
%           Specifies the window or FFT length NFFT used to calculate the
%           spectrum. If 'win' is a scalar, it specifies the FFT length,
%           and a Hann window is applied to each segment. If 'win' is a
%           vector, NFFT is the length of the vector, and the vector is
%           multiplied with each segment.
%
%   [S,F] = IOSR.DSP.LTAS(...) returns the frequencies F for each
%   corresponding bin of S.
%
%   Example
%
%       % Plot the 1/6th-octave-smoothed LTAS of the Handel example
%       load handel.mat
%       figure
%       iosr.dsp.ltas(y,Fs,'noct',6,'graph',true);
%
%   See also IOSR.DSP.STFT, IOSR.DSP.SMOOTHSPECTRUM.

%   Copyright 2016 University of Surrey.

    %% parse input

    if nargin < 2
        error('iosr:ltas:nargin''Not enough input arguments')
    end

    options = struct(...
        'win',4096,...
        'hop',[],...
        'noct',3,...
        'dim',[],...
        'units','db',...
        'scaling','none',...
        'graph',false);

    % read parameter/value inputs
    if nargin>2 % if parameters are specified
        % read the acceptable names
        optionNames = fieldnames(options);
        % count arguments
        nArgs = length(varargin);
        if round(nArgs/2)~=nArgs/2
           error('iosr:ltas:nameValuePair','LTAS needs propertyName/propertyValue pairs')
        end
        % overwrite defults
        for pair = reshape(varargin,2,[]) % pair is {propName;propValue}
           IX = strcmpi(pair{1},optionNames); % find match parameter names
           if any(IX)
              % do the overwrite
              options.(optionNames{IX}) = pair{2};
           else
              error('iosr:ltas:unknownOption','%s is not a recognized parameter name',pair{1})
           end
        end
    end

    %% check and assign input

    % required inputs
    assert(isnumeric(x), 'iosr:ltas:invalidX', 'X must be numeric.');
    assert(isscalar(fs), 'iosr:ltas:invalidFs', 'FS must be a scalar.');
    assert(fs>0, 'iosr:ltas:invalidFs', 'FS must be greater than 0.');
    assert(isint(fs), 'iosr:ltas:invalidFs', 'FS must be an integer.');

    % determine fft parameters
    if numel(options.win)>1 && isvector(options.win)
        NFFT = length(options.win);
        win = options.win;
    elseif isscalar(options.win)
        NFFT = options.win;
        win = NFFT;
    else
        error('iosr:ltas:invalidWin','''WIN'' must be a vector or a scalar');
    end
    assert(isint(NFFT) && NFFT>0, 'iosr:ltas:invalidNfft', '''WIN'' must be a positive integer');

    % determine hop
    hop = options.hop;
    if isempty(hop)
        hop = fix(NFFT/2);
    end

    % smoothing
    Noct = options.noct;

    % dimension to operate along
    dim = options.dim;
    dims = size(x);
    if isempty(dim)
        dim = find(dims>1,1,'first');
    else
        assert(isnumeric(dim),'iosr:ltas:invalidDim', 'DIM must be an integer');
        assert(isint(dim), 'iosr:ltas:invalidDim', 'DIM must be an integer or empty');
        assert(dim>0, 'iosr:ltas:invalidDim', 'DIM must be greater than 0')
    end

    %% permute and rehape x to operate down columns

    % number of useful coefficients
    if mod(NFFT,2)==0
        Nout = (NFFT/2)+1;
    else
        Nout = (NFFT+1)/2;
    end

    % reorder and permute
    order = mod(dim-1:dim+length(dims)-2,length(dims))+1;
    dims_shift = dims(order);
    x = rearrange(x,order,[dims_shift(1) prod(dims_shift(2:end))]);
    dims_out_shift = dims_shift;
    dims_out_shift(1) = Nout;

    %% calculate spectra

    % choose units function
    switch lower(options.units)
        case 'db'
            units = @(x) 10*log10(x);
            max0scale = @(s) s-max(s(:));
            labelY = 'Power spectral density [dBFS]';
            yscale = 'linear';
        case 'none'
            units = @(x) x;
            max0scale = @(s) s./max(s(:));
            labelY = 'Power spectral density';
            yscale = 'log';
        otherwise
            error('iosr:ltas:unknownUnits','Unknown units option ''%s''',options.units);
    end

    % do calculations
    s = zeros(dims_out_shift);
    for c = 1:size(x,2)
        [S,f] = iosr.dsp.stft(x(:,c),win,hop,fs); % short-time ft
        s(:,c) = mean(abs(S/NFFT).^2,2); % mean PSD
        s(:,c) = units(s(:,c)); % put into dB
        s(:,c) = iosr.dsp.smoothSpectrum(s(:,c),f,Noct); % smooth
    end

    % invert permutation
    s = irearrange(s,order,dims_out_shift);

    % scale output
    switch lower(options.scaling)
        case 'max0'
            s = max0scale(s);
        case 'none'
            % do nothing
        otherwise
            error('iosr:matchEQ:unknownScaling','Unknown scaling option ''%s''',options.scaling);
    end

    %% plot

    assert(islogical(options.graph) && numel(options.graph)==1, 'iosr:ltas:invalidGraph', '''graph'' option must be logical.')
    if options.graph
        semilogx(f,rearrange(s,order,[dims_out_shift(1) prod(dims_out_shift(2:end))]));
        xlabel('Frequency [Hz]');
        ylabel(labelY);
        set(gca,'yscale',yscale);
        grid on
        if prod(dims_out_shift(2:end)) > 1
            legend(num2str((1:prod(dims_out_shift(2:end)))'));
        end
    end

end

function y = rearrange(x,order,shape)
%REARRANGE reshape and permute to make target dim column
    y = permute(x,order);
    y = reshape(y,shape);
end

function y = irearrange(x,order,shape)
%IREARRANGE reshape and permute to original size
    y = reshape(x,shape);
    y = ipermute(y,order);
end

function y = isint(x)
%ISINT check if input is whole number
    y = x==round(x);
end


%CentralCamera.flowfield Optical flow
%
% C.flowfield(V) displays the optical flow pattern for a sparse grid
% of points when the camera has a spatial velocity V (6x1).
%
% See also QUIVER.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function flowfield(cam, vel)
    a = 50:100:1000;
    [U,V] = meshgrid(a, a);
    du=[]; dv=[];
    for i=1:numcols(U)
        for j=1:numrows(U)
            pdot = cam.visjac_p( [U(i,j); V(i,j)], 2 ) * vel(:);
            du(i,j) = pdot(1); dv(i,j) = pdot(2);
        end
    end

    quiver(U, V, du, dv, 0.4)
    axis equal
        axis([1 cam.npix(1) 1 cam.npix(2)]);

    set(gca, 'Ydir', 'reverse');
    xlabel('u (pixels)');
    ylabel('v (pixels)');
    grid


function result=libra_mahalanobis(x,locvct,varargin)

%MAHALANOBIS computes the distance of each observation in x
%   from the location estimate (locvct) of the data,
%   relative to the shape of the data.
%
% Required input arguments:
%                  x : data matrix (n observations in rows, p variables in columns)
%             locvct : location estimate of the data (p-dimensional vector)
%      cov or invcov : scatter estimate of the data or the inverse of the scatter estimate (pxp matrix)
%
% I/O: result=libra_mahalanobis(x,locvct,'cov',covmat,'n',size(x,1),'p',size(x,2))
%   The user should only give the input arguments that have to change their default value.
%   The name of the input arguments needs to be followed by their value.
%   The order of the input arguments is of no importance.
%
% Examples:
%   result=libra_mahalanobis(x,loc,'cov',covx,'n',10)
%   result=libra_mahalanobis(x,loc,'p',2,'invcov',invcovx)
%   result=libra_mahalanobis(x,loc,'invcov',invcovx)
%
% Output:
%   A vector containing the distances of all the observations to locvct.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Katrien Van Driessen
% Revisions by Sabine Verboven
% Last update on 18/09/2003
%

%Initialisation
n = size(x,1);
p = size(x,2);
if nargin<3
    error('Missing a required input variable')
end
counter=1;
default=struct('cov',0,'invcov',NaN);
list=fieldnames(default);
options=default;
IN=length(list);
i=1;
%reading the user's input
if nargin>2
    %
    %placing inputfields in array of strings
    %
    for j=1:nargin-2
        if rem(j,2)~=0
            chklist{i}=varargin{j};
            i=i+1;
        end
    end
    %
    %Checking which default parameters have to be changed
    % and keep them in the structure 'options'.
    %
    while counter<=IN
        index=strmatch(list(counter,:),chklist,'exact');
        if ~isempty(index) %in case of similarity
            for j=1:nargin-2 %searching the index of the accompanying field
                if rem(j,2)~=0 %fieldnames are placed on odd index
                    if strcmp(chklist{index},varargin{j})
                        I=j;
                    end
                end
            end
            options=setfield(options,chklist{index},varargin{I+1});
            index=[];
        end
        counter=counter+1;
    end
end
if size(locvct,2)==1
    locvct=locvct'; %converting to a rowvector
end
if options.cov==0 & options.invcov==0
    error('The scatter matrix or its inverse is a required input argument.')
end
%%%%%%MAIN%%%%%%%%%
if ~isnan(options.invcov)
    covmat=options.invcov;
    if min(size(covmat))==1
        covmat=diag(covmat);
    end
else
    if min(size(options.cov))==1
        options.cov=diag(options.cov);
    end
    covmat=pinv(options.cov);
end
hlp=x-repmat(locvct,n,1);
dist=sum(hlp*covmat.*hlp,2)';
result=dist;


function state = dss_core_mim(state)
% MIM DSS algorithm
%   state = dss_core_mim(state)
%     Calculates linear denoising source separation with MIM.
%     Performs calculation defined by state structure and
%     returns the result as state structure.
%     Used CPU time is recorded in the state.

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id: dss_core_pca.m,v 1.13 2005/12/02 12:23:18 jaakkos Exp $

dss_message(state,2,'Extracting components in PCA DSS\n');

% -- Initialize local variables
start_time = cputime;
interrupt_iteration = 0;
interrupt_component = 0;
% Dimension of the whitened data
if iscell(state.Y)
  wdim = size(state.Y{1},1);
else
  wdim = size(state.Y, 1);
end
sdim = state.sdim;

% -- Denoising
[state.denf.params, Y] = feval(state.denf.h, state.denf.params, state.Y, state);

% -- PCA
if iscell(Y)
  C = cellcov(Y,2,0);
  %[E,D] = eig(cellcov(Y,2,0));
else
  C = (Y*Y')/size(Y,2);
  %[E,D] = eig(Y * Y' / size(Y,2));
end

i1 = state.indx==1;
i2 = state.indx==2;

% create whitening matrix T
T = C;
T(i1, i2) = 0;
T(i2, i1) = 0;
T = inv(sqrtm(real(T)));

E = imag(T(i1, i1) * C(i1, i2) *T(i2, i2)');

[e1, d1] = eig(E*E');
[e2, d2] = eig(E'*E);

d1 = diag(d1);
d2 = diag(d2);
n  = min(sum(i1), sum(i2));

% sort eigenvalues in descending order
[d1,order] = sort(-d1);
d1 = -d1(1:n);
e1 = e1(:,order(1:n));

[d2,order] = sort(-d2);
d2 = -d2(1:n);
e2 = e2(:,order(1:n));

state.D = [d1;d2];
state.sdim = numel(state.D);
state.W = blkdiag(e1,e2)'/T;
state.S = state.W * state.Y;

% -- record the used cpu time
if ~isfield(state, 'cputime'); state.cputime = 0; end
state.cputime = state.cputime + cputime - start_time;


%Maps any angle to the equivalent between -pi and pi
function a=modAngle(a)
a=mod(a+pi,2*pi)-pi;


function fiber_torsion_vals=dtiFiberTorsion(fiber)

%Compute fiber torsion (at every segment long the fiber)
%For 3xn fiber as defined here
%http://en.wikipedia.org/wiki/Torsion_of_curves

%
%ER 12/2007
xprime=gradient(fiber(1, :));
yprime=gradient(fiber(2, :));
zprime=gradient(fiber(3, :));

xpp=gradient(xprime);
ypp=gradient(yprime);
zpp=gradient(zprime);

fiber_torsion_vals=(gradient(zpp).*(xprime.*ypp-yprime.*xpp)+zpp.*(gradient(xpp).*yprime-xprime.*gradient(ypp))+zprime.*(xpp.*gradient(ypp)-gradient(xpp).*ypp))./((xprime.^2+yprime.^2+zprime.^2).*(xpp.^2+ypp.^2+zpp.^2))


function [V,F] = schwarz_lantern(m,n)
  % SCHWARZ_LANTERN Construct a Schwarz lattern with m axial slices and n radial
  % vertices.
  %
  % Inputs:
  %   m  number of axial "slices"
  %   n  number of vertices around each slice
  % Outputs:
  %   V  mn by 3 list of vertex positions
  %   F  #F by 3 list of triangle indices into V
  %
  [X,Y,Z] = cylinder(ones(m,1),n);
  %[F,V] = surf2patch(X,Y,Z,'triangles');
  [Q,V] = surf2patch(X,Y,Z);
  R = axisangle2matrix([0 0 1],-2*pi/n*0.5);
  other = ((1:n+1)-1)*m+((2:2:m-mod(m,2)+1)');
  V(other,:) = V(other,:)*R;
  other = ((1:n)-1)*(m-1)+((1:2:m-1)');
  %tsurf(Q,V,'Tets',false,'FaceIndices',1,'CData',sparse(other,1,1,size(Q,1),1));
  %view(90,0);
  other = ismember(1:size(Q,1),other);
  F = [Q(~other,[1 2 3]);Q(~other,[1 3 4]);Q(other,[1 2 4]);Q(other,[4 2 3])];
  %tsurf(F,V);
  %view(90,0);
  % Heavy-handed way of removing duplicates...
  [V,~,IM] = remove_duplicate_vertices(V,eps);
  F = IM(F);
end


function VaR = garchkvar(data, model, ar, ma, p, q, max_forecast, alpha)
%{
-----------------------------------------------------------------------
 PURPOSE:
 Value-at-Risk Estimation for both long and short positions (Model Estimation)
-----------------------------------------------------------------------
 USAGE:
 VaR = garchvar(data, model, ar, ma, p, q, max_forecast, alpha)

 INPUTS:
 data:          (T x 1) vector of data
 model:         'GARCH', 'GJR', 'AGARCH','NAGARCH'
 ar:            positive scalar integer representing the order of AR
 am:            positive scalar integer representing the order of MA
 p:             positive scalar integer representing the order of ARCH
 q:             positive scalar integer representing the order of GARCH
 max_forecasts: maximum number of forecasts (i.e. 1-trading months 22 days)
 alpha:         confidence level

 OUTPUTS:
 VaR:           a vector of VaR forecasts
-----------------------------------------------------------------------
 Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date:     11/2011
-----------------------------------------------------------------------
%}

if nargin == 0
    error('Data, GARCH Model, AR, MA, ARCH, GARCH, Maximum Number of Forecasts, Confidence Level')
end

if size(data,2) > 1
   error('Data vector should be a column vector')
end

if (length(ar) > 1) | (length(ma) > 1) | ar < 0 | ma < 0
    error('AR and MA should be positive scalars')
end

if (length(p) > 1) | (length(q) > 1) | p < 0 | q < 0 | alpha < 0 | alpha > 1
    error('P,Q and alpha should be positive scalars')
end

% Estimate the model
[parameters, stderrors, LLF, ht, kt, nu, resids] = garchk(data, strcat(model), ar, ma, 0, p, q, 0);

% Calling garchkfor2 function to pass the parameters
[MF, VF, KF, MC, VC] = garchkfor2(data, resids, ht, kt, parameters, strcat(model),ar, ma, p, q, max_forecast);

% Estimating Inverce-CDF using the degrees of freedom which are described
% as a function of the forecasted conditional kurtosis
cdfqtile1 = tinv(alpha,(4*KF-6)./(KF-3));
cdfqtile2 = tinv(1-alpha,(4*KF-6)./(KF-3));

% Estimating VaR
VaR = [MC + cdfqtile1.*VC, MC + cdfqtile2.*VC];

end


function [results] = hyperHud(M, B, S)
% HYPERHUD Performs the hybrid unstructured detector (HUD) algorithm
%   Performs the hybrid unstructured detector algorithm for target
% detection.
%
% Usage
%   [results] = hyperHud(M, B, S)
% Inputs
%   M - 2d matrix of HSI data (p x N)
%   B - 2d matrix of background endmembers (p x q)
%   S - 2d matrix of target endmembers (p x #target_sigs)
% Outputs
%   results - vector of detector output (N x 1)
%
% References
%   J Broadwater & R Chellappa.  "Hybrid Detectors for Subpixel Targets."
% IEEE PAMI. Vol 29. No 11. November 2007.

[p, N] = size(M);
% Remove mean from data
u = mean(M.').';
M = M - repmat(u, 1, N);
S = S - repmat(u, 1, size(S,2));

numTargets = size(S,2);
%sigma = 1e-5;
E = [S B];
%E = [sigma*E; ones(1,size(E,2))];
q = size(E, 2);

R_hat = (M*M.')/N;
G = inv(R_hat);

results = zeros(1, N);

R = ones(q,1);
P = R - 1;
% TODO - put in the whitened version of fcls
a_hat_tmp = hyperNnls(M, E);
%a_hat_tmp = hyperFcls(M, E);
for k=1:N
    x = M(:,k);
    a_hat = a_hat_tmp(:,k);
    % Take the top r values from a_hat where r is number of targets.  We
    % are only interested in the abundances for the targets.  From J
    % Broadwater email 11/17/09.
    a_hat = a_hat(1:numTargets);
% %    x = [sigma*x; 1];
%     % FCLS optimzation
%     lambda = zeros(q,1);
%     aPrev = lambda;
%     for kk=1:100
%         a_hat = inv(E.'*G*E)*E.'*G*x - inv(E.'*G*E)*lambda;
%         norm(a_hat-aPrev)
%         lambda = E.'*G*(x-E*a_hat);
%         idx = find(a_hat>0);
%         P(idx) = 1;
%         R(idx) = 0;
%         aPrev = a_hat;
%     end
%     a_hat = a_hat(1:numTargets);
    results(k) = (x.'*G*S*a_hat) / (x.'*G*x);
end


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

if ( nargin == 0 )
    pref = chebfunpref;
end
tol = 1000*pref.cheb3Prefs.chebfun3eps;

f = chebfun3t(@(x,y,z) cos(x.*y.*z) + sin(x.*y.*z) + y -.1);

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

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

% Cosine
exact = @(x,y,z) cos(cos(x.*y.*z) + sin(x.*y.*z) + y -.1);
g = chebfun3t(@(x,y,z) exact(x,y,z));
pass(3) = norm(g - cos(f)) < tol;

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

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

% Tanh
exact = @(x,y,z) tanh(-(cos(x.*y.*z) + sin(x.*y.*z) + y -.1));
g = chebfun3t(@(x,y,z) exact(x,y,z));
pass(6) = norm(g - tanh(-f)) < tol;

% Exp
exact = @(x,y,z) exp(cos(x.*y.*z) + sin(x.*y.*z) + y -.1);
g = chebfun3t(@(x,y,z) exact(x,y,z));
pass(7) = norm(g - exp(f)) < tol;

% Multiple operations:
f = chebfun3t(@(x,y,z) sin(10*x.*y.*z), [-1 2 -1 1 -3 -1]);
pass(8) = norm(f+f+f-3*f) < 100*tol;

pass(9) = norm(f.*f-f.^2) < tol;

end


function eframes = vl_frame2oell(frames)
% VL_FRAMES2OELL   Convert a geometric frame to an oriented ellipse
%   EFRAME = VL_FRAME2OELL(FRAME) converts the generic FRAME to an
%   oriented ellipses EFRAME. FRAME and EFRAME can be matrices, with
%   one frame per column.
%
%   A frame is either a point, a disc, an oriented disc, an ellipse,
%   or an oriented ellipse. These are represented respectively by 2,
%   3, 4, 5 and 6 parameters each, as described in VL_PLOTFRAME().  An
%   oriented ellipse is the most general geometric frame; hence, there
%   is no loss of information in this conversion.
%
%   If FRAME is an oriented disc or ellipse, then the conversion is
%   immediate. If, however, FRAME is not oriented (it is either a
%   point or an unoriented disc or ellipse), then an orientation must
%   be assigned. The orientation is chosen in such a way that the
%   affine transformation that maps the standard oriented frame into
%   the output EFRAME does not rotate the Y axis. If frames represent
%   detected visual features, this convention corresponds to assume
%   that features are upright.
%
%   If FRAME is a point, then the output is an ellipse with null area.
%
%   See: feature frames,
%   VL_PLOTFRAME(), VL_HELP().

% Author: Andrea Vedaldi

% Copyright (C) 2013 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).

[D,K] = size(frames) ;
eframes = zeros(6,K) ;

switch D
  case 2
    eframes(1:2,:) = frames(1:2,:) ;

  case 3
    eframes(1:2,:) = frames(1:2,:) ;
    eframes(3,:)   = frames(3,:) ;
    eframes(6,:)   = frames(3,:) ;

  case 4
    r = frames(3,:) ;
    c = r.*cos(frames(4,:)) ;
    s = r.*sin(frames(4,:)) ;

    eframes(1:2,:) = frames(1:2,:) ;
    eframes(3:6,:) = [c ; s ; -s ; c] ;

  case 5
    eframes(1:2,:) = frames(1:2,:) ;
    eframes(3:6,:) = mapFromS(frames(3:5,:)) ;

  case 6
    eframes = frames ;

  otherwise
     error('FRAMES format is unknown.') ;
end

% --------------------------------------------------------------------
function A = mapFromS(S)
% --------------------------------------------------------------------
% Returns the (stacking of the) 2x2 matrix A that maps the unit circle
% into the ellipses satisfying the equation x' inv(S) x = 1. Here S
% is a stacked covariance matrix, with elements S11, S12 and S22.
%
% The goal is to find A such that AA' = S. In order to let the Y
% direction unaffected (upright feature), the assumption is taht
% A = [a b ; 0 c]. Hence
%
%  AA' = [a^2, ab ; ab, b^2+c^2] = S.

A = zeros(4,size(S,2)) ;
a = sqrt(S(1,:));
b = S(2,:) ./ max(a, 1e-18) ;

A(1,:) = a ;
A(2,:) = b ;
A(4,:) = sqrt(max(S(3,:) - b.*b, 0)) ;


function [ cmat, seed ] = c4mat_uniform_01 ( m, n, seed )

%*****************************************************************************80
%
%% C4MAT_UNIFORM_01 returns a unit pseudorandom C4MAT.
%
%  Discussion:
%
%    The angles should be uniformly distributed between 0 and 2 * PI,
%    the square roots of the radius uniformly distributed between 0 and 1.
%
%    This results in a uniform distribution of values in the unit circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the matrix.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, complex CMAT(M,N), the pseudorandom complex matrix.
%
%    Output, integer SEED, a seed for the random number generator.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'C4MAT_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'C4MAT_UNIFORM_01 - Fatal error!' );
  end

  for i2 = 1 : n
    for i1 = 1 : m

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      r = sqrt ( seed * 4.656612875E-10 );

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + i4_huge;
      end

      theta = 2.0 * pi * seed * 4.656612875E-10;

      cmat(i1,i2) = r * ( cos ( theta ) + i * sin ( theta ) );

    end

  end

  return
end


function PDIFF = chisquare_solve(XGUESS,P,V);
%CHISQUARE_SOLVE  Internal function used by CHISQUARE_INV
%
%   PDIFF=chisquare_solve(XGUESS,P,V)  Given XGUESS, a percentile P,
%   and degrees-of-freedom V, return the difference between
%   calculated percentile and P.

% Uses GAMMAINC
%
% Written January 1998 by C. Torrence

% extra factor of V is necessary because X is Normalized
	PGUESS = gammainc(V*XGUESS/2,V/2);  % incomplete Gamma function

	PDIFF = abs(PGUESS - P);            % error in calculated P

	TOL = 1E-4;
	if (PGUESS >= 1-TOL)  % if P is very close to 1 (i.e. a bad guess)
		PDIFF = XGUESS;   % then just assign some big number like XGUESS
	end


%FuNLMS
%Marko Stamenovic
%April 28, 2016

mus = [0.01 0.05 0.1 0.5 1 2];% 0.5 1];
for j = 1:length(mus)
    for i = 1:100
%% initialize values
% generate input signal
mu=0.13; %learning rate
M=128; %buffer size (num filter weights)
x=randn(10000,1); %input signal
x=x/max(x); %sample rate
fs=8000; %number of samples of the input signal
N=length(x); %length of input signal

% generate known filter coefficients
Pz=0.5*[0:127];  %linear coefficients
%Pz=randn(128,1); %random coefficients
ylim = max(Pz)*1.20;
ymin = min(Pz)-.2*max(Pz);
% generate filtered input signal == desired signal
d=conv(Pz,x); %input signal filtered by known filter Pz (primary path)

%% ESTIMATE SECONDARY PATH SIGNAL USING LMS %%
%generate dummy secondary path response Sz
Sz = Pz/2;
%run known signal through filter
yp=conv(Sz,x);
%initalize Sz hat values
Szh=zeros(M,1);

    for n=M:N
        ypvec=x(n:-1:n-M+1); %input has to be in reverse orxer has to be
        mu = 1/(ypvec'*ypvec);
        e(n)=yp(n)-Szh'*ypvec; %update error
        %plot(e)
        Szh=Szh+mu*ypvec*(e(n)); %update filter coefficient
    end
    Szh = abs(ifft(1./abs(fft(Szh))));

%% LMS FOR MAIN ANC %%
mu=2.6;
%initialize signal
y = zeros(N,1);
%filter input by learned filter to get x prime
xp = conv(Szh,x);
yp = conv(Szh,y);
%initialize delayed cancellation signal
ypvecLast = zeros(M,1);
yvecLast = zeros(M,1);
%initalize adaptive filter values
Az=zeros(M,1);
Bz=zeros(M,1);
%Make sure that x and d are column vectors
x=x(:);
d=d(:);
%LMS
    for n=M:N
        %flip buffer order
        yvec = y(n:-1:n-M+1);
        ypvec=yp(n:-1:n-M+1);
        xvec=x(n:-1:n-M+1)+.8*ypvec; %add feedback to input
        xpvec=xp(n:-1:n-M+1)+.8*ypvec; %add feedback to input
        %update mu
        mu = 1/(xvec'*xvec);
        %calculate output
        y(n) = Az'*xvec + Bz'*yvecLast;
        %update error
        e(n)=d(n)-y(n);
        %update adaptive filter weights
        Az=Az+mu*xpvec*(e(n));
        Bz=Bz+mu*ypvecLast*(e(n));
        %update delayed cancellation signal
        ypvecLast = ypvec;
        yvecLast = yvec;
        %draw the learned filter in realtime
%         plot(Pz)
%         hold on
%         plot(Az)
%         axis([0 inf ymin ylim])
%         title(sprintf('n=%f time=%fs error = %f mu = %f',n-M, (n-M)/fs, e(n),mu))
%         hold off
%         legend('Input coefficients','Learned Coefficients')
%         drawnow;
    end
    e=e(:);

%% PLOT RESULTS %%
figure
subplot(2,1,1)
plot(e)
title('Convergence Time in Cycles')
ylabel('Amplitude');
xlabel('Cycles');
legend('Error');
subplot(2,1,2)
stem(Pz)
hold on
stem(Az, 'r*')
title('Input Coefficients vs Learned Coefficients')
ylabel('Amplitude');
xlabel('Numbering of filter tap');
legend('Input Coefficients', 'learned coefficients')


function [M, Mx, My] = diffusionTermRadial2D(D)
% This function uses the central difference scheme to discretize a 2D
% diffusion term in the form \grad . (D \grad \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = D.domain.dims(1);
Ntheta = D.domain.dims(2);
G=reshape(1:(Nr+2)*(Ntheta+2), Nr+2, Ntheta+2);
DR = repmat(D.domain.cellsize.x, 1, Ntheta);
DTHETA = repmat(D.domain.cellsize.y', Nr, 1);
dr = 0.5*(DR(1:end-1,:)+DR(2:end,:));
dtheta = 0.5*(DTHETA(:,1:end-1)+DTHETA(:,2:end));
rp = repmat(D.domain.cellcenters.x, 1, Ntheta);
rf = repmat(D.domain.facecenters.x, 1, Ntheta);

% define the vectors to store the sparse matrix data
iix = zeros(3*(Nr+2)*(Ntheta+2),1);	iiy = zeros(3*(Nr+2)*(Ntheta+2),1);
jjx = zeros(3*(Nr+2)*(Ntheta+2),1);	jjy = zeros(3*(Nr+2)*(Ntheta+2),1);
sx = zeros(3*(Nr+2)*(Ntheta+2),1);	sy = zeros(3*(Nr+2)*(Ntheta+2),1);
mnx = Nr*Ntheta;	mny = Nr*Ntheta;

% reassign the east, west, north, and south diffusivity vectors for the
% code readability
De = rf(2:Nr+1,:).*D.xvalue(2:Nr+1,:)./(rp.*dr(2:Nr+1,:).*DR(2:Nr+1,:));
Dw = rf(1:Nr,:).*D.xvalue(1:Nr,:)./(rp.*dr(1:Nr,:).*DR(2:Nr+1,:));
Dn = D.yvalue(:,2:Ntheta+1)./(rp.*rp.*dtheta(:,2:Ntheta+1).*DTHETA(:,2:Ntheta+1));
Ds = D.yvalue(:,1:Ntheta)./(rp.*rp.*dtheta(:,1:Ntheta).*DTHETA(:,2:Ntheta+1));

% calculate the coefficients for the internal cells
AE = reshape(De,mnx,1);
AW = reshape(Dw,mnx,1);
AN = reshape(Dn,mny,1);
AS = reshape(Ds,mny,1);
APx = reshape(-(De+Dw),mnx,1);
APy = reshape(-(Dn+Ds),mny,1);

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nr+1,2:Ntheta+1),mnx,1); % main diagonal x
iix(1:3*mnx) = repmat(rowx_index,3,1);
rowy_index = reshape(G(2:Nr+1,2:Ntheta+1),mny,1); % main diagonal y
iiy(1:3*mny) = repmat(rowy_index,3,1);
jjx(1:3*mnx) = [reshape(G(1:Nr,2:Ntheta+1),mnx,1); reshape(G(2:Nr+1,2:Ntheta+1),mnx,1); reshape(G(3:Nr+2,2:Ntheta+1),mnx,1)];
jjy(1:3*mny) = [reshape(G(2:Nr+1,1:Ntheta),mny,1); reshape(G(2:Nr+1,2:Ntheta+1),mny,1); reshape(G(2:Nr+1,3:Ntheta+2),mny,1)];
sx(1:3*mnx) = [AW; APx; AE];
sy(1:3*mny) = [AS; APy; AN];

% build the sparse matrix
kx = 3*mnx;
ky = 3*mny;
Mx = sparse(iix(1:kx), jjx(1:kx), sx(1:kx), (Nr+2)*(Ntheta+2), (Nr+2)*(Ntheta+2));
My = sparse(iiy(1:ky), jjy(1:ky), sy(1:ky), (Nr+2)*(Ntheta+2), (Nr+2)*(Ntheta+2));
M = Mx + My;


% This function executes IPOPT to find the maximum likelihood solution to
% least squares regression with L1 regularization or the "Lasso". The inputs
% are the data matrix A (in which each row is an example vector), the vector
% of regression outputs y, and the penalty parameter lambda, a number
% greater than zero. The output is the estimated vector of regression
% coefficients.
%
% Copyright (C) 2008 Peter Carbonetto. All Rights Reserved.
% This code is published under the Eclipse Public License.
%
% Author: Peter Carbonetto
%         Dept. of Computer Science
%         University of British Columbia
%         September 18, 2008
function w = lasso (A, y, lambda)

  % Get the number of examples (n) and the number of regression
  % coefficients (m).
  [n m] = size(A);

  % The starting point.
  x0 = { zeros(m,1)
         ones(m,1) };

  % The constraint functions are bounded from below by zero.
  options.cl = zeros(2*m,1);
  options.cu = repmat(Inf,2*m,1);

  % Set up the auxiliary data.
  options.auxdata = { n m A y lambda };

  % Set the IPOPT options.
  options.ipopt.jac_d_constant   = 'yes';
  options.ipopt.hessian_constant = 'yes';
  options.ipopt.mu_strategy      = 'adaptive';
  options.ipopt.max_iter         = 100;
  options.ipopt.tol              = 1e-8;

  % The callback functions.
  funcs.objective         = @objective;
  funcs.constraints       = @constraints;
  funcs.gradient          = @gradient;
  funcs.jacobian          = @jacobian;
  funcs.jacobianstructure = @jacobianstructure;
  funcs.hessian           = @hessian;
  funcs.hessianstructure  = @hessianstructure;

  % Run IPOPT.
  [x info] = ipopt(x0,funcs,options);
  w        = x{1};

% ------------------------------------------------------------------
function f = objective (x, auxdata)
  [n m A y lambda] = deal(auxdata{:});
  [w u] = deal(x{:});
  f     = norm(y - A*w)^2/2 + lambda*sum(u);

% ------------------------------------------------------------------
function c = constraints (x, auxdata)
  [w u] = deal(x{:});
  c     = [ w + u; u - w ];

% ------------------------------------------------------------------
function g = gradient (x, auxdata)
  [n m A y lambda] = deal(auxdata{:});
  w = x{1};
  g = { -A'*(y - A*w)
        repmat(lambda,m,1) };

% ------------------------------------------------------------------
function J = jacobianstructure (auxdata)
  m = auxdata{2};
  I = speye(m);
  J = [ I I
        I I ];

% ------------------------------------------------------------------
function J = jacobian (x, auxdata)
  m = auxdata{2};
  I = speye(m);
  J = [  I  I
        -I  I ];

% ------------------------------------------------------------------
function H = hessianstructure (auxdata)
  m = auxdata{2};
  H = [ tril(ones(m))  zeros(m)
          zeros(m)     zeros(m) ];
  H = sparse(H);

% ------------------------------------------------------------------
function H = hessian (x, sigma, lambda, auxdata)
  [n m A y lambda] = deal(auxdata{:});
  H = [ tril(A'*A)  zeros(m)
         zeros(m)   zeros(m) ];
  H = sparse(sigma * H);


function [d] = lbfgsProd(g,S,Y,YS,lbfgs_start,lbfgs_end,Hdiag)
% BFGS Search Direction
%
% This function returns the (L-BFGS) approximate inverse Hessian,
% multiplied by the negative gradient

% Set up indexing
[nVars,maxCorrections] = size(S);
if lbfgs_start == 1
	ind = 1:lbfgs_end;
	nCor = lbfgs_end-lbfgs_start+1;
else
	ind = [lbfgs_start:maxCorrections 1:lbfgs_end];
	nCor = maxCorrections;
end
al = zeros(nCor,1);
be = zeros(nCor,1);

d = -g;
for j = 1:length(ind)
	i = ind(end-j+1);
	al(i) = (S(:,i)'*d)/YS(i);
%     al(i) = sum(S(:, i) .*d(:)) / YS(i);
	d = d-al(i)*Y(:,i);
end

% Multiply by Initial Hessian
d = Hdiag*d;

for i = ind
	be(i) = (Y(:,i)'*d)/YS(i);
%     be(i) = sum(Y(:, i) .* d(:)) / YS(i);
	d = d + S(:,i)*(al(i)-be(i));
%     if i == 1
%         fprintf("albe %.15f %.15f %.15f %.15f\n", al(i)-be(i), al(i), be(i), S(i));
%     end
end


function pcut = addBilinearVariableCuts(p)

if isempty(p.bilinears)
    pcut = p;
else
pcut = emptyNumericalModel;

z = p.bilinears(:,1);
x = p.bilinears(:,2);
y = p.bilinears(:,3);

nn = length(p.c);

still_uncertain = find(abs(p.lb(z)-p.ub(z))>1e-8);
if ~isempty(still_uncertain)

    x_lb = p.lb(x);
    x_ub = p.ub(x);
    y_lb = p.lb(y);
    y_ub = p.ub(y);
    m = length(x);
    one = ones(m,1);
    general_vals =[x_lb.*y_lb one -y_lb -x_lb,x_ub.*y_ub one -y_ub -x_ub,-x_ub.*y_lb -one y_lb x_ub,-x_lb.*y_ub -one y_ub x_lb]';
    general_cols = [one z+1 x+1 y+1 one z+1  x+1 y+1 one z+1 x+1 y+1 one z+1 x+1 y+1]';
    general_row = [1;1;1;1;2;2;2;2;3;3;3;3;4;4;4;4];

    quadratic_row = [1;1;1;2;2 ;2;3; 3; 3;4;4;4;5;5;5;6;6;6];
    quadratic_cols =  [one  z+1 x+1 ,one z+1 x+1 ,one  z+1 x+1, one z+1 x+1 ,one z+1 x+1, one z+1 x+1]';
    x1 = (3*x_ub+x_lb)/4;
    x2 = (x_ub+3*x_lb)/4;
    x3 = (x_ub+x_lb)/2;
    quadratic_vals = [-x_ub.*x_lb -one x_lb+x_ub x_lb.*y_lb one -y_lb-x_lb x_ub.*y_ub one -y_ub-x_ub  x1.*x1 one -x1-x1 x2.*x2 one -x2-x2 x3.*x3 one -x3-x3]';
    m = 1+length(p.c);
    rows = [];
    cols = [];
    vals = [];
    nrow = 0;

    for i =still_uncertain(:)'
        x = p.bilinears(i,2);
        y = p.bilinears(i,3);
        if x~=y
            rows = [rows;general_row+nrow];
            vals = [vals;general_vals(:,i)];
            cols = [cols;general_cols(:,i)];
            nrow = nrow + 4;
        else
            col = quadratic_cols(:,i);
            val = quadratic_vals(:,i);

            rows = [rows;quadratic_row+nrow];
            vals = [vals;val];
            cols = [cols;col];
            nrow = nrow + max(quadratic_row);
        end
    end

    F_temp = sparse(rows,cols,vals,nrow,m);
    keep = find(~isinf(F_temp(:,1)) & ~isnan(F_temp(:,1)));
    F_temp = F_temp(keep,:);
    pcut.F_struc = [F_temp;pcut.F_struc];
    pcut.K.l = pcut.K.l+size(F_temp,1);
end

pcut = mergeNumericalModels(p,pcut);
end


classdef Entropy < Algorithm

    methods (Access = public)

        function obj = Entropy()
            obj.name = 'Entropy';
            obj.inputPort = DataType.kSignal;
            obj.outputPort = DataType.kFeature;
        end

        function result = compute(~,signal)

            alphabet = unique(signal);
            freq = zeros(size(alphabet));

            for symbol = 1:length(alphabet)
                freq(symbol) = sum(signal == alphabet(symbol));
            end

            P = freq / sum(freq);
            result = -sum(P .* log2(P));
        end

        function metrics = computeMetrics(~,input)
            n = size(input,1);
            flops = n * n;
            memory = n;
            outputSize = Constants.kFeatureBytes;
            metrics = Metric(flops,memory,outputSize);
        end
    end

end


function [weights, th1, th2] = gncWeightsUpdate(weights, mu, residuals, barc2)
%GNC - GNC weights update for the TLS cost function
% Reference:
%    - "Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection"
%      Yang, Antonante, Tzoumas, Carlone (2020). IEEE Robotics and Automation Letters (RA-L), 5(2), 1127–1134.
%      https://arxiv.org/pdf/1909.08605.pdf
%    - "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications"
%      Antonante, Tzoumas, Yang, Carlone (2020).
%      https://arxiv.org/pdf/2007.15109.pdf
%

% Author: Pasquale Antonante
% email: antonap@mit.edu
% Date: 2021-01-06

th1 = (mu+1)/mu * barc2;
th2 = (mu)/(mu+1) * barc2; % th1 > th2
for k = 1:length(residuals)
    if residuals(k) - th1 >= 0
        weights(k) = 0;
    elseif residuals(k) - th2 <= 0
        weights(k) = 1;
    else
        weights(k) = sqrt( barc2*mu*(mu+1)/residuals(k) ) - mu;
        assert(weights(k)>= 0 && weights(k) <=1, 'weights %g calculation wrong!', weights(k));
    end
end
end


function [w,run] = train_cg2(x,w)
% Conjugate gradient method
% Data is columns of x, each column already scaled by the output y (+1 or -1).
% w is the starting guess for the parameters (a column).

% Written by Thomas P Minka

flops(0);
old_g = zeros(size(w));
old_u = [];
for iter = 1:1000
  old_w = w;
  % s1 = 1-sigma
  s1 = 1./(1+exp(w'*x));
  g = x*s1';
  a = s1.*(1-s1);
  if iter == 1
    u = g;
  else
    h = x*diag(a)*x';
    u = orthA([old_u g],h);
    u = u(:,cols(u));
  end

  % line search along u
  ug = u'*g;
  ux = u'*x;
  uhu = (ux.^2)*a';
  w = w + (ug/uhu)*u;
  old_g = g;

  % update memory
  old_u = [old_u u];
  if(cols(old_u) > 1)
    old_u(:,1) = [];
  end

  fl = flops;
  run.w(:,iter) = w;
  run.flops(iter) = fl;
  run.e(iter) = logProb(x,w);
  flops(fl);

  if max(abs(w - old_w)) < 1e-5
    break
  end
end
figure(2)
plot(run.e)
if iter == 1000
  warning('not enough iters')
end


% Coupled nonlinear PDE's
% Buckley Leverett equation
% dependent variables: pressure and water saturation
% Prepared for educational purposes by ** AAE **
% Works fine; slow!
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
%% define the geometry
well_radius = 0.1; % [m]
x_well_radius = 10*well_radius; % area close to the well
Nx_mid = 50; % number of cells in x direction
Ny_mid = 50; % number of cells in y direction
Lx = 50; % [m]
Ly = 50; % [m]
x = [0:well_radius:x_well_radius linspace(x_well_radius+well_radius, Lx-x_well_radius, Nx_mid) Lx-x_well_radius+well_radius:well_radius:Lx];
y = [0:well_radius:x_well_radius linspace(x_well_radius+well_radius, Ly-x_well_radius, Ny_mid) Ly-x_well_radius+well_radius:well_radius:Ly];
m = createMesh2D(x, y); % creates a 2D mesh
Nx = length(x)-1;
Ny = length(y)-1;
%% define the physical parametrs
krw0 = 1.0;
kro0 = 0.76;
nw = 2.4;
no = 2.0;
sor=0.12;
swc=0.09;
sws=@(sw)((sw>swc).*(sw<1-sor).*(sw-swc)/(1-sor-swc)+(sw>=1-sor).*ones(size(sw)));
kro=@(sw)((sw>=swc).*kro0.*(1-sws(sw)).^no+(sw<swc).*(1+(kro0-1)/swc*sw));
krw=@(sw)((sw<=1-sor).*krw0.*sws(sw).^nw+(sw>1-sor).*(-(1-krw0)/sor.*(1.0-sw)+1.0));
dkrwdsw=@(sw)((sw<=1-sor).*nw.*krw0.*(1/(1-sor-swc)).*sws(sw).^(nw-1)+(sw>1-sor)*((1-krw0)/sor));
dkrodsw=@(sw)((sw>=swc).*(-kro0*no*(1-sws(sw)).^(no-1))/(-swc-sor+1)+(sw<swc).*((kro0-1)/swc));
p0 = 100e5; % [bar] pressure
pin = 150e5; % [bar] injection pressure at the left boundary
u_in= 1.0/(24*3600); % [m/s] equal to 1 m/day
sw0 = swc+0.1; % initial water saturation
sw_in = 1;
mu_oil = 2e-3; % [Pa.s] oil viscosity
mu_water = 1e-3; % [Pa.s] water viscosity
% reservoir
k0 = 0.1e-12; % [m^2] average reservoir permeability
phi0 = 0.2; % average porosity
clx=1.2;
cly=0.2;
V_dp=0.1; % Dykstra-Parsons coef.
perm_val= field2d(Nx,Ny,k0,V_dp,clx,cly);
k=createCellVariable(m, perm_val);
phi=createCellVariable(m, phi0);
lw = geometricMean(k)/mu_water;
lo = geometricMean(k)/mu_oil;
Lw = @(sw)(krw(sw));
Lo = @(sw)(k/mu_oil*kro(sw));
dLwdsw = @(sw)(k/mu_water*dkrwdsw(sw));
dLodsw = @(sw)(k/mu_oil*dkrodsw(sw));
%% Define the boundaries
BCp = createBC(m); % Neumann BC for pressure
BCs = createBC(m); % Neumann BC for saturation
% bottom left boundary pressure gradient
BCp.left.a(1)=(krw(sw_in)*lw.xvalue(1,1)+kro(sw_in)*lo.xvalue(1,1)); BCp.left.b(1)=0; BCp.left.c(1)=-u_in;
BCp.bottom.a(1)=(krw(sw_in)*lw.yvalue(1,1)+kro(sw_in)*lo.yvalue(1,1)); BCp.bottom.b(1)=0; BCp.bottom.c(1)=-u_in;
% change the top right boandary to constant pressure (Dirichlet)
BCp.right.a(end)=0; BCp.right.b(end)=1; BCp.right.c(end)=p0;
BCp.top.a(end)=0; BCp.top.b(end)=1; BCp.top.c(end)=p0;
% change the bottom left boundary to constant saturation (Dirichlet)
BCs.left.a(1)=0; BCs.left.b(1)=1; BCs.left.c(1)=1;
BCs.bottom.a(1)=0; BCs.bottom.b(1)=1; BCs.bottom.c(1)=1;
%% define the time step and solver properties
% dt = 1000; % [s] time step
dt=(Lx/Nx)/u_in/10; % [s]

eps_p = 1e-5; % pressure accuracy
eps_sw = 1e-5; % saturation accuracy
%% define the variables
sw_old = createCellVariable(m, sw0, BCs);
p_old = createCellVariable(m, p0, BCp);
sw = sw_old;
p = p_old;
uw = -gradientTerm(p_old); % an estimation of the water velocity
%% start the main loop
% generate intial pressure profile (necessary to initialize the fully
% implicit solver)
dp_alwd= 100.0; % Pa
dsw_alwd= 0.1;
t = 0;
oil_init = domainInt(1-sw_old);
R(1) = 0;
i=0;
v_domain = domainInt(phi);
n_pv = 4; % injecting 4 pv of water
t_end = n_pv*v_domain/(u_in*well_radius*2); % final simulation time
while (t<t_end)
    error_p = 1e5;
    error_sw = 1e5;
    % Implicit loop
    loop_count=0;
    while ((error_p>eps_p) || (error_sw>eps_sw))
        loop_count=loop_count+1;
        if loop_count>10
            break
        end
        % calculate parameters
        pgrad = gradientTerm(p);
        sw_face = upwindMean(sw, -pgrad); % average value of water saturation
        labdao = lo.*funceval(kro, sw_face);
        labdaw = lw.*funceval(krw, sw_face);
        dlabdaodsw = lo.*funceval(dkrodsw, sw_face);
        dlabdawdsw = lw.*funceval(dkrwdsw, sw_face);
        labda = labdao+labdaw;
        dlabdadsw = dlabdaodsw+dlabdawdsw;
        % compute [Jacobian] matrices
        Mdiffp1 = diffusionTerm(-labda);
        Mdiffp2 = diffusionTerm(-labdaw);
        Mconvsw1 = convectionUpwindTerm(-dlabdadsw.*pgrad);
        Mconvsw2 = convectionUpwindTerm(-dlabdawdsw.*pgrad);
        [Mtranssw2, RHStrans2] = transientTerm(sw_old, dt, phi);
        % Compute RHS values
        RHS1 = divergenceTerm(-dlabdadsw.*sw_face.*pgrad);
        RHS2 = divergenceTerm(-dlabdawdsw.*sw_face.*pgrad);
        % include boundary conditions
        [Mbcp, RHSbcp] = boundaryCondition(BCp);
        [Mbcsw, RHSbcsw] = boundaryCondition(BCs);
        % Couple the equations; BC goes into the block on the main diagonal
        M = [Mdiffp1+Mbcp Mconvsw1; Mdiffp2 Mconvsw2+Mtranssw2+Mbcsw];
        RHS = [RHS1+RHSbcp; RHS2+RHStrans2+RHSbcsw];
        % solve the linear system of equations
        x = M\RHS;
        % x = agmg(M, RHS, [], 1e-10, 500, [], [p.value(:); sw.value(:)]);
        % separate the variables from the solution
        p_new = reshapeCell(m,full(x(1:(Nx+2)*(Ny+2))));
        sw_new = reshapeCell(m,full(x((Nx+2)*(Ny+2)+1:end)));
        % calculate error values
        error_p = max(abs((p_new(:)-p.value(:))./p_new(:)));
        error_sw = max(abs(sw_new(:)-sw.value(:)));
        % assign new values of p and sw
        p.value = p_new;
        sw.value = sw_new;
    end
    if loop_count>10
      p=p_old;
      sw=sw_old;
      dt=dt/5;
      continue
    end
    dsw=max(abs(sw_new(:)-sw_old.value(:))./sw_new(:));
    t=t+dt
    dt=min([dt*(dsw_alwd/dsw), 2*dt, t_end-t]);
    p_old = p;
    sw_old = sw;
    oil = domainInt(1-sw);
    i=i+1;
    R(i) = (oil_init-oil)/oil_init;
    p_inj(i) = p.value(2,2);
    t_series(i) = t;
    figure(1);visualizeCells(sw); drawnow;
end


function [denoised, noise] = denoise(noisy_signal,prior,std_noise)
  % Given a noisy image and a similar prior image and the standard deviation of
  % the noise (assumes gaussian distribution) denoise the iamge by building a
  % steerable freq pyramid and using probabilistic algorithms to build a coring
  % function (highest 5 bands only) to remap the image to the original. Also
  % prints and shows plots of the estimators.
  %
  % [denoised, noise] = denoise(noisy_signal,prior,std_noise)
  %
  % Inputs:
  %   noisy_signal  image with noise
  %   prior  similar image without noise
  %   std_noise  the standard deviation of the noise distribution
  % Output:
  %   denoised  the denoised result image
  %   noise  the generated noise image
  %

  % create noise image
  noise = std_noise.*randn(size(noisy_signal));

  % build steerable pyramids for each image
  [prior_pyr,prior_ind]=buildSFpyr(prior,3,3);
  [noisy_signal_pyr,noisy_signal_ind]=buildSFpyr(noisy_signal,3,3);
  [noise_pyr,noise_ind]=buildSFpyr(noise,3,3);

  % reconstruct right away (for debugging)
  %before = reconSFpyr(noisy_signal_pyr,noisy_signal_ind);

  % open a new figure for plotting the estimators
  %screen_size = get(0, 'ScreenSize');
  estimator_figure = figure;
  %set(estimator_figure, 'Position', [0 0 screen_size(3) screen_size(4) ] );
  set(estimator_figure, 'Position', [0 0 1000 200 ] );

  for band_index = 1:5
    % extract top five bands of the prior and noise pyramids
    noise_band = pyrBand(noise_pyr,noise_ind,band_index);
    prior_band = pyrBand(prior_pyr,prior_ind,band_index);
    noisy_signal_band = pyrBand(noisy_signal_pyr,noisy_signal_ind,band_index);

    % small value to add to bins to avoid mayhem
    %EPSILON = 0.1;
    EPSILON = 0.000001;
    % histogram the coefficients
    histogram_bins = -0.3:0.01:0.3;
    histogram_prior = EPSILON+ ...
      hist(reshape(prior_band,prod(size(prior_band)),1),histogram_bins);
    histogram_noise = EPSILON+ ...
      hist(reshape(noise_band,prod(size(noise_band)),1),histogram_bins);
    % compute estimate of bayesian estimator using formula from simoncelli96c.pdf:
    %        ∫ Pn(y-z) z Px(z) dz
    % b(y) = ---------------------
    %         ∫ Pn(y-z) Px(z) dz
    %
    % where Pn is the pdf of noise and Px of image. We estimate Pn with
    % histogram_noise and Px with histogram_prior. Then both top and bottom are
    % convolutions ( z is histogram_bins)
    %
    bayesian_estimator = ...
      conv2(histogram_noise,histogram_bins.*histogram_prior,'same') ./ ...
      conv2(histogram_noise,histogram_prior,'same');

    % plot  the estimator with a straight line for comparison
    subplot(1,5,band_index);
    plot(histogram_bins,bayesian_estimator,histogram_bins,histogram_bins);
    grid on
    %legend('bayesian estimator','straight line for comparison');

    if(band_index==1)
      title('hifi residual');
    else
      title(['hifi orientation ' num2str(band_index-1)])
    end

    % (re)map signal coefficients using bayesian_estimator
    denoised_band = interp1(histogram_bins,bayesian_estimator,noisy_signal_band);

    % add back to pyramid
    noisy_signal_pyr = ...
      setPyrBand(noisy_signal_pyr,noisy_signal_ind,denoised_band,band_index);

  end

  % print the plots
  estimator_figure;
  print('-depsc','bayesian_estimator_plots')

  % reconstruct image from modified pyramid
  denoised = reconSFpyr(noisy_signal_pyr,noisy_signal_ind);

end


% Coupled nonlinear PDE's
% Buckley Leverett equation
% dependent variables: pressure and water saturation
% Prepared for educational purposes by ** AAE **
% TVD scheme, really sharp fronts! Not too bad computationally.
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc
%% define the geometry
Nx = 200; % number of cells in x direction
Ny = 150; % number of cells in y direction
W = 50; % [m] length of the domain in x direction
H = 30; % [m] length of the domain in y direction
m = createMesh2D(Nx, Ny, W, H); % creates a 2D mesh
%% define the physical parametrs
p0 = 1e5; % [bar] pressure
pin = 50e5; % [bar] injection pressure at the left boundary
sw0 = 0; % initial water saturation
swin = 1;
mu_oil = 10e-3; % [Pa.s] oil viscosity
mu_water = 1e-3; % [Pa.s] water viscosity
% reservoir
k0 = 2e-12; % [m^2] average reservoir permeability
phi0 = 0.2; % average porosity
clx=0.05;
cly=0.05;
V_dp=0.6; % Dykstra-Parsons coef.
perm_val= field2d(Nx,Ny,k0,V_dp,clx,cly);
k=createCellVariable(m, perm_val);
phi=createCellVariable(m, phi0);
krw0 = 1;
kro0 = 1;
nw = 2;
no = 2;
krw = @(sw)(krw0*sw.^nw);
dkrwdsw = @(sw)(krw0*nw*sw.^(nw-1));
kro = @(sw)(kro0*(1-sw).^no);
dkrodsw = @(sw)(-kro0*no*(1-sw).^(no-1));
lw = geometricMean(k)/mu_water;
lo = geometricMean(k)/mu_oil;
Lw = @(sw)(krw(sw));
Lo = @(sw)(k/mu_oil*kro(sw));
dLwdsw = @(sw)(k/mu_water*dkrwdsw(sw));
dLodsw = @(sw)(k/mu_oil*dkrodsw(sw));
%% Define the boundaries
BCp = createBC(m); % Neumann BC for pressure
BCs = createBC(m); % Neumann BC for saturation
% change the left and right boandary to constant pressure (Dirichlet)
BCp.left.a(:)=0; BCp.left.b(:)=1; BCp.left.c(:)=pin;
BCp.right.a(:)=0; BCp.right.b(:)=1; BCp.right.c(:)=p0;
% change the left boundary to constant saturation (Dirichlet)
BCs.left.a(:)=0; BCs.left.b(:)=1; BCs.left.c(:)=1;
%% define the time step and solver properties
dt = 1000; % [s] time step
t_end = 1000*dt; % [s] final time
eps_p = 1e-5; % pressure accuracy
eps_sw = 1e-5; % saturation accuracy
%% define the variables
sw_old = createCellVariable(m, sw0, BCs);
p_old = createCellVariable(m, p0, BCp);
sw = sw_old;
p = p_old;
uw = -gradientTerm(p_old); % an estimation of the water velocity
%% start the main loop
% generate intial pressure profile (necessary to initialize the fully
% implicit solver)
FL = fluxLimiter('SUPERBEE');
t = 0;
while (t<t_end)
    error_p = 1e5;
    error_sw = 1e5;
    % estimation loop: sequential
%     for i = 1:2
%         pgrad = gradientTerm(m, p.value);
%         sw_face = upwindMean(m, -pgrad, sw.value); % average value of water saturation
%         labdao = lo.*funceval(kro, sw_face);
%         labdaw = lw.*funceval(krw, sw_face);
%         labda = labdao+labdaw;
%         Mdiffp1 = diffusionTerm(m, -labda);
%         [Mbcp, RHSbcp] = boundaryCondition(m, BCp);
%         p.value = reshapeCell(m, full((Mdiffp1+Mbcp)\RHSbcp));
%         for j = 1:2
%             pgrad = gradientTerm(m, p.value);
%             sw_face = upwindMean(m, -pgrad, sw.value);
%             labdaw = lw.*funceval(krw, sw_face);
%             dlabdawdsw = lw.*funceval(dkrwdsw, sw_face);
%             [Mbcsw, RHSbcsw] = boundaryCondition(m, BCs);
%             [Mtranssw2, RHStrans2] = transientTerm(m, phi, dt, sw);
%             Mconvsw2 = convectionUpwindTerm(m, -dlabdawdsw.*pgrad);
%             RHSs = RHSbcsw+RHStrans2+divergenceTerm(m, (labdaw-dlabdawdsw.*sw_face).*pgrad);
%             Ms = Mbcsw+Mconvsw2+Mtranssw2;
%             sw.value = reshapeCell(m, full(Ms\RHSs));
%         end
%     end
    % Implicit loop
    while ((error_p>eps_p) || (error_sw>eps_sw))
        % calculate parameters
        pgrad = gradientTerm(p);

        % sw_face = upwindMean(m, -pgrad, sw.value); % average value of water saturation
        sw_face = tvdMean(sw, -pgrad, FL); % average value of water saturation
        labdao = lo.*funceval(kro, sw_face);
        labdaw = lw.*funceval(krw, sw_face);
        dlabdaodsw = lo.*funceval(dkrodsw, sw_face);
        dlabdawdsw = lw.*funceval(dkrwdsw, sw_face);
        labda = labdao+labdaw;
        dlabdadsw = dlabdaodsw+dlabdawdsw;
        % compute [Jacobian] matrices
        [Mconvsw1, RHSconvsw1] = convectionTvdTerm(-dlabdadsw.*pgrad, sw, FL);
%         sw_face = tvdMean(m, sw.value, -pgrad, FL); % average value of water saturation
%         labdao = lo.*funceval(kro, sw_face);
%         labdaw = lw.*funceval(krw, sw_face);
%         dlabdaodsw = lo.*funceval(dkrodsw, sw_face);
%         dlabdawdsw = lw.*funceval(dkrwdsw, sw_face);
%         labda = labdao+labdaw;
%         dlabdadsw = dlabdaodsw+dlabdawdsw;
        [Mconvsw2, RHSconvsw2] = convectionTvdTerm(-dlabdawdsw.*pgrad, sw, FL);

        [Mtranssw2, RHStrans2] = transientTerm(sw_old, dt, phi);
        Mdiffp1 = diffusionTerm(-labda);
        Mdiffp2 = diffusionTerm(-labdaw);
        % Compute RHS values
        RHS1 = divergenceTerm(-dlabdadsw.*sw_face.*pgrad);
        RHS2 = divergenceTerm(-dlabdawdsw.*sw_face.*pgrad);
        % include boundary conditions
        [Mbcp, RHSbcp] = boundaryCondition(BCp);
        [Mbcsw, RHSbcsw] = boundaryCondition(BCs);
        % Couple the equations; BC goes into the block on the main diagonal
        M = [Mdiffp1+Mbcp Mconvsw1; Mdiffp2 Mconvsw2+Mtranssw2+Mbcsw];
        RHS = [RHS1+RHSbcp+RHSconvsw1; RHS2+RHStrans2+RHSbcsw+RHSconvsw2];
        % solve the linear system of equations
        x = M\RHS;
        % separate the variables from the solution
        p_new = reshapeCell(m,full(x(1:(Nx+2)*(Ny+2))));
        sw_new = reshapeCell(m,full(x((Nx+2)*(Ny+2)+1:end)));
        % calculate error values
        error_p = max(max(abs((p_new-p.value)./p_new)));
        error_sw = max(max(abs(sw_new-sw.value)));
        % assign new values of p and sw
        p.value = p_new;
        sw.value = sw_new;
    end
    t=t+dt;
    p_old = p;
    sw_old = sw;
    figure(1);visualizeCells(sw); drawnow;%shading interp
end


function [A,fint] = glm_phi (phi,dt,fb)
% Estimate connectivity parameters using GLM/EMA method
% FORMAT [A,fint] = glm_phi (phi,dt,fb)
%
% phi       [N x Nr] matrix of phase time series
%           (N time points, Nr regions)
% dt        sample period
% fb        bandwidth parameter
%
% A         [Nr x Nr] normalised connectivities
% fint      [Nr x 1] intrinsic frequencies

if iscell(phi)
    Nt=length(phi);
    tmp=[];
    for i=1:Nt,
        tmp=[tmp;phi{i}];
    end
    phi=tmp;
end

[N,Nr]=size(phi);

for i=1:Nr,
    ddphi(:,i)=diff(phi(:,i));
end

% Ignore clocking points in any region
rem=[];
for i=1:Nr,
    rem=[rem;find(abs(ddphi(:,i))>5)];
end
keep=[1:N-1];
keep(rem)=[];

dphi=ddphi(keep,:)/(2*pi*dt);
my_phi=phi(keep,:);

A=zeros(Nr,Nr);
for i=1:Nr,
    y=dphi(:,i);
    N=length(y);
    X=[];
    regs=[];
    % Assemble design matrix
    for j=1:Nr,
        if ~(i==j)
            X=[X,sin(my_phi(:,i)-my_phi(:,j))];
            regs=[regs,j];
        end
    end
    X=[X,ones(N,1)];
    beta=pinv(X)*y;
    A(i,regs)=-beta(1:Nr-1);
    fint(i)=beta(Nr);
    yfit(:,i)=X*beta;
end


% estimateWorldCameraPose Estimate camera pose from 3-D to 2-D point correspondences
%   [worldOrientation, worldLocation] = estimateWorldCameraPose(imagePoints, worldPoints, cameraParams)
%   returns the orientation and location of a calibrated camera in the world coordinate
%   system in which worldPoints are defined.
%
%   The function solves the Perspective-n-Point (PnP) problem using the P3P algorithm.
%   The function eliminates spurious correspondences using the M-estimator SAmple Consensus
%   (MSAC) algorithm
%
%   Inputs            Description
%   ------            -----------
%   imagePoints       M-by-2 array of [x,y] coordinates of undistorted image points,
%                     with M >= 4.
%
%   worldPoints       M-by-3 array of [x,y,z] coordinates of world points.
%
%   cameraParams      a cameraParameters or cameraIntrinsics object
%
%   Outputs           Description
%   -------            -----------
%   worldOrientation  orientation of the camera in the world coordinates specified
%                     as a 3-by-3 matrix
%   worldLocation     location of the camera in the world coordinates specified as a
%                     1-by-3 vector
%
%   [..., inlierIdx] = estimateWorldCameraPose(...) additionally returns the indices of the
%   inliers used to compute the camera pose. inlierIdx is an M-by-1 logical vector,
%   with the values of true corresponding to the inliers.
%
%   [..., status] = estimateWorldCameraPose(...) additionally returns a status code. If the
%   status output is not specified, the function will issue an error if the number of
%   input points or the number of inliers is less than 4. The status can have the following
%   values:
%
%       0: No error.
%       1: imagePoints and worldPoints do not contain enough points.
%          At least 4 points are required.
%       2: Not enough inliers found. At least 4 inliers are required.
%
%   [...] = estimateWorldCameraPose(..., Name, Value) specifies additional name-value pair
%   arguments described below:
%
%   'MaxNumTrials'          Positive integer scalar.
%                           Specifies the number of random trials for finding
%                           the outliers. The actual number of trials depends
%                           on imagePoints, worldPoints, and the values of the
%                           MaxReprojectionError and Confidence parameters. Increasing
%                           this value will improve the robustness of the output
%                           at the expense of additional computation.
%
%                           Default: 1000
%
%   'Confidence'            Scalar value greater than 0 and less than 100.
%                           Specifies the desired confidence (in percentage)
%                           for finding the maximum number of inliers. Increasing
%                           this value will improve the robustness of the output
%                           at the expense of additional computation.
%
%                           Default: 99
%
%   'MaxReprojectionError'  Positive numeric scalar specifying the reprojection
%                           error threshold in pixels for finding outliers. Increasing
%                           this value will make the algorithm converge faster,
%                           but may reduce the accuracy of the result.
%
%                           Default: 1
%
%    Notes
%    -----
%    The function does not account for lens distortion. You can either
%    undistort the images using the undistortImage function before
%    detecting the image points, or you can undistort the image points
%    themselves using the undistortPoints function.
%
%    Class Support
%    -------------
%    imagePoints and worldPoints must be of the same class, which can be
%    double or single. cameraParams must be a cameraParameters or
%    cameraIntrinsics object. orientation and location are of class double.
%
%    Example: Determine camera pose from world-to-image correspondences
%    ------------------------------------------------------------------
%    data = load('worldToImageCorrespondences.mat');
%    [worldOrientation, worldLocation] = estimateWorldCameraPose(...
%        data.imagePoints, data.worldPoints, data.cameraParams);
%    pcshow(data.worldPoints, 'VerticalAxis', 'Y', 'VerticalAxisDir', 'down', ...
%        'MarkerSize', 30);
%    hold on
%    plotCamera('Size', 10, 'Orientation', worldOrientation, 'Location',...
%        worldLocation);
%
%    See also relativeCameraPose, viewSet, triangulateMultiview, bundleAdjustment,
%             plotCamera, pcshow, extrinsics, triangulate,
%             cameraPoseToExtrinsics

%  Copyright 2015 The MathWorks, Inc.

% References:
% ----------
% [1] X.-S. Gao, X.-R. Hou, J. Tang, and H.-F. Cheng, "Complete Solution
%     Classification for the Perspective-Three-Point Problem", IEEE Trans.
%     Pattern Analysis and Machine Intelligence, vol. 25, no. 8, pp. 930-943,
%     2003.

%#codegen

function [orientation, location, inlierIdx, status, err] = ...
    estimateWorldCameraPoseNew(imagePoints, worldPoints, cameraParams, varargin)

[params, outputClass, imagePts, worldPts] = parseInputs(imagePoints, worldPoints, cameraParams, ...
    varargin{:});

if isa(cameraParams, 'cameraIntrinsics')
    cameraParams = cameraParams.CameraParameters;
end

% List of status codes
statusCode = struct(...
    'NoError',           int32(0),...
    'NotEnoughPts',      int32(1),...
    'NotEnoughInliers',  int32(2));

% Additional RANSAC parameters
params.sampleSize = 4;
params.recomputeModelFromInliers = false;
params.defaultModel.R = nan(3);
params.defaultModel.t = nan(1, 3);

% RANSAC function handles
funcs.fitFunc = @solveCameraPose;
funcs.evalFunc = @evalCameraPose;
funcs.checkFunc = @check;

numPoints = size(worldPts, 1);
if numPoints < params.sampleSize
   status = statusCode.NotEnoughPts;
   [orientation, location, inlierIdx] = badPose(numPoints);
else
    % Compute the pose using RANSAC
    points = pack(imagePts, worldPts);
    [isFound, pose, inlierIdx] = vision.internal.ransac.msac(...
        points, params, funcs, cameraParams.IntrinsicMatrix, outputClass);

%     isFound = true;
%     pose = solveCameraPose_world_image(worldPts, imagePts
    if isFound
        % Convert from extrinsics to orientation and location
        orientation =  pose.R';
        location    = -pose.t * pose.R';
        err = pose.err / numPoints;
        status = statusCode.NoError;
    else
        % Could not compute the pose
        status = statusCode.NotEnoughInliers;
        [orientation, location, inlierIdx] = badPose(numPoints);
        err = nan;
    end
end

if nargout < 4
    checkRuntimeStatus(statusCode, status);
end

%==========================================================================
% Check runtime status and report error if there is one
%==========================================================================
function checkRuntimeStatus(statusCode, status)
coder.internal.errorIf(status==statusCode.NotEnoughPts, ...
    'vision:points:notEnoughMatchedPts', 'imagePoints', 'worldPoints', 4);

coder.internal.errorIf(status==statusCode.NotEnoughInliers, ...
    'vision:points:notEnoughInlierMatches', 'imagePoints', ...
    'worldPoints');

%--------------------------------------------------------------------------
function [orientation, location, inlierIdx] = badPose(numPoints)
orientation = nan(3);
location = nan(1, 3);
inlierIdx = false(numPoints, 1);

function pose = solveCameraPose(points, varargin)
[worldPoints, imagePoints] = unpack(points);

intrinsicMatrix = varargin{1};
% N = size(worldPoints, 1);
% [R,T,Xc,best_solution] = efficient_pnp_gauss([worldPoints, ones(N, 1)], [imagePoints, ones(N, 1)], intrinsicMatrix', true);

% Get up to 4 solutions for the pose using 3 points
[Rs, Ts] = vision.internal.calibration.solveP3P(...
    imagePoints, worldPoints, intrinsicMatrix);

% Choose the best solution using the 4th point
p = [worldPoints(4, :), 1]; % homogeneous coordinates
q = imagePoints(4, :);
% pose_pnp = chooseBestSolution(p, q, R', T', intrinsicMatrix);

pose.R = nan(3);
pose.t = nan(1, 3);
if ~isempty(Rs)
    pose = chooseBestSolution(p, q, Rs, Ts, intrinsicMatrix);
end

function pose = solveCameraPose_world_image(worldPoints, imagePoints, K)

intrinsicMatrix = K;
% N = size(worldPoints, 1);
% [R,T,Xc,best_solution] = efficient_pnp_gauss([worldPoints, ones(N, 1)], [imagePoints, ones(N, 1)], intrinsicMatrix', true);

% Get up to 4 solutions for the pose using 3 points
[Rs, Ts] = vision.internal.calibration.solveP3P(...
    imagePoints, worldPoints, intrinsicMatrix);

% Choose the best solution using the 4th point
p = [worldPoints(4, :), 1]; % homogeneous coordinates
q = imagePoints(4, :);
% pose_pnp = chooseBestSolution(p, q, R', T', intrinsicMatrix);

pose.R = nan(3);
pose.t = nan(1, 3);
if ~isempty(Rs)
    pose = chooseBestSolution(p, q, Rs, Ts, intrinsicMatrix);
end
pose.err = 0;
% pose = pose_pnp;

%--------------------------------------------------------------------------
function pose = solveCameraPose_(points, varargin)
[worldPoints, imagePoints] = unpack(points);

intrinsicMatrix = varargin{1};
NN = size(imagePoints, 1);
bbb = [imagePoints, ones(NN, 1)] / (intrinsicMatrix);
rrr = worldPoints(:, 1 : 3);
% for i = 1 : NN
%     bbb(i, :) = bbb(i, :) ./ norm(bbb(i, :));
%     rrr(i, :) = rrr(i, :) ./ norm(rrr(i, :));
% end
% [RR, tt, ss, xs] = apnp_algebraic(bbb, rrr, 0, 0, intrinsicMatrix', eye(4));

% Get up to 4 solutions for the pose using 3 points
[Rs, Ts] = vision.internal.calibration.solveP3P(...
    imagePoints, worldPoints, intrinsicMatrix);

% Choose the best solution using the 4th point
p = [worldPoints(4, :), 1]; % homogeneous coordinates
q = imagePoints(4, :);

pose.R = nan(3);
pose.t = nan(1, 3);
pose.err = 1;
if ~isempty(Rs)
    pose = chooseBestSolution(p, q, Rs, Ts, intrinsicMatrix);
end

function [R_, t_, s_, xs, min_val] = apnp_algebraic(bbb, rrr, Xw, U, K, Rt)
    N = size(bbb, 1);
    b = zeros(3, N);
    r = zeros(3, N);
    RR = Rt(1 : 3, 1 : 3);
    tt = Rt(1 : 3, 4);
    for i = 1 : N
        b(:, i) = [bbb(i, 1 : 3)]';
        r(:, i) = rrr(i, 1 : 3)';
%
%         b(:, i) - RR * r(:, i) - tt
    end
    b_bar = zeros(3, 1);
    r_bar = zeros(3, 1);
    for i = 1 : N
        b_bar = b_bar + 1 / N * b(:, i);
        r_bar = r_bar + 1 / N * r(:, i);
    end

    P = zeros(4, 4);
    for i = 1 : N
        for j = 1 : 3
            str = sprintf('M = M%d_matrix(b(:, i) - b_bar);', j);
            eval(str);
            P = P + 1 / N * r(j, i) * M;
        end
    end
    [V, ~] = eig(P);

    q11 = V(:, 1); q_1 = q11 ./ norm(q11);
    q22 = V(:, 2); q_2 = q22 ./ norm(q22);
    q33 = V(:, 3); q_3 = q33 ./ norm(q33);
    q44 = V(:, 4); q_4 = q44 ./ norm(q44);
    x_count = 4 + 1;
    xs_ = [
        q_1';
        q_2';
        q_3';
        q_4';
        ];
    xs = zeros(4, 9);

    for i = 1 : x_count - 1
        q0 = xs_(i, 1);
        q1 = xs_(i, 2);
        q2 = xs_(i, 3);
        q3 = xs_(i, 4);
        R = quat2dcm([q0, q1, q2, q3]);
        aa = 0;
        bb = 0;
        for j = 1 : N
            aa = aa + 1 / N * (b(:, j)' * (b(:, j) - b_bar));
            bb = bb + 1 / N * (b(:, j)' * R * (r(:, j) - r_bar));
        end
        ss = bb / aa;
        s = ss;
        t = s * b_bar - R * r_bar;
        xs(i, :) = [q0, q1, q2, q3, t', s, ss];
    end

    Ls = zeros(x_count - 1, 1);
    for i = 1 : x_count - 1
        RR = quat2dcm(xs(i, 1 : 4));
        tt = xs(i, 5 : 7)';
        Ls(i) = J_func(RR, tt, bbb, rrr, K);
    end
    [minimum, idx] = sort(Ls);
    R_ = quat2dcm(xs(idx(1), 1 : 4));
    t_ = xs(idx(1), 5 : 7)';
    s_ = xs(idx(1), 9);
    min_val = minimum;

function J = J_func(R, t, b, r, K)
J = 0;
len = size(b, 2);
for j = 1 : len
    xb = b(j, :);
    bb = xb';
    xr = r(j, :)';
    xr = (R * xr + t);
    res = bb - xr;
    J = J + 1 / len * trace(res' * res);
end

%--------------------------------------------------------------------------
% Find the solution that results in smallest squared reprojection error for
% the 4th point.
% worldPoint must be in homogeneous coordinates (1-by-4)
function pose = chooseBestSolution(worldPoint, imagePoint, Rs, Ts, intrinsicMatrix)

pose.R = zeros(3);
pose.t = zeros(1, 3);

numSolutions = size(Ts, 1);
errors = zeros(numSolutions, 1, 'like', worldPoint);

for i = 1:numSolutions
    cameraMatrix = [Rs(:,:,i); Ts(i,:)] * intrinsicMatrix;
    projectedPoint = worldPoint * cameraMatrix;
    projectedPoint = projectedPoint(1:2) ./ projectedPoint(3);
    d = imagePoint - projectedPoint;
    errors(i) = d * d';
end

[~, idx] = min(errors);
idx = idx(1); % in case we have two identical errors
pose.t = Ts(idx, :);
pose.R = Rs(:,:,idx);
pose.err = errors(idx);

%--------------------------------------------------------------------------
% Compute reprojection errors
function dis = evalCameraPose(pose, points, varargin)
[worldPoints, imagePoints] = unpack(points);

intrinsicMatrix = varargin{1};
cameraMatrix = [pose.R; pose.t] * intrinsicMatrix;

% Project world points into the image
numPoints = size(worldPoints, 1);
worldPointsHomog = [worldPoints, ones(numPoints, 1, 'like', worldPoints)];
projectedPointsHomog = worldPointsHomog * cameraMatrix;
projectedPoints  = bsxfun(@rdivide, projectedPointsHomog(:, 1:2), ...
    projectedPointsHomog(:, 3));

% Compute reprojection errors
diffs = imagePoints - projectedPoints;
dis = sum(diffs.^2, 2);

%--------------------------------------------------------------------------
% Pack points into a single entity for RANSAC
function points = pack(imagePoints, worldPoints)
points = [imagePoints, worldPoints];

%--------------------------------------------------------------------------
% Unpack the points
function [worldPoints, imagePoints] = unpack(points)
imagePoints = points(:, 1:2);
worldPoints = points(:, 3:end);

%--------------------------------------------------------------------------
function r = check(pose, varargin)
r = ~isempty(pose) && ~isempty(pose.R) && ~isempty(pose.t);

%--------------------------------------------------------------------------
function [ransacParams, outputClass, imagePts, worldPts] = ...
    parseInputs(imagePoints, worldPoints, cameraParams, varargin)

validatePoints(imagePoints, worldPoints);
imagePts = double(imagePoints);
worldPts = double(worldPoints);
outputClass = class(imagePts);
validateattributes(cameraParams, {'cameraParameters','cameraIntrinsics'}, ...
    {'scalar'}, mfilename, 'cameraParams');

defaults = struct('MaxNumTrials',1000, 'Confidence',99, 'MaxDistance', 1);

if isempty(coder.target)
    ransacParams = parseRANSACParamsMatlab(defaults, varargin{:});
else
    ransacParams = parseRANSACParamsCodegen(defaults, varargin{:});
end

%--------------------------------------------------------------------------
function ransacParams = parseRANSACParamsMatlab(defaults, varargin)
parser = inputParser;
parser.FunctionName = mfilename;
parser.addParameter('MaxNumTrials', defaults.MaxNumTrials, @checkMaxNumTrials);
parser.addParameter('Confidence', defaults.Confidence, @checkConfidence);
parser.addParameter('MaxReprojectionError', defaults.MaxDistance, @checkMaxDistance);

parser.parse(varargin{:});
ransacParams.confidence = parser.Results.Confidence;
ransacParams.maxDistance = parser.Results.MaxReprojectionError^2;
ransacParams.maxNumTrials = parser.Results.MaxNumTrials;
ransacParams.verbose = false;

%--------------------------------------------------------------------------
function ransacParams = parseRANSACParamsCodegen(defaults, varargin)
% Instantiate an input parser
parms = struct( ...
    'MaxNumTrials',       uint32(0), ...
    'Confidence',         uint32(0), ...
    'MaxReprojectionError',        uint32(0));

popt = struct( ...
    'CaseSensitivity', false, ...
    'StructExpand',    true, ...
    'PartialMatching', false);

% Specify the optional parameters
optarg = eml_parse_parameter_inputs(parms, popt, varargin{:});
ransacParams.maxNumTrials = eml_get_parameter_value(optarg.MaxNumTrials,...
    defaults.MaxNumTrials, varargin{:});
ransacParams.confidence   = eml_get_parameter_value(optarg.Confidence,...
    defaults.Confidence, varargin{:});
ransacParams.maxDistance  = eml_get_parameter_value(...
    optarg.MaxReprojectionError, defaults.MaxDistance, varargin{:});
ransacParams.verbose  = false;

checkMaxNumTrials(ransacParams.maxNumTrials);
checkConfidence  (ransacParams.confidence);
checkMaxDistance (ransacParams.maxDistance);

ransacParams.maxDistance = ransacParams.maxDistance^2;

%--------------------------------------------------------------------------
function validatePoints(imagePoints, worldPoints)
validateattributes(imagePoints, {'double', 'single'}, ...
    {'real', 'nonsparse', 'nonempty', '2d', 'ncols', 2}, ...
    mfilename, 'imagePoints');

validateattributes(worldPoints, {'double', 'single'}, ...
    {'real', 'nonsparse', 'nonempty', '2d', 'ncols', 3}, ...
    mfilename, 'worldPoints');

coder.internal.errorIf(~isa(imagePoints, class(worldPoints)), ...
    'vision:points:ptsClassMismatch', 'imagePoints', 'worldPoints');
coder.internal.errorIf(size(imagePoints, 1) ~= size(worldPoints, 1), ...
    'vision:points:numPtsMismatch', 'imagePoints', 'worldPoints');

%--------------------------------------------------------------------------
function tf = checkMaxNumTrials(value)
validateattributes(value, {'numeric'}, ...
    {'scalar', 'nonsparse', 'real', 'integer', 'positive'}, mfilename, ...
    'MaxNumTrials');
tf = true;

%--------------------------------------------------------------------------
function tf = checkConfidence(value)
validateattributes(value, {'numeric'}, ...
    {'scalar', 'nonsparse', 'real', 'positive', '<', 100}, mfilename, ...
    'Confidence');
tf = true;

%--------------------------------------------------------------------------
function tf = checkMaxDistance(value)
validateattributes(value,{'single','double'}, ...
    {'real', 'nonsparse', 'scalar','nonnegative','finite'}, mfilename, ...
    'MaxDistance');
tf = true;


function xdot = MathieuEquation(t,x)
% Mathieu Equation is y''(z)+eta.y'(z)+(a-2qcos(2z))sin(y) = 0
% Written into two first order differential equations
% y'(z) = x
% x'(z) = -eta.y'(z)-(a-2qcos(2z))sin(y)
%
n = length(x) ;
xdot=zeros(n,1);
theta = x(1) ;
Dtheta = x(2) ;
q = x(3) ;
a = x(4) ;
eta = x(5) ;
xdot(1) = Dtheta;
xdot(2) = -eta*Dtheta-(a-2*q*cos(2*t))*sin(theta);


%%
% test for resolution of analysis prior denoising
% min_x 1/2*|x-y|^2 + lambda*|K*x|_1
% min_x F(x) + G(K*x)   with   G=lambda*|.|_1,  F=1/2*|.-y|^2

addpath('../');
addpath('../toolbox/');

n = 100;
p = 200;
lambda = 1;
K = randn(p,n);
KS = K';

ProxG = @(u,tau)perform_soft_thresholding(u, lambda*tau);
ProxGS = compute_dual_prox(ProxG);
GradFS = @(x)x-y;
F = @(x)1/2*norm(x-y)^2;
G = @(u)lambda*norm(u,1);
options.FS = F;
options.GS = @(u)norm(u,'inf')/lambda;

L = norm(K)^2;
options.method = 'fista';
options.niter = 2000;
[x, fs, Constraint, Hu] = perform_fb_strongly(zeros(n,1), K, KS, GradFS, ProxGS, L, options);

%%
% Compute the primal error.

E = [];
for i=1:size(Hu,2)
    x1 = GradFS(KS*Hu(:,i));
    E(i) = F(x1) + G(K*x1);
end

sel = 1:round(options.niter/10);
clf;
loglog( E(sel)-min(E(:)) );
axis tight;