function [t,y,etim] = pc_dhbvm(fun,k,s,t0,dtau,nu,n)
%
%  [t,y,etim] = pc_dhbvm(fun,k,s,t0,dtau,nu,n)
%
%  Solve the Functional Differential Equation with Piecewise Continuous 
%  Arguments (FDEPCA):
%
%           y'(t) = f(t,floor(t),y(t),y(floor(t))), t>0, y(t)=fi0(t), t<=0,
%
%  with step size  h = 1/nu, performing n integration steps of the HBVM
%  strategy defined in [RP].
%
%  INPUT:
%  ======
%  fun:   the identifier (or the function handling) of the function 
%         evaluating the right-hand side term of the FDECPA or the initial 
%         conditions (also in vector mode), such that:
%      
%         fun(t) -> compute fi0(t) (also in vector mode)
%         fun(t,ttau,y,ytau) -> compute the vector field at the right-hand
%                               side of the FDEPCA 
%                               (ttau=floor(t), ytau approximates 
%                               y(floor(t))
%   k:    the integer k value of the adopted HBVM(k,s) strategy;
%   s:    the integer s value of the adopted HBVM(k,s) strategy (the order 
%         of the method is 2s);
%   t0:   the integer starting time;
%   dtau: integer at whose multiples, starting from t0, the memory term 
%         changes.  
%   nu:   the integer needed to recover the step size h=dtau/nu;
%   n:    the number of integration steps.
%
%  OUTPUT:
%  =======
%  t:     the vector of considered time instants;
%  y:     the computed solution (matrix) along the considered time instants
%         y=[q(t) p(t)] with q(t) and p(t) the (q,p) solution components.
%  etim:  the elapsed computational time.
%
%  ====================
%  Reference Paper [RP]
%  - G. Gurioli, W. Wang, X. Yan (2024). Hamiltonian Boundary Value Methods 
%    (HBVMs) for functional differential equations with piecewise continuous 
%    arguments. Numerical Algorithms (DOI: 10.1007/s11075-024-01994-7).
%
tic;
y0     = feval(fun,0);
m      = length(y0);
y      = zeros(n+1,m);
y(1,:) = y0(:)';
gamma  = zeros(s,m,n);
h      = dtau/nu;
t      = t0+h*(0:n)';
[x,~,PO,~,Is] = notree(k,s);
x      = x*h;
Is     = h*Is;
for i = 1:nu
    [y(i+1,:),gamma(:,:,i)] = step( fun, t(i), y(i,:), x, h, Is, PO );
end
for i = nu+1:n
    if mod(i-1,nu)==0
        id=i;
    end
        [y(i+1,:),gamma(:,:,i)] = step( fun, t(i), y(i,:), x, h, Is, PO, y(id,:),gamma(:,:,id) );
end
etim = toc;
return
end
    
function [y1,gamma] = step( fun, t0, y0, hc, h, hIs, PO, y0tau, gammatau)
%
%   Integration step for the variational local problem.
%  
tol0   = 2*eps;
tol    = tol0*max(10,norm(y0));
itmax  = 1000;
k      = size(hIs,1);
t      = t0+hc;
ttau   = floor(t);
ey0    = ones(k,1)*y0;
y      = ey0;
err    = inf;
if nargin==7
    ytau = feval( fun, ttau);
else
    ytau = ones(k,1)*y0tau;
end  
f      = feval( fun, t, ttau, y, ytau ); 
scal   = diag(1./(1+abs(y0)));
for iter = 1:itmax
   gamma = PO*f;
   yold  = y;         
   y     = ey0 +hIs*gamma;   
   f     = feval( fun, t, ttau, y, ytau );
   err0  = err; 
   err   = norm((y-yold)*scal);
   if ( (err<=tol) && (err>=err0) )||( err<=tol0 ), break
   elseif isnan(err), break, end  
end
if iter>=itmax 
   warning('itmax iterations exceeded, err = %g at t = %g\n', err, t0 )
end
gamma = PO*f;
y1    = y0 +h*gamma(1,:); 
return
end

function [x,beta,PO,Xs,Is,Ps] = notree(k,s)
%
%  Gauss-Legendre weights, abscissae, and matrices for HBVM(k,s).
%
%  INPUT:
%  k,s  : parameters for HBVM(k,s).
%
%  OUTPUT:
%  x    : k Gauss-Legendre abscissae in [0,1];
%  beta : corresponding quadrature weights;
%  PO   : PO = P_s^T * Omega, with P_s(i,j) = P_{j-1}(x_i),
%         Omega = diag(beta);
%  Xs   : matrix X_s relating I_s and P_{s+1}: I_s == P_{s+1}[X_s; 0^T];
%  Is   : matrix I_s(i,j) = \int_0^{x_i} P_{j-1}(x)dx;
%  Ps   : matrix P_s(i,j) = P_{j-1}(x_i), i = 1...k, j = 1...s.
%
[x,beta,xx] = lgwt01(k); 
[x,ind]     = sort(x); 
beta        = beta(ind); 
xx          = xx(ind);  
Ps          = zeros(k,s+1);  
for i = 0:s
    y = legendre(i,xx); Ps(:,i+1) = y(1,:).'*sqrt(2*i+1);
end   
j  = 1:s-1; xis = 0.5/sqrt(4*s^2-1);
Xs = (0.5)./sqrt(4*j.^2-1);
Xs = diag( Xs, -1 ); 
Xs = Xs-Xs.'; Xs(1,1) = .5;  
Is = Ps*[Xs; zeros(1,s-1) xis];
Ps = Ps(:,1:s);
PO = ( diag(beta)*Ps ).';  
return
end

function [x,w,y] = lgwt01(N)
%
% Auxiliary function for notree.
%  
% This function computes the Legendre-Gauss nodes (x) and weights (w)
% on the interval [0,1] with truncation order N.
% Also the nodes (y) on [-1,1] are computed.
%
%
a  = 0; b = 1;  % normalization on the interval [a,b]==[0,1] %
N  = N-1;
N1 = N+1; N2 = N+2;
xu = linspace(-1,1,N1)';
% Initial guess %
y = cos((2*(0:N)'+1)*pi/(2*N+2))+(0.27/N1)*sin(pi*xu*N/N2);
L = zeros(N1,N2);   % Gauss-Legendre Vandermonde Matrix %
Lp = zeros(N1,N2);  % Derivative of the GL-VM %
% Compute the zeros of the N+1 Legendre Polynomial using %
% the recursion relation and the Newton-Raphson method   %
y0 = 2;
while max(abs(y-y0))>eps       % Iterate until new points are uniformly %
    L(:,1) = 1;  L(:,2) = y;   %  within epsilon of old points          %
    for k = 2:N1
        L(:,k+1) = ( (2*k-1)*y.*L(:,k)-(k-1)*L(:,k-1) )/k;
    end
    Lp = (N2)*( L(:,N1)-y.*L(:,N2) )./(1-y.^2);   
    y0 = y;  y = y0-L(:,N2)./Lp;
end
x = (a*(1-y)+b*(1+y))/2;      % tranform from [-1,1] to [a,b] %    
w = (b-a)./((1-y.^2).*Lp.^2)*(N2/N1)^2; % Compute the weights %
w = ( w + w(end:-1:1) )/2;              % and symmetrize them %
return
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%% Example on Problem 1 ([RP]) %%%%%%%%%%%%%%%%%%%%%%%%
%
% function y1 = Problem1(t,ttau,y,ytau)
% %
% % function to evaluate the right-hand side term of the FDECPA (Problem 1
% % in [RP]) or the initial conditions(also in vector mode):
% %
% % Problem1(t) -> compute the initial condition (also in vector mode)
% % Problem1(t,ttau,y,ytau) -> compute the vector field at the right-hand
% %                            side of the FDEPCA.
% %
% alfa = 1e-2; % given parameter alpha
% if nargin==1    
%     y1 = ones(length(t),1)*[sqrt(2) 0];    % initial conditions
% elseif nargin==4 
%     % vector field
%     y1 = [y(:,2).^3 -y(:,1).^3]+[ytau(:,2).^3 -ytau(:,1).^3]*alfa;  
% end    
% return
% end

% Example of calling sequence of the main code pc_dhbvm for Problem1:
%
% [t,y,etim] = pc_dhbvm('Problem1',4,2,0,1,50,1e4*50);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%