classdef DASCMOP8 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Difficulty-adjustable and scalable constrained benchmark MOP

%------------------------------- Reference --------------------------------
% Z. Fan, W. Li, X. Cai, H. Li, C. Wei, Q. Zhang, K. Deb, and E. Goodman,
% Difficulty adjustable and scalable constrained multi-objective test
% problem toolkit, Evolutionary Computation, 2020, 28(3): 339-378.
%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Wenji Li

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 3;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            x_all = X(:,3:1:end);
            g_1   = sum((x_all - 0.5) .* (x_all - 0.5) - cos(20.0 * pi .* ( x_all - 0.5)),2);
            sum1  = obj.D - 2 + g_1;
            PopObj(:,1) = cos(0.5 * pi * X(:,1)) .* cos(0.5 * pi * X(:,2)) + sum1;
            PopObj(:,2) = cos(0.5 * pi * X(:,1)) .* sin(0.5 * pi * X(:,2)) + sum1;
            PopObj(:,3) = sin(0.5 * pi * X(:,1)) + sum1;
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,X)
            x_all  = X(:,3:1:end);
            g_1    = sum((x_all - 0.5) .* (x_all - 0.5) - cos(20.0 * pi .* ( x_all - 0.5)),2);
            sum1   = obj.D - 2 + g_1;
            PopCon = Constraint(X(:,1),X(:,2),sum1);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,3);
            R = R./repmat(sqrt(sum(R.^2,2)),1,3);
            X(:,2) = atan(R(:,2)./R(:,1))/0.5/pi;
            X(:,1) = acos(R(:,1)./cos(0.5*pi*X(:,2)))/0.5/pi;
            C(:,1) = -sin(20*pi*X(:,1));
            C(:,2) = -cos(20*pi*X(:,2));
            R(any(C>1e-2,2),:) = [];
            R = R + 0.5;
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [X1,X2] = meshgrid(linspace(0,1,100));
            x = cos(0.5*pi*X1).*cos(0.5*pi*X2);
            y = cos(0.5*pi*X1).*sin(0.5*pi*X2);
            z = sin(0.5*pi*X1);
            fes = all(Constraint(X1(:),X2(:),0.5)<=0,2);
            z(reshape(~fes,size(z))) = nan;
            R = {x+0.5,y+0.5,z+0.5};
        end
    end
end

function PopCon = Constraint(X1,X2,sum1)
    % set the parameters of constraints
    DifficultyFactors = [0.5,0.5,0.5];
    % Type-I parameters
    a = 20;
    b = 2 * DifficultyFactors(1) - 1;
    % Type-II parameters
    d = 0.5;
    if DifficultyFactors(2) == 0.0
        d = 0.0;
    end
    e = d - log(DifficultyFactors(2));
    if isfinite(e) == 0
        e = 1e+30;
    end
    % Type-III parameters
    r = 0.5 * DifficultyFactors(3);
    % Calculate objective values
    PopObj(:,1) = cos(0.5 * pi * X1) .* cos(0.5 * pi * X2) + sum1;
    PopObj(:,2) = cos(0.5 * pi * X1) .* sin(0.5 * pi * X2) + sum1;
    PopObj(:,3) = sin(0.5 * pi * X1) + sum1;
    % Type-I constraints
    PopCon(:,1) = b - sin(a * pi * X1);
    PopCon(:,2) = b - cos(a * pi * X2);
    % Type-II constraints
    PopCon(:,3) = -(e - sum1) .* (sum1 - d);
    if DifficultyFactors(2) == 1.0
        PopCon(:,3) = 1e-4 - abs(sum1 - e);
    end
    % Type-III constraints
    x_k = [1.0, 0.0, 0.0, 1.0 / sqrt(3.0)];
    y_k = [0.0, 1.0, 0.0, 1.0 / sqrt(3.0)];
    z_k = [0.0, 0.0, 1.0, 1.0 / sqrt(3.0)];
    for k=1:length(x_k)
        PopCon(:,3+k) = r * r - ((PopObj(:,1) - x_k(k))).^2  -...
            ((PopObj(:,2) - y_k(k))).^2 - ((PopObj(:,3) - z_k(k))).^2;
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  MATLAB Code for                                                  %
%                                                                   %
%  Multi-Objective Particle Swarm Optimization (MOPSO)              %
%  Version 1.0 - Feb. 2011                                          %
%                                                                   %
%  According to:                                                    %
%  Carlos A. Coello Coello et al.,                                  %
%  "Handling Multiple Objectives with Particle Swarm Optimization," %
%  IEEE Transactions on Evolutionary Computation, Vol. 8, No. 3,    %
%  pp. 256-279, June 2004.                                          %
%                                                                   %
%  Developed Using MATLAB R2009b (Version 7.9)                      %
%                                                                   %
%  Programmed By: S. Mostapha Kalami Heris                          %
%                                                                   %
%         e-Mail: sm.kalami@gmail.com                               %
%                 kalami@ee.kntu.ac.ir                              %
%                                                                   %
%       Homepage: http://www.kalami.ir                              %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function z=MyCost2(x)

    z1=sum(-10*exp(-0.2*sqrt(x(1:end-1).^2+x(2:end).^2)));

    z2=sum(abs(x).^0.8+5*(sin(x.^3)));

    z=[z1 z2];

end


function max_index_last = i4vec_max_index_last ( n, a )

%*****************************************************************************80
%
%% I4VEC_MAX_INDEX returns the index of the last largest entry in an I4VEC.
%
%  Discussion:
%
%    An I4VEC is a vector of integer values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(N), the vector to be searched.
%
%    Output, integer MAX_INDEX_LAST, the index of the last largest entry.
%
  if ( n <= 0 )

    max_index_last = 0;

  else

    amax = a(1);
    max_index_last = 1;

    for i = 2 : n

      if ( amax <= a(i) )
        amax = a(i);
        max_index_last = i;
      end

    end

  end

  return
end


function [xi] = log_multiSE3(chi)
C = chi(1:3,1:3);
r = chi(1:3,4:end);

[vecs,eigs] = eig(C);
[~,idx] = min(abs(diag(eigs)-1));
a = vecs(:,idx);

phi = acos(1/2*(trace(C)-1));
if phi == 0
    a = zeros(3,1);
    iJ = eye(3);
else
    A = [0 -a(3) a(2);a(3) 0 -a(1);-a(2) a(1) 0];
    a_ta = a*transpose(a);
    C1 = cos(phi)*eye(3) + (1-cos(phi))*a_ta + sin(phi)*A;
    C2 = cos(-phi)*eye(3) + (1-cos(-phi))*a_ta + sin(-phi)*A;
    if norm(C2-C)<norm(C1-C)
        phi = -phi;
    end
    iJ = phi/2*cot(phi/2)*eye(3) + (1-phi/2*cot(phi/2))*a_ta - phi/2*A;
end
rho = iJ*r;
xi = real([phi*a;rho(:)]);
end


function [pass, u1, u2, info1, info2] = test_scalarODE_breakpoints(pref)
% A nonlinear CHEBOP test. This test tests a scalar ODE, where there are two
% breakpoints in the domain of the CHEBOP.  It solves the problem using chebcolloc1,
% chebcolloc2 and ultraS discretizations.
%
% Asgeir Birkisson, May 2014.

%% Setup
if ( nargin == 0 )
    pref = cheboppref;
end

dom = [0 1 2 pi];
pref.damping = 1;

N = chebop(@(x,u) diff(u,2) + sin(u-.2), dom);
N.lbc = @(u) u - 2;
N.rbc = @(u) u - 3;
rhs = 0;

%% Try different discretizations
% Start with chebcolloc2
pref.discretization = @chebcolloc2;
[u1, info1] = solvebvp(N, rhs, pref);

%% Change to ultraS
pref.discretization = @ultraS;
[u2, info2] = solvebvp(N, rhs, pref);

%% Change to chebcolloc1
pref.discretization = @chebcolloc1;
[u3, info3] = solvebvp(N, rhs, pref);

%% Did we pass?
% To pass, both residuals have to be small, but we should not expect u1 and u2
% to be identical!
tol = 5e4*pref.bvpTol;
err1 = normest(N(u1));
err2 = normest(N(u2));
err3 = normest(N(u3));
% TODO: This used to be 1*, 10* and 1*. Should try to restore once we tune the
% algorithms better.
pass(1) = err1 < tol;
pass(2) = err2 < tol;
pass(3) = err3 < tol;
pass(4) = ( (norm(u1 - u2) ~= 0) && (norm(u2 - u3) ~= 0) && ...
    (norm(u1 - u3) ~= 0));

end


function [cal] = kJ2cal(kJ)
% Convert energy or work from kilojoules to calories.
% Chad A. Greene 2012
cal = kJ*238.84589663;


function Q = spm_dcm_csd_Q(csd)
% Precision of cross spectral density
% FORMAT Q = spm_dcm_csd_Q(csd)
%
% csd{i}   - [cell] Array of complex cross spectra
% Q        - normalised precision
%--------------------------------------------------------------------------
% This routine returns the precision of complex cross spectra based upon
% the asymptotic results described in Camba-Mendez & Kapetanios (2005):
% In particular, the scaled difference between the sample spectral density
% (g) and the predicted density (G);
%
% e = vec(g - G)
%
% is asymptotically complex normal, where the covariance between e(i,j) and
% e(u,v) is given by Q/h and:
%
% Q = G(i,u)*G(j,u):  h = 2*m + 1
%
% Here m represent the number of averages from a very long time series.The
% inverse of the covariance is thus a scaled precision, where the
% hyperparameter (h) plays the role of the degrees of freedom (e.g., the
% number of averages comprising the estimate). In this routine, we use the
% sample spectral density to create a frequency specific precision matrix
% for the vectorised spectral densities - under the assumption that the
% former of this sample spectral density resembles the predicted spectral
% density (which will become increasingly plausible with convergence).
%
% Camba-Mendez, G., & Kapetanios, G. (2005). Estimating the Rank of the
% Spectral Density Matrix. Journal of Time Series Analysis, 26(1), 37-48.
% doi: 10.1111/j.1467-9892.2005.00389.x
%__________________________________________________________________________
% Copyright (C) 2018 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_csd_Q.m 7751 2019-12-06 11:59:09Z peter $

%-Check for cell arrays
%--------------------------------------------------------------------------
if iscell(csd)
    CSD   = spm_zeros(csd{1});
    n     = numel(csd);
    for i = 1:n
        CSD = CSD + csd{i};
    end
    Q  = spm_dcm_csd_Q(CSD/n);
    Q  = kron(eye(n,n),Q);
    return
end

%-Get precision
%--------------------------------------------------------------------------
SIZ     = size(csd);
Qn      = spm_length(csd);
Q       = spalloc(Qn,Qn,SIZ(1)*prod(SIZ(2:end))^2);
[w,i,j] = ind2sub(SIZ,1:Qn);
for Qi  = 1:Qn
    for Qj = 1:Qn
        if w(Qi) == w(Qj)
            Q(Qi,Qj) = csd(w(Qi),i(Qi),i(Qj))*csd(w(Qi),j(Qi),j(Qj));
        end
    end
end
Q       = inv(Q + norm(Q,1)*speye(size(Q))/32);


%
% References:
%
% C. Lu. A Library of ADMM for Sparse and Low-rank Optimization. National University of Singapore, June 2016.
% https://github.com/canyilu/LibADMM.
% C. Lu, J. Feng, S. Yan, Z. Lin. A Unified Alternating Direction Method of Multipliers by Majorization
% Minimization. IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 40, pp. 527-541, 2018
%

addpath(genpath(cd))
clear

%% Examples for testing the sparse models
% For detailed description of the sparse models, please refer to the Manual.

%% generate toy data
d = 10;
na = 200;
nb = 100;

A = randn(d,na);
X = randn(na,nb);
B = A*X;
b = B(:,1);

opts.tol = 1e-6;
opts.max_iter = 1000;
opts.rho = 1.1;
opts.mu = 1e-4;
opts.max_mu = 1e10;
opts.DEBUG = 0;

%% l1
[X2,obj,err,iter] = l1(A,B,opts);
iter
obj
err
stem(X2(:,1))

%% group l1
g_num = 5;
g_len = round(na/g_num);
for i = 1 : g_num-1
    G{i} = (i-1)*g_len+1 : i*g_len;
end
G{g_num} = (g_num-1)*g_len+1:na;

[X2,obj,err,iter] = groupl1(A,B,G,opts);
iter
obj
err
stem(X2(:,1))

%% elastic net
lambda = 0.01;
[X2,obj,err,iter] = elasticnet(A,B,lambda,opts);
iter
obj
err
stem(X2(:,1))

%% fused Lasso
lambda = 0.01;
[x,obj,err,iter] = fusedl1(A,b,lambda,opts);
iter
obj
err
stem(x)

%% trace Lasso
[x,obj,err,iter] = tracelasso(A,b,opts);
iter
obj
err
stem(x)

%% k-support norm
k = 10;
[X,err,iter] = ksupport(A,B,k,opts);
iter
err
stem(X(:,1));

%% --------------------------------------------------------------

%% regularized l1
lambda = 0.01;
opts.loss = 'l1';
[X,E,obj,err,iter] = l1R(A,B,lambda,opts);
iter
obj
err
stem(X(:,1))

%% regularized group Lasso
g_num = 5;
g_len = round(na/g_num);

for i = 1 : g_num-1
    G{i} = (i-1)*g_len+1 : i*g_len;
end
G{g_num} = (g_num-1)*g_len+1:na;
lambda = 1;
opts.loss = 'l1';
[X,E,obj,err,iter] = groupl1R(A,B,G,lambda,opts);
iter
obj
err
stem(X(:,1))

%% regularized elastic net
lambda1 = 10;
lambda2 = 10;
opts.loss = 'l1';
[X,E,obj,err,iter] = elasticnetR(A,B,lambda1,lambda2,opts);
iter
obj
err
stem(X(:,1))
% stem(E(:,1))

%% regularized fused Lasso
lambda1 = 10;
lambda2 = 10;
opts.loss = 'l1';
[X,E,obj,err,iter] = fusedl1R(A,b,lambda1,lambda2,opts);
iter
obj
err
stem(X(:,1))
stem(E(:,1))

%% regularized trace Lasso
lambda = 0.1;
opts.loss = 'l1';
tic
[x,e,obj,err,iter] = tracelassoR(A,b,lambda,opts);
toc
iter
obj
err
stem(x)

%% regularized k-support norm
lambda = 0.1;
k = 10;
[X,E,err,iter] = ksupportR(A,B,lambda,k,opts);
iter
err
stem(X(:,1));


function c = intersect(a,b)
%INTERSECT    Intersection of intervals; empty components set to NaN
%
%   c = intersect(a,b)
%
%Result is an ***outer*** inclusion:
%   c includes the true intersection of a and b
%   components of c are set to NaN, if the corresponding components of
%     a and b are NaN, or if the intersection is empty
%   Thus a component NaN of the result c does NOT mean an empty intersection,
%     but rather that no information is available.
%
%===> To check empty intersection please use the INTLAB function
%===>   "EmptyIntersect"
%
%Input a and b must be both real or both complex
%

% written  10/16/98     S.M. Rump
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 12/06/05     S.M. Rump  correction of result (thanks to Andreas Rauh, Ulm, for hint)
% modified 06/11/06     S.M. Rump  correction of result (thanks to Andreas Rauh, Ulm, for hint)
% modified 09/10/07     S.M. Rump  redesign
% modified 04/22/09     S.M. Rump  comment changed, based on emptyintersect
%

  [empty,c] = emptyintersect(a,b);


function [ output_maps ] = convolution( input_maps, kernels, biases )
%CONVOLUTION Summary of this function goes here
%   Detailed explanation goes here

    % If MatConvNet is not installed use Matlab (much slower)
    if(exist('vl_nnconv', 'file') == 3)
        output_maps = vl_nnconv(single(input_maps), kernels, biases);
    else
        n_filters = size(kernels, 4);

        kernels2 = kernels(:,:,end:-1:1,:);
        for i=1:n_filters
            for n_in_maps=1:size(kernels,3)
                kernels2(:,:,n_in_maps,i) = fliplr(squeeze(kernels2(:,:,n_in_maps,i)));
                kernels2(:,:,n_in_maps,i) = flipud(squeeze(kernels2(:,:,n_in_maps,i)));
            end
        end
        output_maps = [];
        for i=1:n_filters
            output_maps = cat(3, output_maps, convn(input_maps, kernels2(:,:,:,i), 'valid') + biases(i));
        end
    end
end


function [Int, p, mask, w3] = estPolIntGrad(inp, N, logFlag);
% ESTPOLINTGRAD - Estimates and corrects the intensity gradient, using a polynomial model
%
% [Int pol] = estPolIntGrad(inp, N, logFlag);
%
% Inputs:
%  inp - input inplanes affected by the intensity gradient
%  N - order of the polynomial [Nx Ny Nz]
%  logFlag - operates in the logarithm of the intensity
%

if ~exist('N')
   N = [3,3,3];
end
if ~exist('logFlag')
   logFlag = 1;
end

[Ny, Nx, Nz] = size(inp);
inp(find(inp==0)) = NaN;

% chosing appropriate thresholds
II = find(~isnan(inp));
% fitting a GMM
inpMu = mean(inp(II));
inpStd = std(inp(II));
outlim = [min(inp(II)) max(inp(II))];
initCov(1,1,1) = inpStd^2/4;
size(initCov)
[Mu Cov W] = fitGMM(inp(II)', 1, inpMu+2.5*inpStd, initCov, [0.9 0.1], outlim);
%initmu = [inpMu-2*inpStd inpMu+2.5*inpStd];
%initCov(:,:,1:2) = 2*inpStd^2;
%initW = [0.45 0.45 0.1];
%outlim = [min(inp(II)) max(inp(II))];
%[Mu Cov W] = fitGMM(inp(II)', 2, initmu, initCov, initW, outlim);

[hh xx] = hist(inp(II),256);
hh = hh/sum(hh)/(xx(2)-xx(1));
p1 = W(1)*pgauss1d(xx,Mu(1),Cov(:,:,1));
%p2 = W(2)*pgauss1d(xx,Mu(2),Cov(:,:,2));
p3 = W(2)/(max(inp(II))-min(inp(II))) * ones(size(p1));
figure(20); clf
plot(xx,hh,xx,p1+p3)

% chosing as thresholds mu2+-2std2
LoT = Mu(1) - 3*sqrt(Cov(:,:,1))
UpT = Mu(1) + 3*sqrt(Cov(:,:,1))
inp(find( (inp<LoT) | (inp>UpT) )) = NaN;

figure(20)
hold on
stem([LoT UpT], [max(hh),max(hh)]);
hold off

% Change polynomial order to be 2 less than number of non-NaN slices
NzEff = 0;
for k=1:Nz
    tmp = inp(:,:,k);
    if ~all(isnan(tmp(:)))
        NzEff = NzEff + 1;
    end
end
N(3) = min(max(1,NzEff-2), N(3));

% taking logarithm
if logFlag
   inp = log(inp);
end

% selecting valid set of indices
II = find(~isnan(inp));

% building A matrix for the appropriate polynomial order
[x y z] = meshgrid(1:Nx,1:Ny,1:Nz);
A = [];
for m=0:N(1)
  for n=0:max(0,(N(2)-m))
    for o=0:max(0,(N(3)-m-n))
         A = [A x(II).^m .* y(II).^n .* z(II).^o];
      end
   end
end
clear x y z

% robust estimation of the polynomial coefficients
% NOTE - The values of the last two parameters in this function can affect the
% performance of the solution. In general, the recommended value for CB = 4.685
% is very conservative with outliers (almost nothing is considered outlier).
% For this reason I choose a value of 2.5. I also set a bit lower the value of SC
% (1.2 instead of 1.4), so that more outliers are rejected.
[p w]= robustMest(A, inp(II), 3, 1.2);
w3 = zeros(size(inp));
w3(II) = w;

% computation of the intensity correction
il = zeros(Ny,Nx,Nz); k=1;
[x y z] = meshgrid(1:Nx, 1:Ny, 1:Nz);
for m=0:N(1)
  for n=0:max(0,(N(2)-m))
    for o=0:max(0,(N(3)-m-n))
         il = il + p(k)*x.^m.*y.^n.*z.^o;
         k = k+1;
      end
   end
end

if logFlag
   Int = exp(il);
else
   Int = il;
end

mask = zeros(size(inp));
mask(II) = 1;

return

clear
load testdata
[Int, p, mask, w3] = estPolIntGrad(inp, [3 3 1], 1);
inpc = inp./Int;
sinpc = reshape(inpc, [size(inp,1) size(inp,2)*size(inp,3)]);
figure(1)
subplot(4,1,1)
vis(inp)
subplot(4,1,2)
imshow(sinpc, [0.5 1.5], 'notruesize')
subplot(4,1,3)
vis(Int.*mask)
subplot(4,1,4)
vis(w3)
figure(2)
II = find((inpc~=0)&(~isnan(inpc)));
hist(inpc(II),256)


function [x,fval,exitflag,info] = mosekqcqp(H,f,A,rl,ru,Q,l,qrl,qru,lb,ub,x0,opts)
%MOSEKQCQP Solve a QCQP using MOSEK
%
%   min 0.5*x'*H*x + f'*x      subject to:     rl <= A*x <= ru
%    x                                         qrl <= x'Qx + l'x <= qru
%                                              lb <= x <= ub
%
%
%   x = mosekqcqp(H,f,A,rl,ru,Q,l,qrl,qru,lb,ub) solves a QCQP where H and f
%   are the objective matrix and vector respectively, A,rl,ru are the
%   linear constraints, Q,l,qrl,qru are the quadratic constraints (see below)
%   and lb,ub are the bounds.
%
%   x = mosekqcqp(H,...,ub,x0) uses x0 as the initial solution guess.
%
%   x = mosekqcqp(H,...,x0,opts) uses opts to pass mosekset options to the
%   MOSEK solver.
%
%   [x,fval,exitflag,info] = mosekqcqp(...) returns the objective value at
%   the solution, together with the solver exitflag, and an information
%   structure.
%
%   Quadratic Constraint Form:
%       - Single Quadratic Constraint
%           Q is matrix
%           l is a column vector
%           qrl is a scalar
%           qru is a scalar
%       - Multiple Quadratic Constraints
%           Q is a row cell array, each cell is a quadratic constraint Q
%           l is a matrix, each column for each quadratic constraint l
%           qrl is a column vector, each row for each quadratic constraint qrl
%           qru is a column vector, each row for each quadratic constraint qru

%   This function is based in parts on examples from the MOSEK Toolbox,
%   Copyright (c) 1998-2011 MOSEK ApS, Denmark.

%   Copyright (C) 2012 Jonathan Currie (IPL)

t = tic;

% Handle missing arguments
if nargin < 13, opts = mosekset('warnings','off'); else opts = mosekset(opts); end
if nargin < 12, x0 = []; end
if nargin < 11, ub = []; end
if nargin < 10, lb = []; end
if nargin < 9, qru = []; end
if nargin < 8, qrl = []; end
if nargin < 7, l = []; end
if nargin < 6, Q = []; end
if nargin < 5, error('You must supply at least 5 arguments to mosekqcqp'); end

%Build MOSEK Command
[cmd,param] = mosekBuild(opts);

%Create Problem Structure
prob = mosekProb(H,f,A,rl,ru,Q,l,qrl,qru,lb,ub,[],[],x0,opts);

%Call MOSEK
[rcode,res] = mosekopt(cmd,prob,param);

%Extract Results
[x,fval,exitflag,info] = mosekRes(prob,rcode,res,t);


function [kWh] = Nm2kWh(Nm)
% Convert energy or work from newtons-meters to kilowatt-hours.
% Chad A. Greene 2012
kWh = Nm*2.7777777778e-7;


function S = DSAFinit_Morgan(M,mu,N,J)

% DSAFinit_Morgan   Initialize Parameter Structure for the Delayless
%                   Subband Adaptive Filter Proposed by Morgan and Thi
% Arguments:
% M                 Length of corresponding fullband filter
% mu                Step size
% N                 Number of subbands
% J                 Update rate, typically in the range 1 to 8
%
% Subband Adaptive Filtering: Theory and Implementation
% by Lee, Gan, and Kuo, 2008
% Publisher: John Wiley and Sons, Ltd

D = N/2;                            % 2x oversampling (recommended)
OverFac = 4;                        % Overlapping factor (Try other integer values)
L = 2*OverFac*N-1;                  % Length of analysis filters, L = 2KN-1

% DFT filter bank with fractional delay (see Section 4.4.3.2)

wc = 1/N;                           % Cut-off frequency
hopt = fir1(L-1,wc);                % Generate prototype filter
[H,F] = make_bank_DFT(hopt,N);      % Complex modulation

% Assign structure fields
S.FULLcoeffs    = zeros(M,1);       % Fullband adaptive filter
S.SUBcoeffs     = zeros(M/D,N/2+1); % Adaptive subfilters (initialized to zeros)
                                    %  consider only the first N/2 + 1 subfilters
S.step          = mu;               % Step size
S.decfac        = D;                % Decimation factor
S.analysis      = H;                % Analysis filter bank
S.synthesis     = F;                % Synthesis filter bank
S.iter          = 0;                % Iteration count
S.alpha         = 1e-4;             % Small positive constant
S.AdaptStart    = L + M;            % Running effect of the analysis and adaptive filter, minimum L+M
S.UpdateRate    = round(M/J);       % J is typically in the range 1 to 8


% P(X) = GAUSSIAN(MU0,COV0)
%
% Q(X) = GAUSSIAN(MU,COV)
%
% where the parameters are actually functions
%   MU = MU(THETA),
%   COV = COV(THETA).
%
% More generally, the whole joint distribution Q(X,MU0,COV0) can be a
% function of THETA.
%
% Evaluates KL(Q||P) w.r.t. THETA up to a constant, and the gradient.
%
% vb_rotationcost_gaussian(logdet_Cov, x_invCov0_x, x_invCov0_mu, ...
%                          mu_invCov0_mu, logdet_Cov0, grad_logdet_Cov, ...
%                          grad_x_invCov0_x, grad_x_invCov0_mu, ...
%                          grad_mu_invCov0_mu, grad_logdet_Cov0)
%
% LOGDET_COV         : <LOG(DET(COV))>
% X_INVCOV0_X        : <X'*INV(COV0)*X>
% X_INVCOV0_MU       : <X'*INV(COV0)*MU>
% MU_INVCOV0_MU      : <MU'*INV(COV0)*MU>
% LOGDET_COV0        : <LOG(DET(COV0))>
% GRAD_LOGDET_COV    : D(<LOG(DET(COV))>) / D(THETA)
% GRAD_X_INVCOV0_X   : D(<X'*INV(COV0)*X>) / D(THETA)
% GRAD_X_INVCOV0_MU  : D(<X'*INV(COV0)*MU>) / D(THETA)
% GRAD_MU_INVCOV0_MU : D(<MU'*INV(COV0)*MU>) / D(THETA)
% GRAD_LOGDET_COV0   : D(<LOG(DET(COV0))>) / D(THETA)
%
% where the expectations <...> are taken over the distribution
% Q(X,MU0,COV0). Note that the terms can be evaluated up to a constant
% w.r.t. THETA, which often gives huge savings.

% Last modified 2010-06-24
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function [KL,dKL] = vb_rotationcost_gaussian(logdet_Cov,         ...
                                             x_invCov0_x,        ...
                                             x_invCov0_mu,       ...
                                             mu_invCov0_mu,      ...
                                             logdet_Cov0,        ...
                                             grad_logdet_Cov,    ...
                                             grad_x_invCov0_x,   ...
                                             grad_x_invCov0_mu,  ...
                                             grad_mu_invCov0_mu, ...
                                             grad_logdet_Cov0)

% Cost from <log q(X)>
log_qX = -0.5 * logdet_Cov;
dlog_qX = -0.5 * grad_logdet_Cov;

% Cost from <log p(x)>
log_pX = -0.5 * (logdet_Cov0 + x_invCov0_x - 2*x_invCov0_mu + mu_invCov0_mu);
dlog_pX = -0.5 * (grad_logdet_Cov0 + grad_x_invCov0_x - 2*grad_x_invCov0_mu ...
                  + grad_mu_invCov0_mu);

KL = log_qX - log_pX;
dKL = dlog_qX - dlog_pX;

% $$$ function [f,df_dR] = vb_rotationcost_gaussian(R,logdet_R,invT_R, ...
% $$$                                               XX_R_invCov,X_Mu_invCov)
% $$$
% $$$ [D,N] = size(X);
% $$$
% $$$ % TODO: Check that the sizes match
% $$$
% $$$ % Cost from <log q(X)>
% $$$ log_qX = -N*logdet_R;
% $$$ dlog_qX = -N*invT_R;
% $$$
% $$$ % Cost from <log p(x)>
% $$$ log_pX = -0.5 * (traceprod(XX_R_invCov,R) - 2*traceprod(X_Mu_invCov,R));
% $$$ dlog_pX = X_Mu_invCov - XX_R_invCov;
% $$$
% $$$ f = log_pX -


function [IMAGE_3D_DATA] = image3Ddata(M)
% function [IMAGE_3D_DATA]=image3Ddata(M)
% ------------------------------------------------------------------------
%
% This simple function creates a structure array containing coordinate and
% colour data for 3D images. It allows one to use the patch function to
% plot the whole image or a selection of voxels in 3D.
%
% N.B. The function has not been optimised for large images. Large images
% (function has been tested for images under 100x100x100) may produce
% memory problems.
%
%
% EXAMPLE
%
% M = rand(15,15,15);
% [IMAGE_3D_DATA] = image3D(M);
%
% Getting faces and vertices for full image:
% voxel_no=1:1:numel(M);
% voxel_face_no=IMAGE_3D_DATA.voxel_patch_face_numbers(voxel_no,:);
% M_faces=IMAGE_3D_DATA.voxel_patch_faces(voxel_face_no,:);
% M_vertices=IMAGE_3D_DATA.corner_coordinates_columns_XYZ;
%
% Getting faces and vertices for selection of voxels:
% voxel_no2=M>0.95;
% voxel_face_no2=IMAGE_3D_DATA.voxel_patch_face_numbers(voxel_no2,:);
% M_faces2=IMAGE_3D_DATA.voxel_patch_faces(voxel_face_no2,:);
% M_vertices2=IMAGE_3D_DATA.corner_coordinates_columns_XYZ;
%
% figure;fig=gcf; clf(fig); colordef (fig, 'white'); units=get(fig,'units'); set(fig,'units','normalized','outerposition',[0 0 1 1]); set(fig,'units',units);
% set(fig,'Color',[1 1 1]);
%
% subplot(1,2,1);
% hp=patch('Faces',M_faces,'Vertices',M_vertices,'EdgeColor','black', 'CData',IMAGE_3D_DATA.voxel_patch_CData(voxel_face_no,:),'FaceColor','flat');
% hold on; view(45,30); axis equal; axis tight; colormap jet; colorbar; caxis([0 1]);
% xlabel('J'); ylabel('I'); zlabel('K');
% title('Full image');
%
% subplot(1,2,2);
% hp2=patch('Faces',M_faces2,'Vertices',M_vertices2,'EdgeColor','black', 'CData',IMAGE_3D_DATA.voxel_patch_CData(voxel_face_no2,:),'FaceColor','flat');
% hold on; view(45,30); axis equal; axis tight; colormap jet; colorbar; caxis([0 1]); grid on;
% set(hp2,'FaceAlpha',0.8);
% xlabel('J'); ylabel('I'); zlabel('K');
% title('Selection of voxels');
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 11/05/2009
% ------------------------------------------------------------------------

% Copyright (c) 2009, Kevin Mattheus Moerman
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

% Setting up meshgrid of voxel centre coordinates
[X,Y,Z] = meshgrid(1:1:size(M,2),1:1:size(M,1),1:1:size(M,3));
IMAGE_3D_DATA.center_coordinates_meshgrid_X=X;
IMAGE_3D_DATA.center_coordinates_meshgrid_Y=Y;
IMAGE_3D_DATA.center_coordinates_meshgrid_Z=Z;

X=X(:); Y=Y(:); Z=Z(:);
IMAGE_3D_DATA.center_coordinates_columns_XYZ=[X Y Z];

% Creating coordinates for voxel corners
[X,Y,Z] = meshgrid(0.5:1:(size(M,2)+0.5),0.5:1:(size(M,1)+0.5),0.5:1:(size(M,3)+0.5));

IMAGE_3D_DATA.corner_coordinates_meshgrid_X=X;
IMAGE_3D_DATA.corner_coordinates_meshgrid_Y=Y;
IMAGE_3D_DATA.corner_coordinates_meshgrid_Z=Z;

X=X(:); Y=Y(:); Z=Z(:);
IMAGE_3D_DATA.corner_coordinates_columns_XYZ=[X Y Z];

clear X Y Z;

% Creating path face and color data
nodes_first_voxel = [    1 ...
                         2 ...
                        ( ( (size(M,1)+1)*(size(M,2)+1) ) +2) ...
                        ( ( (size(M,1)+1)*(size(M,2)+1) ) +1) ...
                        ( 1 + (size(M,1)+1) ) ...
                        ( 2 + (size(M,1)+1) ) ...
                        ( 2 + (size(M,1)+1) + ( (size(M,1)+1)*(size(M,2)+1) ) ) ...
                        ( 1 + (size(M,1)+1) + ( (size(M,1)+1)*(size(M,2)+1) ) )   ];

nodes_first_row_voxels=((0:1:(size(M,1)-1))' * ones(1,8)) + (ones(1,size(M,1))') * nodes_first_voxel;
A = repmat(nodes_first_row_voxels,(size(M,2)),1);
B = repmat(((size(M,1)+1)*(0:(size(M,2)-1))),(size(M,1)),1);
B = reshape(B,1, numel(B))' *ones(1,8);
nodes_first_slice_voxels=A+B;
A = repmat(nodes_first_slice_voxels,(size(M,3)),1);
B = repmat((((size(M,1)+1)*(size(M,2)+1))*(0:(size(M,3)-1))),((size(M,1))*(size(M,2))),1);
B = reshape(B,1,numel(B))'*ones(1,8);

IMAGE_3D_DATA.corner_numbers=A+B;
IMAGE_3D_DATA.voxel_patch_face_numbers=reshape(1:1:(6*numel(M)),6,numel(M))';
IMAGE_3D_DATA.voxel_patch_CData=reshape(((M(:)*ones(1,6)))',(6*numel(M)),1);

face_no=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]';
faces=reshape((IMAGE_3D_DATA.corner_numbers(:,face_no))',4,[])';
IMAGE_3D_DATA.voxel_patch_faces=faces;


function spdemovarout
% SPDEMOVAROUT   Sparse interpolation demo for multiple outputs
%    Sparse grid interpolation demo for a function with multiple
%    output parameters and non-vectorized processing. In this case,
%    we use the Maximum-norm-based grid.

% Author : Andreas Klimke, Universitaet Stuttgart
% Date   : January 27, 2004
% Version: 1.3

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

% Define problem dimension
d = 2;

% Define number of output parameters of fvarout
nout = 3;

% Get options structure for sparse interpolation
options = spset('NumberOfOutputs', nout, ...
								'GridType', 'Maximum');

% Create full grid for plotting
gs = 33;
[X,Y] = meshgrid(linspace(0,2,gs),linspace(-1,1,gs));

% Compute sparse grid weights over domain [0,2]x[-1,1]
z = spvals(@fvarout, d, [0 2; -1 1], options, nout);

% Compute inpterpolated values at full grid
for k = 1:nout
	z.selectOutput = k;
	ip{k} = spinterp(z, X, Y);
end

% Plot interpolated results
for k = 1:nout
	subplot(1,nout,k);
	mesh(X, Y, ip{k});
	title(['interpolated out',num2str(k)]);
end

%------------------------------------------------------------------
function [varargout] = fvarout(x,y,nout)
% FVARARGOUT    function with multiple output arguments, the number
% of output arguments is determined by the parameter nout.

out = zeros(1,nout);
out(1:nout) = 1./((x-(1:nout)/nout).^2+(y)^2+(1:nout)./nout);
varargout = num2cell(out);


function [y, z, p] = mixLinPred(model, X, t)
% Prediction function for mxiture of linear regression
% input:
%   model: trained model structure
%   X: d x n data matrix
%   t:(optional) 1 x n responding vector
% output:
%   y: 1 x n prediction
%   z: 1 x n cluster label
%   p: 1 x n predict probability for t
% Written by Mo Chen (sth4nth@gmail.com).
W = model.W;
alpha = model.alpha;
beta = model.beta;

X = [X;ones(1,size(X,2))]; % adding the bias term
y = W'*X;
D = bsxfun(@minus,y,t).^2;
logRho = (-0.5)*beta*D;
logRho = bsxfun(@plus,logRho,log(alpha));
T = logsumexp(logRho,1);
p = exp(T);
logR = bsxfun(@minus,logRho,T);
R = exp(logR);
z = max(R,[],1);


function F = calc_normalCDF(vX, vXdata)
    % Compute CDF of normal probability distribution for diverse parameters
    % REQUIRES STATISTICS TOOLBOX
    %
    % F = calc_normalCDF(vX, vXdata);
    % ----------------------------------------
    % Compute CDF of normal probability distribution for diverse parameters; use as function in lsqcurvefit (or equivalent) to find
    % best fitting parameters
    %
    % Distribution parameters:
    % vX(1) : mu
    % vX(2) : sigma
    %
    % Author: J. Woessner
    % woessner@seismo.ifg.ethz.ch
    % updated: 11.06.04

    F = normcdf(vXdata,vX(1),vX(2));
    % Check F on NAN
    if isnan(F(1))
        F = zeros(length(F),1);
        warning('Probabilities set to zero')
    end
end


function quadrule_test39 ( )

%*****************************************************************************80
%
%% QUADRULE_TEST39 tests NCO_SET and SUM_SUB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 9;

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  a = 0.0;
  b = 1.0;

  nsub = 1;
  xlo = -1.0;
  xhi = +1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_TEST39\n' );
  fprintf ( 1, '  NCO_SET sets up an open Newton-Cotes rule;\n' );
  fprintf ( 1, '  SUM_SUB carries it out.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for norder = 1 : order_max

      if ( norder == 8 )
        continue
      end

      fprintf ( 1, '  %2d', norder );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = nco_set ( norder );

        result(i) = sum_sub ( @func, a, b, nsub, norder, xlo, xhi, xtab, ...
          weight );

        fprintf ( 1, '  %12f', result(i) );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function y=ar_med_filter(signal,Fs)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function AR_MED_FILTER takes input SIGNAL with Sampling Frequency, %
% Fs, and applies the Yule Walker method based AR filter. The order iof the
% filter is found by Maximum kurtosis. After the application od AR filter,
% the signal is passed through Minimum Entropy Deconvolution. This combined
% AR+MED method brings out the Bearing faults hidden in Noise.
%
% The function plots two figures for AR alone and another for AR+MED
%
% Example:
%   load('s4.mat');
%   signal=s4;
%   Fs=12000;
%   ar_med_filter(signal,Fs);

% The File 's4.mat' is the vibration signal recorded from a OR faulty
% bearing with a sampling frequency of 12000Hz. The Fault frequency is 161
% Hz and is brought out.
%
% This program isa based on the paper:
% Sawalhi N, Randall RB and Endo H (2007) The enhancement of fault detection
% and diagnosis in rolling element bearings using minimum entropy
% deconvolution combined with spectral kurtosis. Mechanical Systems and
% Signal Processing. 21:2616-2633
%
% This function is basically written for Bearing fault diagnosis from
% Vibration signal.
%
%Dont forget to rate or comment on the matlab central site
%http://www.mathworks.in/matlabcentral/fileexchange/authors/258518
%
%Author:Santhana Raj.A
%https://sites.google.com/site/santhanarajarunachalam/
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sig=signal;

clear y y_e A

for order= 1:100

[A,E]=aryule(sig,order);
y(:,order)=filter(1,A,sig);
kurt(:, order)=kurtosis(y(:,order));

end

[~,index]=sort(kurt,1,'descend');
output=y(:,index(1));

figure();
N=4*2048;T=N/Fs;sig_f=abs(fft(output(1:N),N));
sig_n=sig_f/(norm(sig_f));freq_s=(0:N-1)/T;
plot(freq_s(3:350),sig_n(3:350)); title('AR alone');

%MED
[ar_med f_armed kurt_armed] = med2d(output,30,[],0.01,0);

figure();
N=4*2048;T=N/Fs;sig_f=abs(fft(ar_med(1:N),N));
sig_n=sig_f/(norm(sig_f));freq_s=(0:N-1)/T;
plot(freq_s(3:350),sig_n(3:350)); title('AR +MED');

y=ar_med;

end


function [B, model] = circulant_learning(X, para)
% CBE-opt
% X training data
% model.r optimized circulant vector
% model.bernoulli random bernoulli vector
% B CBE code for X

%N = size(X,1);
d = size(X,2);
rr = randn(1,d);
rr(rr > 0) = 1;
rr(rr <= 0 ) = -1;
% randomly flipping the sign
for i = 1:size(X,1)
    X(i,:) = X(i,:).*rr;
end
model.bernoulli = rr;

if ~isfield(para, 'bit')
   para.bit = size(X,2);
end

if ~isfield(para, 'iter')
   para.iter = 500;
end

if ~isfield(para, 'lambda')
   para.lambda = 1;
end

if ~isfield(para, 'verbose')
   para.verbose = 0;
end

% pre-compute the fft of X
fft_X = fft(X,[],2);

% pre-compute m
m = sum(real(fft_X).^2,1)+ sum(imag(fft_X).^2,1);
m = m'/d;

% initilization
if (~isfield(para, 'init_r'))
    r = randn(d,1);
else
    r = para.init_r;
end

fft_r = fft(r);
B = optimize_B(fft_r, fft_X, para);
ops = 100;
obj = inf;
iter = 0;

while(ops > 0.1 && iter <= para.iter)
    fprintf('iteration %d, obj = %f\n', iter, obj);
    if (para.verbose)
        fprintf('obj = %f \n', obj);
        fprintf('obj = %f \n', compute_obj_time(B, X, real(ifft(fft_r)), para.lambda));
        fft_B = fft(B, [], 2);
        fprintf('obj = %f \n', compute_obj_freq(fft_B, fft_X, fft_r, para.lambda));
    end

    [fft_r, obj_new] = optimize_R(B, fft_X, fft_r, m, para);
    B = optimize_B(fft_r, fft_X, para);
    ops = obj - obj_new;
    obj = obj_new;
    iter = iter + 1;
end

r = ifft(fft_r);
r = real(r);
model.r = r;

end

function obj = compute_obj_time(B, X, r, lambda)
   R = circulant(r, 1);
   obj = sum(sum((B - X*R').^2)) + lambda * sum(sum((R'*R - diag(ones(size(R,1), 1))).^2));
end

function obj = compute_obj_freq(fft_B, fft_X, fft_r, lambda)
    d = length(fft_r);
    Z = fft_B - repmat(fft_r.', size(fft_X,1) ,1).*fft_X;
    obj =  1/d * sum(sum(real(Z).^2 + imag(Z).^2));
    obj = obj + lambda*sum((real(fft_r).^2 + imag(fft_r).^2 - 1).^2);
end


classdef Adam < ALGORITHM
% <single> <real> <large/none>
% Adaptive moment estimation
% alpha ---     1 --- Learning rate
% beta1 ---   0.9 --- A parameter within [0 1)
% beta2 --- 0.999 --- A parameter within [0 1)

%------------------------------- Reference --------------------------------
% D. P. Kingma and J. Ba, Adam: A method for stochastic optimization, arXiv
% preprint arXiv:1412.6980, 2014.
%------------------------------- 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
        function main(Algorithm,Problem)
            %% Parameter setting
            [alpha,beta1,beta2] = Algorithm.ParameterSet(1,0.9,0.999);

            %% Generate random solution
            X  = Problem.Initialization(1);
            m0 = zeros(1,Problem.D);
            v0 = zeros(1,Problem.D);

            %% Optimization
            k = 1;
            while Algorithm.NotTerminated(X)
                gk = Problem.CalObjGrad(X.dec);
                m  = beta1*m0 + (1-beta1)*gk;
                v  = beta2*v0 + (1-beta2)*gk.^2;
                X  = Problem.Evaluation(X.dec-alpha*(m/(1-beta1.^k))./(sqrt(v/(1-beta2.^k))+1e-8));
                k  = k + 1;
            end
        end
    end
end


function sift_arr = sp_normalize_sift(sift_arr)
% normalize SIFT descriptors (after Lowe)
% URL: http://www.cs.illinois.edu/homes/slazebni/research/SpatialPyramid.zip

% find indices of descriptors to be normalized (those whose norm is larger than 1)
tmp = sqrt(sum(sift_arr.^2, 2));
normalize_ind = find(tmp > 1);

sift_arr_norm = sift_arr(normalize_ind,:);
sift_arr_norm = sift_arr_norm ./ repmat(tmp(normalize_ind,:), [1 size(sift_arr,2)]);

% suppress large gradients
sift_arr_norm(find(sift_arr_norm > 0.2)) = 0.2;

% finally, renormalize to unit length
tmp = sqrt(sum(sift_arr_norm.^2, 2));
sift_arr_norm = sift_arr_norm ./ repmat(tmp, [1 size(sift_arr,2)]);

sift_arr(normalize_ind,:) = sift_arr_norm;


%KMEANS PRTools k-means clustering, deprecated
%
%   [LABELS,B] = KMEANS(A,K,MAXIT,INIT)
%
% INPUT
%  A       Dataset
%  K       Number of clusters to be found (optional; default: 2)
%  MAXIT   maximum number of iterations (optional; default: 50)
%  INIT    Labels for initialisation, or
%          'rand'     : take at random K objects as initial means, or
%          'kcentres' : use KCENTRES for initialisation (default)
%
% OUTPUT
%  LABELS  Cluster assignments, 1..K
%  B       Dataset with original data and labels LABELS:
%          B = PRDATASET(A,LABELS)
%
% DESCRIPTION
% K-means clustering of data vectors in A. This routine calls PRKMEANS
%
% SEE ALSO (PRTools Guide)
% DATASETS, PRKMEANS, HCLUST, KCENTRES, MODESEEK, EMCLUST, PRPROGRESS


function [grad] = l2rowscaledg(x,y,outderv, alpha)

normeps = 1e-5;
if (~exist('outderv','var')||isempty(outderv))
    error('Requires outderv of previous layer to compute gradient!');
end

epssumsq = sum(x.^2,2) + normeps;

l2rows = sqrt(epssumsq)*alpha;

if (~exist('y','var')||isempty(y))
     y = bsxfun(@rdivide,x,l2rows);
end

grad = bsxfun(@rdivide, outderv, l2rows) - ...
       bsxfun(@times, y, sum(outderv.*x, 2) ./ epssumsq);


% VL_RODR  Rodrigues' formula
%   R = VL_RODR(OM) where OM a 3-dimensional column vector computes the
%   Rodrigues' formula of OM, returning the rotation matrix R =
%   expm(vl_hat(OM)).
%
%   [R,DR] = VL_RODR(OM) computes also the derivative of the Rodrigues
%   formula. In matrix notation this is the expression
%
%           d(vec expm(vl_hat(OM)) )
%     dR = ----------------------.
%                  d om^T
%
%   [R,DR]=VL_RODR(OM) when OM is a 3xK matrix repeats the operation for
%   each column (or equivalently matrix with 3*K elements). In this
%   case R and DR are arrays with K slices, one per rotation.
%
%   See also: VL_IRODR(), VL_HELP().

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


function Population = STM(Population,W,z,znad)
% Selection based on STM 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  = length(Population);
    NW = size(W,1);

    %% The modified Tchebycheff value of each solution on each subproblem
    g = zeros(N,NW);
    for i = 1 : N
        g(i,:) = max(repmat(abs(Population(i).obj-z),NW,1)./W,[],2)';
    end

    %% The perpendicular distance of each solution on each subproblem
    PopObj   = (Population.objs-repmat(z,N,1))./repmat(znad-z,N,1);
    Cosine   = 1 - pdist2(PopObj,W,'cosine');
    Distance = repmat(sqrt(sum(PopObj.^2,2)),1,NW).*sqrt(1-Cosine.^2);

    %% STM selection
    Fp  = zeros(1,NW);
    FX  = zeros(1,N);
    Phi = false(NW,N);
    while any(Fp==0)
        RemainW  = find(Fp==0);
        i        = RemainW(randi(length(RemainW)));
        RemainX  = find(~Phi(i,:));
        [~,best] = min(g(RemainX,i));
        j        = RemainX(best);
        Phi(i,j) = true;
        if FX(j) == 0
            Fp(i) = j;
            FX(j) = i;
        elseif Distance(j,i) < Distance(j,FX(j))
            Fp(i)     = j;
            Fp(FX(j)) = 0;
            FX(j)     = i;
        end
    end
    Population = Population(Fp);
end


function Population = EnvironmentalSelection(Population,T)
% The environmental selection of PeEA

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

% This function is written by Li Li

    [Fitness,extreme] = CalFitness(Population.objs);
    extreme = unique(extreme); %eliminate the identical solutions
    [~,mm] = size(extreme);

	%% Calculate the angle between each two solutions
    Angle = acos(1-pdist2(Population.objs,Population.objs,'cosine'));
    Angle(logical(eye(length(Population)))) = inf;

    %% Angle-based tournament selection
    Remain = 1 : length(Population);
    Remain(extreme) = []; %eliminate extremes first
    while length(Remain) > (T-mm) %mins mm first
        % Identify the two solutions A and B with the minimum angle
        [sortA,rank1] = sort(Angle(Remain,Remain),2); %sortA is the sorted angle, rank1 NXN index of matric angle
        [~,rank2]     = sortrows(sortA);              %rank2 Nx1 index of sorted angle
        A = rank2(1);                                 %A, with minimum angle
        B = rank1(A,1);                               %B corrponding to A with minimum angle
        % Eliminate one of A and B
        if  Fitness(Remain(A)) > Fitness(Remain(B))
            Remain(A) = [];
        else
            Remain(B) = [];
        end
    end
    % Population for next generation
    Population = Population([Remain,extreme]); %add extremes into pop
end


function J = computeCostMulti(X, y, theta)
%COMPUTECOSTMULTI Compute cost for linear regression with multiple variables
%   J = COMPUTECOSTMULTI(X, y, theta) computes the cost of using theta as the
%   parameter for linear regression to fit the data points in X and y

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

% You need to return the following variables correctly
J = 0;

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost of a particular choice of theta
%               You should set J to the cost.

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

end


function xp = spm_beta_compare(alpha1,alpha2,Nsamp)
% Compute probability that r1 > r2
% FORMAT xp = spm_beta_compare(alpha1,alpha2,Nsamp)
%
% Input:
% alpha1    - Beta parameters for first density
% alpha2    - Beta parameters for second density
% Nsamp     - number of samples used to compute xp [default = 1e4]
%
% Output:
% xp        - exceedance probability
%
% Compute probability that r1 > r2 where p(r1)=Beta(r1|alpha1),
% p(r2)=Beta(r2|alpha2). Uses sampling.
% Useful for comparing groups in RFX model inference
%__________________________________________________________________________
% Copyright (C) 2009 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_beta_compare.m 3458 2009-10-13 10:05:35Z maria $

if nargin < 3
    Nsamp = 1e4;
end

Nk = length(alpha1);

% Perform sampling in blocks
%--------------------------------------------------------------------------
blk = ceil(Nsamp*Nk*8 / 2^28);
blk = floor(Nsamp/blk * ones(1,blk));
blk(end) = Nsamp - sum(blk(1:end-1));

xp = 0;
for i=1:length(blk)

    % Sample from univariate gamma densities then normalise
    % (see Dirichlet entry in Wikipedia or Ferguson (1973) Ann. Stat. 1,
    % 209-230)
    %----------------------------------------------------------------------
    r1 = zeros(blk(i),Nk);
    for k = 1:Nk
        r1(:,k) = spm_gamrnd(alpha1(k),1,blk(i),1);
    end
    sr1 = sum(r1,2);
    for k = 1:Nk
        r1(:,k) = r1(:,k)./sr1;
    end

    r2 = zeros(blk(i),Nk);
    for k = 1:Nk
        r2(:,k) = spm_gamrnd(alpha2(k),1,blk(i),1);
    end
    sr2 = sum(r2,2);
    for k = 1:Nk
        r2(:,k) = r2(:,k)./sr2;
    end

    % Exceedance probabilities:

    xp = xp + length(find (r1(:,1)>r2(:,1)));

end
xp = xp / Nsamp;


function [tap] = hanning(n, str)

%HANNING   Hanning window.
%   HANNING(N) returns the N-point symmetric Hanning window in a column
%   vector.  Note that the first and last zero-weighted window samples
%   are not included.
%
%   HANNING(N,'symmetric') returns the same result as HANNING(N).
%
%   HANNING(N,'periodic') returns the N-point periodic Hanning window,
%   and includes the first zero-weighted window sample.
%
%   NOTE: Use the HANN function to get a Hanning window which has the
%          first and last zero-weighted samples.
%
%   See also BARTLETT, BLACKMAN, BOXCAR, CHEBWIN, HAMMING, HANN, KAISER
%   and TRIANG.
%
%   This is a drop-in replacement to bypass the signal processing toolbox

% Copyright (c) 2010, Jan-Mathijs Schoffelen, DCCN Nijmegen
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin==1,
  str = 'symmetric';
end

switch str,
case 'periodic'
   % Includes the first zero sample
   tap = [0; hanningX(n-1)];
case 'symmetric'
   % Does not include the first and last zero sample
   tap = hanningX(n);
end

function tap = hanningX(n)

% compute taper
N   = n+1;
tap = 0.5*(1-cos((2*pi*(1:n))./N))';

% make symmetric
halfn = floor(n/2);
tap( (n+1-halfn):n ) = flipud(tap(1:halfn));


function [cum] = lgpdens_cum(bb,x1,x2)
% [CUM] = LGPDENS_CUM(BB)
%
% Description
%   Given Bayesian Bootstrap estimated density, integrates it from point x1
%   to point x2
%
% Copyright (c) 2012 Ernesto Ulloa
% Copyright (c) 2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

ip=inputParser;
ip.addRequired('bb',@(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('x1',@(x) ~isempty(x) && isreal(x))
ip.addRequired('x2', @(x) ~isempty(x) && isreal(x))
ip.parse(bb,x1,x2)

 [p,pq,xt]=lgpdens(bb);
 I1=min(find(xt>x1));
 I2=max(find(xt<x2));
 sd=xt(2)-xt(1);
 cum=sd*trapz(p(I1:I2));

end


% Convert edge list to adjacency matrix.
%
% INPUTS: edge list: mx3, m - number of edges
% OUTPUTS: adjacency matrix nxn, n - number of nodes
%
% Note: information about nodes is lost: indices only (i1,...in) remain
% GB: last updated, Sep 25, 2012

function adj=edgeL2adj(el)

nodes=sort(unique([el(:,1) el(:,2)])); % get all nodes, sorted
adj=zeros(numel(nodes));         % initialize adjacency matrix

% across all edges
for i=1:size(el,1); adj(find(nodes==el(i,1)),find(nodes==el(i,2)))=el(i,3); end


function r = atanh(a)
%ATANH        Hessian (elementwise) inverse hyperbolic tangent
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% 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

  K = prod(size(a.x));
  if K==1                   % scalar hessian

    r.x = atanh(a.x);
    f = 1 / ( 1 - sqr(a.x) );
    r.dx = a.dx * f;
    r.hx = ( a.hx + reshape( (a.x*r.dx) * a.dx.' , size(a.hx) ) ) * f;

  else                      % matrix hessian

    N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');
    N2 = N^2;

    r.x = atanh(a.x);
    if issparse(a.hx)               % input sparse

      ax = 1 ./ ( 1 - sqr(full(a.x(:))) );
      sizeax = length(ax);
      [ia,ja,sa] = find(a.dx);
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if isempty(ia)
        r.dx = sparse([],[],[],N,sizeax);
        r.hx = sparse([],[],[],N2,sizeax);
      else
        adx1 = a.x(:).*ax;
        if isa(a.x,'intval')          % sparse intval
          rdx = times(ax(ja),sa(:),0);
          adx1 = times(adx1(ja),sa(:),0);
          if rdx.complex
            r.dx = intval( sparse(ia,ja,rdx.mid,N,sizeax) , sparse(ia,ja,rdx.rad,N,sizeax) , 'midrad' );
          else
            r.dx = intval( sparse(ia,ja,rdx.inf,N,sizeax) , sparse(ia,ja,rdx.sup,N,sizeax) , 'infsup' );
          end
          if adx1.complex
            adx1 = intval( sparse(ia,ja,adx1.mid,N,sizeax) , sparse(ia,ja,adx1.rad,N,sizeax) , 'midrad' );
          else
            adx1 = intval( sparse(ia,ja,adx1.inf,N,sizeax) , sparse(ia,ja,adx1.sup,N,sizeax) , 'infsup' );
          end
        else                          % sparse point
          r.dx = sparse(ia,ja,ax(ja).*sa(:),N,sizeax);
          adx1 = sparse(ia,ja,adx1(ja).*sa(:),N,sizeax);
        end
        r.hx = adx2rhx(N,sizeax,adx1,r.dx);
      end
      [ia,ja,sa] = find(a.hx);      % sparse point or intval
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if ~isempty(ia)
        if isa(a.x,'intval')
          rhx = times(ax(ja),sa(:),0);
          if rhx.complex
            r.hx = r.hx + intval( sparse(ia,ja,rhx.mid,N2,sizeax) , sparse(ia,ja,rhx.rad,N2,sizeax) , 'midrad' );
          else
            r.hx = r.hx + intval( sparse(ia,ja,rhx.inf,N2,sizeax) , sparse(ia,ja,rhx.sup,N2,sizeax) , 'infsup' );
          end
        else
          r.hx = r.hx + sparse(ia,ja,ax(ja).*sa(:),N2,sizeax);
        end
      end

    else                            % input full

      r.x = atanh(a.x);
      ax = a.x(:).';
      f = 1 ./ ( 1 - sqr(ax) );
      f = f(ones(N*N,1),:);
      r.dx = a.dx .* f(1:N,:);
      adx = repmat(ax,N,1) .* r.dx;
      r.hx = ( a.hx + adx(repmat(1:N,N,1),:) .* a.dx(repmat(1:N,1,N),:) ) .* f;

    end

  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


% Finds the number of k-neighbors (k links away) for every node
%
% INPUTS: adjacency matrix (nxn), start node index, k - number of links
% OUTPUTS: vector of k-neighbors indices
%
% GB: last updated, Oct 7 2012

function kneigh = kneighbors(adj,ind,k)

adjk = adj;
for i=1:k-1; adjk = adjk*adj; end;

kneigh = find(adjk(ind,:)>0);


function [mbar] = mmHg2mbar(mmHg)
% Convert pressure from millimeters of mercury at 0 degrees C to millibars
% Chad Greene 2012
mbar = mmHg*1.33322;


function [pav_trans,score_bounds,llr_bounds] = pav_calibration(tar,non,small_val)
% Creates a calibration transformation function using the PAV algorithm.
% Inputs:
%   tar: A vector of target scores.
%   non: A vector of non-target scores.
%   small_val: An offset to make the transformation function
%     invertible.  small_val is subtracted from the left-hand side
%     of the bin and added to the right-hand side (and the bin
%     height is linear between its left and right ends).
% Outputs:
%   pav_trans: The transformation function.  It takes in scores and
%     outputs (calibrated) log-likelihood ratios.
%   score_bounds: The left and right ends of the line segments
%     that make up the transformation.
%   llr_bounds: The lower and upper ends of the line segments that
%     make up the transformation.

if nargin==0
    test_this()
    return
else
    assert(nargin==3)
    assert(size(tar,1)==1)
    assert(size(non,1)==1)
    assert(length(tar)>0)
    assert(length(non)>0)
end

largeval = 1e6;

scores = [-largeval tar non largeval];
Pideal = [ones(1,length(tar)+1),zeros(1,length(non)+1)];
[scores,perturb] = sort(scores);
Pideal = Pideal(perturb);
[Popt,width,height] = pavx(Pideal);
data_prior = (length(tar)+1)/length(Pideal);
llr = logit(Popt) - logit(data_prior);
bnd_ndx = make_bnd_ndx(width);
score_bounds = scores(bnd_ndx);
llr_bounds = llr(bnd_ndx);
llr_bounds(1:2:end) = llr_bounds(1:2:end) - small_val;
llr_bounds(2:2:end) = llr_bounds(2:2:end) + small_val;
pav_trans = @(s) pav_transform(s,score_bounds,llr_bounds);
end

function scr_out = pav_transform(scr_in,score_bounds,llr_bounds)
scr_out = zeros(1,length(scr_in));
for ii=1:length(scr_in)
    x = scr_in(ii);
    [x1,x2,v1,v2] = get_line_segment_vals(x,score_bounds,llr_bounds);
    scr_out(ii) = (v2 - v1) * (x - x1) / (x2 - x1) + v1;
end
end

function bnd_ndx = make_bnd_ndx(width)
len = length(width)*2;
c = cumsum(width);
bnd_ndx = zeros(1,len);
bnd_ndx(1:2:len) = [1 c(1:end-1)+1];
bnd_ndx(2:2:len) = c;
end

function [x1,x2,v1,v2] = get_line_segment_vals(x,score_bounds,llr_bounds)
p = find(x>=score_bounds,1,'last');
x1 = score_bounds(p);
x2 = score_bounds(p+1);
v1 = llr_bounds(p);
v2 = llr_bounds(p+1);
end

function test_this()
ntar = 10;
nnon = 12;
tar = 2*randn(1,ntar)+2;
non = 2*randn(1,nnon)-2;
tarnon = [tar non];

scores = [-inf tarnon inf];
Pideal = [ones(1,length(tar)+1),zeros(1,length(non)+1)];
[scores,perturb] = sort(scores);
Pideal = Pideal(perturb);
[Popt,width,height] = pavx(Pideal);
data_prior = (length(tar)+1)/length(Pideal);
llr = logit(Popt) - logit(data_prior);
[dummy,pinv] = sort(perturb);
tmp = llr(pinv);
llr = tmp(2:end-1)

pav_trans = pav_calibration(tar,non,0);
pav_trans(tarnon)

end


function h=ref_pconv(f,g,ctype)
%REF_PCONV  Reference PCONV
%   Usage:  h=ref_pconv(f,g)
%
%   PCONV(f,g) computes the periodic convolution of f and g.

% AUTHOR: Peter L. Søndergaard

L=length(f);
h=zeros(L,1);

switch(lower(ctype))
  case {'default'}
    for ii=0:L-1
      for jj=0:L-1
	h(ii+1)=h(ii+1)+f(jj+1)*g(mod(ii-jj,L)+1);
      end;
    end;
  case {'r'}
    for ii=0:L-1
      for jj=0:L-1
	h(ii+1)=h(ii+1)+f(jj+1)*conj(g(mod(jj-ii,L)+1));
      end;
    end;
  case {'rr'}
    for ii=0:L-1
      for jj=0:L-1
	h(ii+1)=h(ii+1)+conj(f(mod(-jj,L)+1))*conj(g(mod(jj-ii,L)+1));
      end;
    end;
end;


function [ J, grad ] = cost_function ( x, e_conn, u, q, M, lam, c )

%*****************************************************************************80
%
%% COST_FUNCTION evaluate the cost function and its gradient.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2011
%
%  Author:
%
%    Jeff Borggaard, John Burkardt, Catalin Trenchea, Clayton Webster
%
%  Parameters:
%
%    Input, real X(N_NODES,1), the nodes of the mesh.
%
%    Input, integer E_CONN(N_ELEMENTS,NEL_DOF),
%
%    Input, real U(N_NODES), the value of the finite element solution at
%    each node.
%
%    Input, real Q(N_NODES,1), the value of the control function at
%    each node of the mesh.
%
%    Input, real M(N_NODES,N_NODES), the mass matrix.
%    M(I,J) = Integral PHI(I) PHI(J).
%
%    Input, real LAM(N_NODES), the value of the adjoint variable at each node.
%
%    Input, real C(N_EQUATIONS), the value of the adjoint equation right hand
%    side at each internal node.
%
%    Output, real J, the cost function, 1/2 sqrt ( integral ( U - U_HAT )^2 ).
%
%    Output, real GRAD(N_NODES,1), the gradient of the cost function (?).
%
%  Local Parameters:
%
%    Local, real ALPHA, a weighting factor in the computation of GRAD.
%
%    Local, real GRAD_LOCAL(NEL_DOF), the local contribution to the gradient
%    at the nodes of a given element.
%
%    Local, real LAMX_G(N_GAUSS), the dLAMdX function at the Gauss points
%    in a given element.
%
%    Local, integer N_ELEMENTS, the number of elements.
%
%    Local, integer N_GAUSS, the number of Gauss points.
%
%    Local, integer N_NODES, the number of nodes.
%
%    Local, integer NEL_DOF, the number of degrees of freedom per element.
%
%    Local, integer NODES_LOCAL, the indices of the nodes in a given element.
%
%    Local, real P_X, the derivative of the basis function evaluated at
%    the Gauss points in a given element.
%
%    Local, real PHI, the basis function evaluated at
%    the Gauss abscissas in a given element.
%
%    Local, real QX_G(N_GAUSS), the value of dQdX at the Gauss points in a
%    given element.
%
%    Local, real R(N_GAUSS), the Gauss points.
%
%    Local, real UX_G(N_GAUSS), the dUdX function at the Gauss points in a
%    given element.
%
%    Local, real W(N_GAUSS), the Gauss weights.
%
%    Local, real W_G(N_GAUSS), the Gauss weights mapped to a given element.
%
%    Local, real X_G(N_GAUSS), the Gauss points mapped to a given element.
%
%    Local, real X_LOCAL(NEL_DOF), the nodes in a given element.
%
  [ n_nodes, n_dimensions ] = size ( x );
  [ n_elements, nel_dof ] = size ( e_conn );
  n_gauss = 3;
  [ r, w ] = oned_gauss ( n_gauss );

  J = 0.5 * sqrt ( c' * M(2:end-1,2:end-1) * c );
%
%  Compute the gradient.
%
  grad = zeros(n_nodes,1);

  alpha = 0.000003;

  for n_el = 1 : n_elements

    nodes_local = e_conn(n_el,:);
    x_local = x(nodes_local,:);
    [ x_g, w_g, phi, p_x ] = oned_shape ( x_local, r, w );

    ux_g = p_x * u(nodes_local);
    qx_g = p_x * q(nodes_local);
    lamx_g = p_x * lam(nodes_local);

    grad_local = alpha * oned_f_int ( qx_g,           p_x, w_g ) ...
                       - oned_f_int ( lamx_g .* ux_g, phi, w_g );
%
%  Assemble local contribution to gradient.
%
    for n_t = 1 : nel_dof
      n_test = nodes_local(n_t);
      grad(n_test) = grad(n_test) + grad_local(n_t);
    end

  end

  return
end


function [u,v,w]=v_sphrharm(m,a,b,c,d)
%V_SPHRHARM  forward and inverse spherical harmonic transform
%
% Usage: (1) y=('f',n,x)      % Calculate complex transform of spatial data x up to order n
%
%        (2) y=('fr',n,x)     % Calculate real transform of spatial data x(ne,na) up to order n
%                             % x is specified on a uniform grid with inclination e=(0.5:ne-0.5)*pi/ne
%                             % and azimuth a=(0:na-1)*2*pi/na. The North pole has inclination 0 and
%                             % azimuths increase going East.
%
%        (3) y=('fd',n,e,a,v) % Calculate transform of impulse at (e(i),a(i)) of height v(i) up to order n
%                             % e(i), a(i) and v(i) should be the same dimension; v defaults to 1.
%
%        (4) x=('i',y,ne,na)  % Calculate spatial data from spherical harmonics y on
%                             % a uniform grid with ne inclinations and na azimuths
%
%        (5) [e,a,w]=('cg',ne,na)   % Calculate the inclinations, azimuths and
%                                   % quadrature weights of a Gaussian sampling grid
%
%        (6) n=2;             % illustrate real basis functions upto order 2
%            for i=0:n
%              for j=-i:i
%                subplot(n+1,2*n+1,i*(2*n+1)+j+n+1);
%                v_sphrharm('irp',[zeros(1,i^2+i+j) 1],25,25);
%              end
%            end
%
%  Inputs:  m string specifying various options:
%               'f' forward transform
%               'i' inverse transform
%               'c' calculate the coordinates of the sampling grid [default if f or i omitted]
%               'r' real spherical harmonics instead of complex
%               'u' uniform inclination grid:  e=(0.5:ne-0.5)*pi/ne (includes neither pole) [default]
%                      for invertibility, ne>=2N+1 for order N
%               'U' uniform inclination grid:  e=(0:ne-1)*pi/ne (includes North pole only)
%                      for invertibility, ne>=2N+1 for order N
%               'g' gaussian inclination grid (non-uniform but fewer samples needed)
%                      for invertibility, ne>=N+1 for order N
%               'a' arbitrary (specified by user - inverse transform only)
%               'd' delta function [forward transform only]
%               'p' plot result
%               'P' plot with colourbar
%
%           The remaining inputs depend on the mode specified:
%               'f'  a         order of transform
%                    b(ne,na)  input data array on the chosen inclination-azimuth sampling grid
%               'fd' a         order of transform
%                    b(k)      inclinations of k delta functions
%                    c(k)      azimuths of k delta functions
%                    d(k)      amplitudes of k delta functions [default=1]
%               'i'  a()       spherical harmonics as a single row vector in the order:
%                                 (0,0),(1,-1),(1,0),(1,1),(2,-2),(2,-1),...
%                              To access as a 2-D harmonic: Y(n,m)=a(n^2+n+m+1) where n=0:N, m=-n:n
%                    b         number of inclination values in output grid
%                    c         number of azimuth values in output grid
%               'c'  a         number of inclination values in output grid
%                    b         number of azimuth values in output grid
%
% Outputs:  u output data according to transform direction:
%                'f': spherical harmonics as a single row vector in the order:
%                        (0,0),(1,-1),(1,0),(1,1),(2,-2),(2,-1),...
%                     To access as a 2-D harmonic: Y(n,m)=u(n^2+n+m+1) where n=0:N, m=-n:n
%                'i': u(ne,na) is spatial data sampled on an azimuth-inclination grid
%                     with ne inclination points (in 0:pi) and na azimuth points (in 0:2*pi)
%                'c': u(ne) gives inclination grid positions with 0 being the North pole
%           v(na) gives azimuth grid positions
%           w(ne) gives the quadrature weights used in the transform
%
% Suppose f(e,a) is a complex-valued function defined on the surface of the
% sphere (0<=e<=pi, 0<=a<2pi) where e=inclination=pi/2-elevation and
% a=azimuth. (0,*) is the North pole, (pi/2,0) is on the equator near
% Ghana and (pi/2,pi/2) is on the equator near India.
%
% We can approximate f(e,a) using complex spherical harmonics as
% f(e,a) = sum(F(n,m)*u(n,m,e,a)) where the sum
% is over n=0:N,m=-n:n giving a total of (N+1)^2 coefficients F(n,m).
%
% If f(e,a) happens to be real-valued, then we can transform the
% coefficients using G(n,0)=F(n,0) and, for m>0,
% [G(n,m); G(n,-m)]=sqrt(0.5)*[1 1; 1i -1i]*[F(n,m); F(n,-m)]
% to give the real spherical harmonic coefficients G(n,m).
%
% The basis functions u(n,m,e,a) are orthonormal under the inner product
% <u(e,a),v(e,a)> = integral(u(e,a)*v'(e,a)*sin(e)*de*da).
%
%  Minimum spatial grid for invertibility:
%     Gaussian grid: ne >= N+1,  na >= 2N+1
%     Uniform grid:  ne >= 2N+1, na >= 2N+1
%     An inverse transform followed by a forward transform will restore the
%     original coefficient values provided that the sampling grid is large
%     enough. The advantage of the Gaussian grid is that it can be smaller.
%
%   Data formats:
%     (1) Spatial Data: x=v_sphrharm('i',y,ne,na)
%            x(1:ne,1:na) is spatial data sampled on an azimuth-inclination grid
%                         with ne inclination points (in 0:pi) and
%                         na azimuth points (in 0:2*pi)
%     (2) Spherical harmonics: y=v_sphrharm('f',n,x)
%            y(1:(n+1)^2)  spherical harmonics as a single row vector
%                          in the order: (0,0),(1,-1),(1,0),(1,1),(2,-2),(2,-1),...
%                          To access as a 2-D harmonic, use
%                          Y(n,m)=y(n^2+n+m+1)   where n=0:N, m=-i:i
%     (3) Sampling Grid:  [e,a,w]=v_sphrharm('c',ne,na)
%            e(1:ne)   monotonically increasing inclination values (0<=e<=pi) where 0 is the North pole
%            a(1:na)   monotonically increasing azimuth values (0<=a<2pi)
%            w(1:ne)   Quadrature weights
%

% future options
%    direction: [m=v_rotation matrix]
%    transform: [h=complex harmonics but only the positive ones specified]
%    grid       d=delta function [forward transform only]
%               [s=sparse azimuth]
%               [z=include both north and south pole]
%
% bugs:
%   (1) we could save space and time by taking advantage of symmetry of legendre polynomials
%   (2) the number of points in the inclination (elevation) grid must, for now, be even if the 'U' option is chosen
%   (3) should ensure that the negative nyquist azimuth frequency is not used
%   (4) save time by manipulating only the necessary 2*m columns of the da matrix
%   (5) should make non-existant coefficients black in plots
%   (6) using 'surf' for plots adds an offset in azimuth and elevation
%       because colours correspond to vertices not faces
%   (7) the normalization for mode 'fd' seems incorrect
%   (8) mode 'fd' should allow multiple impulses to be summed

% errors:

% tests:
% check for inverse transform n=4; m=4; [ve,va]=spvals(8,8,n,m); ve*va-v_sphrharm('iur',[zeros(1,n^2+n+m) 1],8,8)
% check inverse followed by forward: no=4; h=rand(1,(no+1)^2); max(abs(v_sphrharm('fur',v_sphrharm('iur',h,10,10),no)-h))
% same but complex: no=4; h=rand(1,(no+1)^2); max(abs(v_sphrharm('fu',v_sphrharm('iu',h,10,10),no)-h))
% same but gaussian grid: no=4; h=rand(1,(no+1)^2); max(abs(v_sphrharm('fg',v_sphrharm('ig',h,6,10),no)-h))

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

% decode option string
mv='c u ';       % mv(1)=transform, mv(2)=real/complex, mv(3)=grid, mv(4)=plot
if ~nargout
    mv(4)='P';
end
mc='ficruUgadpP';
mi=[1 1 1 2 3 3 3 3 3 4 4];
for i=1:length(m)
    j=find(m(i)==mc);
    if ~isempty(j)
        mv(mi(j))=m(i);
    end
end

switch mv(1)
    case 'f' % forward transform [sp]=('f',order,data)
        if mv(3)~='d'
            % input data has elevations down the columns and azimuths along the rows
            % output data is in the form:
            [ne,na]=size(b);
            da=fft(b,[],2)*sqrt(pi)/na;
            if mv(2)=='r' % only actually need to do this for min(a,floor(na/2)) values (but nyquist is tricky)
                ix=2:ceil(na/2);
                iy=na:-1:na+2-ix(end);
                da(:,ix)=da(:,ix)+da(:,iy);
                da(:,iy)=1i*(da(:,ix)-2*da(:,iy));  % note
                da(:,1)=da(:,1)*sqrt(2);
            else
                da=da*sqrt(2);
            end
            [ue,we,lgp]=sphrharp(mv(3),ne,a);
            da=da.*repmat(we',1,na);
            u=zeros(1,(a+1)^2);
            i=0;
            for nn=0:a
                % we could vectorize this to avoid the inner loop
                % we should ensure the the negative nyquist value is not used
                for mm=-nn:nn
                    i=i+1;
                    u(i)=lgp(1+nn*(nn+1)/2+abs(mm),:)*da(:,1+mod(mm,na));
                end
            end
        else       % forward transform of impulses [sp]=('fd',order,e,a,value)
            if nargin<5
                d=ones(size(b));
            end
            [ue,we,lgp]=sphrharp('a',b(:),a);
            uu=zeros(1,(a+1)^2);     % reserve space for spherical harmonic coefficients
            u=uu;
            for j=1:numel(b)
                i=0;
                if mv(2)=='r'
                    for nn=0:a
                        % we could vectorize this to avoid the inner loop
                        % we should ensure the the negative nyquist value is not used
                        i=i+1;
                        uu(i+nn)=lgp(1+nn*(nn+1)/2,1);
                        for mm=1:nn
                            uu(i+nn-mm)=lgp(1+nn*(nn+1)/2+abs(mm),j)*sin(mm*c(j))*sqrt(2);
                            uu(i+nn+mm)=lgp(1+nn*(nn+1)/2+abs(mm),j)*cos(mm*c(j))*sqrt(2);
                        end
                        i=i+2*nn;
                    end
                else
                    for nn=0:a
                        % we could vectorize this to avoid the inner loop
                        % we should ensure the the negative nyquist value is not used
                        for mm=-nn:nn
                            i=i+1;
                            uu(i)=lgp(1+nn*(nn+1)/2+abs(mm),j)*exp(-1i*mm*c(j));
                        end
                    end
                end
                u=u+d(j)*uu/sqrt(2*pi);
            end
        end
    case 'i' % inverse transform [data]=('i',sp,nincl,nazim)
        %or [data]=('a',sp,e,a) where e is a list of elevations and a is either a corresponding list of azimuths
        % or else a cell array contining several azimuths for each inclination
        % input data is sp=[(0,0) (1,-1) (1,0) (1,1) (2,-2) (2,-1) (2,0) ... ]
        % length is (n+1)^2 where n is the order
        % output data is an array [ne,na]
        nsp=ceil(sqrt(length(a)))-1;
        [ue,we,lgp]=sphrharp(mv(3),b,nsp);
        if mv(3)=='a'
            na=nsp*2+1;
            ne=length(b);
        else
            na=c;
            ne=b;
        end
        i=0;
        da=zeros(ne,na);
        for nn=0:nsp
            % this could be vectorized somewhat to speed it up
            for mm=-nn:nn
                i=i+1;
                if i>length(a)
                    break;
                end
                da(:,1+mod(mm,na))=da(:,1+mod(mm,na))+a(i)*lgp(1+nn*(nn+1)/2+abs(mm),:)';
            end
        end
        if na>1 && mv(2)=='r' % convert real to complex only actually need to do this for min(b,floor(na/2)) values (but nyquist is a bit tricky)
            ix=2:ceil(na/2);
            iy=na:-1:na+2-ix(end);
            da(:,iy)=(da(:,ix)+1i*da(:,iy))*0.5;
            da(:,ix)=da(:,ix)-da(:,iy);
            da(:,1)=da(:,1)/sqrt(2);
        else
            da=da/sqrt(2);
        end
        if mv(3)=='a' % do a slow fft
            if iscell(c)
                u{ne,1}=[]; % reserve space for output cell array
                for i=1:ne
                    ai=c{i};
                    ui=repmat(da(i,1),1,length(ai));
                    for j=1:nsp
                        exj=exp(1i*j*ai(:).'); % we could vectorize this more
                        ui=ui+da(i,j+1).'.*exj+da(i,na+1-j).'.*conj(exj);
                    end
                    u{i}=ui/sqrt(pi);
                end
            else
                u=da(:,1);
                for j=1:nsp
                    exj=exp(1i*j*c(:)); % we could vectorize this more
                    u=u+da(:,j+1).*exj+da(:,na+1-j).*conj(exj);
                end
                u=u/sqrt(pi);
                if mv(2)=='r' && isreal(a)
                    u=real(u);
                end
            end
        else
            u=ifft(da,[],2)*na/sqrt(pi); % could put the scale factor 1/sqrt(pi) earlier
            if mv(2)=='r' && isreal(a)
                u=real(u);
            end
        end
    case 'c' % just output coordinates [inclination,azim,weights]=('c',nincl,nazim)
        % if m!='g', then order, a, must be even
        [u,w]=sphrharp(mv(3),a,0);
        if nargin<3
            b=ceil(a/1+(mv(3)=='g'));
        end
        v=(0:b-1)*2*pi/b;
end
if mv(4)~=' '
    switch mv(1)
        case 'f'
            if mv(2)=='r'
                ua=u;
                tit='Real Coefficients';
            else
                ua=abs(u);
                tit='Complex Coefficient Magnitudes';
            end
            nu=length(ua);
            no=ceil(sqrt(nu))-1;
            yi=zeros(no,2*no+1);
            for i=0:no
                for j=-i:i
                    iy=i^2+i+j+1;
                    if iy<=nu
                        yi(i+1,j+no+1)=ua(iy);
                    end
                end
            end
            imagesc(-no:no,0:no,yi);
            axis 'xy';
            if mv(4)=='P'
                colorbar;
            end
            xlabel('Harmonic');
            ylabel('Order');
            title(tit);
        case 'i'            % [data]=('i',sp,nincl,nazim)
            vv=(0:na)*2*pi/na;  % azimuth array
            gr=sin(ue)';
            surf(gr*cos(vv),gr*sin(vv),repmat(cos(ue)',1,na+1),real(u(:,[1:na 1])));
            axis equal;
            xlabel('X');
            ylabel('Y');
            zlabel('Z');
            if mv(4)=='P'
                colorbar;
            end
            %             v_cblabel('Legendre Weight');
            %             title(sprintf('Sampling Grid: %s, %d, %d',mv(3),length(u),na-1));
        case 'c'            % [inclination,azim,weights]=('c',nincl,nazim)
            vv=[v v(1)];    % replicate initial azimuth point
            na=length(vv);
            gr=sin(u)';
            mesh(gr*cos(vv),gr*sin(vv),repmat(cos(u)',1,na),repmat(w',1,na));
            axis equal;
            xlabel('X');
            ylabel('Y');
            zlabel('Z');
            if mv(4)=='P'
                colorbar;
                v_cblabel('Quadrature Weight');
            end
            title(sprintf('Sampling Grid: %s, %d, %d',mv(3),length(u),na-1));
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ueo,weo,lgpo]=sphrharp(gr,ne,nsp)
% calculate persistent variables for grid points and legendre polynomials
% we recalculate if transform order or inclination grid size or type are changed
% gr = grid type:
%       'u'=uniform inclination starting at pi/2n (default)
%       'U'=uniform but starting at north pole
%       'g'=gaussian inclination
%       'a'=arbitrary (specified by user - inverse transform only)
% ne = number of inclination values
% nsp = maximum order
% Outputs:
%    ueo(ne)     vector containing the inclination values
%    weo(ne)     vector containing the quadrature weights
%    lgpo((nsp+1)*(nsp+2)/2,ne)  evaluates the Schmidt seminormalized
%                                associated Legendre function at each value
%                                of cos(ue) for n=0:nsp, m=0:n.
persistent gr0 lgp ne0 ue we nsp0
if isempty(ne0)
    ne0=-1;
    nsp0=-1;
    gr0='a';
end
if gr=='a'
    ue=ne;
    ne=length(ue);
    ne0=-1;
    gr0=gr;
    nsp0=-1; % delete previous legendre polynomials
    lgp=[];
    we=[];
elseif gr~=gr0 || ne~=ne0
    if gr=='g'
        r = 1:ne-1;
        r = r ./ sqrt(4*r.^2 - 1);
        p = zeros( ne, ne );
        p( 2 : ne+1 : ne*(ne-1) ) = r;
        p( ne+1 : ne+1 : ne^2-1 ) = r;
        [q, ue] = eig(p);
        [ue, k] = sort(diag(ue));
        ue=acos(-ue)';
        we = 2*q(1,k).^2;
    elseif gr=='U'
        if rem(ne,2)
            error('inclination grid size must be even when using ''U'' option');
        end
        ue=(0:ne-1)*pi/ne;
        xx=zeros(1,ne);
        ah=ne/2;
        xx(1:ah)=(1:2:ne).^(-1);
        we=-4*sin(ue).*imag(fft(xx).*exp(-1i*ue))/ne;
    else % default is m='u'
        ue=(1:2:2*ne)*pi*0.5/ne; % vector of elevations
        vq=(ne-abs(ne+1-2*(1:ne))).^(-1).*exp(-1i*(ue+0.5*pi));
        we=(-2*sin(ue).*real(fft(vq).*exp(-1i*(0:ne-1)*pi/ne))/ne);
    end
    gr0=gr;
    ne0=ne;
    nsp0=-1; % delete previous legendre polynomials
    lgp=[];
end
if nsp>nsp0
    lgp((nsp+1)*(nsp+2)/2,ne)=0; % reserve space
    for i=nsp0+1:nsp
        lgp(1+i*(i+1)/2:(i+1)*(i+2)/2,:)=legendre(i,cos(ue),'sch')*sqrt(0.5*i+0.25);
        lgp(1+i*(i+1)/2,:)=lgp(1+i*(i+1)/2,:)*sqrt(2);
    end
    nsp0=nsp;
end
lgpo=lgp;
weo=we;
ueo=ue;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   FANUC LR MATE 10iA, FANUC Robotics Europe.
%   Author: David Garcia Munyoz, Nestor Gomez Lopez, Teresa Pomares
%   Palorames.
%   Universidad Miguel Hernandez de Elche.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function q = inversekinematic_fanuc_mate(robot, T)
%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
d = eval(robot.DH.d);

%See geometry at the reference for this robot
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

A3=a(3); % desfase

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Eslab�n equivalente para simplificar el desfase
l3 = sqrt (A3^2 + L3^2);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = (acos((L2^2+r^2-l3^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_fanuc_mate: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);

A3= a(3); %desfase

%See geometry of the robot
%compute L4
l3 = sqrt(A3^2 + L3^2);

%the angle phi is fixed
phi=acos((A3^2+l3^2-L3^2)/(2*A3*l3));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = (acos((L2^2 + l3^2 - r^2)/(2*L2*l3)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_fanuc_mate: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta;
q3(2) = pi - phi + eta;

%remove complex solutions for q for the q1+pi solutions
function  qreal = arrange_solutions(q)
qreal=q(:,1:4);

%sum along rows if any angle is complex, for any possible solutions, then v(i) is complex
v = sum(q, 1);

for i=5:8,
    if isreal(v(i))
        qreal=[qreal q(:,i)]; %store the real solutions
    end
end

qreal = real(qreal);


% Fig. 6.42   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

alpha=logspace(-2,0,100);
N=size(alpha);
ialpha=ones(N)./alpha;
dum=(ialpha-ones(N))./(ialpha+ones(N));
phimax=asin(dum);
ff=180/pi;
phimax=ff*phimax;
semilogx(ialpha,phimax);
xlabel('1/\alpha');
ylabel('Maximum phase lead (deg)');
grid;
title('Fig. 6.53 Maximum phase increase for lead compensation.');


% div = GRAPH_DIV(S, A, ki, kj)
%
%Divergence of signal residing on edges of a graph. The output resides on
%nodes. The result should be such that
%   graph_div(graph_grad(s)) = L * s,
%where L is the graph Laplacian associated with the adjacency matrix A.
%
%OUTPUT:
%   div: divergence of columns of S defined on vertices
%
%INPUTS:
%   S: signal defined on edges e
%   A: adjacency matrix (weighted or not)
%   ki, kj: indices of edges by graph_adj2vec
%
%
% TODO: does not handle normalized Laplacian case!!
%
%
%note that the norm of the operator is
%
% ||D||^2_2 = ||L||_2, where G is the divergence operator and L is the
% Laplacian used.
%
%
%see also: graph_grad, graph_adj2vec, sgwt_laplacian
%
%code author: Vassilis Kalofolias
%date: Aug 2013

function div = gsp_div_old(G,s)

if ~strcmp(G.lap_type,'combinatorial')
    error('Not implemented yet. However ask Nathanael it is very easy');
end

if size(s,1) ~= G.Ne
    error('Signal size not equal to number of edges');
end

k = size(s,2);

% What happens with loops? (diagonal elements of G, A)
%
% I think both div and grad is zero on the diagonal elements. But then is
% it consistent with the Laplacian? (it has to be div(grad(s)) = 2*L*s) for
% a signal s defined on the vertices.

div = zeros(G.N, k);

for ii = 1 : k
    tmp = sparse(G.v_in, G.v_out, s(:,ii), G.N,G.N);
    tmp = tril(tmp,1) - tril(tmp,1)';

    % the divergence
    div(:,ii) = sum(tmp.* sqrt(G.W), 2);
end

end


function pass = test_stringConstructor(~)
%TEST_STRINGCONSTRUCTOR    Test that we can pass strings to the CHEBOP
%constructor

%% Linear problem, no x dependence
% String syntax
L1 = chebop('u``+u',[0 5]);
L1.lbc = 1;
L1.rbc = 2;
u1 = L1\0;

% Anonymous function syntax
L2 = chebop(@(u) diff(u,2) + u, [0 5]);
L2.lbc = 1;
L2.rbc = 2;
u2 = L2\0;

% These should be identical!
pass(1) = norm(u1-u2) == 0;

%% Linear problem, x dependence
% String syntax
L1 = chebop('u``+x*u',[0 5]);
L1.lbc = 1;
L1.rbc = 2;
u1 = L1\0;

% Anonymous function syntax
L2 = chebop(@(x,u) diff(u,2) + x.*u, [0 5]);
L2.lbc = 1;
L2.rbc = 2;
u2 = L2\0;

% These should be identical!
pass(2) = norm(u1-u2) == 0;

%% Nonlinear problem, no x dependence
% String syntax
tic
N1 = chebop('u``+sin(u)',[0 5]);
N1.lbc = 1;
N1.rbc = 2;
u1 = N1\0;

% Anonymous function syntax
N2 = chebop(@(u) diff(u,2) + sin(u), [0 5]);
N2.lbc = 1;
N2.rbc = 2;
u2 = N2\0;

% These should be identical!
pass(3) = norm(u1-u2) == 0;

%% Linear problem, x dependence
% String syntax
N1 = chebop('u``+x*sin(u)',[0 5]);
N1.lbc = 1;
N1.rbc = 2;
u1 = N1\0;

% Anonymous function syntax
N2 = chebop(@(x,u) diff(u,2) + x.*sin(u), [0 5]);
N2.lbc = 1;
N2.rbc = 2;
u2 = N2\0;

% These should be identical!
pass(4) = norm(u1-u2) == 0;


%
% Nonuniform FFT in R^2.
%   nufft2d1 - Type 1 - Evaluate linearly spaced Fourier modes (Type 1 nuFFT).
%   nufft2d2 - Type 2 - Evaluate function values (inverse of Type 1 nuFFT).
%   nufft2d3 - Type 3 - Evaluate arbitrary located Fourier modes (Type 3 nuFFT).
%
% Direct (slow) evaluation of nonuniform FFT in R^2.
%   dirft2d1 - Type 1 - Evaluate linearly spaced Fourier modes (Type 1 nuFFT).
%   dirft2d2 - Type 2 - Evaluate function values (inverse of Type 1 nuFFT).
%   dirft2d3 - Type 3 - Evaluate arbitrary located Fourier modes (Type 3 nuFFT).
%
% Test for nuFFT in R^2.
%   test_2d1 - Test Type 1 nuFFT.
%   test_2d2 - Test Type 2 nuFFT.
%   test_2d3 - Test Type 3 nuFFT.
%


% funSimLogNormProbCovFade returns SINR-based k-coverage probability
% under Rayleigh (mean one) fading model and log-normal shadowing based on
% repeated simulations of of model outlined in [1]
%
% simPCovFade=funSimLogNormProbCovFade(tValues,betaConst,K,lambda,sigma,W,diskRadius,simNumb)
% simPCovFade is the 1-coverage probability
% tValues are the SINR threshold values. tValues can be a vector
% betaConst is the pathloss exponent
% K = path-loss constant
% lambda = density of base station/nodes of cellular network
% sigma = standard deviation of log-normal shadowing (not in dB)
% W = noise constant
% diskRadius = radius of simulation region (Warning: if too small, edge
% effects will not be sufficiently included and disagreement with analytic
% results may occurr)
% simNumb = number of simulations
% All input values (except tValues) are scalars
%
% Author: H.P. Keeler, Inria Paris/ENS, 2013
%
% References
% [1] H.P. Keeler, B. Błaszczyszyn and M. Karray,
% 'SINR-based k-coverage probability in cellular networks with arbitrary
% shadowing', accepted at ISIT, 2013

function simPCovFade=funSimLogNormProbCovFade(tValues,betaConst,K,lambda,sigma,W,diskRadius,simNumb)

tNumb=length(tValues);

%%% Simulation Section %%%
%(uniformly) randomly places nodes on a disk of radius diskRadius
diskArea=pi*diskRadius^2;
coveredNumb=zeros(size(tValues));
ESTwoBeta=exp(sigma^2*(2-betaConst)/betaConst^2);
%rescale lambda - see foot note 5 in [1]
lambdaSim=lambda*ESTwoBeta;
for i=1:simNumb

    randNumb=poissrnd(lambdaSim*diskArea);
    %shadowing distribution can be constant if lambda is rescaled - see [1], [2] and [3]
    shadowRand=ones(randNumb,1);
    fadeRand=exprnd(1,randNumb,1); %Rayleigh fading corresponds to exponential random variables
    %random distances from the typical node
    rRand=diskRadius*sqrt(rand(randNumb,1)); %uniform in cartesion, not polar coordinates
    signalRand=(K*rRand).^(betaConst)./shadowRand;
    [Y1 indexY1]=min(signalRand);    %find Y_1 (first order statistics)

    interferTotal=sum(fadeRand./signalRand); %total inteference in network
    newExp=fadeRand(indexY1); %use corresponding exponential variable
    SINR=(newExp/Y1)./((interferTotal-(newExp/Y1))+W);
    for j=1:tNumb
        T=tValues(j);
        %counts how many nodes are connected/covered
        if sum(SINR>=T)>=1
            coveredNumb(j)=coveredNumb(j)+1;
        end
    end
end

simPCovFade=coveredNumb/simNumb;


function vos_map = obtain_vos_map(img_q,N)
    % build color cube (color histogram)
    [row,col] = size(img_q(:,:,1));
    color_cube = zeros(N,N,N);
    for i = 1 : row
        for j = 1 : col
            cr = img_q(i,j,1);
            cg = img_q(i,j,2);
            cb = img_q(i,j,3);
            color_cube(cr,cg,cb) = color_cube(cr,cg,cb) + 1;
        end
    end

    % label major color
    num_counted_pixels = 0; % record numder of pixels that have been counted
    num_major_color = 0; % record numder of major colors
    prop = 0.95; % care about 95%
    while num_counted_pixels < prop*row*col
        color_most_n = max(max(max(color_cube))); % number of the most color at this iteration
        same_n = length(color_cube(color_cube == color_most_n));
        num_counted_pixels = num_counted_pixels + same_n*color_most_n;
        num_major_color = num_major_color + same_n;
        % label the counted points in color_cube as -1, meaning that these
        % colors are "major"
        color_cube(color_cube == color_most_n) = -1;
    end

    % assign the rest 5% colors to their near major color
    [r_set,g_set,b_set] = ind2sub(size(color_cube),find(color_cube==-1));
    while ~isempty(color_cube(color_cube>0))
        color_most_n = max(max(max(color_cube)));
        [cr,cg,cb] = ind2sub(size(color_cube),find(color_cube==color_most_n));
        color_cube(color_cube==color_most_n) = -2;
        for i = 1 : length(cr)
            dist = zeros(length(r_set),1);
            for j = 1 : length(r_set)
                dist(j) = (cr(i)-r_set(j))^2 + (cg(i)-g_set(j))^2 + (cb(i)-b_set(j))^2;
            end
            label = find(dist == min(dist));
            r_assign = r_set(label(1));
            g_assign = g_set(label(1));
            b_assign = b_set(label(1));
            for m = 1 : row
                for n = 1 : col
                    if img_q(m,n,1) == cr(i) && img_q(m,n,2)==cg(i) && img_q(m,n,3)==cb(i)
                        img_q(m,n,:) = reshape([r_assign,g_assign,b_assign],1,1,3);
                    end
                end
            end
        end
    end
    % compute vos saliency
    [r_set,g_set,b_set] = ind2sub(size(color_cube),find(color_cube==-1));
    sal = zeros(length(r_set),1);
    vos_map = zeros(row,col);
    for i = 1 : length(r_set)
        for j = 1 : length(r_set)
            if j == i
                continue
            end
            sal(i) = sal(i) + sqrt((r_set(i)-r_set(j))^2+(g_set(i)-g_set(j))^2+(b_set(i)-b_set(j))^2);
        end
        for m = 1 : row
            for n = 1 : col
                if img_q(m,n,1)==r_set(i) && img_q(m,n,2)==g_set(i) && img_q(m,n,3)==b_set(i)
                    vos_map(m,n) = sal(i);
                end
            end
        end
    end
    vos_map = mat2gray(vos_map);
end


function imgOut = DurandTMO(img, target_contrast, filter_type)
%
%       imgOut = DurandTMO(img, target_contrast, filter_type)
%
%
%        Input:
%           -img: input HDR image
%           -target_contrast: how to reduce the dynamic range
%
%        Output:
%           -imgOut: tone mapped image
%
%     Copyright (C) 2010-15  Francesco Banterle
%
%     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/>.
%
%     The paper describing this technique is:
%     "Fast Bilateral Filtering for the Display of High-Dynamic-Range Images"
% 	  by Fredo Durand and Julie Dorsey
%     in Proceedings of SIGGRAPH 2002
%

%is it a three color channels image?
check13Color(img);

checkNegative(img);

%default parameters
if(~exist('target_contrast', 'var'))
    target_contrast = 5; %as in the original paper
end

if(~exist('filter_type', 'var'))
    filter_type = 'approx_importance';
end

%compute luminance channel
L = lum(img);

sigma_s = max(size(L)) * 0.02;

%separate detail and base
[Lbase, Ldetail] = bilateralSeparation(L, sigma_s, 0.4, 'log10', filter_type);

eps = 1e-6;
log_base = log10(Lbase + eps);
max_log_base = max(log_base(:));
log_detail = log10(Ldetail);
c_factor = log10(target_contrast) / (max_log_base - min(log_base(:)));
log_absolute = c_factor * max_log_base;

log_Ld = log_base * c_factor + log_detail  - log_absolute;
Ld = 10.^(log_Ld) - eps;
Ld(Ld < 0.0) = 0.0;

%change luminance
imgOut = ChangeLuminance(img, L, Ld);

end