% Copyright (C) Daphne Koller, Stanford University, 2012

rand('seed', 1);

% Construct the toy network
[toy_network, toy_factors] = ConstructToyNetwork(1,0.2);
toy_evidence = zeros(1, length(toy_network.names));
%toy_clique_tree = CreateCliqueTree(toy_factors, []);
%toy_cluster_graph = CreateClusterGraph(toy_factors,[]);

% Exact Inference
ExactM = ComputeExactMarginalsBP(toy_factors, toy_evidence, 0);
figure, VisualizeToyImageMarginals(toy_network, ExactM,1,'exact');

% Comment this in to run Approximate Inference on the toy network
% Approximate Inference
% % ApproxM = ApproxInference(toy_cluster_graph, toy_factors, toy_evidence);
% figure, VisualizeToyImageMarginals(toy_network, ApproxM);

% MCMC Inference
transition_names = {'Gibbs', 'MHUniform', 'MHGibbs', 'MHSwendsenWang1', 'MHSwendsenWang2'};

for j = 1:length(transition_names)
    samples_list = {};

    num_chains_to_run = 1;
    for i = 1:num_chains_to_run
        % Random Initialization
        A0 = ceil(rand(1, length(toy_network.names)) .* toy_network.card);

        % Initialization to all ones
        % A0 = i * ones(1, length(toy_network.names));

        [M, all_samples] = ...
            MCMCInference(toy_network, toy_factors, toy_evidence, transition_names{j}, 0, 500, 1, A0);
        samples_list{i} = all_samples;
        figure, VisualizeToyImageMarginals(toy_network, M, i, transition_names{j});
    end

    vis_vars = [3];
    VisualizeMCMCMarginals(samples_list, vis_vars, toy_network.card(vis_vars), toy_factors, ...
      500, ExactM(vis_vars),transition_names{j});
    disp(['Displaying results for MCMC with transition ', transition_names{j}]);
    disp(['Hit enter to continue']);
    pause;
end


function test_failed = test_blockfwt()
test_failed = 0;

disp('-------------TEST_BLOCKFWT--------------');

L = 567;
W = [1,3];

Lb = [78,64,58,1021];

wa = {'dden3','ana:symorth1'};
ws = {'dden3','syn:symorth1'};
J = [5];

for wId = 1:numel(W)
for lId = 1:numel(L)
f = tester_rand(L(lId),W(wId));
for lbId = 1:numel(Lb)
for waId=1:numel(wa)

Fa = blockframeaccel(frame('fwt',wa{waId},J),Lb(lbId),'segola');
Fs = blockframeaccel(frame('fwt',ws{waId},J),Lb(lbId),'segola');

a = Fa.g.a(1);
m = numel(Fa.g.g{1}.h);
rmax = (a^J-1)/(a-1)*(m-1);

f = postpad(f,L(lId)+rmax);
block(f,'offline','L',Lb(lbId));

colC = {};
colfhat = {};

for ii=1:ceil(L(lId)/Lb(lbId))
    fb = blockread();
    c  = blockana(Fa,fb);
    ccell = comp_fwtpack2cell(Fa,c);

    colC{end+1} = ccell;

    chat = cell2mat(ccell);

    fhat = blocksyn(Fs,chat,size(fb,1));
    colfhat{end+1} = fhat;
end

err = 0;
cwhole = fwt(f,wa{waId},J,'zero','cell');
for ii=1:numel(colC{1})
   cc{ii} = cell2mat(cellfun(@(cEl) cEl{ii},colC','UniformOutput',0));
   Ltmp = min([size(cwhole{ii},1),size(cc{ii},1)]);
   err = err + norm(cwhole{ii}(1:Ltmp,:)-cc{ii}(1:Ltmp,:));
end

[test_failed,fail]=ltfatdiditfail(err,test_failed);
fprintf('COEFS L:%3i, W:%3i, Lb=%3i, %s, err=%.4e %s\n',L(lId),W(wId),Lb(lbId),wa{waId},err,fail);

fhat = cell2mat(colfhat.');

fhat = fhat(rmax+1:end,:);

Lcrop = min([size(fhat,1),size(f,1)]);

res = norm([f(1:Lcrop,:)-fhat(1:Lcrop,:)]);
[test_failed,fail]=ltfatdiditfail(res,test_failed);
fprintf('REC   L:%3i, W:%3i, Lb=%3i, %s, err=%.4e %s\n',L(lId),W(wId),Lb(lbId),wa{waId},res,fail);

end
end
end
end


%dout=A*din+b;
function [A b]=cp_affine(dout,din)
nvin=size(din,1);
nvout=size(dout,1);
ndat=size(din,2);

data=[din;ones(1,ndat)];
mx_a=(dout*data')/(data*data');

A=mx_a(1:nvout,1:nvin);
b=mx_a(1:nvout,nvin+1);


function [fess,X] = fess_vbmc(vp,gp,X)
%FESS_VBMC Compute fractional effective sample size through importance sampling

if nargin < 3 || isempty(X); X = 100; end

% If a single number is passed, take it as the number of samples
if numel(X) == 1
    N = X;
    X = vbmc_rnd(vp,N,0);
else
    N = size(X,1);
end

% Can directly pass the estimated GP means instead of the full GP
if isstruct(gp)
    [~,~,fbar] = gplite_pred(gp,X,[],[],0,0);
else
    fbar = mean(gp,2);
end

if size(fbar,1) ~= size(X,1)
    error('Mismatch between number of samples from VP and GP.');
end

% Compute effective sample size (ESS) with importance sampling
vlnpdf = max(vbmc_pdf(vp,X,0,1),log(realmin));
logw = fbar - vlnpdf;
w = exp(logw - max(logw));
w = w/sum(w);
fess = 1/sum(w.^2) / N; % fractional ESS

end


function Keff = electrolyteConductivity(ce,T,param,batterySection)
% electrolyteConductivity  Evaluates the conductivity coefficients for the
% electrolyte phase. The measurement unit is [S/m]
%
%   Keff = electrolyteConductivity(ce,T,param) evaluates
%   the conductivity coefficients for the anode, separator and cathode electrolyte phase of the
%   battery. You can modify the script to meet your particular needs.
%
%   The conductivity coefficients can be evaluated in isothermal case
%   (param.TemperatureEnabled=0) or adiabatic case
%   (param.TemperatureEnabled=1 or 2).
%
%   You can modify the way that the conductivity coefficients are computed, as
%   function of electrolyte concentration and temperature. The main script
%   will pass also the param array.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.TemperatureEnabled>=1)
    switch(batterySection)
        case'p'
            Keff = param.eps_p^param.brugg_p *(1e-4*ce.*((-10.5+0.668*1e-3*ce+0.494*1e-6*ce.^2) +...
                (0.074  -1.78*1e-5*ce -8.86*1e-10*ce.^2).*T + (-6.96*1e-5+2.8*1e-8*ce).*T.^2).^2);
        case 's'
            Keff = param.eps_s^param.brugg_s *(1e-4*ce.*((-10.5+0.668*1e-3*ce+0.494*1e-6*ce.^2) +...
                (0.074  -1.78*1e-5*ce -8.86*1e-10*ce.^2).*T + (-6.96*1e-5+2.8*1e-8*ce).*T.^2).^2);
        case 'n'
            Keff = param.eps_n^param.brugg_n *(1e-4*ce.*((-10.5+0.668*1e-3*ce+0.494*1e-6*ce.^2) +...
                (0.074  -1.78*1e-5*ce -8.86*1e-10*ce.^2).*T + (-6.96*1e-5+2.8*1e-8*ce).*T.^2).^2);
    end
else
        switch(batterySection)
        case'p'
            Keff = param.eps_p^param.brugg_p *(4.1253*1e-2 + 5.007*1e-4*ce - 4.7212*1e-7*ce.^2 +1.5094*1e-10*ce.^3 -1.6018*1e-14*ce.^4);
        case 's'
            Keff = param.eps_s^param.brugg_s *(4.1253*1e-2 + 5.007*1e-4*ce - 4.7212*1e-7*ce.^2 +1.5094*1e-10*ce.^3 -1.6018*1e-14*ce.^4);
        case 'n'
            Keff = param.eps_n^param.brugg_n *(4.1253*1e-2 + 5.007*1e-4*ce - 4.7212*1e-7*ce.^2 +1.5094*1e-10*ce.^3 -1.6018*1e-14*ce.^4);
        end
end


%-------------------------------------------------------
% University of Zaragoza
% Centro Politecnico Superior
% Robotics, Perception and Real Time Group
% Author:  Javier Civera -- jcivera@unizar.es
% Date   :  2007-05-09
%-------------------------------------------------------
% Returns a camera data structure containing the calibration
%-------------------------------------------------------

function cam = initialize_cam()

% Unibrain camera, calibrated on 04/09/07
d =     0.0112;
nRows = 240;
nCols = 320;
Cx =    1.7945 / d;
Cy =    1.4433 / d;
k1=     6.333e-2;
k2=     1.390e-2;
f =     2.1735;

% % Visual Compass camera
% d =     0.0112;
% nRows = 240;
% nCols = 320;
% Cx =    1.6888/d;
% Cy =    1.4393/d;
% k1=     6.239e-2;
% k2=     1.359e-2;
% f =     2.1660;

cam.k1 =    k1;
cam.k2 =    k2;
cam.nRows = nRows;
cam.nCols = nCols;
cam.Cx =    Cx;
cam.Cy =    Cy;
cam.f =     f;
cam.dx =    d;
cam.dy =    d;
cam.model = 'two_distortion_parameters';

cam.K =     sparse( [ f/d   0     Cx;
                0  f/d    Cy;
                0    0     1] );


clear,clc

addpath(pwd);
cd cvx;
addpath(genpath(pwd));
cd ..;

load('Ns=2.mat');
% load('Ns=8.mat');

Ns = 2;

NRF = [2,3,4,6,9,12,18];

SNR_dB = 0;
SNR = 10.^(SNR_dB./10);
realization = size(H,3);
smax = length(SNR);% enable the parallel

for r = 1:length(NRF)
    for reali = 1:realization
        [ FRF, FBB ] = SDR_AltMin( Fopt(:,:,reali), NRF(r) );
        [ WRF, WBB ] = Receiver( Wopt(:,:,reali), NRF(r) );
        R(r,reali) = log2(det(eye(Ns) + SNR/Ns * pinv(WRF * WBB) * H(:,:,reali) * FRF * FBB * FBB' * FRF' * H(:,:,reali)' * WRF * WBB));
    end
end
plot(NRF,sum(R,2)/realization,'Marker','diamond','LineWidth',1.5,'Color',[0.87058824300766 0.490196079015732 0]);
grid on
hold on


function [err_norm,n] = InverseKinematics_LM_ver2(to, Target)
% Levenberg-Marquardt, Chan-Lawrence, Sugihara's modification
global uLINK

idx = FindRoute(to);
wn_pos = 1/0.3;
wn_ang = 1/(2*pi);
We = diag([wn_pos wn_pos wn_pos wn_ang wn_ang wn_ang]);
Wn = eye(length(idx));
We2 = We*We;

ForwardKinematics(1);
err = CalcVWerr(Target, uLINK(to));
Ek = err'*We*err;

for n = 1:10
  J  = CalcJacobian(idx);
  Jh = J'*We*J + Wn*(Ek + 0.002);  %Hk + wn

  gerr = J'*We*err;    %gk
  dq   = Jh \ gerr;    %new

  MoveJoints(idx, dq);
  ForwardKinematics(1);
  err = CalcVWerr(Target, uLINK(to));
  Ek2 = err'*We*err;
  if Ek2 < 1E-12
      break;
  elseif Ek2 < Ek
      Ek = Ek2;
  else
      MoveJoints(idx, -dq);  % revert
      ForwardKinematics(1);
      break,
  end
end

err_norm = norm(err);


function Sigma = UpdateCMA(X,Sigma,gen)
% Update the CMA model

%------------------------------- 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".
%--------------------------------------------------------------------------

    n = size(X,2);

    %% Calculate the CMA parameters
    mu    = 4 + floor(3*log(n));
    mu1   = floor(mu/2);
    w     = log((mu+1)/2) - log(1:mu1);
    w     = w./sum(w);
    mueff = 1./sum(w.^2);
    cs    = (mueff+2)./(n+mueff+5);
    ds    = 1 + 2*max(0,sqrt((mueff-1)./(n+1))-1) + cs;
    cc    = (4+mueff/n)./(n+4+2*mueff/n);
    c1    = 2./((n+1.3).^2+mueff);
    cmu   = min(1-c1,2*(mueff-2+1/mueff)./((n+2).^2+mueff)); % Modified
    ENI   = sqrt(n)*(1-1/4/n+1/21/n^2);

    %% Update the CMA model
    y           = (X(1:mu1,:)-repmat(Sigma.x,mu1,1))/Sigma.sigma;
    yw          = w*y;
    Sigma.x     = Sigma.x + Sigma.sigma*yw;
    Sigma.ps    = (1-cs)*Sigma.ps + sqrt(cs*(2-cs)*mueff)*Sigma.C^(-1/2)*yw';
    hs          = norm(Sigma.ps)./sqrt(1-(1-cs).^(2*(gen+1))) < (1.4+2/(n+1))*ENI;
    deltahs     = 1 - hs; % Modified
    Sigma.pc    = (1-cc)*Sigma.pc + hs*sqrt(cc*(2-cc)*mueff)*yw;
    Sigma.sigma = Sigma.sigma*exp(cs/ds*(norm(Sigma.ps)/ENI-1));
    Sigma.C     = (1-c1-cmu)*Sigma.C + c1*(Sigma.pc'*Sigma.pc+deltahs*Sigma.C) + cmu*y'*diag(w)*y;
    Sigma.C     = triu(Sigma.C) + triu(Sigma.C,1)'; % Enforce symmetry

    %% Reset the CMA model if possible
    [B,D] = eig(Sigma.C);
    diagD = diag(D);
    diagC = diag(Sigma.C);
    ConditionCov  = max(diagD) > 1e14*min(diagD);
    NoEffectCoord = any(Sigma.x==Sigma.x+0.2*Sigma.sigma*sqrt(diagC)');
    NoEffectAxis  = all(Sigma.x==Sigma.x+0.1*Sigma.sigma*sqrt(diagD(mod(gen,n)+1))*B(:,mod(gen,n)+1)');
    TolXUp        = any(Sigma.sigma*sqrt(diagC)>1e4);
    if ConditionCov || NoEffectCoord || NoEffectAxis || TolXUp
        Sigma = struct('s',[],'x',[],'sigma',0.5,'C',eye(n),'pc',0,'ps',0);
    end
end


function vesselness_wrapper = PTKComputeVesselnessFromHessianeigenvalues(hessian_eigs_wrapper, voxel_size)
    % PTKComputeVesselnessFromHessianeigenvalues. Vesselness filter for detecting blood vessels
    %
    %     PTKComputeVesselnessFromHessianeigenvalues computes a mutiscale
    %     vesselness filter based on Frangi et al., 1998. "Multiscale Vessel
    %     Enhancement Filtering". The filter returns a value at each point which
    %     in some sense representes the probability of that point belonging to a
    %     blood vessel.
    %
    %     This function takes in a PTKWraper object which can either contain a nx6
    %     matrix containing the 3 Hessian matrix eigenvalues for each of n
    %     points, or it can be an ixjxkx3 matrix representing the 3 Hessian
    %     matrix eigenvalues for an image of dimension ixjxk.
    %
    %     The output is a single vesselness value for each input point.
    %
    %     See the PTKVesselness plugin for example usage.
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    % lam1 = smallest eigenvalue, lam3 = largest eigenvalue

    vesselness_wrapper = CoreWrapper;

    % The input matrix could be a linear set of points, or an image matrix
    if ndims(hessian_eigs_wrapper.RawImage) == 2
        lam1 = hessian_eigs_wrapper.RawImage(:, 1); % smallest
        lam2 = hessian_eigs_wrapper.RawImage(:, 2);
        lam3 = hessian_eigs_wrapper.RawImage(:, 3); % biggest
    else
        lam1 = hessian_eigs_wrapper.RawImage(:,:,:,1); % smallest
        lam2 = hessian_eigs_wrapper.RawImage(:,:,:,2);
        lam3 = hessian_eigs_wrapper.RawImage(:,:,:,3); % biggest
    end

    term_1 = abs(lam2./lam3); % Ra
    alpha = 0.5;
    term_1 = 1 - exp((-term_1.^2)./(2*alpha^2));

    term_2 = abs(lam1)./sqrt(abs(lam2.*lam3)); % Rb
    beta = 0.5;
    term_2 = exp((-term_2.^2)./(2*beta^2));

    term_3 = sqrt(lam1.^2 + lam2.^2 + lam3.^2); % S

    % Frangi et al. choose a noise threshold based on the Hessian mean.
    % However, we find a fixed experimentally-chosen threshold works better, but
    % this must be scaled by the voxel size
    multiple_matrix = voxel_size'*voxel_size;
    c_scaling = 1/mean(multiple_matrix(:));
    c = 400*c_scaling;

    term_3 = 1 - exp((-term_3.^2)./(2*c^2));

    check_signs = (lam2 <= 0) & (lam3 <= 0);

    vesselness_wrapper.RawImage = term_1.*term_2.*term_3.*check_signs;
end


function Wmat = bst_shepards(destLoc, srcLoc, nbNeighbors, excludeParam, expDistance)
% BST_SHEPARDS: 3D nearest-neighbor interpolation using Shepard's weighting.
%
% USAGE:  Wmat = bst_shepards(destLoc, srcLoc, nbNeighbors=8, excludeParam=0, expDistance=2)
%
% INPUT:
%    - srcLoc       : Nx3 array of original locations, or tesselation structure (Faces,Vertices,VertConn)
%    - destLoc      : NNx3 array of locations onto original data will be interpolated, or tesselation structure (Faces,Vertices,VertConn)
%    - nbNeighbors  : Number of nearest neighbors to be considered in the interpolation (default is 8)
%    - excludeParam : If > 0, the source points that are two far away from the destination surface are ignored.
%                     Excluded points #i that have: (minDist(i) > mean(minDist) + excludeParam * std(minDist))
%                     where minDist represents the minimal distance between each source point and the destination surface
%                     If < 0, exclude the vertices that are further from the absolute distance excludeParam  (in millimeters)
%    - expDistance  : Distance exponent (if higher, influence of a value decreases faster)
%
% OUTPUT:
%    - Wmat : Interpolation matrix

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

%% ===== PARSE INPUTS =====
% Check number of arguments
if (nargin < 2)
    error('Usage: Wmat = bst_shepards(destLoc, srcLoc, nbNeighbors, excludeParam, expDistance)');
end
% Check matrices orientation
if ((size(destLoc, 2) ~= 3) || (size(srcLoc, 2) ~= 3)) && ((size(destLoc, 2) ~= 2) || (size(srcLoc, 2) ~= 2))
    error('destLoc and srcLoc must have 2 or 3 columns.');
end
% Argument: Number of neighbors for interpolation
if (nargin < 3) || isempty(nbNeighbors)
    nbNeighbors = 8;
end
% Argument: excludeParam
if (nargin < 4) || isempty(excludeParam)
    excludeParam = 0;
end
% Argument: expDistance
if (nargin < 5) || isempty(expDistance)
    expDistance = 2;
end

%% ===== SHEPARDS INTERPOLATION =====
% Allocate interpolation matrix
nDest = size(destLoc,1);
nSrc  = size(srcLoc,1);
% Maximum number of neighbors = number of electrodes
if (nbNeighbors > nSrc)
    nbNeighbors = nSrc;
end

% Find nearest neighbors
[I,dist] = bst_nearest(srcLoc, destLoc, nbNeighbors, 1);
% Square the distance matrix
dist = dist .^ 2;
% Eliminate zeros in distance matrix for stability
dist(dist == 0) = eps;

% One neighbor
if (nbNeighbors == 1)
    Wmat = sparse(1:nDest, I(:)', ones(1,nDest), nDest, nSrc);

% More complicated cases
elseif (nbNeighbors > 1)
    % Interpolation weights from Shepards method
    W = (bst_bsxfun(@minus, dist(:,nbNeighbors), dist) ./ bst_bsxfun(@times, dist(:,nbNeighbors), dist)) .^ expDistance;
    sumW = sum(W(:,1:nbNeighbors-1),2);
    % Correct zero values: points overlap exactly => take only the first point
    iZeroW = find(sumW == 0);
    if ~isempty(iZeroW)
        sumW(iZeroW) = 1;
        W(iZeroW, 1:nbNeighbors-1) = ones(length(iZeroW),1) * [1,zeros(1,nbNeighbors-2)];
    end
    W = W(:,1:nbNeighbors-1) ./ (sumW * ones(1,nbNeighbors-1));
    % Create sparse matrix with those weights
    i = repmat((1:nDest)', nbNeighbors-1, 1);
    j = reshape(I(:, 1:nbNeighbors-1), [], 1);
    Wmat = sparse(i, j, W(:), nDest, nSrc);
end

%% ===== IGNORE VERTICES TOO FAR AWAY =====
% Set to zero the weights of the vertices that are too far away from the sources
% EEG: Distance relative to the mean distance between sensors
if (excludeParam > 0)
    % Find vertices that are too far from their nearest neighbors
    iTooFarVertices = (dist(:,1) > mean(dist(:,1)) + excludeParam * std(dist(:,1)));
    % Remove them from the interpolation matrix
    Wmat(iTooFarVertices, :) = 0;
% SEEG/ECOG: Absolute distance
elseif (excludeParam < 0)
    % Find vertices that are too far from their nearest neighbors (in millimeters)
    iTooFarVertices = (sqrt(dist(:,1)) >  abs(excludeParam));
    % Remove them from the interpolation matrix
    Wmat(iTooFarVertices, :) = 0;
end


function [ Z, H, dnorm ] = deep_seminmf ( X, layers, varargin )

% Process optional arguments
pnames = { ...
    'z0' 'h0' 'bUpdateH' 'bUpdateLastH' 'maxiter' 'TolFun', 'verbose', 'bUpdateZ', 'cache' ...
};

num_of_layers = numel(layers);

Z = cell(1, num_of_layers);
H = cell(1, num_of_layers);

dflts  = {0, 0, 1, 1, 500, 1e-5, 1, 1, 1};

[z0, h0, bUpdateH, bUpdateLastH, maxiter, tolfun, verbose, bUpdateZ, cache] = ...
        internal.stats.parseArgs(pnames,dflts,varargin{:});

if  ~iscell(h0)
    for i_layer = 1:length(layers)
        if i_layer == 1
            % For the first layer we go linear from X to Z*H, so we use id
            V = X;
        else
            V = H{i_layer-1};
        end

        if verbose
            display(sprintf('Initialising Layer #%d with k=%d with size(V)=%s...', i_layer, layers(i_layer), mat2str(size(V))));
        end

        % For the later layers we use nonlinearities as we go from
        % g(H_{k-1}) to Z*H_k
        [Z{i_layer}, H{i_layer}, init_err{i_layer}] = ...
             seminmf(V, ...
                 layers(i_layer), ...
                 'maxiter', maxiter, ...
                 'bUpdateH', true, 'bUpdateZ', bUpdateZ, 'verbose', verbose);
                 %'bUpdateH', true, 'bUpdateZ', bUpdateZ, 'verbose', verbose, 'save', cache);
    end

else
    Z=z0;
    H=h0;

    if verbose
        display('Skipping initialization, using provided init matrices...');
    end
end

dnorm0 = cost_function(X, Z, H);
dnorm = dnorm0;

if verbose
    display(sprintf('#%d error: %f', 0, dnorm0));
end

%%% Error Propagation
if verbose
    display('Finetuning...');
end
H_err = cell(1, num_of_layers);

for iter = 1:maxiter
    H_err{numel(layers)} = H{numel(layers)};
    for i_layer = numel(layers)-1:-1:1
        H_err{i_layer} = Z{i_layer+1} * H_err{i_layer+1};
    end

    for i = 1:numel(layers)
        if bUpdateZ
            try
                if i == 1
                    Z{i} = X  * pinv(H_err{1});
                else
                    Z{i} = pinv(D') * X * pinv(H_err{i});
                end
            catch
                display(sprintf('Convergance error %f. min Z{i}: %f. max %f', norm(Z{i}, 'fro'), min(min(Z{i})), max(max(Z{i}))));
            end
        end

        if i == 1
            D = Z{1}';
        else
            D = Z{i}' * D;
        end

        if bUpdateH && (i < numel(layers) || (i == numel(layers) && bUpdateLastH))
            A = D * X;
            Ap = (abs(A)+A)./2;
            An = (abs(A)-A)./2;

            B = D * D';

            Bp = (abs(B)+B)./2;
            Bn = (abs(B)-B)./2;

            H{i} = H{i} .* sqrt((Ap + Bn * H{i}) ./ max(An + Bp * H{i}, 1e-10));
        end
    end

    assert(i == numel(layers));

    dnorm = cost_function(X, Z, H);

    if verbose
        display(sprintf('#%d error: %f', iter, dnorm));
    end

    assert(dnorm <= dnorm0 + 1, ...
        sprintf('Rec. error increasing! From %f to %f. (%d)', ...
        dnorm0, dnorm, iter) ...
    );

    if dnorm0-dnorm <= tolfun*max(1,dnorm0)
        if verbose
            display( ...
                sprintf('Stopped at %d: dnorm: %f, dnorm0: %f', ...
                    iter, dnorm, dnorm0 ...
                ) ...
            );
        end
        break;
    end

    dnorm0 = dnorm;
end
end

function error = cost_function(X, Z, H)
    error = norm(X - reconstruction(Z, H), 'fro');
end

function [ out ] = reconstruction( Z, H )

    out = H{numel(H)};

    for k = numel(H) : -1 : 1;
        out =  Z{k} * out;
    end

end


%SerialLink.cinertia Cartesian inertia matrix
%
% M = R.cinertia(Q) is the NxN Cartesian (operational space) inertia matrix which relates
% Cartesian force/torque to Cartesian acceleration at the joint configuration Q, and N
% is the number of robot joints.
%
% See also SerialLink.inertia, SerialLink.rne.

% MOD HISTORY
% 	4/99 add object support
% $Log: not supported by cvs2svn $
% $Revision: 1.2 $

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function Mx = cinertia(robot, q)
	J = jacob0(robot, q);
	Ji = inv(J);                %#ok<*MINV>
	M = inertia(robot, q);
	Mx = Ji' * M * Ji;


%function particles= resampleParticles(particles, Nmin, fp_debug, resample_randstream)
function particles= resampleParticles(particles, Nmin) %, resample_randstream
%
% Resample particles if their weight variance is such that N-effective
% is less than Nmin.
%

N = length(particles);
w = [particles.w];
ws = sum(w);
w  = w/ws;

Neff = 1 / sum(w .^ 2);

if Neff < Nmin
    %fprintf(fp_debug, 'Particles resampled, Neff = %d\n', Neff);
    %disp(['Particles resampled, Neff = ', Neff]);

    %[keep]    = stratified_resample(w, resample_randstream);%, resample_randstream
    [keep]    = stratified_resample(w);
    %fprintf(fp_debug, 'Keep particles: ');
    particles = particles(keep);
%     for i=1:N
%         particles(i).w= 1/N;
%         fprintf(fp_debug, '%d ', keep(i));
%
%     end
    %fprintf(fp_debug, '\n');
    %disp('Keep particles: ');disp(keep);
else
    for i=1:N
        particles(i).w= particles(i).w / ws;
    end
end


%% Anisotropic Diffusion
%
% This sample demonstrates Perona-Malik anisotropic diffusion.
%
% Sources:
%
% * <https://github.com/opencv/opencv_contrib/blob/3.3.0/modules/ximgproc/samples/filterdemo.cpp>
%

function varargout = anisodiff_demo_gui(im)
    % load source image
    if nargin < 1
        src = cv.imread(fullfile(mexopencv.root(),'test','fruits.jpg'), 'Color',true);
    elseif ischar(im)
        src = cv.imread(im, 'Color',true);
    else
        src = im;
    end

    % not too big
    if size(src,1) > 480
        src = cv.resize(src, [round(480*size(src,2)/size(src,1)), 480]);
    end

    % create the UI
    h = buildGUI(src);
    if nargout > 0, varargout{1} = h; end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    niters = round(get(h.slid, 'Value'));
    set(h.txt, 'String',sprintf('No. of time steps: %2d',niters));

    % apply filtering
    dst = cv.anisotropicDiffusion(h.src, ...
        'Alpha',1.0, 'K',0.02, 'Iterations',niters);

    % show result
    set(h.img, 'CData',dst);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    niters = 10;
    sz = size(img);
    sz(2) = max(sz(2), 250);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Anisotropic Diffusion', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+30-1]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 30 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    h.txt = uicontrol('Parent',h.fig, 'Style','text', ...
        'Position',[5 5 150 20], 'FontSize',11, ...
        'String',sprintf('No. of time steps: %2d',niters));
    h.slid = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Position',[155 5 sz(2)-155-5 20], 'Value',niters, ...
        'Min',0, 'Max',30, 'SliderStep',[1 5]./(30-0));

    % hook event handlers, and trigger default start
    set(h.slid, 'Callback',{@onChange,h}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h);
end


function [AF,BF]=framebounds(F,varargin);
%FRAMEBOUNDS  Frame bounds
%   Usage: fcond=framebounds(F);
%          [A,B]=framebounds(F);
%          [...]=framebounds(F,Ls);
%
%   `framebounds(F)` calculates the ratio $B/A$ of the frame bounds of the
%   frame given by *F*. The length of the system the frame bounds are
%   calculated for is given by `L=framelength(F,1)`.
%
%   `framebounds(F,Ls)` additionally specifies a signal length for which
%   the frame should work. The actual length used is `L=framelength(F,Ls)`.
%
%   `[A,B]=framebounds(F)` returns the frame bounds *A* and *B* instead of
%   just their ratio.
%
%
%   'framebounds` accepts the following optional parameters:
%
%     'fac'        Use a factorization algorithm. The function will throw
%                  an error if no algorithm is available.
%
%     'iter'       Call `eigs` to use an iterative algorithm.
%
%     'full'       Call `eig` to solve the full problem.
%
%     'auto'       Choose the `fac` method if possible, otherwise
%                  use the `full` method for small problems and the
%                  `iter` method for larger problems.
%                  This is the default.
%
%     'crossover',c
%                  Set the problem size for which the 'auto' method
%                  switches between `full` and `iter`. Default is 200.
%
%   The following parameters specifically related to the `iter` method:
%
%     'tol',t      Stop if relative residual error of eighs is less than the
%                  specified tolerance. Default is 1e-9
%
%     'maxit',n    Do at most n iterations in eigs. Default is 100.
%
%     'pcgtol',t   Stop if relative residual error of pcg is less than the
%                  specified tolerance. Default is 1e-6
%
%     'pcgmaxit',n Do at most n iterations in pcg. Default is 150.
%
%     'p',p        The number of Lanzcos basis vectors to use.  More vectors
%                  will result in faster convergence, but a larger amount of
%                  memory.  The optimal value of `p` is problem dependent and
%                  should be less than *L*.  The default value chosen
%                  automatically by eigs.
%
%     'print'      Display the progress.
%
%     'quiet'      Don't print anything, this is the default.
%
%   See also: frame, framered

complainif_notenoughargs(nargin,1,'FRAMEBOUNDS');
complainif_notvalidframeobj(F,'FRAMEBOUNDS');

% We handle the container frames first
  if strcmp(F.type,'fusion')
      AF=0;
      BF=0;
      for ii=1:F.Nframes
          [A,B]=framebounds(F.frames{ii},varargin{:});
          AF=AF+(A*F.w(ii)).^2;
          BF=BF+(B*F.w(ii)).^2;
      end;
      AF=sqrt(AF);
      BF=sqrt(BF);

      return;
  end;

  if strcmp(F.type,'tensor')
    AF=1;
    BF=1;
    for ii=1:F.Nframes
      [A,B]=framebounds(F.frames{ii},varargin{:});
      AF=AF*A;
      BF=BF*B;
    end;

    return;
  end;

  definput.keyvals.Ls=1;
  definput.keyvals.maxit=100;
  definput.keyvals.tol=1e-9;
  definput.keyvals.pcgmaxit=150;
  definput.keyvals.pcgtol=1e-6;
  definput.keyvals.crossover=200;
  definput.keyvals.p=[];
  definput.flags.print={'quiet','print'};
  definput.flags.method={'auto','fac','iter','full'};

  [flags,kv]=ltfatarghelper({'Ls'},definput,varargin);

  F=frameaccel(F,kv.Ls);
  L=F.L;

  % Default values, works for the pure frequency transforms.
  AF=1;
  BF=1;

  % Simple heuristic: If F.g is defined, the frame uses windows.
  if isfield(F,'g')
      if isempty(F.g)
          error('%s: No analysis frame is defined.', upper(mfilename));
      end;
      g=F.g;
      op    = @frana;
      opadj = @frsyn;
  end;

  F_isfac = isfield(F,'isfac') && F.isfac;

  if flags.do_fac && ~F_isfac
    error('%s: The type of frame has no factorization algorithm.',upper(mfilename));
  end;

  if (flags.do_auto && F_isfac) || flags.do_fac
    switch(F.type)
     case 'gen'
      V=svd(g);
      AF=min(V)^2;
      BF=max(V)^2;
     case {'dgt','dgtreal'}
      [AF,BF]=gabframebounds(g,F.a,F.M,L);
     case {'dwilt','wmdct'}
      [AF,BF]=wilbounds(g,F.M,L);
     case {'filterbank','ufilterbank'}
      [AF,BF]=filterbankbounds(g,F.a,L);
     case {'filterbankreal','ufilterbankreal'}
      [AF,BF]=filterbankrealbounds(g,F.a,L);
     case 'fwt'
      [AF,BF]=wfbtbounds({g,F.J,'dwt'},L);
     case 'wfbt'
      [AF,BF]=wfbtbounds(g,L);
     case 'ufwt'
      [AF,BF]=wfbtbounds({g,F.J,'dwt'},L,F.flags.scaling);
     case 'uwfbt'
      [AF,BF]=wfbtbounds(g,L,F.flags.scaling);
     case 'wpfbt'
      [AF,BF]=wpfbtbounds(g,L,F.flags.interscaling);
     case 'uwpfbt'
      [AF,BF]=wpfbtbounds(g,L,F.flags.interscaling,F.flags.scaling);
    end;
  end;

  if (flags.do_auto && ~F_isfac && F.L>kv.crossover) || flags.do_iter

    if flags.do_print
      opts.disp=1;
    else
      opts.disp=0;
    end;
    opts.isreal = F.realinput;
    opts.maxit  = kv.maxit;
    opts.tol    = kv.tol;
    opts.issym  = 0;
    if ~isempty(kv.p)
       opts.p      = kv.p;
    end

    pcgopts.maxit = kv.pcgmaxit;
    pcgopts.tol = kv.pcgtol;

    % Upper frame bound
    frameop = @(x) F.frsyn(F.frana(x));
    BF = real(eigs(frameop,L,1,'LM',opts));

    % Lower frame bound
    frameop2 = @(x) F.frsyn(F.frana(x));
    invfrop = @(x) pcgwrapper(frameop2,x,pcgopts.tol,pcgopts.maxit);

    % Test convergence of pcg
    test = randn(L,1);
    if ~F.realinput, test = test +1i*randn(L,1); end
    [~,flag] = invfrop(test);

    % If PCG converges, estimate the smallest eigenvalue, otherwise assume
    % AF = 0;
    if ~flag
        AF = real(eigs(invfrop,L,1,'SM',opts));
    else
        AF = 0;
    end

  end;

  if (flags.do_auto && ~F_isfac && F.L<=kv.crossover) || flags.do_full
    % Compute thee transform matrix.
    bigM=opadj(F,op(F,eye(L)));

    D=eig(bigM);

    % Clean the eigenvalues, we know they are real
    D=real(D);
    AF=min(D);
    BF=max(D);
  end;

  if nargout<2
    % Avoid the potential warning about division by zero.
    if AF==0
      AF=Inf;
    else
      AF=BF/AF;
    end;
  end;

end

% In order to mute pprinting out pcg progress
function [y,flag]=pcgwrapper(varargin)
[y,flag,dummyrelres,dummyiter] = pcg(varargin{:});
end

% The function has been written in this way, because Octave (at the time
% of writing) does not accept additional parameters at the end of the
% line of input arguments for eigs
function y=afun(x,F_in,op_in,opadj_in)
  persistent F;
  persistent op;
  persistent opadj;

  if nargin>1
    F     = F_in;
    op    = op_in;
    opadj = opadj_in;
  else
    y=opadj(F,op(F,x));
  end;

end


function [ xmin, ixmin ] = i4row_min ( m, n, x )

%*****************************************************************************80
%
%% I4ROW_MIN returns the minimums of rows of an I4ROW.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer X(M,N), the array to be examined.
%
%    Output, integer XMIN(M), the minimums of the rows of X.
%
%    Output, integer IXMIN(M); IXMIN(I) is the column of X in which
%    the minimum for row I occurs.
%
  for i = 1 : m

    ixmin(i) = 1;
    xmin(i) = x(i,1);
    for j = 2 : n
      if ( x(i,j) < xmin(i) )
        ixmin(i) = j;
        xmin(i) = x(i,j);
      end
    end

  end

  return
end


 function [err, sn, kernel] = nufft1_error(om, N1, J1, K1, kernel, sn)
%function [err, sn, kernel] = nufft1_error(om, N1, J1, K1, kernel, sn)
%
% Compute worst-case error for each input frequency for 1D NUFFT
% using specified (inline) `ad hoc' kernel function (e.g., gaussian).
% This is worst-case for a unit-norm signal of length N1.
% in
%	om	[M,1]	digital frequency omega in radians
%	N1		signal length
%	J1		# of neighbors used per frequency location
%	K1		FFT size (should be > N1)
%	kernel		inline kernel function, args (k,J)
%				(or choose from built-in examples - see code!)
%	sn		optional scaling factors (otherwise do-no-harm)
% out
%	err	[M,1]	worst-case error over unit-norm signals
%	sn	[N,1]	scaling factors
%
% examples for kernel:
% linear:		inline('(1 - abs(k/(J/2))) .* (abs(k) < J/2)', 'k', 'J')
% truncated diric:	inline('sinc(k) .* (abs(k) < J/2)', 'k', 'J')
%
% Copyright 2001-12-7, Jeff Fessler, The University of Michigan

% if no arguments, give an example, comparing triangular kernel to min-max
if nargin < 4
	help(mfilename)
	N = 2^7; K = 2*N; gam = 2*pi/K;
	Jlist = [2:10]';
	om = gam * linspace(0,1,101);

	err.linear	= zeros(size(Jlist));
	err.minmaxu	= zeros(size(Jlist));
	err.minmax2	= zeros(size(Jlist));
	err.minmaxo	= zeros(size(Jlist));
	err.minmaxk	= zeros(size(Jlist));	% kaiser sn's
	err.gauss_zn	= zeros(size(Jlist));
	err.gauss_ft	= zeros(size(Jlist));
	err.kaiser	= zeros(size(Jlist));

	for ii=1:length(Jlist)
		J = Jlist(ii);
		printf('J=%d', J)
		err.linear(ii) = ...
			max(nufft1_error(om, N, J, K, 'linear'));
		err.minmaxu(ii) = ...
			max(nufft1_error(om, N, J, K, 'minmax,uniform'));
		err.minmax2(ii) = ...
			max(nufft1_error(om, N, J, K, 'minmax,best,L=2'));
		err.minmaxo(ii) = ...
			max(nufft1_error(om, N, J, K, 'minmax,best'));
		err.gauss_zn(ii) = ...
			max(nufft1_error(om, N, J, K, 'gauss'));
		err.gauss_ft(ii) = ...
			max(nufft1_error(om, N, J, K, 'gauss', 'ft'));
		[tmp, sn] = nufft1_error(om, N, J, K, 'kaiser', 'ft');
		err.kaiser(ii) = max(tmp);
		err.minmaxk(ii) = ...
			max(nufft1_err_mm(om, N, J, K, 'qr', sn));
	end
	clf, semilogy(Jlist, err.linear, 'g-x', ...
		Jlist, err.minmaxu, 'c-+', ...
		Jlist, err.gauss_zn, 'b-*', ...
		Jlist, err.gauss_ft, 'b-o', ...
		Jlist, err.minmax2, 'r-^', ...
		Jlist, err.kaiser, 'm->', ...
		Jlist, err.minmaxo, 'y-<', ...
		Jlist, err.minmaxk, 'w-o'), axis tight
	xlabel J, ylabel 'worst-case error'
	legend('linear', 'min-max, uniform', ...
		'gaussian (zn)', 'gaussian (FT)', ...
		'min-max, best L=2', 'kaiser', 'min-max, optimized', ...
		'min-max, kaiser s', 3)
	clear err
return
end

%
% kernel selection
%
if ischar(kernel)

	% min-max interpolators (uniform, best, etc., see nufft1_err_mm.m)
	if strncmp(kernel, 'minmax,', 7)
		type = kernel(8:end);	% uniform or best or best,L=2 etc.
		[err, sn] = nufft1_err_mm(om, N1, J1, K1, 'qr', type);
		return

	% cos^3-tapered dirichlet
	elseif streq(kernel, 'cos3diric')
		kernel = 'diric(2*pi*k/J, J) .* cos((2*pi*k/J)/2).^3';

	% Dirichlet (truncated)
	elseif streq(kernel, 'diric')
		kernel = 'nufft_diric(k,%d,%d,1) .* (abs(k) < J/2)';
		kernel = sprintf(kernel, N1, N1);

	% gaussian (truncated) with previously numerically-optimized width
	elseif streq(kernel, 'gauss')
		if isvar('sn') & ischar(sn) & streq(sn, 'ft')
			stype = 'ft';
		else
			stype = 'zn';
		end
		[dummy, kernel, kernel_ft] = nufft_best_gauss(J1, K1/N1, stype);
		kernel_ft = inline(kernel_ft, 't');

	% kaiser-bessel with previously numerically-optimized shape
	elseif streq(kernel, 'kaiser')
		[kernel, kb_a, kb_m] = kaiser_bessel('string', J1, 'best', 0, K1/N1);
		kernel_ft = kaiser_bessel_ft('inline', J1, kb_a, kb_m, 1);

	% linear interpolation via triangular function (possibly "wide"!)
	elseif streq(kernel, 'linear')
		kernel = '(1 - abs(k/(J/2))) .* (abs(k) < J/2)';

	else
		error(sprintf('unknown kernel "%s"', kernel))
	end

	kernel = inline(kernel, 'k', 'J');

elseif ~streq('inline', class(kernel))
	error 'need inline kernel'
end

gam = 2*pi/K1;

if 0
	%	plot interpolator
	k = linspace(-J/2-1,J/2+1,101);
	clf, subplot(221), plot(k, kernel(k, J))
	xlabel k, ylabel kernel(k), axis tight, grid
end

%
% Compute scaling factors using the "do no harm" strategy.
% This may not be optimal; analytical FT could also be reasonable.
%
if ~isvar('sn')
	sn = 1 ./ nufft_interp_zn(0, N1, J1, K1, kernel);	% [N]
%	sn = 1 ./ mean(nufft_interp_zn([0 1/2], N1, J1, K1, kernel), 2); % alt

elseif isa(sn, 'inline')
	n = [0:(N1-1)]'-(N1-1)/2;
	sn = 1 ./ sn(n/K1);		% [N]

% trick to use Gaussian FT scaling factors
elseif ischar(sn) & streq(sn, 'ft') & isvar('kernel_ft')
	n = [0:(N1-1)]'-(N1-1)/2;
	sn = 1 ./ kernel_ft(n/K1);		% [N]
	if 0 & J1 > 2
		sn_zn = 1 ./ nufft_interp_zn(0, N1, J1, K1, kernel);	% [N]
		clf, plot(n, [sn reale(sn_zn)])
		keyboard
	end

% trick to use Kaiser-Bessel FT scaling factors
elseif isstruct(sn) & streq(sn.type, 'kaiser')
	n = [0:(N1-1)]'-(N1-1)/2;
	sn = 1 ./ kaiser_bessel_ft(n/K1, J1, sn.alpha, sn.m, 1);

else
	error 'unsupport scaling factors type'
end

%
% interpolator worst-case error for each frequency (scaled by 1/sqrt(N)),
% from equations (46)-(47) in Fessler&Sutton NUFFT paper, T-SP, Feb. 2003
%
zn = nufft_interp_zn(om/gam, N1, J1, K1, kernel);		% [N,M]
err = sqrt(mean(abs(spdiag(sn) * zn - 1).^2, 1))';	% [M]


function [mi3] = oz2mi3(oz)
% Convert volume from US liquid ounces to cubic miles.
% Chad Greene 2012
mi3 = oz*7.0950670584e-15;


%   AUTHOR:
%       Boguslaw Obara, http://boguslawobara.net/
%% Read image
im = imread('triple.tif');
im = im<10;
%% Skeleton
im = bwmorph(im, 'thin', inf);
%% Hit or Miss
out1 = BOHitOrMiss(im, 'end');
out2 = BOHitOrMiss(im, 'triple');
%% Plot
ims = im;
ims = ims + 2*out1;
ims = ims + 8*out2;
imagesc(ims);
%%


function pcut = addEvalVariableCuts(p)

pcut = p;
if ~isempty(p.evalMap)
    pcut = emptyNumericalModel;
    for i = 1:length(p.evalMap)
        y = p.evalVariables(i);
        x = p.evalMap{i}.variableIndex;
        xL = p.lb(x);
        xU = p.ub(x);

        % Generate a convex hull polytope
        if xL<xU
            if ~isempty(p.evalMap{i}.properties.convexhull)
                % A convex hull generator function is available!
                % Might be able to reuse hull from last run node
                if isfield(p.evalMap{i},'oldhull') && isequal(p.evalMap{i}.oldhull.xL,xL) && isequal(p.evalMap{i}.oldhull.xU,xU)
                    [Ax,Ay,b,K] = getOldHull(p,i);
                else
                    [Ax,Ay,b,K,p] = updateHull(xL,xU,p,i);
                    if isempty(Ax)
                        % Operator bounder does not cover this interval so
                        % use the sample-based instead
                        [Ax,Ay,b,K,p] = convexhullSampled(xL,xU,p,i);
                    end
                end
            else
               [Ax,Ay,b,K] = convexhullSampled(xL,xU,p,i);
            end
            if ~isempty(b)
                if isempty(K)
                    % Compatibility with old code
                    K.f = 0;
                    K.l = length(b);
                end
                F_structemp = zeros(size(b,1),length(p.c)+1);
                F_structemp(:,1+y) = -Ay;
                F_structemp(:,1+x) = -Ax;
                F_structemp(:,1) = b;
                localModel = createNumericalModel(F_structemp,K);
                pcut = mergeNumericalModels(pcut,localModel);
            end
        end
    end

    pcut = mergeNumericalModels(p,pcut);
end

function [Ax,Ay,b,K] = getOldHull(p,i);

Ax = p.evalMap{i}.oldhull.Ax;
Ay = p.evalMap{i}.oldhull.Ay;
b = p.evalMap{i}.oldhull.b;
K = p.evalMap{i}.oldhull.K;

function [Ax,Ay,b,K,p] = updateHull(xL,xU,p,i);

try
    [Ax,Ay,b,K]=feval(p.evalMap{i}.properties.convexhull,xL,xU, p.evalMap{i}.arg{2:end-1});
catch
    [Ax,Ay,b]=feval(p.evalMap{i}.properties.convexhull,xL,xU, p.evalMap{i}.arg{2:end-1});
    if ~isempty(Ax)
        problem = find(any(isinf([Ax Ay b]),2) | any(isnan([Ax Ay b]),2));
        Ax(problem,:) = [];
        Ay(problem,:) = [];
        b(problem) = [];
    end
    K = [];
end
p = saveOldHull(xL,xU,Ax,Ay,b,K,p,i);

function p = saveOldHull(xL,xU,Ax,Ay,b,K,p,i)
p.evalMap{i}.oldhull.xL = xL;
p.evalMap{i}.oldhull.xU = xU;
p.evalMap{i}.oldhull.Ax = Ax;
p.evalMap{i}.oldhull.Ay = Ay;
p.evalMap{i}.oldhull.b = b;
p.evalMap{i}.oldhull.K = K;

function [Ax,Ay,b,K,p] = convexhullSampled(xL,xU,p,i)

if length(xL)>1
    Ax = [];
    Ay = [];
    b = [];
    K = [];
    return
end
% sample function
z = linspace(xL,xU,100);

if isequal(p.evalMap{i}.fcn,'power_internal2')
    % Special code for automatically converting sigmonial
    % terms to be solvable with bmibnb
    fz = feval(p.evalMap{i}.fcn,z,p.evalMap{i}.arg{2});

else
    arg = p.evalMap{i}.arg;
    arg{1} = z;
    fz = real(feval(p.evalMap{i}.fcn,arg{1:end-1}));
    % end
    [minval,minpos] = min(fz);
    [maxval,maxpos] = max(fz);
    xtestmin = linspace(z(max([1 minpos-5])),z(min([100 minpos+5])),100);
    xtestmax = linspace(z(max([1 maxpos-5])),z(min([100 maxpos+5])),100);
    arg{1} = xtestmin;
    fz1 = real(feval(p.evalMap{i}.fcn,arg{1:end-1}));
    arg{1} = xtestmax;
    fz2 = real(feval(p.evalMap{i}.fcn,arg{1:end-1}));
    z = [z(:);xtestmin(:);xtestmax(:)];
    fz = [fz(:);fz1(:);fz2(:)];
    [z,sorter] = sort(z);
    fz = fz(sorter);
    [z,ii,jj]=unique(z);
    fz = fz(ii);
end

[Ax,Ay,b] = convexhullFromSampled(z,fz,xL,xU);
K = [];

p = saveOldHull(xL,xU,Ax,Ay,b,K,p,i);


% SOLVE_MAXWELL_EIG_MIXED1: Solve the Maxwell eigenvalue problem with a mixed formulation, and a B-spline discretization.
%
% Example to solve the problem
%
%    curl (1/mu(x) curl (u)) = lambda (epsilon(x) u)   in Omega = F((0,1)^n)
%          div (epsilon(x) u) = 0                       in Omega
%       (1/mu(x) curl(u)) x n = 0                       on Gamma_N
%                       u x n = 0                       on Gamma_D
%
% with the variational mixed formulation
%
%    \int (1/mu(x) curl(u) curl(v)) + \int (epsilon(x) v grad(p))
%                = lambda \int (epsilon(x) u v),   \forall v \in H_0(curl),
%                                     \int (epsilon(x) u grad(q)) = 0,
%                                                  \forall q \in H^1_0.
%
% USAGE:
%
%  [geometry, msh, space, sp_mul, eigv, eigf] =
%                  solve_maxwell_eig_mixed1 (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - nmnn_sides:   sides with Neumann boundary condition (may be empty)
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - c_elec_perm:  electric permittivity (epsilon in the equation)
%    - c_magn_perm:  magnetic permeability (mu in the equation)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree of the spline functions.
%    - regularity: continuity of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry: geometry structure (see geo_load)
%  msh:      mesh object that defines the quadrature rule (see msh_cartesian)
%  space:    space object that defines the discrete functions (see sp_vector)
%  sp_mul:   space object for the multiplier (see sp_scalar)
%  eigv:     the computed eigenvalues
%  eigf:     degrees of freedom of the associated eigenfunctions
%
% See also EX_MAXWELL_EIG_MIXED1_SQUARE for an example
%
% Copyright (C) 2010, 2011, 2015 Rafael Vazquez
%
%    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/>.

function [geometry, msh, space, sp_mul, eigv, eigf] = ...
              solve_maxwell_eig_mixed1 (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

% Construct geometry structure
geometry = geo_load (geo_name);

[knots, zeta] = kntrefine (geometry.nurbs.knots, nsub-1, degree, regularity);
[knots_hcurl, degree_hcurl] = knt_derham (knots, degree, 'Hcurl');

% Construct msh structure
rule     = msh_gauss_nodes (nquad);
[qn, qw] = msh_set_quad_nodes (zeta, rule);
msh      = msh_cartesian (zeta, qn, qw, geometry);

% Construct the space structures for the field and the Lagrange multiplier
scalar_spaces = cell (msh.ndim, 1);
for idim = 1:msh.ndim
  scalar_spaces{idim} = sp_bspline (knots_hcurl{idim}, degree_hcurl{idim}, msh);
end
space = sp_vector (scalar_spaces, msh, 'curl-preserving');
sp_mul = sp_bspline (knots, degree, msh);

% Assemble the matrices
if (msh.rdim == 2)
  invmu = @(x,y) 1./c_magn_perm (x,y);
elseif (msh.rdim == 3)
  invmu = @(x,y,z) 1./c_magn_perm (x,y,z);
end

stiff_mat = op_curlu_curlv_tp (space, space, msh, invmu);
mass_mat  = op_u_v_tp (space, space, msh, c_elec_perm);
saddle_mat = op_v_gradp_tp (space, sp_mul, msh, c_elec_perm);

% Apply homogeneous Dirichlet boundary conditions
drchlt_dofs = []; drchlt_dofs_mul = [];
for iside = 1:numel (drchlt_sides)
  drchlt_dofs = union (drchlt_dofs, space.boundary(drchlt_sides(iside)).dofs);
  drchlt_dofs_mul = union (drchlt_dofs_mul, sp_mul.boundary(drchlt_sides(iside)).dofs);
end
if (isempty (drchlt_dofs_mul))
  drchlt_dofs_mul = sp_mul.ndof;
end

int_dofs = setdiff (1:space.ndof, drchlt_dofs);
int_dofs_mul = setdiff (1:sp_mul.ndof, drchlt_dofs_mul);

% Solve the eigenvalue problem
stiff_mat  = stiff_mat (int_dofs, int_dofs);
mass_mat   = mass_mat (int_dofs, int_dofs);
saddle_mat = saddle_mat (int_dofs_mul, int_dofs);

A = [stiff_mat, saddle_mat.'; ...
     saddle_mat, sparse(numel(int_dofs_mul),numel(int_dofs_mul))];
M = [mass_mat, sparse(numel(int_dofs), numel(int_dofs_mul)); ...
     sparse(numel(int_dofs_mul), numel(int_dofs)+numel(int_dofs_mul))];

eigf = zeros (space.ndof + sp_mul.ndof, numel(int_dofs) + numel(int_dofs_mul));
[eigf([int_dofs space.ndof+int_dofs_mul], :), eigv] = eig (full(A), full(M));
eigv = diag (eigv);

end

%!demo
%! ex_maxwell_eig_mixed1_square

%!demo
%! ex_maxwell_eig_mixed1_Lshaped

%!demo
%! ex_maxwell_eig_mixed1_cube

%!demo
%! ex_maxwell_eig_mixed1_thick_L


function J = J_func_pTop(q, t, r, b, nv)
len = size(r, 1);
R = q2R(q);
J = 0;
for i = 1 : len
   rr = r(i, :).';
   bb = b(i, :).';
   J = J + 1 / len * (nv(i, :) * (R * rr + t - bb))^2;
end
end


function X = coeffs2vals( X )
%COEFFS2VALS Convert an array of Chebyshev--Fourier--Fourier of
%            coefficients to an array of values.
%
%   COEFFS2VALS( CFS ) computes the values on a tensor-product doubled-up
%   spherical grid of a function that is represented by a
%   Chebyshev-Fourier-Fourier expansion with expansion coefficients CFS.
%
% See also CHEBFUN.COEFFS2VALS, VALS2COEFFS.

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if isempty( X )
    return
end

[m, n, p] = size( X );

% Old slow approach:
% for k = 1:p
%     X(:,:,k) = chebtech2.coeffs2vals( X(:,:,k) );
%     X(:,:,k) = trigtech.coeffs2vals( X(:,:,k).' ).';
% end
% for j = 1:n
%     vj = reshape( X(:,j,:), m, p );
%     vj = trigtech.coeffs2vals( vj.' ).';
%     X(:,j,:) = reshape( vj, m, 1, p );
% end

% Faster approach, but code less readable:
if ( m > 1 )
    X(2:m-1, :, :) = X(2:m-1, :, :)/2;
    X = fft( vertcat(X, X(m-1:-1:2,:,:)), [], 1 );
    X = X(m:-1:1, :, :);
end

scl_p = (n*p)*even_odd_fix( p );
scl_n = even_odd_fix( n );
Enp = reshape(scl_n.'*scl_p, [1 n p]);

X = ifft(ifft(ifftshift(ifftshift(X.*repmat( Enp, m, 1, 1),2),3), [], 2),[],3);

end

function scl = even_odd_fix( n )

if ( mod(n, 2) )
    scl = (-1).^(-(n-1)/2:(n-1)/2);
else
    scl = (-1).^((-n/2):(n/2-1));
end

end


function y = logsumexp(x, dim)
% compute log(sum(exp(x),dim)) while avoiding numerical underflow
xmax = max(x, [], dim);
y    = xmax + log(sum(exp(bsxfun(@minus, x, xmax)), dim));
ind  = find(~isfinite(xmax));
if ~isempty(ind)
    y(ind) = xmax(ind);
end


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

% Script chap::2::script
% Density CEV Model

t = 10;                                      % maturity
f0 = 0.03;                                  % spot value

figname = 'Risk Neutral Density - CEV';

x = 0:0.001:2;                            % range

sigma_base = 0.2;                           % volatility base scenario
beta_base = 0.5;                            % CEV exponent base scenario

y_base = pcev(t,x,f0,sigma_base, beta_base);
legendname_base = 'Base';
%% Changing sigma_base
sigma_low = 0.15;
sigma_high = 0.25;

y_low = pcev(t,x,f0,sigma_low, beta_base);
y_high = pcev(t,x,f0,sigma_high, beta_base);

legendname_low = 'Changing \sigma low value';
legendname_high = 'Changing \sigma high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);

%% Changing beta_base
beta_low = 0.3;
beta_high = 0.7;

y_low = pcev(t,x,f0,sigma_base, beta_low);
y_high = pcev(t,x,f0,sigma_base, beta_high);

legendname_low = 'Changing \beta low value';
legendname_high = 'Changing \beta high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);


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

% Script chap::2::script
% Density Heston Model
%
%   uses the characteristic function of the Heston model
%
t = 10;                             % maturity
a = 600;                            % spot value
N = 512;                            % number of grid points
x = ( (0:N-1) - N/2 ) / a;          % range

figname = 'Risk Neutral Density - Heston-Hull-White';

vInst_base = 0.02;                  % instantanuous variance of base parameter set
vLong_base = 0.02;                  % long term variance of base parameter set
kappa_base = 0.1;                   % mean reversion speed of variance of base parameter set
omega_base = 0.2;                   % volatility of variance of base parameter set
rho_base = 0;                       % correlation of base parameter set

lambda_base = 0.1;
eta_base = 0.02;

icurveData = [0.999884333380315;0.996803132736937;0.993568709230647;0.990285301195274;0.986945903402709;0.983557350486521;0.980185549124449;0.976782934344041;0.973361992614499;0.969976793305220;0.966616749933289;0.962914317958160;0.959904777446077;0.920091903961326;0.882870065420196;0.847186544281939;0.812742515687365;0.779459552415061;0.747152463119429;0.715745016074346;0.685138723808460;0.655753392359115;0.627333845297308;0.599226698198774;0.572763319281569;0.547259133751455;0.523441996253080;0.499646068368557;0.477507905873099;0.456481811728753;0.436385788738282;0.417350253831050;0.399187111819286;0.381865611666566;0.365435617455498;0.349786183601181;0.334806921914717;0.320548897004994;0.306983265264429;0.294081800917050;0.282443547729164;0.269929224010243];
icurveDates = [734472;734501;734534;734562;734591;734622;734653;734683;734713;734744;734775;734807;734836;735202;735567;735931;736298;736663;737028;737393;737758;738125;738489;738854;739219;739585;739949;740316;740680;741046;741411;741776;742140;742507;742872;743237;743602;743967;744334;744698;745063;745429];
icurveInterpMethod = 'spline';
icurveType = 'Discount';
icurveSettle = 734471;
irdc = IRDataCurve(icurveType,icurveSettle,icurveDates,icurveData);
% cf (characteristic function) for the heston hull white model
f = @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0) .* cf_hullwhite(x+1i,t,0,lambda_base,eta_base,irdc);

legendname_base = 'Base parameter set';
y_base = fftdensity(f,a,N);         % density calculated from cf for base parameter set
%% Changing lambda_base
lambda_low = .005;                    % changing parameter (low value)
lambda_high = .5;                   % changing parameter (high value)

f_low = @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_low,eta_base,irdc);
f_high = @(x)  cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_high,eta_base,irdc);

y_low = fftdensity(f_low,a,N);
y_high = fftdensity(f_high,a,N);

legendname_low = 'Changing \lambda low value';
legendname_high = 'Changing \lambda high value';

% output density as figure
createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);
%% Changing eta_base
eta_low = .01;
eta_high = .03;

f_low =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_low,irdc);
f_high =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_high,irdc);

y_low = fftdensity(f_low,a,N);
y_high = fftdensity(f_high,a,N);

legendname_low = 'Changing \eta low value';
legendname_high = 'Changing \eta high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);

%% Changing curve_base
zero = -log(icurveData)./(icurveDates-icurveSettle)*360 - 0.01;
icurveData_low = exp(-zero .*(icurveDates-icurveSettle)/360);
zero = -log(icurveData)./(icurveDates-icurveSettle)*360 + 0.01;
icurveData_high = exp(-zero .*(icurveDates-icurveSettle)/360);

irdc_low = IRDataCurve(icurveType,icurveSettle,icurveDates,icurveData_low);
irdc_high = IRDataCurve(icurveType,icurveSettle,icurveDates,icurveData_high);

f_low =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_base,irdc_low);
f_high =  @(x) cf_heston(x,vInst_base,vLong_base,kappa_base,omega_base,rho_base,t,0).* cf_hullwhite(x+1i,t,0,lambda_base,eta_base,irdc_high);

y_low = fftdensity(f_low,a,N);
y_high = fftdensity(f_high,a,N);

legendname_low = 'Changing curve parallel shift - low value';
legendname_high = 'Changing curve parallel shift - high value';

createfigure_density(x,y_base,y_low,y_high,...
    figname, legendname_base,legendname_low,legendname_high);


function mesh = genMesh3DType2(domain, nx, ny, nz)

%% Usage: mesh structure of a uniform cubic partition
%
% INPUTS:
% domain --- cubic domain = [xmin, xmax, ymin, ymax, zmin, zmax].
% nx --- the number of uniform partition in x direction.
% ny --- the number of uniform partition in y direction.
% nz --- the number of uniform partition in z direction.
%
% option.meshinfo --- basic (default): generate only p and t.
%                     all: enriched mesh information
%
% OUTPUTS:
% mesh --- a struct data contains mesh information.
%
% Last Modified: 08/07/2020 by Xu Zhang
%
%                A8-------------------A7        The Cube is divided into
%                /|                   /|        six congruent tetrahedrons
%               / |                  / |
%              /  |                 /  |
%             /   |                /   |        (1) A1-A2-A3-A7
%            /    |               /    |        (2) A1-A6-A2-A7
%          A5-----+-------------A6     |        (3) A1-A5-A6-A7
%           |     |             |      |        (4) A1-A8-A5-A7
%           |     |             |      |        (5) A1-A4-A8-A7
%           |     |             |      |        (6) A1-A3-A4-A7
%           |     A4------------+------A3
%           |     /             |      /
%           |    /              |     /
%           |   /               |    /
%           |  /                |   /
%           | /                 |  /
%           |/                  | /
%          A1-------------------A2
%
%% 1. Generate basic mesh info: p t
[p,T] = genMesh3DRectPT(domain, nx, ny, nz);
c1 = zeros(5,4);
c1(1,:) = [1,2,4,5];
c1(2,:) = [2,7,5,6];
c1(3,:) = [3,2,4,7];
c1(4,:) = [5,7,4,8];
c1(5,:) = [2,4,5,7];
c2 = zeros(5,4);
c2(1,:) = [1,6,8,5];
c2(2,:) = [1,3,6,2];
c2(3,:) = [8,6,3,7];
c2(4,:) = [1,8,3,4];
c2(5,:) = [1,3,8,6];

CBxy = zeros(nx,ny);
CBxy(1:2:nx,2:2:ny)=1;
CBxy(2:2:nx,1:2:ny)=1;
CB = repmat(CBxy,1,1,nz);
CB(:,:,1:2:nz) = repmat(1-CBxy,1,1,length(1:2:nz));
CB = reshape(CB,[],1);
tid1 = find(CB==1); tid2 = find(CB==0);

t = T(:,c1(1,:));
t(tid2,:) = T(tid2,c2(1,:));
for i = 2:5
    tmp = T(:,c1(i,:));
    tmp(tid2,:) = T(tid2,c2(i,:));
    t = [t;tmp];
end

mesh = struct('p',p,'t',t,'T',T);


% DEMOILFGPLVM9 Oil data with three dimensions and variational sparse approximation.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'oil';
experimentNo = 9;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('dtcvar');
options.kern = {'rbf', 'bias', 'whitefixed'};
options.optimiser = 'scg';
latentDim = 3;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);
model.kern.comp{3}.variance = 1e-4;
% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display them.
lvmScatterPlot(model, lbls,[],[1,2]); % default [1,2], change to plot
                                      % a different pair of lantent dims

% compute the nearest neighbours errors in latent space.
errors = fgplvmNearestNeighbour(model, lbls);


% generates a rounded rectangle as a test track for tracking control applications

l1 = 100;
l2 = 20;
lstep = 2;
r = 12.5;
phistep = 0.1;
phi = phistep:phistep:(pi/2);
b0 = 10;

% build track snippets
trackparts{1} = [lstep:lstep:l1; zeros(1, l1/lstep)];
trackparts{2} = [lstep:lstep:l2; zeros(1, l2/lstep)];
trackparts{3} = [r*cos(phi+3/2*pi); r*sin(phi+3/2*pi)+r];

% order of track snippets
trackgenerator = [1, 3, 2, 3, 1, 3, 2, 3];
% rotation of track snippets
rotgenerator = [0, 0, pi/2, pi/2, pi, pi, 3*pi/2, 3*pi/2] + pi/2;

% build up track
x_m = 0;
y_m = 0;
for i = 1:1:length(trackgenerator)
  R = [cos(rotgenerator(i)), -sin(rotgenerator(i));...
    sin(rotgenerator(i)), cos(rotgenerator(i))];
  % rotate track part to the right position
  x_loc = R(1, :)*trackparts{trackgenerator(i)};
  y_loc = R(2, :)*trackparts{trackgenerator(i)};
  % transform trackpart origin to the last point
  x_m = [x_m, x_loc+x_m(end)];
  y_m = [y_m, y_loc+y_m(end)];
end
% generate width vector
b_m = ones(1, length(x_m))*b0;

% write to csv
dlmwrite('roundedRectangle.csv', [x_m(1:end-1)',y_m(1:end-1)',b_m(1:end-1)'], ';');

figure;
plot(x_m(1:end-1), y_m(1:end-1));
axis equal;
grid on;


function r = VBA_spm_gamrnd(a,b,varargin)
% Random arrays from gamma distribution - a compiled routine
% FORMAT r = spm_gamrnd(a,b,m,n,...)
%
% a        - shape parameter
% b        - scale parameter
% m,n,...  - dimensions of the output array [optional]
%
% r        - array of random numbers chosen from the gamma distribution
%__________________________________________________________________________
%
% Reference
%
% George Marsaglia and Wai Wan Tsang, "A Simple Method for Generating Gamma
% Variables": ACM Transactions on Mathematical Software, Vol. 26, No. 3,
% September 2000, Pages 363-372
% http://portal.acm.org/citation.cfm?id=358414
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_gamrnd.m 3251 2009-07-06 17:29:44Z guillaume $

%-This is merely the help file for the compiled routine
error('spm_gamrnd.c not compiled - see Makefile');


function [B0,BV] = spm_MDP_DP(MDP)
% dynamic programming using active inference
% FORMAT [B0,BV] = spm_MDP_DP(MDP)
%
% MDP.A(O,N)      - Likelihood of O outcomes given N hidden states
% MDP.B{M}(N,N)   - transition probabilities among hidden states (priors)
% MDP.C(N,1)      - prior preferences (prior over future states)
%
% MDP.V(T - 1,P)  - P allowable policies (control sequences)
%
% B0      - optimal state action policy or transition matrix
% BV      - corresponding policy using value iteration
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_DP.m 6598 2015-11-11 19:48:30Z karl $

% set up and preliminaries
%==========================================================================

% generative model and initial states
%--------------------------------------------------------------------------
T     = size(MDP.V,1) + 1;        % number of outcomes
Ns    = size(MDP.B{1},1);         % number of hidden states
Nu    = size(MDP.B,2);            % number of hidden controls
p0    = exp(-8);                  % smallest probability

% likelihood model (for a partially observed MDP implicit in G)
%--------------------------------------------------------------------------
try
    A = MDP.A + p0;
catch
    A = speye(Ns,Ns) + p0;
end
A     = A*diag(1./sum(A));        % normalise
lnA   = log(A);                   % log probabilities
H     = sum(A.*lnA)';             % negentropy of observations

% transition probabilities (priors)
%--------------------------------------------------------------------------
for j = 1:Nu
    B{j}   = MDP.B{j} + p0;
    B{j}   = B{j}*diag(1./sum(B{j}));
    sB{j}  = B{j};
    rB{j}  = spm_softmax(log(B{j})');
    lnB{j} = log(B{j});
end

% terminal probabilities over outcomes (priors)
%--------------------------------------------------------------------------
try
    C = MDP.C;
catch
    C = zeros(No,1);
end

% asume constant preferences over states
%--------------------------------------------------------------------------
if size(C,2) ~= T
    C = C(:,end)*ones(1,T);
end
C = A'*diag(1./sum(A,2))*spm_softmax(C);
C = log(C);

% policies, states and their expectations
%--------------------------------------------------------------------------
V     = MDP.V;
Np    = size(V,2);                % number of allowable policies

% policy iteration
%==========================================================================
for s = 1:Ns

    % Variational iterations (hidden states)
    %======================================================================
    x     = zeros(Ns,T,Np) + 1/Ns;
    for k = 1:Np

        % gradient descent on free energy
        %------------------------------------------------------------------
        for i = 1:16

            % hiddens states (x)
            %--------------------------------------------------------------
            x(:,1,k) = 0;
            x(s,1,k) = 1;

            for j = 2:T

                % current state
                %----------------------------------------------------------
                xj   = x(:,j,k);
                qx   = log(xj);
                v    = 0;

                % evaluate free energy and gradients (v = dFdx)
                %----------------------------------------------------------
                if j > 1, v = v + qx - log(sB{V(j - 1,k)}*x(:,j - 1,k)); end
                if j < T, v = v      - log(rB{V(j    ,k)}*x(:,j + 1,k)); end

                % update
                %----------------------------------------------------------
                x(:,j,k) = spm_softmax(qx - v/4);
                F(j,k)   = xj'*v;

            end

            % convergence
            %--------------------------------------------------------------
            if i > 1
                dF = F0 - sum(F(:,k));
                if dF > 1/128, F0 = F0 - dF; else, break, end
            else
                F0 = sum(F(:,k));
            end

        end
    end

    % value of policies (Q)
    %======================================================================
    Q     = zeros(Np,1);
    for k = 1:Np

        % path integral of expected free energy
        %------------------------------------------------------------------
        for j = 2:T
            v    = C(:,j) - log(x(:,j,k)) + H;
            Q(k) = Q(k) + v'*x(:,j,k);

        end
    end

    % optimal transition from this state
    %======================================================================
    [u,k]   = max(Q);
    B0(:,s) = lnB{V(1,k)}(:,s);

end

if nargout < 2, return, end

% value iteration
%==========================================================================
V     = zeros(Ns,1);
C     = C(:,end);
g     = 1 - 1/T;
for i = 1:32
    for s  = 1:Ns

        % value of actions (Q)
        %------------------------------------------------------------------
        Q     = zeros(Nu,1);
        for k = 1:Nu
            Q(k)   = B{1,k}(:,s)'*(C + g*V);
        end

        % optimal transition from this state
        %------------------------------------------------------------------
        [u,k]   = max(Q);
        BV(:,s) = B{1,k}(:,s);

    end

    % optimal transition from this state
    %----------------------------------------------------------------------
    dV   = BV'*(C + g*V) - V;
    V    = V + dV;

    % convergence
    %----------------------------------------------------------------------
    if norm(dV) < 1e-2, break, end

end


function varargout = atan(varargin)
%ATAN (overloaded)

switch class(varargin{1})

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = struct('convexity','none','monotonicity','increasing','definiteness','none','model','callback');
        operator.convexhull = @convexhull;
        operator.bounds = @bounds;
        operator.derivative = @(x)((1+x.^2).^-1);
        operator.inverse = @(x)(tan(x));
        operator.range = [-pi/2 pi/2];

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error('SDPVAR/ATAN called with CHAR argument?');
end

function [L,U] = bounds(xL,xU)
L = atan(xL);
U = atan(xU);

function [Ax, Ay, b] = convexhull(xL,xU)
fL = atan(xL);
fU = atan(xU);
dfL = 1/(1+xL^2);
dfU = 1/(1+xU^2);
if xL >= 0
    % Concave region
    [Ax,Ay,b] = convexhullConcave(xL,xU,fL,fU,dfL,dfU);
elseif xU <= 0
    % Convex region
    [Ax,Ay,b] = convexhullConvex(xL,xU,fL,fU,dfL,dfU);
else
    % Changes convexity. We're lazy and let YALMIP sample instead
    Ax = [];
    Ay = [];
    b = [];
end


function [X, present] = block(X, past, weights, hyperParameters)
% block   Transformer block for GPT-2
%
%   [X, present] = block(X, past, weights, hyperParameters) computes a
%   GPT-2 style transformer block on the input X as described in [1] (see
%   Section 2.3). One difference between this style of transformer block
%   and others is that this block uses layer normalization at the
%   beginning.
%
%   Inputs:
%       X               - A (numFeatures*numHeads)-by-numInputSubwords
%                         input array.
%       past            - A numFeatures-by-numPastSubwords-by-numHeads-by-2
%                         array. This contains the 'keys' and 'values' for
%                         past subwords. These are needed to predict future
%                         outputs in an autoregressive manner. 'keys' are
%                         stored in past(:,:,:,1) and 'values' are stored
%                         in past(:,:,:,2).
%       weights         - The weights for the transformer block stored in a
%                         struct. In this block we have:
%                           - ln_1_g_0: Weight vector for the first layer
%                             normalization.
%                           - ln_1_b_0: Bias vector for the first layer
%                             normalization.
%                           - ln_2_g_0: Weight vector for the second layer
%                             normalization.
%                           - ln_2_b_0: Bias vector for the second layer
%                             normalization.
%                         In the attention sub-block:
%                           - attn_c_attn_w_0: A weight matrix for the
%                             first fully connected layer.
%                           - attn_c_attn_b_0: A bias vector for the first
%                             fully connected layer.
%                           - attn_c_proj_w_0: A weight matrix for the
%                             final fully connected layer.
%                           - attn_c_proj_b_0: A bias vector for the final
%                             fully connected layer.
%                         In the multi-layer perceptron block:
%                           - mlp_c_fc_w_0: A weight matrix for the first
%                             fully connected layer.
%                           - mlp_c_fc_b_0: A bias vector for the first
%                             fully connected layer.
%                           - mlp_c_proj_w_0: A weight matrix for the
%                             second fully connected layer.
%                           - mlp_c_proj_b_0: A bias vector for the second
%                             fully connected layer.
%       numHeads        - The number of attention heads. This is a
%                         hyper-parameter.
%
%   Outputs:
%       Z               - A (numFeatures*numHeads)-by-numInputSubwords
%                         output array.
%       present         - A numFeatures-by-numAllSubwords-by-numHeads-by-2
%                         array. This contains the 'keys' and 'values' that
%                         are created from inputs. These need to passed
%                         back in as the 'past' input if we want to predict
%                         future outputs in an autoregressive manner. 'keys'
%                         are stored in present(:,:,:,1) and 'values' are
%                         stored in present(:,:,:,2).
%
%   References:
%
%   [1] Alec Radford, Jeffrey Wu, Rewon Child, David Luan, Dario Amodei,
%       Ilya Sutskever, "Language Models are Unsupervised Multitask
%       Learners",
%       https://cdn.openai.com/better-language-models/language_models_are_unsupervised_multitask_learners.pdf

XNorm1 = transformer.layer.normalization(X, ...
    weights.ln_1_g_0, weights.ln_1_b_0);

[A, present] = transformer.layer.attention(XNorm1, past, weights, hyperParameters);

X = X + A;

XNorm2 = transformer.layer.normalization(X, ...
    weights.ln_2_g_0, weights.ln_2_b_0);

M = transformer.layer.multiLayerPerceptron(XNorm2, weights);

X = X + M;

end


%% Read shape

[X,T] = readOff('../data/meshes/sphere_102.off');
M = getMeshData(X,T,10); % compute 10 LB eigenfunctions for fun

%% Set up Gaussian blur function

blurTime = .005; % if this gets too small, distances get noisy
blurSteps = 50;

% blur = @(x) blurOnMesh(x,M,blurTime,blurSteps)+1e-300; % faster than pre-factored?
% blurTranspose = @(x) blurOnMesh(x,M,blurTime,blurSteps,1)+1e-300;

% structure = prefactorMeshBlur(M,blurTime,blurSteps,5000);
% blur = @(x) prefactoredBlur(x,structure,0)+1e-300;
% blurTranspose = @(x) prefactoredBlur(x,structure,1)+1e-300;

h = blurTime/blurSteps;
nv = M.numVertices;

% Sphere is small enough that we can just write down the heat kernel explicitly
blurInverse = spdiags(M.areaWeights,0,nv,nv) - h*M.cotLaplacian;
mtx = full(blurInverse) \ diag(M.areaWeights);
mtx = mtx^blurSteps;

mtx(mtx<1e-50) = 1e-50;

blur = @(x) mtx*x;
blurTranspose = @(x) mtx'*x;

%% Make boundary

[~,fixedVerts(1)] = min(X(:,1));
[~,fixedVerts(2)] = min(X(:,2));
[~,fixedVerts(3)] = min(X(:,3));
[~,fixedVerts(4)] = max(X(:,1));
[~,fixedVerts(5)] = max(X(:,2));
[~,fixedVerts(6)] = max(X(:,3));

nFixed = length(fixedVerts);

fixedDistributions = zeros(M.numVertices,nFixed);

close all;
for i=1:nFixed
    fixedDistributions(fixedVerts(i),i) = 1/M.areaWeights(fixedVerts(i));
end

fixedDistributions = fixedDistributions+1e-10;
% fixedDistributions = blur(fixedDistributions);
showDescriptor(M,fixedDistributions(:,1));

%% Compute average entropy of data

averageEntropy = -mean(sum(bsxfun(@times,M.areaWeights,(fixedDistributions.*log(fixedDistributions))),1));

%% Solve barycenter problem using this machinery

edges = [2 1; 3 1; 4 1];
edgeWeights = [1 1 1 1 1 1];

targetEntropy = averageEntropy + 1;

result = convolutionalPropagation(edges, edgeWeights, [2 3 4], ...
    fixedDistributions(:,1:3),M.areaWeights,blur,blurTranspose,targetEntropy);

barycenter = convolutionalBarycenter(fixedDistributions(:,1:3),[1 1 1],...
    M.areaWeights,blur,blurTranspose,targetEntropy);

close all

f = subplot(1,3,1);
showDescriptor(M,sum(fixedDistributions(:,1:3),2),[],[],[],f);
title('Boundary distributions');
colorbar off;

f = subplot(1,3,2);
showDescriptor(M,result(:,1),[],[],[],f);
title('Barycenter computed using graph algorithm');
colorbar off;

f = subplot(1,3,3);
showDescriptor(M,barycenter,[],[],[],f);
title('Barycenter computed using specialized barycenter algorithm')
colorbar off;

%% Solve displacement problem using this machinery

k = 9;

edges = [(1:(k-1))' (2:k)'];
edges = [edges ; edges(:,2) edges(:,1)];
edgeWeights = ones(1,size(edges,1));

targetEntropy = averageEntropy + 1.5;

result = convolutionalPropagation(edges, edgeWeights, [1 k], ...
    fixedDistributions(:,[1 6]),M.areaWeights,blur,blurTranspose,targetEntropy);

figure

for i=1:k
    f = subplot(1,k,i);
    showDescriptor(M,result(:,i),[],[],[],f);
    colorbar off;
end

%% Solve soft mapping problem

edges = [T(:,1) T(:,2); T(:,2) T(:,3); T(:,3) T(:,1)];
edges = sort(edges,2);
edges = unique(edges,'rows');
edges = [edges; edges(:,2) edges(:,1)];

edgeWeights = ones(size(edges,1),1);

targetEntropy = averageEntropy+1.5;

result = convolutionalPropagation(edges, edgeWeights, fixedVerts, ...
                    fixedDistributions,M.areaWeights,blur,blurTranspose,targetEntropy);

%% Show result

testVerts = randperm(M.numVertices);
nTests = 5;
testVerts = testVerts(1:nTests);

figure;
for i=1:length(testVerts)
    f = subplot(2,length(testVerts),i);
    ind = zeros(M.numVertices,1);
    ind(testVerts(i)) = 1;
    showDescriptor(M,ind,[],[],[],f);
    colorbar off;
    colormap hot;

    f = subplot(2,length(testVerts),i+length(testVerts));
    showDescriptor(M,result(:,testVerts(i)),[],[],[],f);
    colorbar off;
    colormap hot;
end


% pivot_input.m // Jon Lee
% data for pivoting example

A = [1 2 1 0 0 0; 3 1 0 1 0 0; 1.5 1.5 0 0 1 0; 0 1 0 0 0 1];
c = [6 7 -2 0 4 4.5]';
b = [7 9 6 3.3]';
beta = [1,2,4,6];
[m,n] = size(A);
eta = setdiff(1:n,beta); % lazy eta initialization


function [eMat,f2eMat,f2eIsDirMat] = mapface2edge(vMat,fMat)
% MAPFACE2EDGE creates a mapping from faces to edges
%
% Input:
%   regular:
%       vMat: double[nVerts,3] - coordinates of vertices
%       fMat: double[nFace,3] - indices of face vertices in vMat
%
% Output:
%   eMat: double[nEdges,2] - contains indices of vertices corresponding
%       to each edge
%
%   f2eMat: double[nFaces,3] - contains indices of edges for
%       each face in this order (1-2, 2-3, 1-3)
%
%   f2eIsDirMat: logical[nFaces,3] - contains true if face
%       references edge in a direct order (i.e. 1-2 for instance)
%       and false if reference is in an opposite order
%
% $Author: Peter Gagarinov, PhD  <pgagarinov@gmail.com> $
% $Copyright: Peter Gagarinov, PhD,
%            Moscow State University,
%            Faculty of Computational Mathematics and Computer Science,
%            System Analysis Department 2011-2016 $
%
trObj = triangulation(fMat,vMat);
eMat=trObj.edges;
nEdges=size(eMat,1);
nFaces=size(fMat,1);
%% Build Face to Edges map and edge orientation map for each face
fMidCVec=trObj.edgeAttachments(eMat);
indShiftVec=cellfun('length',fMidCVec);
indF2EVec=zeros(sum(indShiftVec),1);
indF2EVec(1)=1;
indF2EVec(1+cumsum(indShiftVec(1:end-1)))=ones(nEdges-1,1);
indF2EVec=cumsum(indF2EVec);
indFVec=[fMidCVec{:}].';
%edges for each face are expected to be oriented as
%1-2, 2-3 , 3-1
indEdgeNumVec=...
    all(fMat(indFVec,[1,2])==eMat(indF2EVec,:),2)...
    -all(fMat(indFVec,[2,1])==eMat(indF2EVec,:),2)...
    +2*all(fMat(indFVec,[2,3])==eMat(indF2EVec,:),2)...
    -2*all(fMat(indFVec,[3,2])==eMat(indF2EVec,:),2)...
    +3*all(fMat(indFVec,[1,3])==eMat(indF2EVec,:),2)...
    -3*all(fMat(indFVec,[3,1])==eMat(indF2EVec,:),2);
%
[~,indSortVec]=sortrows([indFVec,abs(indEdgeNumVec)]);
indF2EVec=indF2EVec(indSortVec);
f2eMat=reshape(indF2EVec,3,nFaces).';
f2eIsDirMat=reshape(indEdgeNumVec(indSortVec)>0,3,nFaces).';


function dick_grid_display ( ng, xy )

%*****************************************************************************80
%
%% DISK_GRID_DISPLAY displays grid points inside a disk.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NG, the number of grid points inside the disk.
%
%    Input, real XY(2,NG), the grid points.
%
  scatter ( xy(1,:), xy(2,:), 'b.' );
  axis equal
  title ( sprintf ( '%d grid points inside a disk', ng ) )
  grid on

  return
end


function [xhk, pf] = particle_filter(sys, yk, pf, resampling_strategy)
%% Generic particle filter
%
% Note: when resampling is performed on each step this algorithm is called
% the Bootstrap particle filter
%
% Usage:
% [xhk, pf] = particle_filter(sys, yk, pf, resamping_strategy)
%
% Inputs:
% sys  = function handle to process equation
% yk   = observation vector at time k (column vector)
% pf   = structure with the following fields
%   .k                = iteration number
%   .Ns               = number of particles
%   .w                = weights   (Ns x T)
%   .particles        = particles (nx x Ns x T)
%   .gen_x0           = function handle of a procedure that samples from the initial pdf p_x0
%   .p_yk_given_xk    = function handle of the observation likelihood PDF p(y[k] | x[k])
%   .gen_sys_noise    = function handle of a procedure that generates system noise
% resampling_strategy = resampling strategy. Set it either to
%                       'multinomial_resampling' or 'systematic_resampling'
%
% Outputs:
% xhk   = estimated state
% pf    = the same structure as in the input but updated at iteration k
%
% Reference:
% [1] Arulampalam et. al. (2002).  A tutorial on particle filters for
%     online nonlinear/non-gaussian bayesian tracking. IEEE Transactions on
%     Signal Processing. 50 (2). p 174--188

%% Programmed by:
% Diego Andres Alvarez Marin (diegotorquemada@gmail.com)
% Universidad Nacional de Colombia at Manizales, February 29, 2012

%%
k = pf.k;
if k == 1
   error('error: k must be an integer greater or equal than 2');
end

%% Initialize variables
Ns = pf.Ns;                              % number of particles
nx = size(pf.particles,1);               % number of states

wkm1 = pf.w(:, k-1);                     % weights of last iteration
if k == 2
   for i = 1:Ns                          % simulate initial particles
      pf.particles(:,i,1) = pf.gen_x0(); % at time k=1
   end
   wkm1 = repmat(1/Ns, Ns, 1);           % all particles have the same weight
end

%%
% The importance sampling function:
% PRIOR: (this method is sensitive to outliers)   THIS IS THE ONE USED HERE
% q_xk_given_xkm1_yk = pf.p_xk_given_xkm1;

% OPTIMAL:
% q_xk_given_xkm1_yk = q_xk_given_xkm1^i_yk;
% Note this PDF can be approximated by MCMC methods: they are expensive but
% they may be useful when non-iterative schemes fail

%% Separate memory
xkm1 = pf.particles(:,:,k-1); % extract particles from last iteration;
xk   = zeros(size(xkm1));     % = zeros(nx,Ns);
wk   = zeros(size(wkm1));     % = zeros(Ns,1);

%% Algorithm 3 of Ref [1]
for i = 1:Ns
   % xk(:,i) = sample_vector_from q_xk_given_xkm1_yk given xkm1(:,i) and yk
   % Using the PRIOR PDF: pf.p_xk_given_xkm1: eq 62, Ref 1.
   xk(:,i) = sys(k, xkm1(:,i), pf.gen_sys_noise());

   % Equation 48, Ref 1.
   % wk(i) = wkm1(i) * p_yk_given_xk(yk, xk(:,i))*p_xk_given_xkm1(xk(:,i), xkm1(:,i))/q_xk_given_xkm1_yk(xk(:,i), xkm1(:,i), yk);

   % weights (when using the PRIOR pdf): eq 63, Ref 1
   wk(i) = wkm1(i) * pf.p_yk_given_xk(k, yk, xk(:,i));

   % weights (when using the OPTIMAL pdf): eq 53, Ref 1
   % wk(i) = wkm1(i) * p_yk_given_xkm1(yk, xkm1(:,i)); % we do not know this PDF
end;

%% Normalize weight vector
wk = wk./sum(wk);

%% Calculate effective sample size: eq 48, Ref 1
Neff = 1/sum(wk.^2);

%% Resampling
% remove this condition and sample on each iteration:
% [xk, wk] = resample(xk, wk, resampling_strategy);
%if you want to implement the bootstrap particle filter
resample_percentaje = 0.50;
Nt = resample_percentaje*Ns;
if Neff < Nt
   disp('Resampling ...')
   [xk, wk] = resample(xk, wk, resampling_strategy);
   % {xk, wk} is an approximate discrete representation of p(x_k | y_{1:k})
end

%% Compute estimated state
xhk = zeros(nx,1);
for i = 1:Ns;
   xhk = xhk + wk(i)*xk(:,i);
end

%% Store new weights and particles
pf.w(:,k) = wk;
pf.particles(:,:,k) = xk;

return; % bye, bye!!!

%% Resampling function
function [xk, wk, idx] = resample(xk, wk, resampling_strategy)

Ns = length(wk);  % Ns = number of particles

% wk = wk./sum(wk); % normalize weight vector (already done)

switch resampling_strategy
   case 'multinomial_resampling'
      with_replacement = true;
      idx = randsample(1:Ns, Ns, with_replacement, wk);
%{
      THIS IS EQUIVALENT TO:
      edges = min([0 cumsum(wk)'],1); % protect against accumulated round-off
      edges(end) = 1;                 % get the upper edge exact
      % this works like the inverse of the empirical distribution and returns
      % the interval where the sample is to be found
      [~, idx] = histc(sort(rand(Ns,1)), edges);
%}
   case 'systematic_resampling'
      % this is performing latin hypercube sampling on wk
      edges = min([0 cumsum(wk)'],1); % protect against accumulated round-off
      edges(end) = 1;                 % get the upper edge exact
      u1 = rand/Ns;
      % this works like the inverse of the empirical distribution and returns
      % the interval where the sample is to be found
      [~, idx] = histc(u1:1/Ns:1, edges);
   % case 'regularized_pf'      TO BE IMPLEMENTED
   % case 'stratified_sampling' TO BE IMPLEMENTED
   % case 'residual_sampling'   TO BE IMPLEMENTED
   otherwise
      error('Resampling strategy not implemented')
end;

xk = xk(:,idx);                    % extract new particles
wk = repmat(1/Ns, 1, Ns);          % now all particles have the same weight

return;  % bye, bye!!!


function im = ifft2c(d)
% Function performs a centered ifft2
im = ifftshift(ifft2(ifftshift(d)));


function Fitness = Fit(PopObj,PopCon)
% Calculate the fitness 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".
%--------------------------------------------------------------------------

    N  = size(PopObj,1);
    CV = sum(max(0,PopCon),2);

    %% Detect the dominance relation between each two solutions
    Dominate = false(N);
    for i = 1 : N-1
        for j = i+1 : N
            if CV(i) < CV(j)
                Dominate(i,j) = true;
            elseif CV(i) > CV(j)
                Dominate(j,i) = true;
            else
                k = any(PopObj(i,:)<PopObj(j,:))-any(PopObj(i,:)>PopObj(j,:));
                if k == 1
                    Dominate(i,j) = true;
                elseif k == -1
                    Dominate(j,i) = true;
                end
            end
        end
    end

    %% Calculate S(i)
    S = sum(Dominate,2);

    %% Calculate R(i)：
    R = zeros(1,N);
    for i = 1 : N
        R(i) = sum(S(Dominate(:,i)));
    end

    %% Calculate D(i)
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Distance = sort(Distance,2);
    D = 1./(Distance(:,floor(sqrt(N)))+2);

    %% Calculate the fitnesses
    Fitness = R + D';
end


function [retdat,algo] =  training(algo,dat)

retdat=dat;

 disp(['training ' get_name(algo) '.... '])
 switch algo.optimizer
  case {'svmtorch'}
  %%<<----------------svmtorch optimizer-------------------->>

    multi = 0;
    regression = 1;
    degree = 1;
    gamma = 1;
    eps = algo.epsilon;
    C = algo.C;
    if strcmp(algo.child.ker,'linear')
       kernelType = 0;
    elseif strcmp(algo.child.ker,'poly')
       kernelType = 1;
       degree = algo.child.kerparam;
    elseif strcmp(algo.child.ker,'rbf')
    	kernelType = 2;
	gamma = algo.child.kerparam;
    end;
    [x y] = get_xy(dat);
    numEx = get_dim(dat);
    [alpha,threshold0,xSV] = SVMTorch(x,y,regression,multi,kernelType,degree,gamma,C,eps);
    if isempty(alpha)
    	alpha = zeros(numEx,1);
    end;

 %<<--------------sparse svr------------------>>%

   case{'sparse'}
   	kern1=get_kernel(algo.child,dat,dat);
	y=get_y(dat);
	kNum=size(kern1,1);
	c=ones(kNum*4,1);
	c(2*kNum+1:4*kNum)=algo.C;
	kern2 = [-kern1 kern1 -1*eye(kNum) zeros(kNum); kern1 -kern1 zeros(kNum) -1*eye(kNum); -1*eye(4*kNum)];
	cst = zeros(6*kNum,1);
	cst(1:kNum) = (algo.epsilon+algo.b0) - y;
	cst(kNum+1:2*kNum) = (algo.epsilon-algo.b0) + y;
	opts= optimset('display','off','MaxIter',10000,'LargeScale','off');
	[alphaTemp,fval,exit,out,lambda] = linprog(c,kern2,cst,[],[],[],[],[],opts);
	threshold0=lambda.ineqlin(1);
	alpha=alphaTemp(1:kNum)-alphaTemp(kNum+1:2*kNum);

  %<<------------andre optimizer--------------------->>

  case {'andre'}
   kern1=get_kernel(algo.child,dat,dat);   %% <--- calculate kernel
   y=get_y(dat);
   yLen = length(y(:,1));
   xSV = get_x(dat);
   kern2 = [kern1 , -kern1 ; -kern1 , kern1];
   cst = ones(2*yLen,1);
   cst(yLen+1:2*yLen) = -1;
   if algo.nu ==0,
       c = zeros(2*yLen,1);
       c(1:yLen) = algo.epsilon*ones(yLen,1) - y;
       c(yLen+1:2*yLen) = algo.epsilon*ones(yLen,1) + y;
       [alphaTemp,threshold] = quadsolve(kern2,c,cst',0,algo.C);
       alpha=alphaTemp(1:yLen)-alphaTemp(yLen+1:2*yLen);
       threshold0 = -threshold;
   else
     if algo.C==Inf,
         algo.C=10000;
     end;

        c = zeros(2*yLen+1,1);
        c(1:yLen) = -y;
        c(yLen+1:2*yLen) = y;
        cst2 = ones(1,2*yLen+1)/(yLen*algo.nu);
        cst2(2*yLen+1)=-1;
        kern2 = [kern2,zeros(2*yLen,1);zeros(1,2*yLen+1)];
        cst=[cst',0;cst2];
        [alphaTemp,threshold] = quadsolve(kern2,c,cst,[0;0],algo.C); threshold0 = -threshold(1);
        alpha=alphaTemp(1:yLen)-alphaTemp(yLen+1:2*yLen);
        epsilon = -threshold(2);
    end

  %<<------------quadprog optimizer--------------------->>

  case {'quadprog'}

   kern1=get_kernel(algo.child,dat,[]);   %% calculate kernel
   kNum=size(kern1,1);  y=get_y(dat);
   kern2 = [kern1 , -kern1 ; -kern1 , kern1];
   c = zeros(2*kNum,1);
   c(1:kNum) = algo.epsilon*ones(kNum,1) - y;
   c(kNum+1:2*kNum) = algo.epsilon*ones(kNum,1) + y;
   cst(kNum+1:2*kNum) = -1;
   cst = ones(2*kNum,1);
   opts= optimset('display','off','MaxIter',10000,'LargeScale','off');
   [alphaTemp,fval,exit,out,lambda] = quadprog(kern2,c,[],[],cst',0,...
				       zeros(2*kNum,1),algo.C*ones(2*kNum,1),[],opts);
   threshold0=lambda.eqlin(1);
   alpha=alphaTemp(1:kNum)-alphaTemp(kNum+1:2*kNum);

%<<------------libsvm optimizer--------------------->>
case {'libsvm'}

        %
        x=[];
        y=[];
        svm_type=3;
        kernelType=0;
        degree=3;
        gamma=0;
        coef0=0;

        nu=algo.nu;
        if(nu>0)
            svm_type=4;
        end

        cachesize=40;
        C=algo.C;
        eps=algo.epsilon;
        p=0.05;
        shrinking=1;

        weight_label=[];
        weight=[];
        nr_weight=0;

        if strcmp(algo.child.ker,'linear')
            kernelType = 0;
        end;
        if strcmp(algo.child.ker,'poly')
            kernelType = 1;
            degree = algo.child.kerparam;
            coef0 = 1;
            gamma = 1;
        end;
        if strcmp(algo.child.ker,'rbf'),
            kernelType = 2;
            sigma = algo.child.kerparam;
            gamma = 1/(2*sigma^2);
        end;

        y=get_y(dat);
        x=get_x(dat);

        if strcmp(algo.child.ker,'custom'),
          kernelType = 4;
          K= algo.child.kerparam;
          l = get_dim( retDat);
          x = get_index( retDat);
          x = [ reshape( x, l, 1) [ 1:l]']; % using x to pass indices in Matrix and real indices
        end;

        s=whos('libsvm_cachesize','global');

        if (length(s)>0)
            global libsvm_cachesize;
            cachesize=libsvm_cachesize;
        else
            cachesize=40;
        end
        if algo.algorithm.verbosity>1
         fprintf('Using %d MB Cache for Libsvm\n',cachesize)
        end

         if( kernelType == 4)
          [alpha,xSV,bias0]=libsvm_regressor_spider(x,y,svm_type,kernelType,...
                     degree,gamma,coef0,nu,cachesize,C,eps,p,weight_label,weight,nr_weight,K);
          algo.Xsv=get(retDat, xSV( :, 2));
        else
          [alpha,xSV,bias0]=libsvm_regressor_spider(x,y,svm_type,kernelType,...
                     degree,gamma,coef0,nu,cachesize,C,eps,p,weight_label,weight,nr_weight);
          algo.Xsv=data(xSV);
        end

        threshold0 = bias0 * y(1);

        algo.b0=bias0;

        algo.epsilon = eps;

        algo.Xsv = data(xSV);
        algo.alpha=alpha;

        if algo.algorithm.do_not_evaluate_training_error==1
            retdat=set_x(dat,get_y(dat));
        else
            retdat=test(algo,dat);
        end

        return

        %         fin=find(abs(alpha)>algo.alpha_cutoff);

% case {'libsvm'}
%
%    if algo.nu ==0,
%         svm_type = 3;
%         C = algo.C;
%         epsilon = algo.epsilon;
%         nu=0;
%     else
%         svm_type = 4;
%         nu = algo.nu;
%         C = algo.C;
%         epsilon = -1;
%     end;
%     %% default values for libsvm
%     cacheSize = 40;
%     eps = 0.001;
%     shrinking=1;
%     nrWeight = 0;
%     weightLabel =0;
%     weight = 1;
%     gamma=1;
%     deg = 0;
%     coef0 = 0;
%     kerTmp = algo.child;
%     if strcmp(kerTmp.ker,'linear')
%       kernelType = 0;
%     end;
%     if strcmp(kerTmp.ker,'poly')
%       kernelType = 1;
%       ptmp = kerTmp.kerparam;
%       deg = ptmp;
%       coef0 = 1;
%     end;
%    if strcmp(kerTmp.ker,'rbf'),
%          kernelType = 2;
%          ptmp = kerTmp.kerparam;
%          gamma = 1/(2*ptmp^2);
%    end;
% %    if algo.balanced_ridge~=0,
% %         disp('Warning: balanced ridge not implemented for libsvm.');
% %    end;
%    y=get_y(dat);
%    x=get_x(dat);
%    [alpha,threshold0,xSV,eps,CC] = svmlibtrain(x,y,svm_type,kernelType,deg,gamma,coef0,nu,cacheSize,C,eps,epsilon,...
%        shrinking,nrWeight,weightLabel,weight,0);
%    threshold0=-threshold0;% from libsvm
%    % alpha is reordered in order to have the same xsp for all runs (important for one_vs_rest)
%    alphaTemp = zeros(size(xSV,1),1);
%    indTemp = find(xSV(:,size(xSV,2))~=0);
%    indTemp2 = xSV(indTemp,size(xSV,2));
%    alphaTemp(indTemp2) = alpha(indTemp);
%    alpha = alphaTemp;
%    epsilon=eps;
%  end
end

 if algo.nu~=0,
   algo.epsilon = epsilon;
 end;

 algo.b0=threshold0;
 fin=find(abs(alpha)>algo.alpha_cutoff);
 algo.alpha=alpha(fin);
 algo.Xsv = get(dat,fin);

 if algo.algorithm.do_not_evaluate_training_error==1
   retdat=set_x(dat,get_y(dat));
 else
   retdat=test(algo,dat);
 end


function J = lotkaObjectiveFCN_models(u,x,N,xref,u0,p,Q,R,Ru, select_model)
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   xref:   state references, constant from time k+1 to k+N
%   u0:     previous controller output at time k-1
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters
% Q = diag([1,1,1]);
% R = 0.01;

%% Integrate system
if strcmp(select_model,'DelayDMDc')
    [xk,~] = lsim(p.sys,[p.udelay(1:N);u'],[0:N-1].*p.dt,[p.xdelay(:,1); x]-[p.xmean; p.xmean]);
    xk = xk(:,3:4);
    xk = xk + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'DMDc')
    [xk,~] = lsim(p.sys,[u' 0],[0:N].*p.dt,x-p.xmean);
    xk = xk(2:end,:) + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'SINDYc')
    Ns = size(x,1);
    xk = zeros(Ns,N+1); xk(:,1) = x;
    for ct=1:N
        % Obtain plant state at next prediction step.
        xk(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xk(:,ct),u(ct),p.dt,1,[],p);
    end
    xk = xk(:,2:N+1);
elseif strcmp(select_model,'NARX')
    Hu = [u',0];
    Hx = zeros(2,length(Hu)); Hx(:,1) = x;
    [Us,Ui,Si] = preparets(p.net,con2seq(Hu),{},con2seq(Hx));
    xk = p.net(Us,Ui,Si);
    xk = cell2mat(xk);
end

%% Cost Calculation
% Set initial plant states, controller output and cost
uk = u(1);
J = 0;
% Loop through each prediction step
for ct=1:N
    % Obtain plant state at next prediction step
    xk1 = xk(:,ct);

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J = J + (xk1-xref)'*Q*(xk1-xref);
    % Accumulate MV rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J = J + (uk-u0)'*R*(uk-u0) + uk'*Ru*uk;
    else
        J = J + (uk-u(ct-1))'*R*(uk-u(ct-1)) + uk'*Ru*uk;
    end
    % Update uk for the next prediction step
    if ct<N
        uk = u(ct+1);
    end
end