function [maxCI minCI] = calculateConfidenceInterval(tau, sPeriod, rCount,alpha,stabDev)
%This function calculates max and min confidence intervals for the XDEV
%calculation. The method used is found in the NIST Handbook of Frequency
%Stability Analysis page 37 section 5.3.1. tau is the array of tau values
%used in the stability calculation. Sample Period is used to calculate the
%number of values used in a particular tau calculation. rCount is the
%number of total readings at the sample period. alpha is an array of values
%for determine power law noise type. stabDev is the array of XDEV
%calculations.

tCount = length(tau); %get total number of calculations
maxCI = zeros(1,tCount); %initiate arrays
minCI = zeros(1,tCount);

for i = 1:tCount %for number of calculations
    avgCount = tau(i) / sPeriod; %gets count for averaging
    vCount = rCount / avgCount; %get number of loop iterations needed
    vCount = floor(vCount); %convert loopCount to integer in case it is a non int

    %perform confidence interval calculation based on noise type
    if alpha(i) >= 1.5 %dominant noise type is white PM
        temp = .99*(stabDev(i)/sqrt(vCount));
    elseif alpha(i) < 1.5 && alpha(i) >= .5 %dominant noise type is flicker PM
        temp = .99*(stabDev(i)/sqrt(vCount));
    elseif alpha(i) < .5 && alpha(i) >= -.5 %dominant noise type is white FM
        temp = .87*(stabDev(i)/sqrt(vCount));
    elseif alpha(i) < -.5 && alpha(i) >= -1.5 %dominant nosie type is flicker FM
        temp = .77*(stabDev(i)/sqrt(vCount));
    else %dominant noise type is random walk FM
        temp = .75*(stabDev(i)/sqrt(vCount));
    end

    %set min and max confidence limits
    maxCI(i) = stabDev(i) + temp;
    minCI(i) = stabDev(i) - temp;

end


function [obs, hidden] = pomdp_sample(initial_prob, transmat, obsmat, act)
% SAMPLE_POMDP Generate a random sequence from a Partially Observed Markov Decision Process.
% [obs, hidden] = sample_pomdp(prior, transmat, obsmat, act)
%
% Inputs:
% prior(i) = Pr(Q(1)=i)
% transmat{a}(i,j) = Pr(Q(t)=j | Q(t-1)=i, A(t)=a)
% obsmat(i,k) = Pr(Y(t)=k | Q(t)=i)
% act(a) = A(t), so act(1) is ignored
%
% Output:
% obs and hidden are vectors of length T=length(act)

len = length(act);
hidden = mdp_sample(initial_prob, transmat, act);
obs = zeros(1, len);
for t=1:len
  obs(t) = sample_discrete(obsmat(hidden(t),:));
end


function wishart_test06 ( )

%*****************************************************************************80
%
%% WISHART_TEST06 compares the Wishart and Bartlett sample matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2013
%
%  Author:
%
%    John Burkardt
%

%
%  Access the PDFLIB and RNGLIB libraries.
%
  addpath ( '../pdflib' );
  addpath ( '../rnglib' );
%
%  Initialize the RNGLIB library.
%  Normally, we would do this just once, here at the beginning.
%  In this example, however, we really want to do it just before
%  we call each of the sampling routines, so that they both access
%  the same set of random numbers...
%
  initialize ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'WISHART_TEST06:\n' );
  fprintf ( 1, '  Verify that, if using the same set of random numbers,\n' );
  fprintf ( 1, '    W = T'' * T,\n' );
  fprintf ( 1, '  where\n' );
  fprintf ( 1, '    W = wishart_sample ( n, df, sigma );\n' );
  fprintf ( 1, '    T = bartlett_sample ( n, df, sigma );\n' );
%
%  Set the parameters.
%
  n = 3;
  df = 5;
  r = [ 5.0, 1.0, 3.0; ...
        0.0, 4.0, 2.0; ...
        0.0, 0.0, 6.0 ];
  sigma = r' * r;
  r8mat_print ( n, n, sigma, '  Covariance SIGMA:' );
%
%  Initialize the random number package and compute W.
%
  initialize ( );
  w = wishart_sample ( n, df, sigma );
%
%  Initialize the random number package again, and compute T.
%
  initialize ( );
  t = bartlett_sample ( n, df, sigma );
%
%  Compute T' * T and compare it to W.
%
  tt = t' * t;

  diff = r8mat_norm_fro_affine ( n, n, w, tt );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of error is %g\n', diff );
%
%  Release the PDFLIB and RNGLIB libraries.
%
  rmpath ( '../pdflib' );
  rmpath ( '../rnglib' );

  return
end


function D = hmm_kl (hmm_p,hmm_q)
% Computes Kullback-Leibler divergence between two Hidden Markov Model
% distributions, through an approximation (an upper bound) as proposed in
% M. Do (2003). IEEE Signal Processing Letters 10
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2018)

K = length(hmm_p.Pi);
if K~=length(hmm_q.Pi)
    error(['The two HMMs must have the same number of states, ' ...
        'and their order must correspond'])
end
if (hmm_p.train.order ~= hmm_q.train.order) || ...
        (~strcmpi(hmm_p.train.covtype,hmm_q.train.covtype)) || ...
        (length(hmm_p.train.embeddedlags) ~= length(hmm_q.train.embeddedlags)) || ...
        (any(hmm_p.train.embeddedlags ~= hmm_q.train.embeddedlags)) || ...
        (hmm_p.train.zeromean ~= hmm_q.train.zeromean)
   error('The state configuration of the two HMMs must be identical')
end
hmm = hmm_p; setstateoptions;
if isfield(hmm_p.state(1),'W')
    ndim = size(hmm_p.state(1).W.Mu_W,2);
else
    ndim = size(hmm_p.state(1).Omega.Gam_rate,2);
end
S = hmm.train.S==1;
regressed = sum(S,1)>0;

D = 0;
if hmm_p.train.id_mixture
   hmm_p.P = 1/K*ones(K); hmm_q.P = 1/K*ones(K);
end
nu = compute_nu (hmm_p.Pi,hmm_p.P); % weight vector

% Non-state specific stuff
switch train.covtype
    case {'uniquediag','shareddiag'}
        for n = 1:ndim
            if ~regressed(n), continue; end
            D = D + gamma_kl(hmm_p.Omega.Gam_shape,hmm_q.Omega.Gam_shape, ...
                hmm_p.Omega.Gam_rate(n),hmm_q.Omega.Gam_rate(n));
        end
    case {'uniquefull','sharedfull'}
        D = D + wishart_kl(hmm_p.Omega.Gam_rate(regressed,regressed),...
            hmm_q.Omega.Gam_rate(regressed,regressed), ...
            hmm_p.Omega.Gam_shape,hmm_q.Omega.Gam_shape);
    case 'pca'
        D = D + gamma_kl(hmm_p.Omega.Gam_shape,hmm_q.Omega.Gam_shape, ...
                hmm_p.Omega.Gam_rate,hmm_q.Omega.Gam_rate);
end

% State specific stuff
for k = 1:K

    % Trans probabilities
    kk = hmm.train.Pstructure(k,:);
    D = D + nu(k) * dirichlet_kl(hmm_p.Dir2d_alpha(k,kk),hmm_q.prior.Dir2d_alpha(k,kk));

    % State distribution
    hs = hmm_p.state(k);
    hs0 = hmm_q.state(k);

    if ~isempty(hs.W.Mu_W)
        if train.uniqueAR || ndim==1
            if train.uniqueAR || ndim==1
                D = D + nu(k) * gauss_kl(hs.W.Mu_W, hs0.W.Mu_W, hs.W.S_W, hs0.W.S_W);
            else
                D = D + nu(k) * gauss_kl(hs.W.Mu_W, hs0.W.Mu_W, ...
                    permute(hs.W.S_W,[2 3 1]), permute(hs0.W.S_W,[2 3 1]));
            end
        elseif strcmp(train.covtype,'diag') || ...
                strcmp(train.covtype,'uniquediag') || strcmp(train.covtype,'shareddiag') || ...
                strcmp(train.covtype,'pca')
            for n = 1:ndim
                D = D + nu(k) * gauss_kl(hs.W.Mu_W(Sind(:,n),n),hs0.W.Mu_W(Sind(:,n),n), ...
                    permute(hs.W.S_W(n,Sind(:,n),Sind(:,n)),[2 3 1]),...
                    permute(hs0.W.S_W(n,Sind(:,n),Sind(:,n)),[2 3 1]));
            end
        else % full or sharedfull
            mu_w = hs.W.Mu_W';
            mu_w = mu_w(:);
            mu_w0 = hs0.W.Mu_W';
            mu_w0 = mu_w0(:);
            D = D + nu(k) * gauss_kl(mu_w,mu_w0, hs.W.S_W, hs0.W.S_W);
        end
    end

    switch train.covtype
        case 'diag'
            for n=1:ndim
                if ~regressed(n), continue; end
                D = D + nu(k) * gamma_kl(hs.Omega.Gam_shape,hs0.Omega.Gam_shape, ...
                    hs.Omega.Gam_rate(n),hs0.Omega.Gam_rate(n));
            end
        case 'full'
            try
                D = D + nu(k) * wishart_kl(hs.Omega.Gam_rate(regressed,regressed),...
                    hs0.Omega.Gam_rate(regressed,regressed), ...
                    hs.Omega.Gam_shape,hs0.Omega.Gam_shape);
            catch
                error(['Error computing kullback-leibler divergence of the cov matrix - ' ...
                    'Something strange with the data?'])
            end
    end

    if ~isempty(orders) && ~train.uniqueAR && ndim>1
        for n1=1:ndim
            for n2=1:ndim
                if (train.symmetricprior && n2<n1) || S(n1,n2)==0, continue; end
                D = D + nu(k) * gamma_kl(hs.sigma.Gam_shape(n1,n2),hs0.sigma.Gam_shape(n1,n2), ...
                    hs.sigma.Gam_rate(n1,n2),hs0.sigma.Gam_rate(n1,n2));
            end
        end
    end
    if ~isempty(orders)
        for i=1:length(orders)
            D = D + nu(k) * gamma_kl(hs.alpha.Gam_shape,hs0.alpha.Gam_shape, ...
                hs.alpha.Gam_rate(i),hs0.alpha.Gam_rate(i));
        end
    end
end

end

function nu = compute_nu (Pi,P)
eps = 1e-6;
nu = Pi * P;
while true
    nu0 = nu;
    nu = nu * P;
    if mean(nu(:)-nu0(:))<eps, break; end
end
end


function [u] = spm_erp_u(t,P,M)
% returns the [scalar] input for EEG models (Gaussian function)
% FORMAT [u] = spm_erp_u(t,P,M)
% t      - PST (seconds)
% P      - parameter structure
%   P.R  - scaling of [Gaussian] parameters
%
% u   - stimulus-related (subcortical) input
%
% See spm_fx_erp.m and spm_erp_priors.m
%__________________________________________________________________________
%
% David O, Friston KJ (2003) A neural mass model for MEG/EEG: coupling and
% neuronal dynamics. NeuroImage 20: 1743-1755
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_erp_u.m 7679 2019-10-24 15:54:07Z spm $

% preliminaries - check durations (ms)
%--------------------------------------------------------------------------
try
    if length(M.dur) ~= length(M.ons)
        M.dur = M.dur(1) + M.ons - M.ons;
    end
catch
    M.dur = 32 + M.ons - M.ons;
end

% check sustained input (0,1)
%--------------------------------------------------------------------------
try
    if length(M.sus) ~= length(M.ons)
        M.sus = M.sus(1) + M.ons - M.ons;
    end
catch
    M.sus = 0 + M.ons - M.ons;
end

% stimulus - Gaussian (subcortical) impulse
%--------------------------------------------------------------------------
nu    = length(M.ons);
u     = sparse(length(t),nu);
t     = t*1000;
for i = 1:nu

    % Gaussian bump function
    %----------------------------------------------------------------------
    delay  = M.ons(i) + 128*P.R(i,1);
    scale  = M.dur(i) * exp(P.R(i,2));
    U      = exp(-(t - delay).^2/(2*scale^2));

    % sustained inputs
    %----------------------------------------------------------------------
    try
        prop = M.sus(i)*exp(P.R(i,3));
    catch
        prop = M.sus(i);
    end
    U      = prop*cumsum(U)/sum(U) + U*(1 - prop);
    u(:,i) = 32*U;
end


% Information-Flow Matte Refinement
% This function implements the matte refinement approach described in
% Yagiz Aksoy, Tunc Ozan Aydin, Marc Pollefeys, "Designing Effective
% Inter-Pixel Information Flow for Natural Image Matting", CVPR, 2017.
% 'alphaHat' and 'confidences' parameters are typically obtained by a
% sampling-based natural matting algorithm. 'confidences' is filled by
% ones if not provided. Optional input parameter 'params' can be
% customized by editing the default values in the struct returned
% by 'getMattingParams('IFM').
% - **_K parameters represent the number of nonlocal neighbors found
%   for color mixture,  and intra-U flows, while **_xyw define the effect of
%   spatial proximity.
% - **_mult define the weight of each information flow.
% - loc_*** define the parameters for the matting Laplacian.
% - refinement_mult determines how much trust is given to the initial
%   alpha estimation

function alpha = informationFlowMatteRefinement(image, trimap, alphaHat, confidences, params, suppressMessages)
    abmtSetup
    tic;
    if ~exist('confidences', 'var') || isempty(confidences)
        confidences = ones(size(alphaHat(:,:,1)));
    end
    if ~exist('params', 'var') || isempty(params)
        params = getMattingParams('IFM');
    end
    if ~exist('suppressMessages', 'var') || isempty(suppressMessages)
        suppressMessages = false;
    end
    if(~suppressMessages) display('Matte refinement via Information-Flow Matting...'); end

    image = im2double(image);
    trimap = im2double(trimap(:,:,1));
    alphaHat = im2double(alphaHat(:,:,1));

    % Compute L_IFM
    unk = trimap < 0.8 & trimap > 0.2;
    dilUnk = imdilate(unk, ones(3, 3));
    if(~suppressMessages) display('     Computing color mixture flow...'); end
    Lap = affinityMatrixToLaplacian(colorMixtureAffinities(image, params.cm_K, dilUnk, [], params.cm_xyw));
    Lap = params.cm_mult * (Lap' * Lap);
    if(~suppressMessages) display('     Computing matting Laplacian...'); end
    Lap = Lap + params.loc_mult * affinityMatrixToLaplacian(mattingAffinity(image, dilUnk, params.loc_win, params.loc_eps));
    if(~suppressMessages) display('     Computing intra-U flow...'); end
    Lap = Lap + params.iu_mult * affinityMatrixToLaplacian(colorSimilarityAffinities(image, params.iu_K, unk, unk, params.iu_xyw));

    if(~suppressMessages) display('     Solving for alphas...'); end
    alpha = solveForAlphas(Lap, trimap, params.lambda, params.usePCGtoSolve, alphaHat, confidences, params.refinement_mult);

    alpha = reshape(alpha, [size(image, 1), size(image, 2)]);

    dur = toc;
    if(~suppressMessages) display(['Done. It took ' num2str(dur) ' seconds.']); end
end


classdef UF5 < PROBLEM
% <multi> <real> <large/none>
% Unconstrained benchmark MOP

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, S. Zhao, P. N. Suganthan, W. Liu, and S. Tiwari,
% Multiobjective optimization test instances for the CEC 2009 special
% session and competition, School of CS & EE, University of Essex, Working
% Report CES-487, 2009.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = [0,zeros(1,obj.D-1)-1];
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            J1 = 3 : 2 : obj.D;
            J2 = 2 : 2 : obj.D;
            Y  = X - sin(6*pi*repmat(X(:,1),1,obj.D)+repmat(1:obj.D,size(X,1),1)*pi/obj.D);
            hY = 2*Y.^2 - cos(4*pi*Y) + 1;
            PopObj(:,1) = X(:,1)   + (1/20+0.1)*abs(sin(20*pi*X(:,1)))+2*mean(hY(:,J1),2);
            PopObj(:,2) = 1-X(:,1) + (1/20+0.1)*abs(sin(20*pi*X(:,1)))+2*mean(hY(:,J2),2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = (0:1:20)'/20;
            R(:,2) = 1 - R(:,1);
        end
    end
end


function days = month_length_republican ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_REPUBLICAN returns the number of days in a Republican month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of days
%    in the month.
%

%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_republican ( y2, m2 );

  if ( ierror ~= 0 )
    days = 0;
    return
  end
%
%  Get the number of days in the month.
%
  if ( 1 <= m2 && m2 <= 12 )
    days = 30;
  else if ( m2 == 13 )
    if ( year_is_leap_republican ( y2 ) )
      days = 6;
    else
      days = 5;
    end
  end

  return
end


function r = sqr(a)
%SQR          Taylor (elementwise) square  sqr(a)
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K1 = getappdata(0,'INTLAB_TAYLOR_ORDER') + 1;

  r = a;
  r.t(1,:) = sqr(a.t(1,:));
  for j=2:K1
    r.t(j,:) = sum(a.t(1:j,:).*a.t(j:-1:1,:),1);
  end

  if rndold
    setround(rndold)
  end


function [C,CompositeObjectiveValue, SingleObjectiveValues]=ITERevaluate_X0...
    (Problem,Constraints,Spec,Objectives,askflag,printflag)

if nargin < 5; askflag = true; end
if nargin < 6; printflag = true; end
if askflag
choice = txtmenu('What do you want to evaluate?',...
    'Current design point','Lower bound','Upper bound');
% choice = 0;
switch choice
    case 0
        x0 = Problem.x0(Problem.activeX);
    case 1
        x0 = Problem.lb(Problem.activeX);
    case 2
        x0 = Problem.ub(Problem.activeX);
end
else
    x0 = Problem.x0(Problem.activeX);
end

[C] = EVALnonlcons(x0,Problem,Constraints,Spec);

if printflag
jnk=Constraints.conLabels(Constraints.active_cons);
fprintf('%30s\t\t%s\n','CONSTRAINT','MARGIN (%)');
for ii = 1:nnz(Constraints.active_cons)
    fprintf('%30s\t\t%-3.1f\n',jnk{ii},-100*C(ii));
end
disp(' ')
end

[CompositeObjectiveValue, SingleObjectiveValues] = EVALobjective...
    (x0,Problem,Objectives,Spec,0,'single');

if printflag
jnk=Objectives.ObjLabels;

fprintf('%15s\t\t%10s\t\t%s\n','OBJECTIVE','WEIGHTING','VALUE');
for ii = 1:Objectives.nObj
    if Objectives.weightings(ii)
     fprintf('%15s\t\t%9.2f\t\t%-3.2d\n',jnk{ii},...
         Objectives.weightings(ii),SingleObjectiveValues(ii));
    end
end

disp(' ')
fprintf('Composite Objective Value:   %d\n',CompositeObjectiveValue')
end


function F = acscd(F, varargin)
%ACSCD   Inverse cosecant of a CHEBFUN, result in degrees.
%   ACSCD(F) computes the inverse cosecant (in degrees) of the CHEBFUN F.
%
%   ACSCD(F, PREF) does the same but uses the CHEBFUNPREF object PREF when
%   computing the composition.
%
% See also CSCD, ACSC.

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

% Call the compose method:
F = compose(F, @acscd, varargin{:});

end


% SCRIPT TEST THE DIRECT DYNAMICS OF A 2 DOF PLANAR ROBOT ROBOT

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.

close all

fprintf('\nTHE SIMULATION PRESENTS THE ROBOT AT AN INITIAL POSITION WHEN NO TORQUES ARE APPLIED\n')

%load robot parameters
robot=load_robot('example', '2dofplanar');

%simulate for 10 seconds, change this depending on your computer speed and
%the total time that you want to simulate
total_simulation_time = 10;

%initial position and joint speed
q0 = [0 0]';
qd0 = [0 0]';

drawrobot3d(robot, q0);
adjust_view(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The student should try different combinations of the following parameters:
%   g: the direction of the gravity vector. In this case, if we select g=[0  0 9.81]';
%       the movement of the arm is not affected by the gravity, since it
%       moves in a plane perpendicular to the gravity vector.
%   tau: the torques applied to each joint. The student should observe the effects of selecting
%       tau = [0 0 0]' or different values in combination with the
%       direction of the vector g.
%   robot.dynamics.friction = 0 selects no friction at the joints, whereas
%       robot.dynamics.friction = 1 considers that there exists friction. This
%       friction is modelled by robot.motors.Viscous (viscous friction) and
%       robot.motors.Coulomb (Coulomb friction). The student should observe
%       that selecting g=[0  9.81 0]' and tau = [0 0 0]' and
%       robot.dynamics.friction = 0 turns into an infinite triple pendulum
%       movement. In addition, selecting selecting g=[0  9.81 0]' and tau = [0 0 0]' and
%       robot.dynamics.friction = 1 simulates the case in which the triple
%       pendulum converges to a steady solution with the three links
%       hanging along the Y direction.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%you may redefine the gravity vector
%in this case you may one of the next two lines, that define
%the gravity acting along the Y axis or the Z axis, respectively.
g=[0  -9.81 0]'; %y0 axis

tau = [20 20]';%no torques applied
%select friction or not
robot.dynamics.friction = 1;

fprintf('\nCOMPUTING FORWARD DYNAMICS (this may take a while)')

%this may take a while, since it requires integration
%of the acceleration at each time step
[t, q, qd] = forwarddynamic(robot, total_simulation_time, q0, qd0, tau, g, []);

figure, plot(t, q), grid, title('Position vs. time')
xlabel('time (s)'), ylabel('Position (rad)')
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6');

figure, plot(t, qd), grid, title('Speed vs. time')
xlabel('time (s)'), ylabel('Speed (rad/s)')
legend('qd_1', 'qd_2', 'qd_3', 'qd_4', 'qd_5', 'qd_6');

%plot results faster in animate!
qq = q(:,1:2:end);

%animate it!!
animate(robot, qq)


classdef RMMEDA_F9 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing RM-MEDA

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, and Y. Jin, RM-MEDA: A regularity model-based
% multiobjective estimation of distribution algorithm, IEEE Transactions on
% Evolutionary Computation, 2008, 12(1): 41-63.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = [1,zeros(1,obj.D-1)+10];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,2:end).^2 - repmat(X(:,1),1,size(X,2)-1);
            g = sum(t.^2/4000,2) - prod(cos(t./repmat(sqrt(1:size(X,2)-1),size(X,1),1)),2) + 2;
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1)./g));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function varargout = contour3( f, varargin )
%CONTOUR3   3-D contour plot of a SPHEREFUN.
%   CONTOUR3(F) is a contour plot of F treating the values of F as heights
%   above the sphere, analagous to SURF(F, 'projection', 'bumpy'). A
%   contour plot shows the level curves of F for some values V. The values
%   V are chosen automatically.
%
%   CONTOUR3(F, N) draws N contour lines, overriding the automatic number.
%   The values V are still chosen automatically.
%
%   CONTOUR3(F, V) draws LENGTH(V) contour lines at the values specified in
%   the vector V. Use CONTOUR3(F, [V V]) to compute a single contour at the
%   level V.
%
%   CONTOUR3(F, 'NUMPTS', N) plots the contour lines on an N by N uniform
%   grid. If NUMPTS is not given then we plot on a 200 by 200 grid.
%
% See also CONTOUR, CONTOURF.

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

if ( isempty(f) )  % Empty check.
    contour3([]);
    return
end

holdState = ishold;

% Minimum number of plotting points:
minplotnum = 200;

% Extract from the inputs the user defined options:
j = 1;
argin = {};
while ( ~isempty(varargin) )
    if ( strcmpi(varargin{1}, 'numpts') ) % If given numpts then use them.
        minplotnum = varargin{2};
        varargin(1:2) = [];
    else
        argin{j} = varargin{1};
        varargin(1) = [];
        j = j+1;
    end
end

if ( isa(f, 'spherefun') )
    dom = f.domain;
    % Evaluate at equally spaced grid:
    x = linspace(dom(1), dom(2), minplotnum);
    y = linspace(dom(3), dom(4), minplotnum);
    vals = sample(f, minplotnum-1, minplotnum);
    vals = [vals vals(:,1)];
else
    error('CHEBFUN:SPHEREFUN:contour3:inputs', ...
        'Input must be a spherefun.');
end

if ( iscolat(f) )
    xc = @(ll,tt,rr) rr.*cos(ll).*sin(tt);
    yc = @(ll,tt,rr) rr.*sin(ll).*sin(tt);
    zc = @(tt,rr) rr.*cos(tt);
else
    xc = @(ll,tt,rr) rr.*cos(ll).*cos(tt);
    yc = @(ll,tt,rr) rr.*sin(ll).*cos(tt);
    zc = @(tt,rr) rr.*sin(tt);
end

% Use contour rather than contourc so that it can handle parsing the inputs
% correctly.
[c, h] = contour( x', y', vals, argin{:} );

% Extract out the options we need to plot the contours with plot3.
LW = 'LineWidth';
lw = h.LineWidth;
LS = 'LineStyle';
ls = h.LineStyle;
LC = 'Color';
lc = h.LineColor;
levelList = h.LevelList;
clrmap = parula(numel(levelList));

% Remove the contour plot that was generated.
delete(h);

scl = 0.15;           % Match the bumpy parameter in SPHEREFUN/SURF().
lim = [-1-scl 1+scl]; % Pad the axis limits.
m = minandmax2(f);

% If the plot is not being added to another, then set the axis properties.
if ( ~holdState )
    xlim(lim), ylim(lim), zlim(lim)
    daspect([1 1 1])
    grid on, box off
    hold on
end
view(3)

cl = size(c,2);
k = 1;
while ( k < cl )
    kl = c(2,k);
    v = k+1:k+kl;

    % Bump out the radial components according to the function values.
    rr = rescale(c(1,k), 1-scl, 1+scl, 'InputMin', m(1), 'InputMax', m(2));
    xv = xc(c(1,v), c(2,v), rr);
    yv = yc(c(1,v), c(2,v), rr);
    zv = zc(c(2,v), rr);

    % If the line color is a float then we are plotting all contours in a
    % single color.
    if ( isfloat(lc) )
        plot3(xv, yv, zv, LW, lw, LC, lc, LS, ls);
    else
        % We need to plot each contour in a color using the default
        % colormap.
        % Determine the color for the level being plotted.
        clr = clrmap(abs(c(1, k) - levelList) < 10*eps, :);
        plot3(xv, yv, zv, LW, lw, LC, clr, LS, ls);
    end

    k = k+kl+1;
end

if ( ~holdState )
    hold off
end

% Return plot handle if appropriate.
if ( nargout >= 1 )
    warning('CHEBFUN:SPHEREFUN:contour3:outputs', ...
        'Outputs from contour3 are not supported');
    varargout = { [], [] };
end

end


% 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

%%begin

% A serial link manipulator comprises a series of links.  Each link is described
% by four Denavit-Hartenberg parameters.
%
% Let's define a simple 2 link manipulator.  The first link is

L1 = Link('d', 0, 'a', 1, 'alpha', pi/2)

% The Link object we created has a number of properties
L1.a
L1.d

% and we determine that it is a revolute joint
L1.isrevolute

% For a given joint angle, say q=0.2 rad, we can determine the link transform
% matrix
L1.A(0.2)

% The second link is
L2 = Link('d', 0, 'a', 1, 'alpha', 0)

% Now we need to join these into a serial-link robot manipulator

bot = SerialLink([L1 L2], 'name', 'my robot')
% The displayed robot object shows a lot of details.  It also has a number of
% properties such as the number of joints
bot.n

% Given the joint angles q1 = 0.1 and q2 = 0.2 we can determine the pose of the
% robot's end-effector

bot.fkine([0.1 0.2])
% which is referred to as the forward kinematics of the robot.  This, and the
% inverse kinematics are covered in separate demos.

% Finally we can draw a stick figure of our robot

bot.plot([0.1 0.2])


% Script demonstrating usage of the ccmod function.
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-04-09
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'Copyright' and 'License' files
% distributed with the library.

% Training images
S0 = zeros(512, 512, 5);
S0(:,:,1) = single(stdimage('lena.grey')) / 255;
S0(:,:,2) = single(stdimage('barbara.grey')) / 255;
S0(:,:,3) = single(stdimage('kiel.grey')) / 255;
S0(:,:,4) = single(rgb2gray(stdimage('mandrill'))) / 255;
tmp = single(stdimage('man.grey')) / 255;
S0(:,:,5) = tmp(101:612, 101:612);

%Reduce images size to speed up demo script
tmp = zeros(256, 256, 5);
for k = 1:size(S0,3),
  tmp(:,:,k) = imresize(S0(:,:,k), 0.5);
end
S0 = tmp;

% Filter input images and compute highpass images
npd = 16;
fltlmbd = 5;
[Sl, Sh] = lowpass(S0, fltlmbd, npd);

% Load dictionary
load([sporco_path '/Data/ConvDict.mat']);
dmap = containers.Map(ConvDict.Label, ConvDict.Dict);
D0 = dmap('12x12x36');

% Set up cbpdn parameters
lambda = 0.1;
opt = [];
opt.Verbose = 1;
opt.MaxMainIter = 200;
opt.AutoRho = 1;
opt.AutoRhoPeriod = 1;
opt.RelaxParam = 1.8;

% Compute sparse representation on current dictionary
[X, optinf] = cbpdn(D0, Sh, lambda, opt);

% Set up ccmod parameters
opt = [];
opt.Verbose = 1;
opt.MaxMainIter = 500;
opt.sigma = size(Sh,3);
opt.AutoSigma = 1;
opt.AutoSigmaPeriod = 1;
opt.RelaxParam = 1.8;
opt.AuxVarObj = 1;

% Update dictionary for training set S
[D1, optinf1] = ccmod(X, Sh, size(D0), opt);

% Plot functional value and residuals
figure;
subplot(1,3,1);
plot(optinf1.itstat(:,2));
xlabel('Iterations');
ylabel('Functional value');
subplot(1,3,2);
semilogy(optinf1.itstat(:,4));
xlabel('Iterations');
ylabel('Primal residual');
subplot(1,3,3);
semilogy(optinf1.itstat(:,5));
xlabel('Iterations');
ylabel('Dual residual');

% Update dictionary with new filter sizes for training set S
dsz = [repmat([12 12]', [1 24]) repmat([8 8]', [1 12])];
[D2, optinf2] = ccmod(X, Sh, dsz, opt);

% Display dictionaries
figure;
subplot(1,3,1);
imdisp(tiledict(D0));
title('D0');
subplot(1,3,2);
imdisp(tiledict(D1));
title('D1');
subplot(1,3,3);
imdisp(tiledict(D2, dsz));
title('D2');

% Plot functional value evolution
figure;
plot(optinf1.itstat(:,2), 'r');
hold on;
plot(optinf2.itstat(:,2), 'b');
hold off;
xlabel('Iterations');
ylabel('Functional value');
legend('D2', 'D3');


function mimgR = red_channel_mean2(ops)

% numPlanes = length(ops.planesToProcess);

%% build file list with red channel

if (isfield(ops, 'SubDirsRed') && ~isempty(ops.SubDirsRed))
   subDirsRed = ops.SubDirsRed;
else
    if (isfield(ops, 'expred') && ~isempty(ops.expred))
        for i = 1:length(ops.expred)
            subDirsRed{i} = sprintf('%d', ops.expred(i));
        end
    else
        warning('could not find red channel info, returning...')
        return;
    end
end
%%
% build file list
fsrootRED = [];

for j = 1:length(subDirsRed)
    fsrootRED{j} = dir(fullfile(ops.RootDir, subDirsRed{j}, '*.tif'));

    for k = 1:length(fsrootRED{j})
        fsrootRED{j}(k).name = fullfile(ops.RootDir, subDirsRed{j}, fsrootRED{j}(k).name);
    end
end

%%
fsRED = [];
for j = 1:length(fsrootRED)
    fsRED = [fsRED {fsrootRED{j}.name}];
end

%%
D = [];
for k = 1:length(fsRED)
    data = loadFramesBuff(fsRED{k});

    data = cat(3, D, data);
end

%%
clear mimgR
for j = 1:ops.nplanes
    i0 = ops.nchannels_red + (j-1)*ops.nchannels_red;
    %mimgR(:,:,j) = mean(data(:,:,i0:ops.nplanes*ops.nchannels:end), 3);
    datj = data(:,:,i0:ops.nplanes*ops.nchannels_red:end);
    ops1  = AlignIterativeKriging(datj, ops);
    mimgR(:,:,j) = ops1.mimg;
end

%%


% DEMOIL5 Oil data with partially independent training conditional.

% FGPLVM

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

dataSetName = 'oil';
experimentNo = 5;

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

% Set up model
options = fgplvmOptions('pitc');
options.optimiser = 'scg';
latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model');

if exist('printDiagram') & printDiagram
  fgplvmPrintPlot(model, lbls, capName, experimentNo);
end

% Load the results and display dynamically.
fgplvmResultsDynamic(dataSetName, experimentNo, 'vector')

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


function [H2,Z2,sLen2,iter]=FGD_H(V,W,H,Z,Lpos,Lneg,tol)

% Fast gradient descent (FGD) with Newton method for NPAF
% Copyright@Naiyang Guan and Dacheng Tao
% Arguments:
%     sLen: step length

% Calculate scaled negative gradient at H
n=size(H,2);
G=H.*(W'*(V./Z)+H*Lneg)./(sum(W)'*ones(1,n)+H*Lpos)-H;
L=Lpos-Lneg;
Z1=W*G;
a=sum(sum(L.*(G'*G)));
d=sum(sum(L.*(G'*H)))+sum(sum(Z1));
C=Z./Z1;

% Newton method
sLen=1;
for iter=1:20,
    [sum1,sum2]=SumC1(V,C,sLen);
    sLen1=sLen-(a*sLen-sum1+d)/(a+sum2);
    if abs(sLen1-sLen)<tol,
        break;
    else
        sLen=sLen1;
    end
end

% Step length to boundary of positive orthant
if min(G(:))>=0
    sLen3=Inf;
else
    C=H./G;
    sLen3=min(-C(C<0));
end

% Best step length
sLen2=max(min(sLen1,0.99*sLen3),1);
H2=H+sLen2*G;
Z2=Z+sLen2*Z1;

return;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% shanChen.m: Multi-component fluid, using a LB method,
%   based on the Shan-Chen model
% [X.Shan and H.Chen, http://dx.doi.org/10.1103/PhysRevE.47.1815].
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Lattice Boltzmann sample, written in Matlab
% Copyright (C) 2008 Orestis Malaspinas, Andrea Parmigiani, Jonas Latt
% Address: EPFL-STI-LIN Station 9
% E-mail: orestis.malaspinas@epfl.ch
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
% You should have received a copy of the GNU General Public
% License along with this program; if not, write to the Free
% Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
% Boston, MA  02110-1301, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear
clf

% GENERAL FLOW CONSTANTS

ly           = 201;
lx           = 201;

G = -1.2;  % Amplitude of the molecular interaction force

omega1 = 1.;  % Relaxation parameter for fluid 1
omega2 = 1.;  % Relaxation parameter for fluid 2

maxT   = 80000;    % total number of iterations
tPlot  = 40;       % iterations between successive graphical outputs

% D2Q9 LATTICE CONSTANTS
tNS   = [4/9, 1/9,1/9,1/9,1/9, 1/36,1/36,1/36,1/36];
cxNS  = [  0,   1,  0, -1,  0,    1,  -1,  -1,   1];
cyNS  = [  0,   0,  1,  0, -1,    1,   1,  -1,  -1];
oppNS = [  1,   4,  5,  2,  3,    8,   9,   6,   7];

[y,x] = meshgrid(1:ly,1:lx);

drho = 0.001;
delta_rho = -drho*(1-2.0*rand(lx));

% INITIAL CONDITION FOR BOTH DISTRIBUTION FUNCTIONS: (T=0) ==> TIn(i) = t(i)
for i=1:9
    fIn(i,1:lx,1:ly) = tNS(i).*(1.0 + delta_rho);
    gIn(i,1:lx,1:ly) = tNS(i).*(1.0 - delta_rho);
end

rho1  = reshape(sum(fIn),lx,ly);
imagesc(rho1');
colorbar
title('Fluid 1 density');
axis equal off; drawnow

% MAIN LOOP (TIME CYCLES)
Gomega1 = G/omega1;
Gomega2 = G/omega2;
for cycle = 1:maxT
    % MACROSCOPIC VARIABLES
    rho1 = sum(fIn);
    rho2 = sum(gIn);
    jx1  = reshape ( (cxNS * reshape(fIn,9,lx*ly)), 1,lx,ly);
    jy1  = reshape ( (cyNS * reshape(fIn,9,lx*ly)), 1,lx,ly);
    jx2  = reshape ( (cxNS * reshape(gIn,9,lx*ly)), 1,lx,ly);
    jy2  = reshape ( (cyNS * reshape(gIn,9,lx*ly)), 1,lx,ly);

    rhoTot_OMEGA = rho1*omega1 + rho2*omega2;
    uTotX = (jx1*omega1+jx2*omega2) ./ rhoTot_OMEGA;
    uTotY = (jy1*omega1+jy2*omega2) ./ rhoTot_OMEGA;

    rhoContrib1x = 0.0;
    rhoContrib2x = 0.0;

    rhoContrib1y = 0.0;
    rhoContrib2y = 0.0;
    for i=2:9
        rhoContrib1x = rhoContrib1x + circshift(rho1*tNS(i), [0,cxNS(i),cyNS(i)])*cxNS(i);
        rhoContrib1y = rhoContrib1y + circshift(rho1*tNS(i), [0,cxNS(i),cyNS(i)])*cyNS(i);

        rhoContrib2x = rhoContrib2x + circshift(rho2*tNS(i), [0,cxNS(i),cyNS(i)])*cxNS(i);
        rhoContrib2y = rhoContrib2y + circshift(rho2*tNS(i), [0,cxNS(i),cyNS(i)])*cyNS(i);
    end

    uTotX1 = uTotX - Gomega1.*rhoContrib2x; %POTENTIAL CONTRIBUTION OF FLUID 2 ON 1
    uTotY1 = uTotY - Gomega1.*rhoContrib2y;

    uTotX2 = uTotX - Gomega2.*rhoContrib1x; %POTENTIAL CONTRIBUTION OF FLUID 2 ON 1
    uTotY2 = uTotY - Gomega2.*rhoContrib1y;

   % COLLISION STEP FLUID 1 AND 2
   for i=1:9
      cuNS1        = 3*(cxNS(i)*uTotX1+cyNS(i)*uTotY1);
      cuNS2        = 3*(cxNS(i)*uTotX2+cyNS(i)*uTotY2);

      fEq(i,:,:)   = rho1 .* tNS(i) .* ...
                       ( 1 + cuNS1 + 0.5*(cuNS1.*cuNS1) - 1.5*(uTotX1.^2+uTotY1.^2) );

      gEq(i,:,:)   = rho2 .* tNS(i) .* ...
                       ( 1 + cuNS2 + 0.5*(cuNS2.*cuNS2) - 1.5*(uTotX2.^2+uTotY2.^2) );

      fOut(i,:,:)  = fIn(i,:,:) - omega1 .* (fIn(i,:,:)-fEq(i,:,:));
      gOut(i,:,:)  = gIn(i,:,:) - omega2 .* (gIn(i,:,:)-gEq(i,:,:));
   end

   % STREAMING STEP FLUID 1 AND 2
   for i=1:9
      fIn(i,:,:) = circshift(fOut(i,:,:), [0,cxNS(i),cyNS(i)]);
      gIn(i,:,:) = circshift(gOut(i,:,:), [0,cxNS(i),cyNS(i)]);
   end

   % VISUALIZATION
   if(mod(cycle,tPlot)==0)
       rho1     = reshape(rho1,lx,ly);
       imagesc(rho1'); colorbar
       title('Fluid 1 density');
       axis equal off; drawnow
   end
end


function maskM = fastPolyFill(pointsM,optS)
%function maskM = fastPolyFill(pointsM,optS)
%fastPolyFill:  fills in images given polygons defining edges.
%Intention is to only fill a voxel if it's center is in the polygon.
%Faster than previous version by a significant factor (about x10).
%JOD; 17 June 05.
%Fixed bug, 5 July 05.
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

imageSizeV = optS.ROIImageSize;

xOffset = optS.xCTOffset;
yOffset = optS.yCTOffset;

numRows = double(imageSizeV(1));
numCols = double(imageSizeV(2));

xInV = pointsM(:,1);
yInV = pointsM(:,2);

maskM = zeros(numRows,numCols);
%could also be a double.

%preallocate a table of voxel indices
%'next to' refers to voxel centers just to the right of where polygon
%edges cross a line connecting voxel centers.
nextToPtsM = sparse(numRows * 4, numCols * 4); %hard to imagine bigger (sparse is faster than full)
numNextToPtsV = zeros(numRows,1);

%convert to "row and col space", that is, a continuous space where
%s is a coord that runs from 1 to numCols (along x axis)
%t is a coord that runs from 1 to numRows (along -y axis)
sInV = (xInV - xOffset)/optS.ROIxVoxelWidth + (numCols + 1)/2;
tInV = (yInV - yOffset)/optS.ROIyVoxelWidth + (numRows + 1)/2;

% % APA: snap points to integer-grid using nearest neighbor interpolation and
% % delete duplicates
% [S,T] = meshgrid(1:numCols+1,1:numRows+1);
% indSnap = interp2(S,T,reshape(1:(numCols+1)*(numRows+1),numRows+1,numCols+1),sInV,tInV,'*nearest');
% STpts = [S(ind2sub([numCols+1 numRows+1],indSnap)) T(ind2sub([numCols+1 numRows+1],indSnap))];
% goodPtsV = [1;any(diff(STpts),2)];
% sInV = STpts(find(goodPtsV),1);
% tInV = STpts(find(goodPtsV),2);

%For each polygonal element, compute and add one to 'row starters,' centers of
%voxels (assumed at image coords) whose rows cross the polygonal elements
%and are the first element to the right of that polygonal element's row crossing.
%
%Use the parameterization col = n * deltaRow + c + rowURPt    (URPt = upper
%right point).
%
%Algorithm:
%1.  Determine n
%2.  Determine c
%3.  Determine vector of row crossings
%4.  Determine vector of column crossings
%5.  Add one to row starters.

%Loop over polygonal elements
%For polygonal elements:

shift_sV = [sInV(end); sInV(1:end-1)];
shift_tV = [tInV(end); tInV(1:end-1)];

sM = [sInV(:), shift_sV];
tM = [tInV(:), shift_tV];

%Loop over polygonal edges, put ones where voxel centers are inside
%polygon.
for i = 1 : length(sInV)

    if tM(i,1) ~= tM(i,2)   %skip horizontal lines

        tMax = max([tM(i,1),tM(i,2)]);
        tMin = min([tM(i,1),tM(i,2)]);

        %determine n (line parameterization: s = n * t + c )
        n = (sM(i,1) - sM(i,2))/(tM(i,1) - tM(i,2));
        %determine c, could vectorize these two
        c = sM(i,1) - n * tM(i,1);

        %get delta_tV
        tPtsV = ceil(tMin) : floor(tMax);
%         tPtsV = tMin+1 : tMax; % APA
        delta_tV = tPtsV - tMax;
        sPtsV = n * delta_tV + c + n * tMax; %these are s values at edge 'crossings'

        %derive s values 'next to the right' of the crossings
        sVoxelsV = ceil(sPtsV);

        %catalogue
        for j = 1: length(sVoxelsV)
            num = numNextToPtsV(tPtsV(j));
            numNextToPtsV(tPtsV(j)) = num + 1;
            nextToPtsM(tPtsV(j),num+1) = sVoxelsV(j);
        end

    end

end

%This should be faster than the oft-used cumsum trick:
for i = 1 : numRows
    num = numNextToPtsV(i);
    if num ~=0
        %get 'em
        nextToPtsV = nextToPtsM(i,1:num);
        %sort 'em
        sortPtsV = sort(full(nextToPtsV));
        %fill image
        for j = 1 : length(sortPtsV)/2
            indV = sortPtsV(2*j-1) : sortPtsV(2*j) - 1;
            maskM(i * ones(1,length(indV)),indV) = 1;
        end
    end
end

%lastly, correct an oversight in writing the code: rows need to be flipped:
maskM = flipud(maskM);  %fast operation.


% [INPUT]
% va = A vector of floats [0,Inf) of length n representing the market values of assets.
% vap = Input argument representing the distributional parameters of assets whose type depends on the chosen option pricing model:
%   - for Black-Scholes-Merton, a float [0,Inf) representing the annualized volatility of assets;
%   - for Gram-Charlier, a row vector of floats (-Inf,Inf) of length 3 whose values represent respectively the annualized volatility, skewness and excess kurtosis of assets.
% cds = A vector of floats [0,Inf) of length n representing the credit default swap spreads.
% db = A float or a vector of floats [0,Inf) of length n representing the default barrier.
% r = A float or a vector of floats (-Inf,Inf) of length n representing the annualized risk-free interest rate.
% t = A float or a vector of floats (0,Inf) of length n representing the time to maturity of default barrier.
%
% [OUTPUT]
% el = A column vector of floats [0,Inf) of length n representing the expected losses.
% cl = A column vector of floats [0,Inf) of length n representing the contingent liabilities.
% a = A column vector of floats [0,1] of length n representing the contingent alphas.

function [el,cl,a] = contingent_claims_analysis(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('va',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' '2d' 'nonempty'}));
        ip.addRequired('vap',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('cds',@(x)validateattributes(x,{'double'},{'real' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('db',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('t',@(x)validateattributes(x,{'double'},{'real' 'finite' '>' 0 'vector' 'nonempty'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [va,vap,cds,db,r,t] = validate_input(ipr.va,ipr.vap,ipr.cds,ipr.db,ipr.r,ipr.t);

    nargoutchk(3,3);

    [el,cl,a] = contingent_claims_analysis_internal(va,vap,cds,db,r,t);

end

function [el,cl,a] = contingent_claims_analysis_internal(va,vap,cds,db,r,t)

    s = vap(1);
    st = s * sqrt(t);

    dbd = db .* exp(-r .* t);

    d1 = (log(va ./ db) + ((r + (0.5 * s^2)) .* t)) ./ st;
    d2 = d1 - st;

    put_price = (dbd .* normcdf(-d2)) - (va .* normcdf(-d1));

    if (numel(vap) == 3)
        g = vap(2);
        k = vap(3);

        t1 = (g / 6) .* ((2 * s) - d1);
        t2 = (k / 24) .* (1 - d1.^2 + (3 .* d1 .* s) - (3 * s^2));

        put_price = put_price - (va .* normcdf(d1) .* s .* (t1 - t2));
    end

    put_price = max(0,put_price);

    rd = dbd - put_price;

    cds_put_price = dbd .* (1 - exp(-cds .* max(0.5,((db ./ rd) - 1)) .* t));
    cds_put_price = min(cds_put_price,put_price);

    a = max(0,min(1 - (cds_put_price ./ put_price),1));
    a(~isreal(a)) = 0;

    el = put_price;
    cl = el .* a;

end

function [va,vap,cds,db,r,t] = validate_input(va,vap,cds,db,r,t)

    va = va(:);
    va_len = numel(va);

    if (va_len < 5)
        error('The value of ''va'' is invalid. Expected input to be a vector containing at least 5 elements.');
    end

    vap_len = numel(vap);

    if ((vap_len ~= 1) && (vap_len ~= 3))
        error('The value of ''vap'' is invalid. Expected input to be a vector containing either 1 or 3 elements.');
    end

    if (vap(1) < 0)
        error('The value of ''vap'' is invalid. Expected input first element to be greater than or equal to 0.');
    end

    cds = cds(:);

    if (numel(va) ~= va_len)
        error(['The value of ''cds'' is invalid. Expected input to be a vector of length ' num2str(va_len) ' elements.']);
    end

    if (all(cds >= 1))
        cds = cds ./ 10000;
    end

    data = {db(:) r(:) t(:)};

    l = unique(cellfun(@numel,data));
    l_scalar = (l == 1);

    if (any(l_scalar))
        if (any(l(~l_scalar) ~= va_len))
            error(['The number of elements of ''db'', ''r'' and ''t'' must be either 1 or equal to ' num2str(va_len) '.']);
        end
    else
        if (any(l ~= va_len))
            error(['The number of elements of ''db'', ''r'' and ''t'' must be either 1 or equal to ' num2str(va_len) '.']);
        end
    end

    for i = 1:numel(data)
        data_i = data{i};

        if (numel(data_i) == 1)
            data{i} = repmat(data_i,va_len,1);
        end
    end

    [db,r,t] = deal(data{:});

end


function Ximg = preimage_rbf(Xtr,sig2,U,B,type,npcs,maxIts)
%
% function Ximg = preimage_rbf(Xtr,sig2,U,B,type,npcs,maxIts)
%   Reconstruction or denoising after kernel PCA with RBF kernels, i.e. to find the
%   approximate preimage (in the input space) of the corresponding feature space expansions
%
% Inputs
%   Xtr     : N by d matrix of the training data used to find the prinicipal components.
%   sig2    : parameter for the RBF kernel used, k(x,z)=exp(-norm(x-z)^2/sig2).
%   U       : the eigenvectors computed from the kernel PCA using RBF kernel with parameter sig2.
%   B       : for reconstruction, B is the compressed data,
%               i.e. the projections of the data on to the first n PCs;
%             for denoising, B is the Nt by d matrix of original noisy data;
%             if not specified, Xtr is denoised instead.
%   type    : 'reconstruct' or 'denoise'
%   npcs    : number of PCs used for approximation
%   maxIts  : maximum iterations allowed to update the preimage, 1000 by default.
%
% Outputs
%   Ximg    : the reconstructed or denoised data in the input space
%
% Usage e.g.
%   >> [lam,U] = kpca(Xtr,'RBF_kernel',sig2);
%   >> [lam, perm] = sort(-lam); lam = -lam; U = U(:,perm);
%   >> projections = kernel_matrix(Xtr,'RBF_kernel',sig2,Xtest)'*U;
%   >> Xr = preimage_rbf(Xtr,sig2,U,projections(:,1:npcs),'r'); % Reconstruction
%   >> Xd = preimage_rbf(Xtr,sig2,U(:,1:npcs),Xnoisy,'d');      % Denoising
%   >> Xdtr = preimage_rbf(Xtr,sig2,U(:,1:npcs));  % Denoising on the training data
%
% see also:
%    kpca, denoise_kpca, RBF_kernel
%
% Reference
%   Mika S., Schoelkopf B., Smola A., Muller K.-R., Scholz M., Ratsch G. (1999), ``Kernel
%   PCA and de-noising in feature spaces'', Advances in Neural Information Processing
%   Systems 11, 536-542, MIT Press.
%

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

MAXDX=1e-6; % Convergence criterion
[~, dim]=size(Xtr);
mX=mean(Xtr); sX=std(Xtr);

if nargin<4, B = Xtr; end
[Nt, dimB] = size(B);

if nargin<5
    if dimB==dim, type='denoise'; else type='reconstruct'; end
else
    if type(1)~='d'&type(1)~='r'| (type(1)=='d'&dimB~=dim),
        warning('Invalid type specified, default value is used!')
        if dimB==dim, type='denoise'; else type='reconstruct'; end
    end
end

if nargin<6
    if type(1)=='r'; npcs=dimB; else npcs=size(U,2); end
else
    if npcs>size(U,2) | (type(1)=='r' & npcs~=dimB),
        warning('Invalid number of PCs, default value is used!'),
        if type(1)=='r'; npcs=dimB; else npcs=size(U,2); end
    end
end

if nargin<7, maxIts=1000; end

U=U(:,1:npcs);

for n=1:Nt
    cont=1; t=0; ts=0;
    if type(1)=='r'
        % reconstuction
        rs = U*B(n,:)';
        x = zeros(1, dim); % set the initial value of approximate preimage for reconstruction
        k = RBF_kernel(x,Xtr,sig2);
    else
        % denoise
        x = B(n,:); % initial value of the approximate preimage for denoising is the noisy data
        k = RBF_kernel(x,Xtr,sig2);
        rs = U*(k'*U)';
    end

    % iteratively update the approximate preimage x
    %
    while cont,

        d=rs'*k; % the reconstruction error is (const-2d)

        if d==0;
            % choose a different starting value
%            [k,id]=min(k); x_new = Xtr(id,:);
            randn('state',cputime+ts); x_new=(randn(1,dim)+mX).*sX;
            fprintf('%5d> Starting value changed!(d=0) \n', ts);
        else
            % update approximate preimage with a linear combination of kpca training data
            x_new = sum(rs.*k*ones(1,dim).*Xtr)/d;
        end

        dx = norm(x_new - x);
        x = x_new;

        t=t+1; ts=ts+1;

        if dx<MAXDX;
            cont = 0;
         % fprintf('%5d> Converged! \n', ts);
        else
            if ts>=maxIts;
                cont=0;
                fprintf('%5d> Maximum iteration reached!\n', ts);
            elseif t>=500;
                % choose a different starting value
%                [k,id]=min(k); x = Xtr(id,:);
                randn('state',cputime+ts); x=(randn(1,dim)+mX).*sX;
                t=0;
            end
        end
        k = RBF_kernel(x,Xtr,sig2);

    end % while
    Ximg(n,:)=x_new;
end % for


function A2 = project_to_simplex(A)
%PROJECT_TO_SIMPLEX Summary of this function goes here
%   Detailed explanation goes here
[N,M] = size(A);
[A1,IX] = sort(A',1,'descend');
J = ones(N,1)*(1:M); J = J';
A1cum = cumsum(A1,1)-1*ones(M,N);
A1minus = (A1 - (A1cum ./ J)) > 0;

rho = max(double(A1minus).*J,[],1);
theta = A1cum(sub2ind([M N],rho,(1:N)))./rho;

A1 = A1 - ones(M,1)*theta;
A1(A1<0) = 0;

% reorder
A2 = zeros(N,M);
for i = 1:M
    A2(:,i) = A1(IX==i);
end


function [mGal] = nmps22mGal(nmps2)
% Convert acceleration from nanometers per square-second to milligalileos
% Chad A. Greene 2012
mGal = nmps2*1e-4;


function G =  getGroupOverlapColor27(row, col)

    N = row*col;

    g = sparse(zeros(N,1));
    g = diag(g);

    %% build overlapping group

    % top let corner group

    g(1, 1 ) = 1;
    g(1, 2 ) = 1;
    g(1, 1+row ) = 1;
    g(1, 2+row ) = 1;

     % bottom let corner group

    g(row, row-1 ) = 1;
    g(row, row ) = 1;
    g(row, row-1+row ) = 1;
    g(row, row+row ) = 1;

    % top right corner group

    g((col-1)*row+1, (col-1)*row+1 ) = 1;
    g((col-1)*row+1, (col-2)*row+1 ) = 1;
    g((col-1)*row+1, (col-1)*row+2 ) = 1;
    g((col-1)*row+1, (col-2)*row+2 ) = 1;

    % bottom right corner group

    g((col-1)*row+row , (col-1)*row+row-1 ) = 1;
    g((col-1)*row+row , (col-2)*row+row-1 ) = 1;
    g((col-1)*row+row , (col-1)*row+row ) = 1;
    g((col-1)*row+row , (col-2)*row+row ) = 1;

    % boundary group

    for i=2:col-1
        % top row rgoup

        j = 1;

        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;

        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;

        g( (i-1)*row+j  , i*row+j   ) = 1;
        g( (i-1)*row+j  , i*row+j+1 ) = 1;

        % bottom row group

        j = row;

        g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;

        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;

        g( (i-1)*row+j  , i*row+j-1 ) = 1;
        g( (i-1)*row+j  , i*row+j   ) = 1;

    end

    for j=2:row-1
          % left column rgoup

        i=1;

        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;
        g( (i-1)*row+j  , i*row+j-1 ) = 1;
        g( (i-1)*row+j  , i*row+j   ) = 1;
        g( (i-1)*row+j  , i*row+j+1 ) = 1;

        % right  column group

        i=col;

        g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;

    end

    for i=2:col-1
        for j=2:row-1

            g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
            g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
            g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;
            g( (i-1)*row+j  , i*row+j-1 ) = 1;
            g( (i-1)*row+j  , i*row+j   ) = 1;
            g( (i-1)*row+j  , i*row+j+1 ) = 1;

        end
    end

     G =[g; g; g];

end


function [fp, fm] = partition(f)
% PARTITION   Partition a DISKFUN into its even/periodic odd/anti-periodic
% parts.
%
% [FP, FM] = partition(F) partitions F into two diskfuns FP & FM with the
% following properties:
%
%   FP has a CDR decomposition such that C is even and R is pi periodic.
%   FM has a CDR decomposition such that C is odd and R is pi
%   anti-periodic.
%
% See also DISKFUN/COMBINE.

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

if ( ~isa(f,'diskfun') )
    error('DISKFUN:partition:unknown',['Undefined function ''partition'' for ' ...
        'input argument of type %s.'], class(f));
end

if ( isempty(f) )
    fp = diskfun();
    fm = diskfun();
    return
end

% Do the even-pi-periodic case first.
id = f.idxPlus;

if isempty(id)
    fp = diskfun();
else
    fp = f;
    fp.cols = fp.cols(:, id);
    fp.rows = fp.rows(:, id);
    fp.pivotValues = fp.pivotValues(id, :);
    fp.pivotLocations = fp.pivotLocations(id, :);
    fp.idxPlus = 1:length(id);
    fp.idxMinus = [];
end

% Now the odd case.
id = f.idxMinus;

if ( isempty(id) )
    fm = diskfun();
else
    fm = f;
    fm.cols = fm.cols(:, id);
    fm.rows = fm.rows(:, id);
    fm.pivotValues = fm.pivotValues(id);
    fm.pivotLocations = fm.pivotLocations(id, :);
    fm.idxMinus = 1:length(id);
    fm.idxPlus = [];
    fm.nonZeroPoles = 0;
end

end


function p = square_uniform ( n )

%*****************************************************************************80
%
%% SQUARE_UNIFORM returns sample points from the unit square.
%
%  Discussion:
%
%    This routine returns N points sampled uniformly at random
%    from within the unit square.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points to generate.
%
%    Output, real P(N,2), the sample points.
%
  p = rand ( n, 2 );

  return
end


% SP_HCURL_ERROR: Evaluate the error in H(curl) norm.
%
%   [errhcurl, errl2, errcurl] = sp_hcurl_error (space, msh, u, uex, curluex);
%
% INPUT:
%
%    space:   object defining the space of discrete functions (see sp_vector)
%    msh:     object defining the domain partition and the quadrature rule (see msh_cartesian)
%    u:       vector of dof weights
%    uex:     function handle to evaluate the exact solution
%    curluex: function handle to evaluate the curl of the exact solution
%
% OUTPUT:
%
%     errhcurl: error in H(curl) norm
%     errl2:    error in L^2 norm
%     errcurl:  error of the curl in L^2 norm
%
% Copyright (C) 2010 Carlo de Falco, Rafael Vazquez
% Copyright (C) 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [errhcurl, errl2, errcurl] = sp_hcurl_error (space, msh, u, uex, curluex)

  if (numel(u) ~= space.ndof)
    error ('Wrong size of the vector of degrees of freedom')
  end

  errl2 = 0;
  errcurl = 0;

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp_col  = sp_evaluate_col (space, msh_col, 'value', true, 'curl', true);

    [~, err_l2, err_curl] = sp_hcurl_error (sp_col, msh_col, u, uex, curluex);

    errcurl = errcurl + err_curl.^2;
    errl2 = errl2 + err_l2.^2;
  end

  errhcurl = sqrt (errl2 + errcurl);
  errl2 = sqrt (errl2);
  errcurl = sqrt (errcurl);

end


function b = r8vec_bracket6 ( nd, xd, ni, xi )

%*****************************************************************************80
%
%% R8VEC_BRACKET6 brackets data between successive entries of a sorted R8VEC.
%
%  Discussion:
%
%    We assume XD is sorted.
%
%    If XI(I) is contained in the interval [XD(1),XD(N)], then the value of
%    B(I) indicates that XI(I) is contained in [ XD(B(I)), XD(B(I)+1) ].
%
%    If XI(I) is not contained in the interval [XD(1),XD(N)], then B(I) = -1.
%
%    This code implements a version of binary search which is perhaps more
%    understandable than the usual ones.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ND, the number of data values.
%
%    Input, real XD(N,1), the sorted data.
%
%    Input, integer NI, the number of inquiry values.
%
%    Input, real XI(NI,1), the query values.
%
%    Output, integer B(NI,1), the bracket information.
%
  xd = xd(:);
  xi = xi(:);
  b = zeros ( ni, 1 );

  for i = 1 : ni

    if ( xi(i) < xd(1) || xd(nd) < xi(i) )

      b(i) = -1;

    else

      l = 1;
      r = nd;

      while ( l + 1 < r )
        m = floor ( ( l + r ) / 2 );
        if ( xi(i) < xd(m) )
          r = m;
        else
          l = m;
        end
      end

      b(i) = l;

    end

  end

  return
end


% This is the main FastSLAM loop. This script calls all the required
% functions in the correct order.
%
% You can disable the plotting or change the number of steps the filter
% runs for to ease the debugging. You should however not change the order
% or calls of any of the other lines, as it might break the framework.
%
% If you are unsure about the input and return values of functions you
% should read their documentation which tells you the expected dimensions.

% Turn off pagination:
more off;

clear all;
close all;

% Make tools available
addpath('tools');

% Read world data, i.e. landmarks. The true landmark positions are not given to the robot
landmarks = read_world('../data/world.dat');
% Read sensor readings, i.e. odometry and range-bearing sensor
data = read_data('../data/sensor_data.dat');

% Get the number of landmarks in the map
N = size(landmarks,2);

noise = [0.005, 0.01, 0.005]';

% how many particles
numParticles = 100;

% initialize the particles array
particles = struct;
for i = 1:numParticles
  particles(i).weight = 1. / numParticles;
  particles(i).pose = zeros(3,1);
  particles(i).history = cell();
  for l = 1:N % initialize the landmarks aka the map
    particles(i).landmarks(l).observed = false;
    particles(i).landmarks(l).mu = zeros(2,1);    % 2D position of the landmark
    particles(i).landmarks(l).sigma = zeros(2,2); % covariance of the landmark
  end
end

% toogle the visualization type
%showGui = true;  % show a window while the algorithm runs
showGui = false; % plot to files instead

% Perform filter update for each odometry-observation pair read from the
% data file.
for t = 1:size(data.timestep, 2)
%for t = 1:50
    printf('timestep = %d\n', t);

    % Perform the prediction step of the particle filter
    particles = prediction_step(particles, data.timestep(t).odometry, noise);

    % Perform the correction step of the particle filter
    particles = correction_step(particles, data.timestep(t).sensor);

    % Generate visualization plots of the current state of the filter
    plot_state(particles, landmarks, t, data.timestep(t).sensor, showGui);

    % Resample the particle set
    particles = resample(particles);
end


function value = grad(psi,co2)

N = psi.bandwidth;
A = psi.A(:);

k = 1:0.5:(N+1)/2;
k(2:2:end) = 0;
l = (N+1)/2:0.5:N;
l(end-1:-2:1)=0;
M = triu(hankel(k,l)-toeplitz([0,k(1:end-1)]));

Ahat = (-4) * (M*A(2:end));

Dpsi = SO3Kernel(Ahat);

% TODO: Interpolation error: Try
% psi = SO3DeLaValleePoussinKernel(90);
% psi2=SO3Kernel(psi.A);
% omega = 35*degree:0.01:pi;
% plot(omega/degree,psi.grad(cos(omega/2)) )
% hold on
% plot(omega/degree, psi2.grad(cos(omega/2)) )
% hold off

if nargin == 2
  value = Dpsi.eval(co2).*co2.*sqrt(1-co2.^2);
else
  value = SO3KernelHandle(@(co2) Dpsi.eval(co2).*co2.*sqrt(1-co2.^2));
end


% Driver script for solving the 3D advection equations
Globals3D;

% Order of polymomials used for approximation
N = 8;

% Generate simple mesh
filename = 'cubeK6.neu'
[Nv, VX, VY, VZ, K, EToV] = MeshReaderGambit3D(filename);

% Initialize solver and construct grid and metric
StartUp3D;

% set initial conditions
u = exp(-1*(x.^2 + y.^2 + z.^2));

% Solve Problem
FinalTime = 0.8;
[u] = Advec3D(u,FinalTime);


function [dLdp,iCpY,L] = mci_approach_deriv (P,M,U,Y)
% Gradient of log-likelihood for approach model
% FORMAT [dLdp,iCpY,L] = mci_approach_deriv (P,M,U,Y)
%
% dLdp      gradient of log joint
% iCpY      curvature (Fisher Information)
% L         log joint
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_approach_deriv.m 6548 2015-09-11 12:39:47Z will $

G = mci_approach_gen (P,M,U);
if isstruct(Y)
    e = Y.y-G;
else
    e = Y-G;
end

V=exp(P(1));
tau=exp(P(2));
t=U.X;

y=-60+V*(1-exp(-t/tau));

dydp = [V*(1-exp(-t/tau)),-V*exp(-t/tau).*(t/tau)];
dLdp = dydp'*M.iCe*e;
iCpY = dydp'*M.iCe*dydp;

if nargout > 2
    L = mci_approach_like (P,M,U,Y);
end


function [parent, post] = cs_etree (A, mode)                                %#ok
%CS_ETREE elimination tree of A or A'*A.
%   parent = cs_etree (A) returns the elimination tree of A.
%   parent = cs_etree (A,'col') returns the elimination tree of A'*A.
%   parent = cs_etree (A,'sym') is the same as cs_etree(A).
%   For the symmetric case (cs_etree(A)), only triu(A) is used.
%
%   [parent,post] = cs_etree(...) also returns a postorder of the tree.
%
%   Example:
%       Prob = UFget ('HB/bcsstk01') ; A = Prob.A ;
%       parent = cs_etree (A) ; treeplot (parent) ;
%
%   See also ETREE, TREEPLOT.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

error ('cs_etree mexFunction not found') ;


classdef RodAlign < dagnn.Layer

    properties
        subdivisions = [6 6]
        transformation = 1/8
    end

    methods
        function outputs = forward(obj, inputs, params)
          % calculation on cpu is faster for matlab
          inputs{1} = gather(inputs{1});
          inputs{2} = gather(inputs{2});

          feat_h = size(inputs{1},1);
          feat_w = size(inputs{1},2);
          num_roi = size(inputs{2},2);
          roi_h_start = max(round(inputs{2}(2,:) * obj.transformation),1);
          roi_w_start = max(round(inputs{2}(3,:) * obj.transformation),1);
          roi_h_end = min(round(inputs{2}(4,:) * obj.transformation),feat_h);
          roi_w_end = min(round(inputs{2}(5,:) * obj.transformation),feat_w);

          scale_h = feat_h / obj.subdivisions(1);
          scale_w = feat_w / obj.subdivisions(2);
          bin_h = ((1:obj.subdivisions(1))-0.5) * scale_h;
          bin_W = ((1:obj.subdivisions(2))-0.5) * scale_w;

          floor_bin_h = max(floor(bin_h),1);
          ceil_bin_h = floor_bin_h+1;
          floor_bin_w = max(floor(bin_W),1);
          ceil_bin_w = floor_bin_w+1;
          c11 = min(ceil_bin_h-bin_h,1)'*min(ceil_bin_w-bin_W,1);
          c12 = min(ceil_bin_h-bin_h,1)'*max(bin_W-floor_bin_w,0);
          c21 = max((bin_h-floor_bin_h),0)'*min(ceil_bin_w-bin_W,1);
          c22 = max(bin_h-floor_bin_h,0)'*max(bin_W-floor_bin_w,0);
          feats = repmat(inputs{1},1,1,1,num_roi);
          for i = 1:num_roi
              feats(roi_h_start(i):roi_h_end(i),roi_w_start(i):roi_w_end(i),:,i) = 0;
          end
          outputs = c11.*feats(floor_bin_h,floor_bin_w,:,:) ...
                  + c12.*feats(floor_bin_h,ceil_bin_w,:,:) ...
                  + c21.*feats(ceil_bin_h,floor_bin_w,:,:) ...
                  + c22.*feats(ceil_bin_h,ceil_bin_w,:,:);
          outputs = {gpuArray(outputs)};

        end

        function [derInputs, derParams] = backward(obj, inputs, param, derOutputs)
          %When assigning into a GPUArray, the subscripts must contain unique values.
          inputs{1} = gather(inputs{1});
          inputs{2} = gather(inputs{2});
          derOutputs{1} = gather(derOutputs{1});

          feat_h = size(inputs{1},1);
          feat_w = size(inputs{1},2);
          ch = size(inputs{1},3);
          num_roi = size(inputs{2},2);
          roi_h_start = max(round(inputs{2}(2,:) * obj.transformation),1);
          roi_w_start = max(round(inputs{2}(3,:) * obj.transformation),1);
          roi_h_end = min(round(inputs{2}(4,:) * obj.transformation),feat_h);
          roi_w_end = min(round(inputs{2}(5,:) * obj.transformation),feat_w);

          scale_h = feat_h / obj.subdivisions(1);
          scale_w = feat_w / obj.subdivisions(2);
          bin_h = ((1:obj.subdivisions(1))-0.5) * scale_h;
          bin_W = ((1:obj.subdivisions(2))-0.5) * scale_w;

          floor_bin_h = max(floor(bin_h),1);
          ceil_bin_h = floor_bin_h+1;
          floor_bin_w = max(floor(bin_W),1);
          ceil_bin_w = floor_bin_w+1;
          c11 = min(ceil_bin_h-bin_h,1)'*min(ceil_bin_w-bin_W,1);
          c12 = min(ceil_bin_h-bin_h,1)'*max(bin_W-floor_bin_w,0);
          c21 = max((bin_h-floor_bin_h),0)'*min(ceil_bin_w-bin_W,1);
          c22 = max(bin_h-floor_bin_h,0)'*max(bin_W-floor_bin_w,0);

          derbilinear = zeros([feat_h,feat_w,ch,num_roi],'single');
          for i = 1:num_roi
              derbilinear(floor_bin_h,floor_bin_w,:,i) = derbilinear(floor_bin_h,floor_bin_w,:,i) + c11 .* derOutputs{1}(:,:,:,i);
              derbilinear(floor_bin_h,ceil_bin_w,:,i) = derbilinear(floor_bin_h,ceil_bin_w,:,i) + c12 .* derOutputs{1}(:,:,:,i);
              derbilinear(ceil_bin_h,floor_bin_w,:,i) = derbilinear(ceil_bin_h,floor_bin_w,:,i) + c21 .* derOutputs{1}(:,:,:,i);
              derbilinear(ceil_bin_h,ceil_bin_w,:,i) = derbilinear(ceil_bin_h,ceil_bin_w,:,i) + c22 .* derOutputs{1}(:,:,:,i);
              derbilinear(roi_h_start(i):roi_h_end(i),roi_w_start(i):roi_w_end(i),:,i) = 0;
          end
          derInputs{1} = gpuArray(sum(derbilinear,4));

          derInputs{2} = [];
          derParams = {} ;
        end

        function obj = RodAlign(varargin)
          obj.load(varargin);
        end
    end
end


function [thrs, fars, frrs] = slroc(scores, signs, thres, op)
%SLROC Computes the ROC
%
% $ Syntax $
%   - [thrs, fars, frrs] = slroc(scores, signs, thres, op)
%
% $ Arguments $
%   - scores:           the scores representing the signal intensities
%   - signs:            the signs representing the groundtruth (0 or 1)
%   - thres:            the descriptor indicating how to sample the
%                       thresholds at which the rate is computed
%   - op:               the option stating the attributes of the scores.
%   - thrs:             the sampled threshold values
%   - fars:             the false accept rates at the sampled thresholds
%   - frrs:             the false reject rates at the sampled thresholds
%
% $ Description $
%   - [thrs, fars, frrs] = slroc(scores, signs, thres, op) Computes the
%     ROC of a receiver from the scores and groudtruth signs. The argument
%     thres specifies the sampled thresholds at which the false accept
%     rate and false reject rate is evaluated. If thres is an integer,
%     say n, then n equal-interval integers from lowest to highest scores
%     are taken as samples. The thres can also be a vector containing the
%     sampled thresholds, which should be arranged in ascending order.
%     op states the attributes of the scores, which
%     takes either of the two values: 'high' or 'low'. If op is 'high', a
%     higher score indicates a better match; if op is 'low', a lower score
%     indicates a better match. The cumulative score will be computed
%     up to the number of all classes.
%     For output, if n thresholds are sampled, then thrs, fars, and frrs
%     for all n x 1 vectors, containing the sampled threshold values,
%     and the corresponding false accept rates, and false reject rates.
%
% $ History $
%   - Created by Dahua Lin on Jun 9th, 2005
%   - Modified by Dahua Lin on May 1st, 2006
%     - Base on the sltoolbox v4
%   - Modified by Dahua Lin on Aug 8th, 2006
%     - Base on slhistroc
%

%% parse and verify the input arguments
if nargin < 4
    raise_lackinput('slroc', 4);
end
if ~isequal(size(scores), size(signs))
    error('sltoolbox:sizmismatch', ...
        'The sizes of scores and signs are not match');
end

% the following two statements are disabled in the 2006-08-08 modification
% since scores(signs) will automatic serialize the values
% there is no need to write this statement, just a waste of time and mem
% scores = scores(:);
% signs = logical(signs(:));

if numel(thres) == 1
    n = thres;
    highscore = max(max(scores));
    lowscore = min(min(scores));
    thrs = linspace(lowscore, highscore, n)';
else
    thrs = thres(:);
end

%% compute
scores_a = scores(signs);
scores_r = scores(~signs);

hist_a = histc(scores_a, thrs);
hist_r = histc(scores_r, thrs);

[thrs, fars, frrs] = slhistroc(hist_a, hist_r, thrs, op);


function d = month_to_ides_roman ( m )

%*****************************************************************************80
%
%% MONTH_TO_IDES_ROMAN returns the day of the ides of a Roman month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the month index.
%
%    Output, integer D, the day of the ides of the month.
%
  ides = [ 13, 13, 15, 13, 15, 13, 15, 13, 13, 15, 13, 13 ];

  if ( m < 1 || 12 < m )
    d = -1;
  else
    d = ides(m);
  end

  return
end


function [L,S] = gosus(M,opts)
  regularizer = 0.1;
  param.superpixel.slicParam = [10, regularizer; 20, regularizer; 40,regularizer; 80, regularizer;];
  %  param.superpixel.slicParam = [5,regularizer;  10, regularizer; 20, regularizer;  40,regularizer ; ];
  param.maxIter = 200;
  param.tol = 1e-4;
  param.lambda = 0.33;    %   bestAccu = 0.9762, lambda = 0.33; threshold = 8e-6;
  param.rank = 2; % subspace dimension
  param.eta = 1e-3;   % stepsize for subspace updating
  %param.sampleSize = 20; % number of sample frames to learn the background
  %param.trainSize = 24; % sample from the fist 240 frames to learn the background
  %param.startIndex = 1;   %  which frame to start
  %param.randomStart = true;
  param.admm.I = speye(size(M,1)*3);

  %U = orth(randn(size(M,1)*3, param.rank));
  [U, ~] = svds([M(:,1:25);M(:,1:25);M(:,1:25)], param.rank);

  for i = 1:size(M,2)
    disp(i);
    I = M(:,i);
    im = im2uint8(reshape(I,[opts.rows opts.cols]));
    im = repmat(im,[1 1 3]);

    param.admm.G = getGroupSuperColor27(im, param);
    param.admm.Z = sparse(size(M,1)*3, size(param.admm.G, 2), 0);
    param.admm.Y = param.admm.Z;

    v = double(im(:))/255.0;

    [x, w] = solveWXADMM(U, v, param);

    pL = U*w; % low-rank
    pS = pL-v; % sparse
    %E = v;

    imgL = reshape(pL, size(im));
    imgS = reshape(pS, size(im));

    vL = imgL(:,:,1);
    vS = imgS(:,:,1);

    vL = vL(:);
    vS = vS(:);

    L(:,i) = vL;
    S(:,i) = vS;

    %L = U*w; % low-rank
    %E = v;

    %fg = L-v;
    %fg(abs(x)<=8e-10) = 0;
    %fg(abs(x)>8e-10) = 1;

    %subplot(1,3,1), imshow(im,[]);
    %subplot(1,3,2), imshow(reshape(L, size(im)),[]);
    %subplot(1,3,3), imshow(reshape(fg, size(im)),[]);

    residual = param.lambda*(U*w + x - v);
    U = updateSubspace(U, residual, w, param);

    %pause(.01);
  end
end


function i4vec_order_type_test ( )

%*****************************************************************************80
%
%% I4VEC_ORDER_TYPE_TEST tests I4VEC_ORDER_TYPE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 4;
  test_num = 6;
%
%  Each ROW of the definition is a COLUMN of the matrix.
%
  x_test = [ ...
    1, 3, 2, 4; ...
    2, 2, 2, 2; ...
    1, 2, 2, 4; ...
    1, 2, 3, 4; ...
    4, 4, 3, 1; ...
    9, 7, 3, 0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_ORDER_TYPE_TEST\n' );
  fprintf ( 1, '  I4VEC_ORDER_TYPE classifies an integer vector as\n' );
  fprintf ( 1, '  -1: no order\n' );
  fprintf ( 1, '   0: all equal;\n' );
  fprintf ( 1, '   1: ascending;\n' );
  fprintf ( 1, '   2: strictly ascending;\n' );
  fprintf ( 1, '   3: descending;\n' );
  fprintf ( 1, '   4: strictly descending.\n' );

  for test = 1 : test_num

    x(1:n) = x_test(1:n,test);

    order = i4vec_order_type ( n, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  The following vector has order type %d\n', order );
    fprintf ( 1, '\n' );
    for j = 1 : n
      fprintf ( 1, '  %6d  %6d\n', j, x(j) );
    end

  end

  return
end


function [ n_data, a, x, fx ] = jacobi_dn_values ( n_data )

%*****************************************************************************80
%
%% JACOBI_DN_VALUES returns some values of the Jacobi elliptic function DN(A,X).
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      JacobiDN[ x, a ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real A, the parameter of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  a_vec = [ ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.5E+00, ...
     0.5E+00, ...
     0.5E+00, ...
     0.5E+00, ...
     0.5E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00 ];

  fx_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.9975093485144243E+00, ...
     0.9901483195224800E+00, ...
     0.9429724257773857E+00, ...
     0.8231610016315963E+00, ...
     0.7108610477840873E+00, ...
     0.9950207489532265E+00, ...
     0.9803279976447253E+00, ...
     0.8868188839700739E+00, ...
     0.6480542736638854E+00, ...
     0.2658022288340797E+00, ...
     0.3661899347368653E-01, ...
     0.9803279976447253E+00, ...
     0.8868188839700739E+00, ...
     0.6480542736638854E+00, ...
     0.2658022288340797E+00  ];

  x_vec = [ ...
      0.1E+00, ...
      0.2E+00, ...
      0.5E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      0.1E+00, ...
      0.2E+00, ...
      0.5E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      0.1E+00, ...
      0.2E+00, ...
      0.5E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      4.0E+00, ...
     -0.2E+00, ...
     -0.5E+00, ...
     -1.0E+00, ...
     -2.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [] = plotWavefield(f, c, fs, T, thetaArrivalAngles, phiArrivalAngles, amplitudes)

if isscalar(f)
    f = f*ones(1,numel(thetaArrivalAngles));
end

if ~exist('amplitudes','var')
    amplitudes = ones(1,numel(thetaArrivalAngles));
end

% Grid-points for calculating the field
[x, y] = meshgrid(-0.5:0.02:0.5,-0.5:0.02:0.5);
nPoints = size(x,1);
xPoints = x(:)';
yPoints = y(:)';
nSamples = T*fs;
t = 0:1/fs:nSamples/fs-1/fs;

signal_field = 0;
for k = 1:numel(thetaArrivalAngles)
    doa = squeeze(steeringVector(xPoints, yPoints, f(k), c, thetaArrivalAngles(k), phiArrivalAngles(k)));
    signal = 10^(amplitudes(k)/20)*doa*exp(1j*2*pi*f(k)*t);
    signal_field = signal_field + signal;
end

figure(11);clf
set(gcf,'color','w')
colormap('spring')
cmap = [1 1 1]*0.5;
%plot(0.5*cos(0:pi/50:2*pi),0.5*sin(0:pi/50:2*pi),'Color',[1 1 1],'linewidth',1.5)
hold on
set(gca,'color',[0 0 0],'xcolor',cmap,'ycolor',cmap,'zcolor',cmap)
set(gca,'XTickLabel',[],'YTickLabel',[],'ZTickLabel',[])
set(gca,'XMinorGrid','on','YMinorGrid','on','ZMinorGrid','on','MinorGridColor',[1 1 1],'MinorGridLineStyle','-')
axis([-0.5 0.5 -0.5 0.5 -0.5 0.5])
view(110,25)
text(0.46,-0.5,-0.45,'x','color',[1 1 1])
text(-0.5,0.46,-0.45,'y','color',[1 1 1])
text(-0.5,-0.5,0.46,'z','color',[1 1 1])

z = reshape(real(signal_field(:,1))'/25,nPoints,nPoints);
h = surf(x,y,z,'edgecolor','none','FaceAlpha',0.7');
h.ZDataSource = 'z';

title(['t = ' num2str(sprintf('%0.1f',t(1)*1e3)) ' ms'],'fontweight','normal')

for sample = 2:nSamples
    pause(0.01)

    z = reshape(real(signal_field(:,sample))'/25,nPoints,nPoints);
    refreshdata(h, 'caller')

    title(['t = ' num2str(sprintf('%0.1f',t(sample)*1e3)) ' ms'],'fontweight','normal')

end