function [x,zo]=overlapadd(f,win,inc)
%OVERLAPADD join overlapping frames together X=(F,WIN,INC)
%
% Inputs:  F(NR,NW) contains the frames to be added together, one
%                   frame per row.
%          WIN(NW)  contains a window function to multiply each frame.
%                   WIN may be omitted to use a default rectangular window
%                   If processing the input in chunks, WIN should be replaced by
%                   ZI on the second and subsequent calls where ZI is the saved
%                   output state from the previous call.
%          INC      gives the time increment (in samples) between
%                   succesive frames [default = NW].
%
% Outputs: X(N,1) is the output signal. The number of output samples is N=NW+(NR-1)*INC.
%          ZO     Contains the saved state to allow a long signal
%                 to be processed in chunks. In this case X will contain only N=NR*INC
%                 output samples.
%
% Example of frame-based processing:
%          INC=20       													% set frame increment
%          NW=INC*2     													% oversample by a factor of 2 (4 is also often used)
%          S=cos((0:NW*7)*6*pi/NW);								% example input signal
%          W=sqrt(hamming(NW+1)); W(end)=[];      % sqrt hamming window of period NW
%          F=enframe(S,W,INC);               			% split into frames
%          ... process frames ...
%          X=overlapadd(F,W,INC);           			% reconstitute the time waveform (omit "X=" to plot waveform)

%	   Copyright (C) Mike Brookes 2009
%      Version: $Id: overlapadd.m,v 1.2 2009/06/08 16:21:49 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nr,nf]=size(f);            % number of frames and frame length
if nargin<2
    win=nf;                 % default increment
end
if isstruct(win)
    w=win.w;
    if ~numel(w) && length(w)~=nf
        error('window length does not match frames size');
    end
    inc=win.inc;
    xx=win.xx;
else
    if nargin<3
        inc=nf;
    end
    if numel(win)==1 && win==fix(win) && nargin<3       % win has been omitted
        inc=win;
        w=[];
    else
        w=win(:).';
        if length(w)~=nf
            error('window length does not match frames size');
        end
        if all(w==1)
            w=[];
        end
    end
    xx=[];      % partial output from previous call is null
end
nb=ceil(nf/inc);        % number of overlap buffers
no=nf+(nr-1)*inc;       % buffer length
z=zeros(no,nb);                      % space for overlapped output speech
if numel(w)
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f.*repmat(w,nr,1);
else
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f;
end
x=sum(z,2);
if ~isempty(xx)
    x(1:length(xx))=x(1:length(xx))+xx;     % add on leftovers from previous call
end
if nargout>1            % check if we want to preserve the state
    mo=inc*nr;          % completed output samples
    if no<mo
        x(mo,1)=0;
        zo.xx=[];
    else
        zo.xx=x(mo+1:end);
        zo.w=w;
        zo.inc=inc;
        x=x(1:mo);
    end
elseif ~nargout
    if isempty(xx)
        k1=nf-inc;  % dubious samples at start
    else
        k1=0;
    end
    k2=nf-inc;      % dubious samples at end
    plot(1+(0:nr-1)*inc,x(1+(0:nr-1)*inc),'>r',nf+(0:nr-1)*inc,x(nf+(0:nr-1)*inc),'<r', ...
        1:k1+1,x(1:k1+1),':b',k1+1:no-k2,x(k1+1:end-k2),'-b',no-k2:no,x(no-k2:no),':b');
    xlabel('Sample Number');
    title(sprintf('%d frames of %d samples with %.0f%% overlap = %d samples',nr,nf,100*(1-inc/nf),no));
end


function zL = extrap(z, limtype);	% Slope-limited extrapolation (MUSCL)
					% Staggered scheme
% Function called: limiter

% Numbering for pressure nodes:
%   1 1 2 2                     J J
% |-o-|-o-|-o-|-o-|-o-|-o-|-o-|-o-|
% Numbering for momentum nodes:
% 1 1 2 2                        J-1  J
% |-o-|-o-|-o-|-o-|-o-|-o-|-o-|---o---|

J = length(z);
zL(1) = z(1);
for j = 2:J-1
  b     = (z(j+1) - z(j)); a = (z(j) - z(j-1));
  zL(j) = z(j) + 0.5*limiter(a, b, limtype)*(z(j+1)-z(j));
end
zL(J) = z(J);


function x = fnlCg(x0,params)
%-----------------------------------------------------------------------
%
% res = fnlCg(x0,params)
%
% implementation of a L1 penalized non linear conjugate gradient reconstruction
%
% The function solves the following problem:
%
% given k-space measurments y, and a fourier operator F the function
% finds the image x that minimizes:
%
% Phi(x) = ||F* W' *x - y||^2 + lambda1*|x|_1 + lambda2*TV(W'*x)
%
%
% the optimization method used is non linear conjugate gradient with fast&cheap backtracking
% line-search.
%
% (c) Michael Lustig 2007
%-------------------------------------------------------------------------
x = x0;

% line search parameters
maxlsiter = params.lineSearchItnlim ;
gradToll = params.gradToll ;
alpha = params.lineSearchAlpha; ,    beta = params.lineSearchBeta;
t0 = params.lineSearchT0;
k = 0;
t = 1;

% copmute g0  = grad(Phi(x))

g0 = wGradient(x,params);

dx = -g0;

% iterations
while(1)

% backtracking line-search

	% pre-calculate values, such that it would be cheap to compute the objective
	% many times for efficient line-search
	[FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx] = preobjective(x, dx, params);
	f0 = objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx,x,dx, 0, params);
	t = t0;
        [f1, ERRobj, RMSerr]  =  objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx,x,dx, t, params);

	lsiter = 0;

	while (f1 > f0 - alpha*t*abs(g0(:)'*dx(:)))^2 & (lsiter<maxlsiter)
		lsiter = lsiter + 1;
		t = t * beta;
		[f1, ERRobj, RMSerr]  =  objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx,x,dx, t, params);
	end

	if lsiter == maxlsiter
		disp('Reached max line search,.... not so good... might have a bug in operators. exiting... ');
		return;
	end

	% control the number of line searches by adapting the initial step search
	if lsiter > 2
		t0 = t0 * beta;
	end

	if lsiter<1
		t0 = t0 / beta;
	end

	x = (x + t*dx);

	%--------- uncomment for debug purposes ------------------------
	disp(sprintf('%d   , obj: %f, RMS: %f, L-S: %d', k,f1,RMSerr,lsiter));

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

    %conjugate gradient calculation

	g1 = wGradient(x,params);
	bk = g1(:)'*g1(:)/(g0(:)'*g0(:)+eps);
	g0 = g1;
	dx =  - g1 + bk* dx;
	k = k + 1;

	%TODO: need to "think" of a "better" stopping criteria ;-)
	if (k > params.Itnlim) | (norm(dx(:)) < gradToll)
		break;
	end

end

return;

function [FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx] = preobjective(x, dx, params)

% precalculates transforms to make line search cheap

FTXFMtx = params.FT*(params.XFM'*x);
FTXFMtdx = params.FT*(params.XFM'*dx);

if params.TVWeight
    DXFMtx = params.TV*(params.XFM'*x);
    DXFMtdx = params.TV*(params.XFM'*dx);
else
    DXFMtx = 0;
    DXFMtdx = 0;
end

function [res, obj, RMS] = objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx, x,dx,t, params);
%calculated the objective function

p = params.pNorm;

obj = FTXFMtx + t*FTXFMtdx - params.data;
obj = obj(:)'*obj(:);

if params.TVWeight
    w = DXFMtx(:) + t*DXFMtdx(:);
    TV = (w.*conj(w)+params.l1Smooth).^(p/2);
else
    TV = 0;
end

if params.xfmWeight
   w = x(:) + t*dx(:);
   XFM = (w.*conj(w)+params.l1Smooth).^(p/2);
else
    XFM=0;
end

TV = sum(TV.*params.TVWeight(:));
XFM = sum(XFM.*params.xfmWeight(:));
RMS = sqrt(obj/sum(abs(params.data(:))>0));

res = obj + (TV) + (XFM) ;

function grad = wGradient(x,params)

gradXFM = 0;
gradTV = 0;

gradObj = gOBJ(x,params);
if params.xfmWeight
gradXFM = gXFM(x,params);
end
if params.TVWeight
gradTV = gTV(x,params);
end

grad = (gradObj +  params.xfmWeight.*gradXFM + params.TVWeight.*gradTV);

function gradObj = gOBJ(x,params)
% computes the gradient of the data consistency

	gradObj = params.XFM*(params.FT'*(params.FT*(params.XFM'*x) - params.data));

gradObj = 2*gradObj ;

function grad = gXFM(x,params)
% compute gradient of the L1 transform operator

p = params.pNorm;

grad = p*x.*(x.*conj(x)+params.l1Smooth).^(p/2-1);

function grad = gTV(x,params)
% compute gradient of TV operator

p = params.pNorm;

Dx = params.TV*(params.XFM'*x);

G = p*Dx.*(Dx.*conj(Dx) + params.l1Smooth).^(p/2-1);
grad = params.XFM*(params.TV'*G);


function pde = TorusTimeInitial4(mum,mup,sigm,sigp,epsm,epsp,omega,x0,y0,z0,r1,r2,a,b,intPt)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'Mu',@Mu,'Mum',@Mum,'Mup',@Mup,'one',@one,...
    'Epslon',@Epslon,'Epslonm',@Epslonm,'Epslonp',@Epslonp,...
    'Sig',@Sig,'Sigm',@Sigm,'Sigp',@Sigp,...
    'E1',@E1,'E2',@E2,'E3',@E3,...
    'Et1',@Et1,'Et2',@Et2,'Et3',@Et3);

pde.mum = mum;
pde.mup = mup;
pde.sigm = sigm;
pde.sigp = sigp;
pde.epsm = epsm;
pde.epsp = epsp;
%% interface function
    function u = intf(x,y,z)
        u = (sqrt((x-x0).^2+(y-y0).^2)-r2).^2 + (z-z0).^2 - r1^2;
    end

%% exact solution
    function u = exactu1(x,y,z,t)
        u = um1(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id),t);
        %u = t*ones(size(x));
    end
    function u = exactu2(x,y,z,t)
        u = um2(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id),t);
        %u = t*ones(size(x));
    end
    function u = exactu3(x,y,z,t)
        u = um3(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id),t);
        %u = t*ones(size(x));
    end
    function u = um1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = um2(x,y,z,t)
        u = exp(-b*(a*(x-intPt)-omega*t).^2);
    end
    function u = um3(x,y,z,t)
        u = zeros(size(x));
    end
    function u = up1(x,y,z,t)
        u = zeros(size(x));
        %u = t*ones(size(x));
    end
    function u = up2(x,y,z,t)
        u = exp(-b*(a*(x-intPt)-omega*t).^2);
    end
    function u = up3(x,y,z,t)
        u = zeros(size(x));
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z,t)
        u = Dxum(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id),t);
    end
    function u = Dyu(x,y,z,t)
        u = Dyum(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id),t);
    end
    function u = Dzu(x,y,z,t)
        u = Dzum(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id),t);
    end
    function u = Dxum(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dyum(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dzum(x,y,z,t)
        u = -exp(-b*(a*(x-intPt)-omega*t).^2)*2*a*b.*(a*(x-intPt)-omega*t);
    end
    function u = Dxup(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dyup(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dzup(x,y,z,t)
        u = -exp(-b*(a*(x-intPt)-omega*t).^2)*2*a*b.*(a*(x-intPt)-omega*t);
    end

%% right hand side function
    function u = f1(x,y,z,t)
        u = fm1(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id),t);
    end
    function u = f2(x,y,z,t)
        u = fm2(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id),t);
    end
    function u = f3(x,y,z,t)
        u = fm3(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id),t);
    end

    function u = fm1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fm2(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fm3(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp2(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp3(x,y,z,t)
        u = zeros(size(x));
    end

%% Diffusion coefficient function
    function u = Mu(x,y,z)
        u = Mum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Mup(x(id),y(id),z(id));
    end
    function u = Mum(x,y,z)
        u = mum^(-1)*ones(size(x));
    end
    function u = Mup(x,y,z)
        u = mup^(-1)*ones(size(x));
    end

%% Mass coefficient function
    function u = Epslon(x,y,z)
        u = Epslonm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Epslonp(x(id),y(id),z(id));
    end
    function u = Epslonm(x,y,z)
        u = epsm*ones(size(x));
    end
    function u = Epslonp(x,y,z)
        u = epsp*ones(size(x));
    end

    function u = Sig(x,y,z)
        u = Sigm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Sigp(x(id),y(id),z(id));
    end
    function u = Sigm(x,y,z)
        u = sigm*ones(size(x));
    end
    function u = Sigp(x,y,z)
        u = sigp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function [D,u,X,div_X,phi,pre,B,t] = heat_geodesic(varargin)
  % HEAT_GEODESIC  geodesic distance approximation following the method of
  % "Geodesics in Heat" [Krane et al. 2013] D from all points V in the domain
  % (V,F) to a source point/set of points, gamma. The method is motivated by
  % heat using a time parameter t to guide the heat diffusion.
  %
  % D = heat_geodesic(V,F,gamma,t)
  % [D,u,X,div_X,phi,pre] = heat_geodesic(V,F,gamma,t,'ParamName',ParamValue)
  %
  % Inputs:
  %   V  #V by 3 set of vertex positions
  %   F  #F by 3 set of face indices
  %     or
  %      #T by 4 set of tetrahedra indices
  %   gamma  #gamma list of vertex indices of source points
  %   t  time parameter
  %   Optional:
  %     'BoundaryConditions'  followed by one of the following strings
  %       {'average'}:
  %       'dirichlet'  domain boundaries set to 0 when computing heat
  %         diffusion
  %       'neumann'  heat diffusion solved with implicit neumann conditions
  %       'average'  Uniform average of neumann and Dirichlet solutions
  %       'robin'  The original ArXiv paper called the method of averaging the
  %         Dirichlet and Neumann solutions 'robin'. This was later corrected
  %         (this solution cannot be created by boundary conditions to the heat
  %         equation alone: average solutions is a non-linear operation).  In
  %         any case, 'robin' is treated as 'average'.
  %     'Precomputation' Followed by pre struct as returned by this function
  %     'IntrinsicDelaunay'  followed by whether to use intrinsic Delaunay
  %       Laplacian {false}
  %     'Legacy' followed by bool telling whether to use Alec's legacy
  %       implmentation. In particular this is useful because the original
  %       paper is unclear how the boundary of the domain should be handle with
  %       respect to the seed locations (gamma). What if they overlap? What
  %       should be the boundary conditions for the final poisson solve?
  % Outputs:
  %   D  #V list of geodesic distances from vertices to gamma
  %   u  #V list of results of heat diffusion step
  %   X  #F by 3 list of reversed normalized gradients of u
  %   div_X  #V list od divergence of X
  %   phi  #V list of solution to final poisson equation solve
  %
  D=[];
  % Note: This is Alec's previous implmentation. He is still convinced that
  % this is more exact/correct despite not what's written in the article (and
  % implying that refactoring is necessary).
  legacy = false;
  use_intrinsic = false;

  % mandatory input
  V = varargin{1};
  F = varargin{2};

  % number of domain vertices
  n = size(V,1);
  % simplex size
  ss = size(F,2);
  gamma = varargin{3};
  if nargin>=4 && ~isempty(varargin{4})
    t = varargin{4};
  else
    switch ss
    case 3
      % Section 3.1.1
      AM = sum(doublearea(V,F))/2;
      sF = size(F,1);
      c = 5;
      t = c * AM / sF;
      %t = 20*mean(doublearea(V,F));
    case 4
      AM = sum(volume(V,F));
      sF = size(F,1);
      c = 1e3;
      t = c * AM / sF;
      %t = 20*mean(volume(V,F));
    end
  end

  % option parameter default values
  bc_type = 'average';
  pre = [];
  pre.L = [];
  % precomputation for Dirichlet solve
  pre.D = [];
  % precomputation for Neumann solve
  pre.N = [];
  % precomputation for Poisson solve
  pre.poisson = [];
  u = [];

  ii = 5;
  while(ii <= nargin)
    switch varargin{ii}
    case 'BoundaryConditions'
      ii = ii + 1;
      assert(ii<=nargin);
      bc_type = varargin{ii};
    case 'Precomputation'
      ii = ii + 1;
      assert(ii<=nargin);
      % skip empty input
      if ~isempty(varargin{ii})
        pre = varargin{ii};
      end
    case 'Legacy'
      ii = ii + 1;
      assert(ii<=nargin);
      legacy = varargin{ii};
    case 'u'
      ii = ii + 1;
      assert(ii<=nargin);
      u = varargin{ii};
    case 'IntrinsicDelaunay'
      ii = ii + 1;
      assert(ii<=nargin);
      use_intrinsic = varargin{ii};
    otherwise
      error(['Unsupported parameter: ' varargin{ii}]);
    end
    ii = ii + 1;
  end

  % Algorithm 1 in Section 3:
  %   Integrate the heat flow ?? = ??u for time t
  %   Evaluate the vector field X = -???u/|???u|
  %   Solve the Poisson equation ???? = ??????X

  % Integrate the heat flow: "We discretize the heat equation from step I of
  % Algorithm 1 using a single backward Euler step"

  % "... with initial conditions u0 = ??(x)"
  % "Note that a Dirac delta appears as a literal one in this system since we
  % are effectively working with integrated quantities"
  u0 = zeros(n,1);
  u0(gamma) = 1;

  % "where ???????? is one third the area of all triangles incident on vertex ...
  % where ???? ??? R|????|??|????| is a diagonal matrix containing the vertex areas"
  if isempty(pre.L)
    if use_intrinsic
    pre.L = intrinsic_delaunay_cotmatrix(V,F);
    else
    pre.L = cotmatrix(V,F);
    end
    pre.M = massmatrix(V,F,'barycentric');
    pre.Q = pre.M - t*pre.L;
    pre.G = grad(V,F);
    pre.Div = div(V,F);
    pre.Tik = 1e-8*speye(size(V,1));
    pre.P = -pre.L*0.5 + pre.Tik;
    % get outline ("boundary") of mesh
    switch ss
    case 3
      pre.out = unique(reshape(outline(F),[],1));
    case 4
      pre.out = unique(boundary_faces(F));
    end
  end

  % 11/7/2017: Keenan writes that the mass matrix should not be here.
  % B = M*u0;
  B = u0;

  if isempty(u)
    if strcmp(bc_type,'dirichlet') || strcmp(bc_type,'average') || strcmp(bc_type,'robin')

      if legacy
        % remove any gamma from outline
        pre.out = setdiff(pre.out(:),gamma(:));
        % find all boundary vertices and append to gamma
        b = [gamma(:); pre.out];
        % "zero Dirichlet conditions"
        bc = [ones(numel(gamma),1); zeros(numel(b)-numel(gamma),1)];
        [uD,pre.D] = min_quad_with_fixed(pre.Q,B,b,bc,[],[],pre.D);
      else
        b = pre.out;
        % Q: How should we deal with gamma ??? out ?
        % This gives *reasonable* results, but doesn't look right for fixing
        % entire boundary. Probably more because of the lack of correct boundary
        % conditions for the final poisson solve.
        bc = -1*ismember(pre.out,gamma);
        %assert(~any(ismember(gamma,out)));
        [uD,pre.D] = min_quad_with_fixed(pre.Q,B,b,bc,[],[],pre.D);
      end
    end
    if strcmp(bc_type,'neumann') || strcmp(bc_type,'average') || strcmp(bc_type,'robin')
      % See Figure 9, pretty sure these are implicit ???x/???n = 0 neumann
      % conditions, though it is not stated in the text. At best, "Neumann
      % conditions prevent heat from flowing out of the domain..."
      if legacy
        b = gamma(:);
        bc = ones(numel(gamma),1);
        [uN,pre.N] = min_quad_with_fixed(pre.Q,B,b,bc,[],[],pre.N);
      else
        [uN,pre.N] = min_quad_with_fixed(pre.Q,B,[],[],[],[],pre.N);
      end
    end

    if strcmp(bc_type,'natural')
      K = keenan(V,F);
      Q = M - t*K;
      B = M*u0;
      G = grad(V,F);
      switch size(F,2)
      case 4
        vol = volume(V,F);
      case 3
        vol = doublearea(V,F);
      end
      vol = vol/sum(vol);
      % kill off affine functions
      A = [
        sum(M)/sum(M(:)); ...
        kron(speye(size(V,2)),vol)'*G];
      uT = min_quad_with_fixed(Q,B,[],[],A,[0;zeros(size(V,2),1)]);
    end

    switch bc_type
    case 'dirichlet'
      u = uD;
    case 'neumann'
      u = uN;
    case {'average','robin'}
      % "We advocate the use of the Robin boundary conditions obtained by taking
      % the mean of the Neumann solution uN and the Dirichlet solution uD, i.e.,
      % u = 0.5*(uN + uD)"
      u = 0.5*(uN+uD);
    case 'natural'
      u = uT;
    otherwise
      error(['Unsupported BoundaryCondtions value: ' bc_type]);
    end
  end

  robust_X = false;
  if robust_X
    X = robust_heat_gradients(V,F,uN,uD,t);
    warning('robust');
  else
    % Evaluate the vector field X
    grad_u = reshape(pre.G*u,size(F,1),size(V,2));
    %grad_u_norm = sqrt(sum(grad_u.^2,2));
    %% normalize grad_u
    %normalized_grad_u = bsxfun(@rdivide,grad_u,grad_u_norm);
    %% correct any zero-norm gradients
    %normalized_grad_u(grad_u_norm == 0,:) = 0;
    % normalize reverse direction; normalizerow will use robust norm that
    % avoids underflow.
    X = -normalizerow(grad_u);
    X(isnan(X)) = 0;
  end

  % Solve the Poisson equation
  % divergence of X
  div_X = pre.Div*X(:);
  if legacy
    [phi,pre.poisson] = min_quad_with_fixed( ...
      -pre.L*0.5,2*div_X,gamma,zeros(numel(gamma),1),[],[],pre.poisson);
  else
    [phi,pre.poisson] = ...
      min_quad_with_fixed(pre.P,2*div_X,[],[],[],[],pre.poisson);
    %Aeq = sparse(ones(1,size(V,1)));
    %Beq = 0;
    %[phi,pre.poisson] = ...
    %  min_quad_with_fixed(-pre.L*0.5,2*div_X,[],[],Aeq,Beq,pre.poisson);
  end
  D = phi;
  % "Note that ???? is unique only up to an additive constant and should be
  % shifted such that the smallest distance value is zero."
  %
  % D should be zero at gamma
  D = D - mean(D(gamma));
  % Flip sign so farthest point is positive distance
  [~,mi] = max(abs(D));
  D = D*sign(D(mi));

end


%% FUNCTION Logistic_CMTL
%   Convex-relaxed Clustered Multi-Task Learning with Logistic Loss.
%
%% OBJECTIVE
%   argmin_{W,M,C} { sum_i^t (- sum(log (1./ (1+ exp(-X{i}*W(:, i) - Y{i} .* C(i)))))/length(Y{i}))
%            + rho1 * eta (1+eta) trace(W (eta I + M)^-1 W')
%     subject to: trace (M) = k, M \preceq I, M \in S_+^t, eta = rho2/rho1
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   k: cluster number
%   rho1: clustering penalty controlling parameter (rho1=0 then tasks are not
%   related)
%   rho2: L2 norm regularization on model W (rho2=0 then reduce to ASO but
%   this solver does not support this case)
%
%% OUTPUT
%   W: model: d * t
%   C: model: 1 * t
%   funcVal: function value vector.
%   M: relaxed F*F', where F is the clustering assignment matrix
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Jianhui Chen and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] J. Zhou, J. Chen and J. Ye, Clustered Multi-Task Learning via
%       Alternating Structure Optimization, NIPS 2011.
%
%% RELATED FUNCTIONS
%   Least_CMTL, init_opts

%% Code starts here
function [W, C, funcVal, M] = Logistic_CMTL(X, Y, rho1, rho2, k, opts)

if nargin <5
    error('\n Inputs: X, Y, rho1, rho2 and k should be specified!\n');
end
X = multi_transpose(X);

if nargin <6
    opts = [];
end

if rho2<=0 || rho1<=0
    error('rho1 and rho2 should both greater than zero.');
end

% if exist('mosekopt','file')==0
%     error('Mosek is not found. Please install Mosek first. \n')
% end

% initialize options.
opts=init_opts(opts);

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

eta = rho2 / rho1;
c = rho1 * eta * (1 + eta);

%initialize a starting point
C0_prep = zeros(1, task_num);
for t_idx = 1: task_num
    m1 = nnz(Y{t_idx} == 1);
    m2 = nnz(Y{t_idx} == -1);
    if ( m1==0 || m2==0 )
        C0_prep(t_idx) = 0;
    else
        C0_prep(t_idx) = log(m1/m2);
    end
end

if opts.init==2
    W0 = zeros(dimension, task_num);
    C0 = zeros(1, task_num);
elseif opts.init== 0
    W0 = randn(dimension, task_num);
    C0 = C0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0 = zeros(dimension, task_num);
    end
    if isfield(opts,'C0')
        C0=opts.C0;
    else
        C0=C0_prep;
    end
end

M0 = speye (task_num) * k / task_num;

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Cz= C0;
Mz = M0;

Wz_old = W0;
Cz_old = C0;
Mz_old = M0;

t = 1;
t_old = 0;
iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;
    Cs = (1 + alpha) * Cz - alpha * Cz_old;
    Ms = (1 + alpha) * Mz - alpha * Mz_old;

    % compute function value and gradients of the search point
    [gWs, gCs, gMs, Fs ]  = gradVal_eval(Ws, Cs, Ms);

    % the Armijo Goldstein line search scheme
    while true
        %[Wzp l1c_wzp] = l1_projection(Ws - gWs/gamma, 2 * rho2 / gamma);
        %Fzp = funVal_eval  (Wzp, Czp, rho1);

        Wzp = Ws - gWs/gamma;
        Czp = Cs - gCs/gamma;
        [Mzp Mzp_Pz Mzp_DiagSigz ] = singular_projection (Ms - gMs/gamma, k);
        Fzp = funVal_eval (Wzp, Czp, Mzp_Pz, Mzp_DiagSigz);

        delta_Wzp = Wzp - Ws;
        delta_Czp = Czp - Cs;
        delta_Mzp = Mzp - Ms;

        nrm_delta_Wzp = norm(delta_Wzp, 'fro')^2;
        nrm_delta_Czp = norm(delta_Czp, 'fro')^2;
        nrm_delta_Mzp = norm(delta_Mzp, 'fro')^2;

        r_sum = (nrm_delta_Wzp+nrm_delta_Czp+nrm_delta_Mzp)/3;

        Fzp_gamma = Fs + sum(sum(delta_Wzp .* gWs)) ...
            + sum(sum(delta_Czp .* gCs))...
            + sum(sum(delta_Mzp .* gMs)) ...
            + gamma/2 * nrm_delta_Wzp ...
            + gamma/2 * nrm_delta_Mzp ...
            + gamma/2 * nrm_delta_Czp;

        if (r_sum <=eps)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Cz_old = Cz;
    Mz_old = Mz;

    Wz = Wzp;
    Cz = Czp;
    Mz = Mzp;

    funcVal = cat(1, funcVal, Fzp);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;
C = Czp;
M = Mzp;

% private functions

    function [Mzp Mzp_Pz Mzp_DiagSigz ] = singular_projection (Msp, k)
        [EVector EValue] = eig(Msp);
        Pz = real(EVector);  diag_EValue = real(diag(EValue));
        %DiagSigz = SingVal_Projection(diag_EValue, k);
        DiagSigz = bsa_ihb(diag_EValue, ones(size(diag_EValue)), k, ones(size(diag_EValue)));
        Mzp = Pz * diag(DiagSigz) *Pz';
        Mzp_Pz = Pz;
        Mzp_DiagSigz = DiagSigz;
    end

    function [grad_W, grad_C, grad_M, funcVal] = gradVal_eval(W, C, M)
        IM = (eta * speye(task_num) + M);
        invEtaMWt = IM\W';

        grad_W = zeros(dimension, task_num);
        grad_C = zeros(1, task_num);
        lossValVect = zeros (1 , task_num);
        if opts.pFlag
            parfor i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
            end
        else
            for i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
            end
        end
        grad_W = grad_W + 2 * c * invEtaMWt';   %W component
        grad_M = - c * (W' * W / IM /IM );      %M component

        funcVal = sum(lossValVect) + c * trace( W * invEtaMWt);
    end

    function [funcVal] = funVal_eval (W, C, M_Pz, M_DiagSigz)
        invIM = M_Pz * (diag( 1./(eta + M_DiagSigz))) * M_Pz';
        invEtaMWt = invIM * W';

        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
            end
        else
            for i = 1: task_num
                funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
            end
        end
        funcVal = funcVal + c * trace( W * invEtaMWt);
    end

end

function [ grad_w, grad_c, funcVal ] = unit_grad_eval( w, c, x, y)
%gradient and logistic evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
weighty = weight.* y;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
pp = 1./ (1+exp(aa));
b = -weighty.*(1-pp);
grad_c = sum(b);
grad_w = x * b;
end
function [ funcVal ] = unit_funcVal_eval( w, c, x, y)
%function value evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
end


function [P,F] = spm_MH(L,B,y,M)
% The Rejection-Metropolis-Hastings Algorithm
% FORMAT [P,F] = spm_MH(L,P,y)
%
% L   - likelihood function: inline(P,y,M)
% B   - free parameter [structure]
% Y   - response  [stucture]
% M   - model [structure]
%
% P   - Sample from posterior p(P|y,M)
% F   - marginal likelihood p(y|M) using harmonic mean
%--------------------------------------------------------------------------
%
% Returns a harmonic mean estimate of the log-marginal likelihood or
% log-evidence and a sample from the posterior density of the free parameters
% of a model.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MH.m 1143 2008-02-07 19:33:33Z spm $

% initialise parameters
%--------------------------------------------------------------------------
P(:,1) = spm_vec(B);

% MCMC - RMH
%--------------------------------------------------------------------------
n     = 2^8;                                          % number of burn in
N     = 2^16;                                         % number of samples
for i = 1:N

    % sample from proposal
    %----------------------------------------------------------------------
    pi = P(:,i);
    pp = pi + randn(size(P,1),1)/32;

    % compute importnace ratio
    %----------------------------------------------------------------------
    Lp = feval(L,spm_unvec(pp,B),y,M);
    Li = feval(L,spm_unvec(pi,B),y,M);
    r  = Lp/Li;

    % accept and marginal likelihood
    %----------------------------------------------------------------------
    if rand < r
        P(:,i + 1) = pp;
        F(i + 1)   = Lp;
    else
        P(:,i + 1) = pi;
        F(i + 1)   = Li;
    end

end

% burn in
%--------------------------------------------------------------------------
P = P(:,n:end);
F = F(n:end);
F = -log(mean(mean(F)./F)) + log(mean(F));


function [ value, t ] = r4_random ( t, n )

%*****************************************************************************80
%
%% R4_RANDOM is a portable pseudorandom number generator.
%
%  Discussion:
%
%    This random number generator is portable amoung a wide variety of
%    computers.  It generates a random number between 0.0 and 1.0
%    according to the algorithm presented by Bays and Durham.
%
%    The motivation for using this scheme, which resembles the
%    Maclaren-Marsaglia method, is to greatly increase the period of the
%    random sequence.  If the period of the basic generator is P,
%    then the expected mean period of the sequence generated by this
%    generator is given by
%
%      new mean P = sqrt ( pi * factorial ( N ) / ( 8 * P ) ),
%
%    where factorial ( N ) must be much greater than P in this
%    asymptotic formula.  Generally, N should be 16 to maybe 32.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Carter Bays, Stephen Durham,
%    Improving a Poor Random Number Generator,
%    ACM Transactions on Mathematical Software,
%    Volume 2, Number 1, March 1976, pages 59-64.
%
%  Parameters:
%
%    Input, integer N.  The absolute value of N is the number
%    of random numbers in an auxiliary table.  Note though that abs(N)+1 is
%    the number of items in array T.  If N is positive and differs from its
%    value in the previous invocation, then the table is initialized for
%    the new value of N.  If N is negative, abs(N) is the number of items
%    in an auxiliary table, but the tables are now assumed already to
%    be initialized.  This option enables the user to save the table T at
%    the end of a long computer run and to restart with the same sequence.
%    Normally, this function would be called at most once with negative N.
%    Subsequent invocations would have N positive and of the correct magnitude.
%
%    Input/output, real T(abs(N)+1), an array of random numbers
%    from a previous invocation of this function.  Whenever N is positive
%    and differs from the old N, the table is initialized.  The first
%    abs(N) numbers are the table discussed in the reference, and the
%    last value is Y.  This array may be saved in order to restart a sequence.
%
%    Output, real VALUE, a random number between 0.0 and 1.0.
%
  persistent floatn
  persistent nold

  if ( isempty ( floatn ) )
    floatn = -1.0;
    nold = -1;
  end

  if ( n ~= nold )

    nold = abs ( n );
    floatn = nold;

    if ( n < 0 )

      dummy = r4_rand ( t(nold+1) );

    else

      for i = 1 : nold
        t(i) = r4_rand ( 0.0 );
      end
      t(nold+1) = r4_rand ( 0.0 );

    end

  end

  j = r4_aint ( t(nold+1) * floatn + 1.0 );
  t(nold+1) = t(j);
  r4_random = t(j);
  t(j) = r4_rand ( 0.0 );

  return
end


function [lat, lon, gam, k] = tranmerc_inv(lat0, lon0, x, y, ellipsoid)
%TRANMERC_INV  Inverse transverse Mercator projection
%
%   [LAT, LON] = TRANMERC_INV(LAT0, LON0, X, Y)
%   [LAT, LON, GAM, K] = TRANMERC_INV(LAT0, LON0, X, Y, ELLIPSOID)
%
%   performs the inverse transverse Mercator projection of points (X,Y) to
%   (LAT,LON) using (LAT0,LON0) as the center of projection.  These input
%   arguments can be scalars or arrays of equal size.  The ELLIPSOID vector
%   is of the form [a, e], where a is the equatorial radius in meters, e is
%   the eccentricity.  If ellipsoid is omitted, the WGS84 ellipsoid (more
%   precisely, the value returned by DEFAULTELLIPSOID) is used.  GEODPROJ
%   defines the projection and gives the restrictions on the allowed ranges
%   of the arguments.  The forward projection is given by TRANMERC_FWD.
%
%   GAM and K give metric properties of the projection at (LAT,LON); GAM is
%   the meridian convergence at the point and K is the scale.
%
%   LAT0, LON0, LAT, LON, GAM are in degrees.  The projected coordinates X,
%   Y are in meters (more precisely the units used for the equatorial
%   radius).  K is dimensionless.
%
%   This implementation of the projection is based on the series method
%   described in
%
%     C. F. F. Karney, Transverse Mercator with an accuracy of a few
%     nanometers, J. Geodesy 85(8), 475-485 (Aug. 2011);
%     Addenda: http://geographiclib.sf.net/tm-addenda.html
%
%   This extends the series given by Krueger (1912) to sixth order in the
%   flattening.  This is a substantially better series than that used by
%   the MATLAB mapping toolbox.  In particular the errors in the projection
%   are less than 5 nanometers withing 3900 km of the central meridian (and
%   less than 1 mm within 7600 km of the central meridian).  The mapping
%   can be continued accurately over the poles to the opposite meridian.
%
%   This routine depends on the MATLAB File Exchange package "Geodesics on
%   an ellipsoid of revolution":
%
%     http://www.mathworks.com/matlabcentral/fileexchange/39108
%
%   See also GEODPROJ, TRANMERC_FWD, GEODRECKON, DEFAULTELLIPSOID.

% Copyright (c) Charles Karney (2012) <charles@karney.com>.
%
% This file was distributed with GeographicLib 1.29.

  if nargin < 4, error('Too few input arguments'), end
  if nargin < 5, ellipsoid = defaultellipsoid; end
  try
    Z = lat0 + lon0 + x + y;
    Z = zeros(size(Z));
  catch err
    error('lat0, lon0, x, y have incompatible sizes')
  end
  if length(ellipsoid(:)) ~= 2
    error('ellipsoid must be a vector of size 2')
  end

  degree = pi/180;
  maxpow = 6;

  a = ellipsoid(1);
  f = ecc2flat(ellipsoid(2));
  e2 = f * (2 - f);
  e2m = 1 - e2;
  cc = sqrt(e2m) * exp(e2 * atanhee(1, e2));
  n = f / (2 -f);
  bet = betf(n);
  b1 = (1 - f) * (A1m1f(n) + 1);
  a1 = b1 * a;

  if isscalar(lat0) && lat0 == 0
    y0 = 0;
  else
    [sbet0, cbet0] = SinCosNorm((1-f) * sind(lat0), cosd(lat0));
    y0 = a1 * (atan2(sbet0, cbet0) + ...
               SinCosSeries(true, sbet0, cbet0, C1f(n)));
  end
  y = y + y0;

  xi = y / a1;
  eta = x / a1;
  xisign = 1 - 2 * (xi < 0 );
  etasign = 1 - 2 * (eta < 0 );
  xi = xi .* xisign;
  eta = eta .* etasign;
  backside = xi > pi/2;
  xi(backside) = pi - xi(backside);

  c0 = cos(2 * xi); ch0 = cosh(2 * eta);
  s0 = sin(2 * xi); sh0 = sinh(2 * eta);
  ar = 2 * c0 .* ch0; ai = -2 * s0 .* sh0;
  j = maxpow;
  xip0 = Z; yr0 = Z;
  if mod(j, 2)
    xip0 = xip0 + bet(j);
    yr0 = yr0 - 2 * maxpow * bet(j);
    j = j - 1;
  end
  xip1 = Z; etap0 = Z; etap1 = Z;
  yi0 = Z; yr1 = Z; yi1 = Z;
  for j = j : -2 : 1
    xip1  = ar .* xip0 - ai .* etap0 - xip1 - bet(j);
    etap1 = ai .* xip0 + ar .* etap0 - etap1;
    yr1 = ar .* yr0 - ai .* yi0 - yr1 - 2 * j * bet(j);
    yi1 = ai .* yr0 + ar .* yi0 - yi1;
    xip0  = ar .* xip1 - ai .* etap1 - xip0 - bet(j-1);
    etap0 = ai .* xip1 + ar .* etap1 - etap0;
    yr0 = ar .* yr1 - ai .* yi1 - yr0 - 2 * (j-1) * bet(j-1);
    yi0 = ai .* yr1 + ar .* yi1 - yi0;
  end
  ar = ar/2; ai = ai/2;
  yr1 = 1 - yr1 + ar .* yr0 - ai .* yi0;
  yi1 =   - yi1 + ai .* yr0 + ar .* yi0;
  ar = s0 .* ch0; ai = c0 .* sh0;
  xip  = xi  + ar .* xip0 - ai .* etap0;
  etap = eta + ai .* xip0 + ar .* etap0;
  gam = atan2(yi1, yr1);
  k = b1 ./ hypot(yr1, yi1);
  s = sinh(etap);
  c = max(0, cos(xip));
  r = hypot(s, c);
  lam = atan2(s, c);
  taup = sin(xip)./r;
  tau = tauf(taup, e2);
  phi = atan(tau);
  gam = gam + atan(tan(xip) .* tanh(etap));
  c = r ~= 0;
  k(c) = k(c) .* sqrt(e2m + e2 * cos(phi(c)).^2) .* ...
         hypot(1, tau(c)) .* r(c);
  c = ~c;
  if any(c)
    phi(c) = pi/2;
    lam(c) = 0;
    k(c) = k(c) * cc;
  end
  lat = phi / degree .* xisign;
  lon = lam / degree;
  lon(backside) = 180 - lon(backside);
  lon = lon .* etasign;
  lon = AngNormalize(lon + AngNormalize(lon0));
  gam = gam/degree;
  gam(backside) = 180 - gam(backside);
  gam = gam .* xisign .* etasign;
end

function bet = betf(n)
  bet = zeros(1,6);
  nx = n^2;
  bet(1) = n*(n*(n*(n*(n*(384796*n-382725)-6720)+932400)-1612800)+ ...
              1209600)/2419200;
  bet(2) = nx*(n*(n*((1695744-1118711*n)*n-1174656)+258048)+80640)/ ...
           3870720;
  nx = nx * n;
  bet(3) = nx*(n*(n*(22276*n-16929)-15984)+12852)/362880;
  nx = nx * n;
  bet(4) = nx*((-830251*n-158400)*n+197865)/7257600;
  nx = nx * n;
  bet(5) = (453717-435388*n)*nx/15966720;
  nx = nx * n;
  bet(6) = 20648693*nx/638668800;
end

function tau = tauf(taup, e2)
  overflow = 1/eps^2;
  tol = 0.1 * sqrt(eps);
  numit = 5;
  e2m = 1 - e2;
  tau = taup / e2m;
  stol = tol * max(1, abs(taup));
  g = ~(abs(taup) < overflow);
  tau(g) = taup(g);
  g = ~g;
  for i = 1 : numit
    if ~any(g), break, end
    tau1 = hypot(1, tau);
    sig = sinh(e2 * atanhee( tau ./ tau1, e2 ) );
    taupa = hypot(1, sig) .* tau - sig .* tau1;
    dtau = (taup - taupa) .* (1 + e2m .* tau.^2) ./ ...
           (e2m * tau1 .* hypot(1, taupa));
    tau(g) = tau(g) + dtau(g);
    g = g & abs(dtau) >= stol;
  end
end


function v=edge_weight_vector(As,A,varargin)
% EDGE_WEIGHT_VECTOR Returns input for the edge_weight option for a weighted
% matrix
%
% Given a structural and weighted matrix pair (As,A), this function returns
% input for the edge_weight option.  The structural matrix As can have
% arbitrary non-zero values, but the non-zero structure of A must be a
% subset of the non-zero structure of As.  In terms of graphs, think about
% it as: the weights come from A, but the edges come from As.
%
% Example:
%   n = 8; u = 1; v = 2;
%   E = [1:n 2:n 1; 2:n 1 1:n]';
%   w = [1 zeros(1,n-1) 1 zeros(1,n-1)]';
%   A = sparse(E(:,1), E(:,2), w, n, n); % create weighted sparse matrix
%   As = sparse(E(:,1), E(:,2), true, n, n); % create structural sparse matrix
%   [d pred] = shortest_paths(As,u,struct('edge_weight',edge_weight_vector(As,A)));
%   d(v)

% David Gleich
% Copyright, Stanford University, 2008

%% History
%  2008-09-24: Initial coding
%%
[trans check] = get_matlab_bgl_options(varargin{:});
if check
    check_matlab_bgl(A,struct());
    if any(size(As)~=size(A)), error('matlab_bgl:edge_weight_vector', ...
            'The structral and weight matrix must be the same size'); end
end
n = size(A,1);
if trans, [j i] = find(As');
else [i j] = find(As);
end
Ai = sparse(i,j,1:nnz(As),n,n);
inds = nonzeros(Ai.*(A~=0));
if length(inds) ~= nnz(A), error('matlab_bgl:edge_weight_vector', ...
    'The matrix A cannot have additional non-zeros not in As.'); end
v = zeros(nnz(As),1);
v(inds) = nonzeros(A);
if check && ~isequal(A,sparse(j,i,v,n,n)')
    error('matlab_bgl:edge_weight_vector','error in weight function');
end

%% Todo
% Check that is actually works with the is trans option, I'm not convinced
% the test coverage works.


function [yd3] = m32yd3(m3)
% Convert volume from cubic meters to cubic yards.
% Chad Greene 2012
yd3 = m3*1.3079506193;


function [pos2, tlight] = geocen (pos1, pe)

% this function moves the origin of coordinates from the
% barycenter of the solar system to the observer (or the
% geocenter).  i.e., this function accounts for parallax
% (annual + geocentric or just annual).

% input

%  pos1 = position vector of star or planet, with respect to
%         origin at solar system barycenter, components in au

%  pe   = position vector of observer (or the geocenter),
%         with respect to origin at solar system barycenter,
%         components in au

% output

%   pos2   = position vector of star or planet, with respect to
%            origin at observer (or the geocenter), components in au

%   tlight = light-time from star or planet to observer (or the
%            geocenter) in days

% ported from NOVAS 3.1

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

% speed of light in au/day

c = astcon('c(au/day)', 1.0);

for j = 1:1:3

    pos2(j) = pos1(j) - pe(j);

end

tlight = sqrt(pos2(1)^2 + pos2(2)^2 + pos2(3)^2) / c;


% OP_CURLV_P: assemble the matrix B = [b(i,j)], b(i,j) = (coeff p_i, curl v_j), exploiting the tensor product structure.
%
%   mat = op_curlv_p_tp (spv, spp, msh, [coeff]);
%   [rows, cols, values] = op_curlv_p (spv, spp, msh, [coeff]);

% INPUT:
%
%   spv:     object that defines the vector-valued space of trial functions (see sp_vector)
%   spp:     object that defines the scalar-valued space of the multiplier (see sp_scalar)
%   msh:     object that defines the domain partition and the quadrature rule (see msh_cartesian)
%   epsilon: function handle to compute some physical coefficient (optional)
%
% OUTPUT:
%
%   mat:    assembled stiffness matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2011, 2016, 2017 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_curlv_p_tp (spv, spp, msh, coeff)

  for idim = 1:msh.ndim
    size2 = size (spp.sp_univ(idim).connectivity);
    for icomp = 1:spv.ncomp_param
      size1 = size (spv.scalar_spaces{icomp}.sp_univ(idim).connectivity);
      if (size1(2) ~= size2(2) || size1(2) ~= msh.nel_dir(idim))
        error ('One of the discrete spaces is not associated to the mesh')
      end
    end
  end

  A = spalloc (spp.ndof, spv.ndof, 3*spv.ndof);

  ndim = numel (msh.qn);

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    spv_col = sp_evaluate_col (spv, msh_col, 'value', false, 'curl', true);
    spp_col = sp_evaluate_col (spp, msh_col);

    if (nargin == 4)
      for idim = 1:msh.rdim
        x{idim} = reshape (msh_col.geo_map(idim,:,:), msh_col.nqn, msh_col.nel);
      end
      coeffs = coeff (x{:});
    else
      coeffs = ones (msh_col.nqn, msh_col.nel);
    end

    A = A + op_curlv_p (spv_col, spp_col, msh_col, coeffs);
  end

  if (nargout == 1)
    varargout{1} = A;
  elseif (nargout == 3)
    [rows, cols, vals] = find (A);
    varargout{1} = rows;
    varargout{2} = cols;
    varargout{3} = vals;
  end

end


function moon_lander()
close all;

t0 = 0;
tfinal = 800;
initial_state = [0 160000 0 0 -100 0 15000]';

[t, x] = runge_kutta(@moonlander, initial_state, [t0 tfinal], 0.05);

figure, plot(x(1,:), x(2,:), '-b*')
title('Landing trajectory')

figure,
plot(t, x(3,:), 'k')
title('Attitude')

figure,
plot(t, x(5,:), 'r')
title('Landing speed')

figure,
plot(t, x(7,:), 'g')
title('Mass')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a second order differential equation.
% Called from function exerciseB()
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = moonlander(t, x)
%target speed and x position
vtarget = -100;
xtarget = 0;

%constants
mass_min = 8000;
g = 1.6;
J = 100000;
ce=3.0*1000;
thlmax = 1000*0.5;
thtmax = 1000*44;
% compute control actions!
Kt = 300;
Kl = -0.000001;
vy = x(5);
posx = x(1);
posy = x(2);

if posy<3000
ut = Kt*(vtarget-vy);
ul = Kl*(xtarget-posx);
ul = sat(ul, -1, 1);
ut = sat(ut, 0, 1);
else
    ut=0;
    ul=0;
end

if x(7)<=mass_min
    Fl = 0;
    Ft = 0;
else
    Fl=thlmax*ul;
    Ft=thtmax*ut;
end

xd1 = x(4);
xd2 = x(5);
xd3 = x(6);
xd4 = (1/x(7))*(Fl*cos(x(3)) - Ft*sin(x(3)));
xd5 = (1/x(7))*(Fl*sin(x(3)) + Ft*cos(x(3))) - g;
xd6 = 4*Fl/J;

if x(7)<=mass_min
    % not changing mass
    display('ran out of fuel')
    % the derivative of mass is zero
    xd7 = 0 ;
else
    xd7 = -(abs(Fl/ce)+abs(Ft/ce));
end
xd = [xd1 xd2 xd3 xd4 xd5 xd6 xd7]';
return

function u= sat(u, min_val, max_val)
if u> max_val
    u = max_val;
end
if u < min_val
    u = min_val;
end


function k = simXsimKernDiagCompute(simKern1, simKern2, t)

% SIMXSIMKERNDIAGCOMPUTE Diagonal of a cross kernel between two SIM kernels.
% FORMAT
% DESC computes diagonal of cross kernel terms between two SIM kernels for
% the multiple output kernel.
% ARG simKern1 : the kernel structure associated with the first SIM
% kernel.
% ARG simKern2 : the kernel structure associated with the second SIM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN k : block of values from the diagonal kernel matrix.
% RETURN sK : normalised diagonal(i.e. unscaled version of K not multiplied
% by sqrt(simKern1.variance), sqrt(simKern2.variance) and
% sqrt(2/simKern.inverseWidth) in the case of the un-normalised kernel).
%
% SEEALSO : multiKernParamInit, multiKernCompute, simKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if size(t, 2) > 1
  error('Input can only have one column');
end

if simKern1.inverseWidth ~= simKern2.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end

if ~isfield(simKern1, 'isNormalised')
    isSim1Normalised = false;
else
    isSim1Normalised = simKern1.isNormalised;
end

if ~isfield(simKern2, 'isNormalised')
    isSim2Normalised = false;
else
    isSim2Normalised = simKern2.isNormalised;
end

% The factor of 2 on the top is because all our derivations are in terms
% of erfs which are defined in terms of exp(-x^2) rather than exp(-0.5x^2).
sigma = sqrt(2/simKern1.inverseWidth);

if ~simKern1.isStationary
    h1 = simXsimComputeDiagH(t, simKern1.decay, simKern2.decay, ...
                     simKern1.delay, simKern2.delay, sigma);
    h2 = simXsimComputeDiagH(t, simKern2.decay, simKern1.decay, ...
                     simKern2.delay, simKern1.delay, sigma);
else
    h1 = simXsimComputeDiagHStat(t, simKern1.decay, simKern2.decay, ...
                         simKern1.delay, simKern2.delay, sigma);
    h2 = simXsimComputeDiagHStat(t, simKern2.decay, simKern1.decay, ...
                         simKern2.delay, simKern1.delay, sigma);
end

sK = 0.5 * (h1 + h2);
if (isSim1Normalised == false) || (isSim2Normalised == false)
  sK = sqrt(pi) * sigma * sK;
end

if isfield(simKern1, 'isVarS') && (simKern1.isVarS)
    k = sK;
else
    if isfield(simKern1, 'isNegativeS') && (simKern1.isNegativeS == true)
        k = simKern1.sensitivity * sK;
    else
        k = sqrt(simKern1.variance) *sK;
    end
    if isfield(simKern2, 'isNegativeS') && (simKern2.isNegativeS == true)
        k = simKern2.sensitivity * k;
    else
        k = sqrt(simKern2.variance) * k;
    end
end


function comp_unrank_grlex_test ( )

%*****************************************************************************80
%
%% COMP_UNRANK_GRLEX_TEST tests COMP_UNRANK_GRLEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COMP_UNRANK_GRLEX_TEST\n' );
  fprintf ( 1, '  A COMP is a composition of an integer N into K parts.\n' );
  fprintf ( 1, '  Each part is nonnegative.  The order matters.\n' );
  fprintf ( 1, '  COMP_UNRANK_GRLEX determines the parts\n' );
  fprintf ( 1, '  of a COMP from its rank.\n' );

  kc = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rank: ->  NC       COMP\n' );
  fprintf ( 1, '  ----:     --   ------------\n' );

  for rank1 = 1 : 71

    xc = comp_unrank_grlex ( kc, rank1 );
    nc = sum ( xc(1:kc) );

    fprintf ( 1, '   %3d: ', rank1 );
    fprintf ( 1, '    %2d = ', nc );
    for j = 1 : kc - 1
      fprintf ( 1, '%2d + ', xc(j) );
    end
    fprintf ( 1, '%2d\n', xc(kc) );
%
%  When XC(1) == NC, we have completed the compositions associated with
%  a particular integer, and are about to advance to the next integer.
%
    if ( xc(1) == nc )
      fprintf ( 1, '  ----:     --   ------------\n' );
    end

  end

  return
end


function [yUpdate,PInvUpdate,innov]=cubInfoUpdate(yPred,PInvPred,z,RInv,h,xi,w,innovTrans,measAvgFun,stateDiffTrans)
%%CUBINFOUPDATE Perform the measurement update step in the cubature
%               information filter.
%
%INPUTS: yPred The xDimX1 predicted information state. The information
%              state is the inverse covariance matrix times the target
%              state.
%     PInvPred The xDimXxDim inverse of the predicted state covariance
%              matrix.
%            z The zDim X 1 vector measurement.
%         RInv The zDim X zDim inverse of the measurement covariance matrix
%              in the native coordinate system of the measurement.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%           xi An xDim X numCubPoints matrix of cubature points. If this
%              and the next parameter are omitted or empty matrices are
%              passed, then fifthOrderCubPoints(xDim) is used. It is
%              suggested that xi and w be provided to avoid needless
%              recomputation of the cubature points.
%            w A numCubPoints X 1 vector of the weights associated with the
%              cubature points.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   measAvgFun An optional function handle that, when given N measurement
%              values with weights, produces the weighted average. This
%              function only has to be provided if the domain of the
%              measurement is not linear. For example, when averaging
%              angular values, then the function meanAng should be used.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states and transforms them however
%              might be necessary. For example, a state containing angular
%              components will generally need to be transformed so that the
%              difference between the angles is wrapped to -pi/pi.
%
%OUTPUTS: yUpdate The xDim X 1 updated (posterior) information state
%                 vector.
%      PInvUpdate The updated xDim X xDim inverse state covariance matrix.
%           innov The zDimX1 innovation. This is the difference between the
%                 measurement and the predicted measurement. This is
%                 sometimes used for gating measurements.
%
%If the function h needs additional parameters beyond the state, then the
%parameters can be passed by using an anonymous function as the function
%handle. For example, suppose that the measurement function is measFunc and
%it needs the additional parameters param1 and param2. In this instance,
%rather than using
%h=@measFunc
%one should use
%h=@(x)measFunc(x,param1,param2)
%This way, every time cubKalUpdate calls measFunc (via h) with a
%different x, those two parameters are always passed.
%
%This function is an implementation of the measurement update step of
%Algorithm 1 in [1].
%
%The optional parameters innovTrans and measAvgFun are not described in
%[1], but allow for possible modifications to the filter as
%described in [2]. The parameters have been added to allow the filter to be
%used with angular quantities. For example, if the measurement consisted of
%range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%measAvgFun=@(z,w)[calcMixtureMoments(z(1,:),w);
%                  meanAng(z(2,:),w')];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] K. P. B. Chandra, D.-W. Gu, and I. Postlethwaite, "Square root
%    cubature information filter," IEEE Sensors Journal, vol. 13, no. 2,
%    pp. 750-758, Feb. 2013.
%[2] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%October 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(yPred,1);

if(nargin<6||isempty(xi))
    [xi,w]=fifthOrderCubPoints(xDim);
end

if(nargin<8||isempty(innovTrans))
    %The function just returns the input.
    innovTrans=@(a,b)bsxfun(@minus,a,b);
end

if(nargin<9||isempty(measAvgFun))
   measAvgFun=@(zPoints,w)calcMixtureMoments(zPoints,w);
end

if(nargin<10||isempty(stateDiffTrans))
   stateDiffTrans=@(x)x;
end

zDim=size(z,1);

numCubPoints=size(xi,2);

%Extract the state
xPred=PInvPred\yPred;
%Get a square root of the inverse covariance matrix.
SPred=cholSemiDef(pinv(PInvPred),'lower');
%Predicted cubature state points
xPredPoints=bsxfun(@plus,SPred*xi,xPred);

%Predicted cubature measurement points
zPredPoints=zeros(zDim,numCubPoints);
for curP=1:numCubPoints
    zPredPoints(:,curP)=h(xPredPoints(:,curP));
end

%Measurement prediction.
zPred=measAvgFun(zPredPoints,w);

%The innovation, transformed as necessary to keep values in a desired
%range.
innov=innovTrans(z,zPred);

Pxz=zeros(xDim,zDim);
for curP=1:numCubPoints
    diff=innovTrans(zPredPoints(:,curP),zPred);
    Pxz=Pxz+w(curP)*stateDiffTrans(xPredPoints(:,curP)-xPred)*diff';
end

I=PInvPred*Pxz*RInv*Pxz'*PInvPred';
i=PInvPred*Pxz*RInv*(innov+Pxz'*PInvPred'*xPred);

yUpdate=yPred+i;
PInvUpdate=PInvPred+I;
%Ensure symmetry
PInvUpdate=(PInvUpdate+PInvUpdate')/2;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


classdef chebfun2 < separableApprox
%CHEBFUN2   CHEBFUN2 class for representing functions on [a,b]x[c,d].
%
%   Class for approximating functions defined on finite rectangles. The
%   functions should be smooth.
%
% CHEBFUN2(F) constructs a CHEBFUN2 object representing the function F on
% [-1,1]x[-1 1]. F can be a string, e.g., 'sin(x.*y)', a function handle, e.g.,
% @(x,y) x.*y + cos(x), or a matrix of numbers. For the first two, F should in
% most cases be "vectorized" in the sense that it may be evaluated at a matrix
% of points and returns a matrix output.
%
% CHEBFUN2(F, [A B C D]) specifies a rectangle [A B]x[C D] where the
% function is defined. A, B, C, D must all be finite.
%
% If F is a matrix, F = (f_ij), the numbers f_ij are used as function
% values at tensor Chebyshev points of the 2nd kind. CHEBFUN2(F, 'equi')
% assumes the function values f_ij come from an equispaced tensor grid.
% CHEBFUN2(F, 'equix') assumes the values come from a tensor grid that is
% equispaced in x and 2nd-kind Chebyshev in y. CHEBFUN2(F, 'equiy') assumes
% the values come from a tensor grid that is 2nd-kind Chebyshev in x and
% equispaced in y.
%
% CHEBFUN2(F, k) returns a rank k approximation to F.
%
% CHEBFUN2(F, [m n]) returns a representation of a bivariate polynomial
% with m coefficients in x and n coefficients in y. The polynomial is
% compressed in low rank form and the rank k is still determined adaptively
% (satisfying k<=min(m,n)+1).
%
% CHEBFUN2(F, k, [A B C D]) or CHEBFUN2(F, [m,n], [A B C D]) is nonadaptive in
% rank or degrees, as above, returning a chebfun2 on the domain [A B]x[C D].
%
% CHEBFUN2(F, 'coeffs') where F is a matrix uses the matrix as coefficients
% in a Chebyshev tensor expansion. CHEBFUN2(F, 'coeffsx') uses an expansion
% of coefficients in x and values in y. CHEBFUN2(F, 'coeffsy') uses an
% expansion of values in x and coefficients in y.
%
% CHEBFUN2(F, 'trig') constructs a CHEBFUN2 object representing a smooth
% and periodic function F on [-1,1]x[-1 1]. The resulting CHEBFUN2 is
% represented using a bivariate Fourier expansion. CHEBFUN2(F, 'trigx')
% constructs a CHEBFUN2 that is periodic in x only, represented as a
% bivariate Fourier-Chebyshev expansion. CHEBFUN2(F, 'trigy') constructs a
% CHEBFUN2 that is periodic in y only, represented as a bivariate
% Chebyshev-Fourier expansion.
%
% CHEBFUN2(F, 'periodic')  is the same as CHEBFUN2(F, 'trig').
% CHEBFUN2(F, 'periodicx') is the same as CHEBFUN2(F, 'trigx').
% CHEBFUN2(F, 'periodicy') is the same as CHEBFUN2(F, 'trigy').
%
% CHEBFUN2(F, pref) constructs a CHEBFUN2 object from F with the options
% determined by the CHEBFUNPREF object pref. CHEBFUN2(F, {prefx, prefy})
% uses the CHEBFUNPREF objects prefx in x and prefy in y, where one of
% prefx and prefy may be [].
%
% The Chebfun2 software system is based on:
%
% A. Townsend and L. N. Trefethen, An extension of Chebfun to two dimensions,
% SIAM J. Sci. Comput., 35 (2013), C495-C518.
%
% See also CHEBFUN, CHEBFUN2V.

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

% TODO: Improve documentation of input options.

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

        function f = chebfun2(varargin)
            % The main CHEBFUN2 constructor!

            % Return an empty CHEBFUN:
            if ( (nargin == 0) || isempty(varargin{1}) )
                return
            end

            % Call the constructor, all the work is done here:
            f = constructor(f, varargin{:});

        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )

        % Convert Chebyshev coefficients to values:
        varargout = coeffs2vals(U, varargin);

        % Convert values to Chebyshev coefficients:
        varargout = vals2coeffs(U, varargin);

        % Padua points to tensor grid:
        [C, V, X, Y] = paduaVals2coeffs( F, dom );

        % Tensor product of Chebyshev points:
        [xx, yy] = chebpts2(nx, ny, domain, kind);

        % Outer-product of two chebfuns:
        F = outerProduct(f, g);

        % Fast spectrally-accurate Poisson solver:
        u = poisson(f, varargin);

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Private Static methods implemented by CHEBFUN2 class.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = private, Static = true )

    end

end


function [ a, ipvt, rcond ] = csico ( a, lda, n )

%*****************************************************************************80
%
%% CSICO factors a complex symmetric matrix.
%
%  Discussion:
%
%    The factorization is done by symmetric pivoting.
%
%    The routine also estimates the condition of the matrix.
%
%    If RCOND is not needed, CSIFA is slightly faster.
%
%    To solve A*X = B, follow CSICO by CSISL.
%
%    To compute inverse(A)*C, follow CSICO by CSISL.
%
%    To compute inverse(A), follow CSICO by CSIDI.
%
%    To compute determinant(A), follow CSICO by CSIDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex A(LDA,N), the symmetric matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); a block diagonal matrix and the multipliers which
%    were used to obtain it.  The factorization can be written A = U*D*U'
%    where U is a product of permutation and unit upper triangular matrices,
%    U' is the transpose of U, and D is block diagonal with 1 by 1 and
%    2 by 2 blocks.  Only the diagonal and upper triangle are used.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal condition of
%    the matrix.  For the system A*X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    (EPSILON/RCOND).  If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%  Local Parameters:
%
%    Local, complex Z(N), a work vector whose contents are usually
%    unimportant.  If A is close to a singular matrix, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%

%
%  Find norm of A using only upper half.
%
  for j = 1 : n
    z(j) = scasum ( j, a(1:j,j), 1 );
    for i = 1 : j-1
      z(i) = real ( z(i) ) + cabs1 ( a(i,j) );
    end
  end

  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, real ( z(j) ) );
  end
%
%  Factor.
%
  [ a, ipvt, info ] = csifa ( a, lda, n );
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where U*D*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve U*D*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  k = n;

  while ( 0 < k )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    kp = abs ( ipvt(k) );
    kps = k + 1 - ks;

    if ( kp ~= kps )
      t      = z(kps);
      z(kps) = z(kp);
      z(kp)  = t;
    end

    if ( cabs1 ( z(k) ) ~= 0.0 )
      ek = csign1 ( ek, z(k) );
    end

    z(k) = z(k) + ek;
    z(1:k-ks) = z(1:k-ks) + z(k) * transpose ( a(1:k-ks,k) );

    if ( ks ~= 1 )

      if ( cabs1 ( z(k-1) ) ~= 0.0 )
        ek = csign1 ( ek, z(k-1) );
      end

      z(k-1) = z(k-1) + ek;
      z(1:k-ks) = z(1:k-ks) + z(k-1) * a(1:k-ks,k-1);

    end

    if ( ks ~= 2 )

      if ( cabs1 ( a(k,k) ) < cabs1 ( z(k) ) )
        s = cabs1 ( a(k,k) ) / cabs1 ( z(k) );
        z(1:n) = z(1:n) * s;
        ek = s * ek;
      end

      if ( cabs1 ( a(k,k) ) ~= 0.0 )
        z(k) = z(k) / a(k,k);
      else
        z(k) = 1.0;
      end

    else

      ak = a(k,k) / a(k-1,k);
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = z(k) / a(k-1,k);
      bkm1 = z(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve U' * Y = W.
%
  k = 1;

  while ( k <= n )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + z(1:k-1) * transpose ( a(1:k-1,k) );

      if ( ks == 2 )
        z(k+1) = z(k+1) + z(1:k-1) * transpose ( a(1:k-1,k+1) );
      end

      kp = abs ( ipvt(k) );

      if ( kp ~= k )
        t     = z(k);
        z(k)  = z(kp);
        z(kp) = t;
      end

    end

    k = k + ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve U*D*V = Y.
%
  k = n;

  while ( 0 < k )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= ks )

      kp = abs ( ipvt(k) );
      kps = k + 1 - ks;

      if ( kp ~= kps )
        t      = z(kps);
        z(kps) = z(kp);
        z(kp)  = t;
      end

      z(1:k-ks) = z(1:k-ks) + z(k) * a(1:k-ks,k);

      if ( ks == 2 )
        z(1:k-ks) = z(1:k-ks) + z(k-1) * transpose ( a(1:k-ks,k-1) );
      end

    end

    if ( ks ~= 2 )

      if ( cabs1 ( a(k,k) ) < cabs1 ( z(k) ) )
        s = cabs1 ( a(k,k) ) / cabs1 ( z(k) );
        z(1:n) = z(1:n) * s;
        ynorm = s * ynorm;
      end

      if ( cabs1 ( a(k,k) ) ~= 0.0 )
        z(k) = z(k) / a(k,k);
      else
        z(k) = 1.0;
      end

    else

      ak = a(k,k) / a(k-1,k);
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = z(k) / a(k-1,k);
      bkm1 = z(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;
%
%  Solve U' * Z = V.
%
  k = 1;

  while ( k <= n )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + z(1:k-1) * transpose ( a(1:k-1,k) );

      if ( ks == 2 )
        z(k+1) = z(k+1) + z(1:k-1) * transpose ( a(1:k-1,k+1) );
      end

      kp = abs ( ipvt(k) );

      if ( kp ~= k )
        t     = z(k);
        z(k)  = z(kp);
        z(kp) = t;
      end

    end

    k = k + ks;

  end
%
%  Make ZNORM = 1.
%
  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


classdef MaOPP_binary < PROBLEM
% <many> <binary> <large/none> <expensive/none>
% Many-objective pathfinding problem based on binary encoding
% xmax                    ---  10 --- xmax
% ymax                    ---  10 --- ymax
% obstacleValue           ---   0 --- obstacleValue
% nh                      ---   1 --- nh
% neighbourhood           ---   2 --- neighbourhood
% backtracking            ---   0 --- backtracking
% allowObstaclesOnPath    ---   1 --- allowObstaclesOnPath
% overheadVariablesFactor --- 1.5 --- overheadVariablesFactor

%------------------------------- Reference --------------------------------
% J. Weise and S. Mostaghim, A scalable many-objective pathfinding
% benchmark suite, IEEE Transactions on Evolutionary Computation, 2022,
% 26(1): 188-194.
%------------------------------- 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".
%--------------------------------------------------------------------------

% The most recent reference fronts and sets can be found at
% https://ci.ovgu.de/Publications/Scalable+Many_Objective+Pathfinding+Benchmark+Suite-p-910.html
% In case of any question, please contact me at
% jens.weise@ovgu.de

    properties(Access = protected)
        x_max;          % size [1,x_max] in x direction
        y_max;          % size [1,y_max] in y direction
        delay;          % expected delay
        nh;             % elevation function {1,2,3,M} -> 1 = 1, 2 = 2, 3 = 3, 4 = M
        neighborhood;   % 2^k, k {2,3}
        backtracking;   % True, False
        obstacle;       %{0,1,2} -> 0 = No, 1 = CH, 2 = LA
        vmax_high;
        vmax_medium;
        vmax_low;
        v_max;
        elevation;
        allowObstaclesOnPath;
        upperBoundsForObjectives;
        overheadVariablesFactor;
        numerOfBits;
    end

    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.x_max, obj.y_max, obstacleValue, obj.nh, obj.neighborhood, obj.backtracking, obj.allowObstaclesOnPath, obj.overheadVariablesFactor] = obj.ParameterSet(10,10,0,1,2,0,1,1);
            if isempty(obj.vmax_high); obj.vmax_high = 130; end
            if isempty(obj.vmax_medium); obj.vmax_medium = 100; end
            if isempty(obj.vmax_low); obj.vmax_low = 50; end
            obj.M = 5;
            obj.setBinaryEncoding();
            obj.v_max     = zeros(obj.x_max, obj.y_max);  % matrix of shape (x_max,y_max), matrix with different v values for each cell
            obj.elevation = zeros(obj.x_max, obj.y_max);  % matrix of shape (x_max,y_max)
            for x = 1 : obj.x_max
                for y = 1 : obj.y_max
                    obj.obstacle(x,y) = obstacleValue;
                    w_x_y = max(sin(x-1), cos(y-1));
                    if w_x_y > 0.9
                        obj.v_max(x, y) = obj.vmax_high;
                    elseif w_x_y < -0.4
                        obj.v_max(x, y) = obj.vmax_low;
                    else
                        obj.v_max(x, y) = obj.vmax_medium;
                    end
                    % velocity matrix obstacles for CH
                    if obj.obstacle(x,y) == 1 && (sign(sin(pi/2 + pi*x)) + sign(sin(pi/2 + pi*y)) - 2* (heaviside(x-obj.x_max+0.5)- heaviside(x-obj.x_max-0.5))* (heaviside(y-obj.y_max+0.5)- heaviside(y-obj.y_max-0.5)) == 2)
                        obj.v_max(x, y) = 0;
                    end
                    % velocity matrix obstacles for LA
                    if obj.obstacle(x,y) == 2 && (x-1-obj.x_max/2)^2 + (y-1-obj.y_max/2)^2 - (0.25*obj.x_max)^2 <0 %Q: using radius of x_max/4 from paper page 4 line 22 A: Yes.
                        obj.v_max(x, y) = 0;
                    end
                    xs = map(x,1,obj.x_max+1,-3,3);
                    ys = map(y,1,obj.y_max+1,-3,3);
                    if obj.nh == 1
                        obj.elevation(x,y) = 5*exp(-(xs-1.5)^2-(ys+1.5)^2);
                    elseif obj.nh == 2
                        obj.elevation(x,y) = 5*exp(-(xs+1.5)^2-(ys+1.5)^2) + 5*exp(-(xs-1.5)^2-(ys-1.5)^2);
                    elseif obj.nh == 3
                        obj.elevation(x,y) = 5*exp(-(xs+1.5)^2-(ys+1.5)^2) + 5*exp(-(xs-1.5)^2-(ys-1.5)^2) + 5*exp(-(xs-1.5)^2-(ys+1.5)^2);
                    elseif obj.nh == 4
                        obj.elevation(x,y) = 3*(1-xs)^2*exp(-(xs^2)-(ys+1)^2)-10*exp(-xs^2-ys^2)*(-xs^3+xs/5-ys^5)-1/3*exp(-(xs+1)^2-ys^2);
                    end
                end
            end
            manhattanSteps = (obj.x_max + obj.y_max - 2);
            obj.upperBoundsForObjectives = [manhattanSteps*1.5, manhattanSteps*1.5, 1.5*5*obj.nh, 1.5*manhattanSteps/50, 0.5*manhattanSteps*pi/2];
        end
        function setBinaryEncoding(obj)
            if obj.neighborhood == 2 && obj.backtracking == 0
                obj.D = 1*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 1;
            elseif obj.neighborhood == 2 && obj.backtracking == 1
                obj.D = 2*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 2;
            elseif obj.neighborhood == 3 && obj.backtracking == 0
                obj.D = 2*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 2;
            elseif obj.neighborhood == 3 && obj.backtracking == 1
                obj.D = 3*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 3;
            end
            obj.encoding = 4 + zeros(1,obj.D);
        end
        function [x_coords, y_coords,D] = decodePath(obj,PopDec,x_max,y_max)
            D = size(PopDec,2);
            n = obj.numerOfBits;
            for i = 0 : (D/n)-1
               temp = PopDec(:,[n*i+1:n*i+n]);
               realPopDec(:,i+1) = b2d(temp);
            end
            realPopDec = 1.0*realPopDec/(2^n);
            [x_coords, y_coords, D] = decodePathWithRealValues(obj,realPopDec,x_max,y_max);
            D = size(realPopDec,2);
        end
        function [x_coords, y_coords, D] = decodePathWithRealValues(obj,PopDec,x_max,y_max)
            [N,D] = size(PopDec);
            % convert the array into a sequence of x,y coordinates
            x_coords = zeros(N,D+1);
            y_coords = zeros(N,D+1);
            x_coords(:,1) = 1;
            y_coords(:,1) = 1;

            for i = 1 : N
                x_current = 1;
                y_current = 1;
                for d = 1 : D
                    possibleNeighours      = obj.getPossNeighbours(x_current,y_current,obj.backtracking,obj.neighborhood,obj.allowObstaclesOnPath);
                    noOfPossibleNeighbours = size(possibleNeighours,1);
                    neighbourToTake        = noOfPossibleNeighbours;
                    for a = 1 : noOfPossibleNeighbours
                        if PopDec(i,d) >= (a-1)/noOfPossibleNeighbours && PopDec(i,d) < (a)/noOfPossibleNeighbours
                            neighbourToTake = a;
                            break;
                        end
                    end
                    if neighbourToTake == 0 % Path ends if there are no neighbours available
                        break;
                    end
                    addvals   = possibleNeighours(neighbourToTake,:);
                    xAdd      = addvals(1);
                    yAdd      = addvals(2);
                    x_current = x_current + xAdd;
                    y_current = y_current + yAdd;
                    x_coords(i,d+1) = x_current;
                    y_coords(i,d+1) = y_current;
                    if x_current == obj.x_max && y_current == obj.y_max
                        break;
                    end
                end
            end
        end
        function neighbours = getPossNeighbours(obj,xCurr,yCurr,backtracking,neighborhood,allowObstaclesOnPath)
            e  = [1 0];
            se = [1 1];
            s  = [0 1];
            sw = [-1 1];
            w  = [-1 0];
            nw = [-1 -1];
            n  = [0 -1];
            ne = [1 -1];
            currentCoordinates = [xCurr yCurr];
            if backtracking == 0
                if neighborhood == 2
                    neighbours = [e;s];
                else
                    neighbours=[e;se;s];
                end
            else
                if neighborhood == 2
                    neighbours = [e;s;w;n];
                else
                    neighbours = [e;se;s;sw;w;nw;n;ne];
                end
            end
            % should we remove the outliers?
            % it is closer to the original implementation
            nextCoordinates = neighbours+currentCoordinates;
            out = nextCoordinates(:,1)<1 | nextCoordinates(:,1)>obj.x_max | nextCoordinates(:,2)<1 | nextCoordinates(:,2)>obj.y_max;
            neighbours(out,:) = [];
            if allowObstaclesOnPath == 0
                nextCoordinates = neighbours+currentCoordinates;
                obstancles = getVmaxValues(nextCoordinates(:,2),nextCoordinates(:,1),obj.v_max) == 0;
                neighbours(obstancles,:) = [];
            end
        end
        %% separate the calculations here to be able to compute the feasibility rate as well (with a lot of extra runtime, but whatever..)
        function [objectives,distToTarget,fObstacle] = objectiveValuesPath(obj,PopDec)
            % read out parameters
            N = size(PopDec,1);
            [x_coords, y_coords, D] = obj.decodePath(PopDec,obj.x_max,obj.y_max);
            % for entire population (one column) iterate over nodes (columns)
            f1 = zeros(N,1); % euclidean distance
            f2 = zeros(N,1); % expected delays
            f3 = zeros(N,1); % elevations
            f4 = zeros(N,1); % traveling time
            f5 = zeros(N,1); % smoothness
            fObstacle = zeros(N,1);
            distances = zeros(N,D); % stores distances that were calculated for f1 for later use in f5
            c = [];
            for d = 1 : D
                x1        = x_coords(:,d);
                x2        = x_coords(:,d+1);
                y1        = y_coords(:,d);
                y2        = y_coords(:,d+1);
                hasEnded  = (x2~=0&x1~=0);
                x2(x2==0) = 1;
                y2(y2==0) = 1;
                x1(x1==0) = 1;
                y1(y1==0) = 1;

                vMaxValues1 = getVmaxValues(x1,y1,obj.v_max);
                vMaxValues2 = getVmaxValues(x2,y2,obj.v_max);

                if obj.allowObstaclesOnPath==1
                    obstancles = vMaxValues1==0|vMaxValues2==0;
                    fObstacle  = fObstacle+obstancles;
                else
                    obstancles = zeros(N,1);
                    fObstacle  = fObstacle+obstancles;
                end

                %---Objective 1: euclidean distance
                distances(:,d) = sqrt((x2-x1).^2+(y2-y1).^2);
                res = (distances(:,d)).*hasEnded;
                res(res>sqrt(2)) = 0;
                f1 = f1 + res;

                %---Objective 2: delays
                inds      = vMaxValues1 ~= vMaxValues2;
                del       = inds*2;
                inds      = vMaxValues1 == vMaxValues2;
                loc       = find(inds==1);
                inds      = zeros(N,1);
                inds(loc) = vMaxValues1(loc);
                inds50    = inds == 50;
                del       = del + inds50*3;
                inds100   = inds == 100;
                del       = del + inds100*1;
                inds0     = del == 0;
                del       = del + inds0*0.2;
                res       = del.*hasEnded;
                f2        = f2 + res;

                %---Objective 3: elevations
                h1   = getVmaxValues(x1,y1,obj.elevation);
                h2   = getVmaxValues(x2,y2,obj.elevation);
                inds = h1 < h2;
                res  = (inds.*(h2-h1)).*hasEnded;
                f3   = f3 + res;

                %---Objective 4: traveling time
                res = (2*distances(:,d)./(vMaxValues1+vMaxValues2));
                res(isinf(res)|isnan(res)) = 0;
                if any(isnan(res))
                    errr;
                end
                res = res.*hasEnded;
                f4  = f4 + res;

                %---Objective 5: smoothness
                if d>1
                    x0       = x_coords(:,d-1);
                    y0       = y_coords(:,d-1);
                    x0(x0<1) = 1;
                    y0(y0<1) = 1;

                    test = transpose(dot([(x0-x1).'; (y0-y1).'],[(x1-x2).'; (y1-y2).'],1))./(distances(:,d-1).*distances(:,d));
                    test(isnan(test)) = 0;
                    try
                    	assert(all(test <= 1) && all(test >= -1))
                    catch
                    end
                    res = (acos(transpose(dot([(x0-x1).'; (y0-y1).'],[(x1-x2).'; (y1-y2).'],1))./(distances(:,d-1).*distances(:,d)))).*hasEnded;
                    res(isnan(res) )= 0;
                    f5 = f5 + res;
                end
            end
            objectives = [f1,f2,f3,f4,f5];
            try
                last = [];
                for i = 1 : N
                    last = [last find(x_coords(i,:)~=0,1,'last')];
                end
                last = last';
                lxc  = [];
                for k = 1 : N
                    lxc(k,1) = x_coords(k,last(k));
                    lxc(k,2) = y_coords(k,last(k));
                end
            catch
            end
            target       = [repmat(obj.x_max,N,1) repmat(obj.y_max,N,1)];
            distToTarget = abs(target(:,1)-lxc(:,1))+abs(target(:,2)-lxc(:,2));
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            [objectives,distToTarget,fObstacle] = objectiveValuesPath(obj,varargin{1});
            objectives(:,1)                     = objectives(:,1) + 2*distToTarget + fObstacle;
            Population = SOLUTION(varargin{1},objectives,distToTarget+fObstacle,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        % Here, the pre-computed fronts are read. If it is not available,
        % an educated-guess Nadir point is computed. Please check the
        % website above in the header to see, if there are new fronts
        % available.
        function R = GetOptimum(obj,N)
            obs = '';
            if obj.obstacle == 0
                obs = 'NO';
            elseif obj.obstacle == 1
                obs = 'CH';
            elseif obj.obstacle == 2
                obs = 'LA';
            end
            sizeX = obj.x_max;
            sizeY = obj.y_max;
            ele   = '';
            if obj.nh == 1
                ele = '1';
            elseif obj.nh == 2
                ele = '2';
            elseif obj.nh == 3
                ele = '3';
            elseif obj.nh == 4
                ele = 'M';
            end
            k = obj.neighborhood;
            if obj.backtracking
                bt = 'T';
            else
                bt = 'F';
            end
            problem = sprintf('ASLETISMAC_%s_X%i_Y%i_P%s_K%i_B%s',obs,sizeX,sizeY,ele,k,bt);
            try
                CallStack = dbstack('-completenames');
                load(fullfile(fileparts(CallStack(1).file),'PF.mat'),problem);
                order = [2 5 1 3 4];
                eval(['R=',problem,';']);
                R      = R(:,order);
                R(:,5) = deg2rad(R(:,5));
            catch
                R = repmat(obj.upperBoundsForObjectives,N,1);
            end
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end

function out= map(x,in_min,in_max,out_min,out_max)
    out=(x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
end

function vList = getVmaxValues(x1,y1,aMatrix)
    len = size(x1,1);
    vList = zeros(len,1);
    for i = 1:len
        vList(i) = aMatrix(x1(i),y1(i));
    end
end

% Converts binary to decimal (MSB 0)!
function dec = b2d(vector)
    l   = size(vector);
    dec = ones(l(1),1);
    for r = 1 : l(1)
        dTemp = 0;
        for i = 1 : l(2)
            dTemp = dTemp + vector(r,i)*2^(i-1);
        end
        dec(r,1) = dTemp;
    end
end


function c = tapas_ehgf_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the enhanced Hierarchical Gaussian Filter (eHGF)
% for binary inputs in the absence of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_ehgf_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_ehgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_ehgf_binary_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'ehgf_binary';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,   2];
c.omsa = [NaN,   4,   4];

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 3*c.n_levels+2*(c.n_levels-1)+1;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_ehgf_binary;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_ehgf_binary_transp;

end