function [y,J,u,q,yopt]=intercept
% INTERCEPT
%
% Solve Intercept Problem by a direct method using fminunc and chebfuns
%
% This example illustrates the solution of Optimal Control Problems (OCP)  
% by a direct method using fminunc and chebfuns
% 
%   Inputs
%   q0     : optional initial start point of 7x1 free parameter vector 
%
%   Outputs  
%   y      : chebfun optimal position   
%   J      : cost function $$ J = \int_0^T u(t)^2 dt $$ 
%   u      : chebfun of optimal control
%   q      : free parameter vector
%   yopt   : chebfun of analytic solution optimal trajectory
%
%% KNOWN ANALYTICAL SOLUTION
% from Lewis et al [1] p 125 
% optimal trajectory solution is solution to intial value problem
%    $$ (T-t)^2 \ddot{y}(t) + 3(T-t) \dot{y}(t)+ 3 y(t) =0 $$          
%   subject to:       
%    $$ y(0) = 1 $$       
%    $$ \dot{y}(0) = 0 $$
% 
%    
%% OPTIMAL CONTROL PROBLEM
% The intercept problem can  be posed as an OCP [2]:
%
%  Find $y(t)$ over $t \in [0, T]$ to minimize
%    $$ J = \int_{0}^{T} u(t)^2 dt $$             
%   subject to:
%    $$ u=\ddot{y} $$            
%    $$ y(0) = 1 $$       
%    $$ \dot{y}(0) = 0 $$
%    $$ y(T) = y0 $$ 
%
% References:
% [1]  Lewis, F. L. Vrabie, D. L. Syrmos V. L.  Optimal Control (3rd Ed) 
% Wiley, Hoboken, NJ, 2012, pp122-125
% 
% [2] J.F. Whidborne, ``Solving optimal control problems using the Chebfun
%    system'', UKACC Control 2016, Belfact, 2016.
%

% (c) James Whidborne, Cranfield University, 15 March 2016
%   revised 11 June 2019
% MATLAB   MATLAB Version: 9.5.0.944444 (R2018b)
% Chebfun  Version Version 5.7.0  
% Optimization Toolbox   Version 8.2 
% 

%% initial parameters
if nargin==0
    q0=randn(7,1); % random start point of free parameter vector
end
 
%  boundary   conditions 
y0=1;    
yf=0;
ydot0=0;
V=1; % closing speed in x-direction
D=1; % horizontal distance to target
T=D/V; % final time with initial time =0
% time axis domain interval
trange=[0,T];
 

%% solution from Lewis [1]
%  $$ u(t)=-3/(T-t)(y(t)/(T-t) -\dot{y}) $$
% rearrange 
%  $$ (T-t)^2 \ddot{y}(t) + 3(T-t) \dot{y}(t)+ 3 y(t) =0  $$
%and solve 
delta=1e-6; % range must be slightly less than T because there is a singularity at t=T
N = chebop(@(t,yopt) (T-t).^2.*diff(yopt, 2) + 3*(T-t).*diff(yopt, 1) + 3*yopt, [0,T-delta]); 
N.lbc = [y0, ydot0];
yopt = N\0;


%% Solution of OCP using direct method   

% Control Function Parametrization:
% Parameterize y(x) by Chebychev orthogonal basis function
%  $$ y(x) = \sum_{n = 0}^\infty a_n T_n(x)   $$                   
% where a_n are the free parameters and T_n(x) are the Chebychev 
% polynomials of the first kind:
%  $$ T_0(x) = 1 $$
%  $$ T_1(x) = x $$
%  $$ T_2(x) = 2x^2 - 1 $$
%  $$ T_3(x) = 4x^3 - 3x $$
%  $$ T_4(x) = 8x^4 - 8x^2 + 1 $$
%  $$ T_5(x) = 16x^5 - 20x^3 + 5x $$
%  $$ T_6(x) = 32x^6 - 48x^4 + 18x^2 - 1 $$
%  $$ T_7(x) = 64x^7 - 112x^5 + 56x^3 - 7x $$
%  $$ T_8(x) = 128x^8 - 256x^6 + 160x^4 - 32x^2 + 1 $$
%  $$ T_9(x) = 256x^9 - 576x^7 + 432x^5 - 120x^3 + 9 $$
%
% NOTES
%  Eq (3) is linear in $a_n$
%  Make problem finite dimensional by truncating series
%  Impose boundary conditions by solving linear equation  
%   $$ \mathbf{y}(x_{0,f})=\mathbf{T}(x_{0,f})\mathbf{a} $$
%  Map free parameters to the unspecified boundary conditions derivatives
%  Hence free parameters become $$ \dot{y}(x_0), \dot{y}(x_f), \ddot{y}(x_0), 
%                                            \ddot{y}(x_f), \cdots $$
%  Solve using non-linear programming with fminunc

% Chebychev inversion matrix
Tcheb = [1   -1    1   -1    1    -1     1     -1      1      -1
     1    1    1    1    1     1     1      1      1       1
     0    1   -4    9   -16    25   -36     49    -64      81
     0    1    4    9    16    25    36     49     64      81
     0    0    4   -24   80   -200   420   -784    1344   -2160
     0    0    4    24   80    200   420    784    1344    2160
     0    0    0    24  -192   840  -2688   7056  -16128   33264
     0    0    0    24   192   840   2688   7056   16128   33264
     0    0    0    0    192  -1920  10368 -40320  126720 -342144
     0    0    0    0    192   1920  10368  40320  126720  342144];
invTcheb=inv(Tcheb);

yb =[y0; yf;ydot0];  %  boundary  condition vector 

%% call fminunc
% set fminunc options
options = optimset('display','iter','maxfunevals',1000,'maxiter',1000,...
   'TolFun',1e-9,'tolcon',1e-9,'tolx',1e-9,...
   'largescale','off','algorithm','active-set');

disp('Start nonlinear programming solver fminunc')    
tic % start clock
[q] = fminunc(@(p) interceptfun(p,invTcheb,yb,trange),...
                      q0,options);
toc % stop clock

%% calculate y=y_min
cy =[yb;q]; % boundary conditions
ay=invTcheb*cy;% Chebychev polynomial coefficients to meet boundary conditions 
y=chebfun(ay,trange,'coeffs'); % solution

J=sum(diff(y,2).^2);  % cost function 
disp(' ');disp(['Solved with cost J =',num2str(J)])

u=diff(y,2); % chebfun of control

%% plot result

% plotting constants
fs=12; % fontsize
lw=1.5; % linewidth

figure % plot optimal trajectories 
plot(yopt,'--r',y,'b','linewidth',lw)
grid on
xlabel('$t$','interpreter','latex','fontsize',fs)
ylabel('$y$','interpreter','latex','fontsize',fs)
title(' OCP solution')
 
figure % plot chebfu of absolute error
error = chebadd( y,-yopt ); % call function to add 2 chebfuns of differing domains
plot(abs(error),'linewidth',lw)
grid
xlabel('$t$','interpreter','latex','fontsize',fs)
ylabel('$|y-y_{\min}|$','interpreter','latex','fontsize',fs)
title(' OCP solution - solution error')

figure % plot control
plot(u,'linewidth',lw)
grid
xlabel('$t$','interpreter','latex','fontsize',fs)
ylabel('$u$','interpreter','latex','fontsize',fs)
title(' OCP solution - control')

end

%%
function [ J ] = interceptfun( q, invT, yb, tdomain)
% intercept cost function
% 
%   q -      free parameter vector
%   invT -   Chebychev inversion matrix
%   yb -     known boundary conditions vector
%   trange - time interval $$ t \in [0, T ]$$
% 
%   $$J = \int_{0}^{T} u^2 dt$$
%   $$u = \ddot{y}$$

cy =[yb;q]; % boundary conditions
ay=invT*cy; % Chebychev polynomial coefficients to meet boundary conditions 

y=chebfun(ay,tdomain,'coeffs'); % calculate trajectory trial chebfun
J=sum(diff(y,2).^2);            % cost function 
end

%%
function [ cheb ] = chebadd( cheb1,cheb2 )
% CHEBADD
%
% [cheb] = chebadd(cheb1,cheb2 )
%
% add 2 chebfuns where domains may differ
% it is assumed that the domain intervals are equal 

% (c) James Whidborne, Cranfield University, 15 March 2016
%   revised 11 June 2019
% MATLAB   MATLAB Version: 9.5.0.944444 (R2018b)
% Chebfun  Version Version 5.7.0  
% Optimization Toolbox   Version 8.2 

chebdomain1=get(cheb1, 'domain');
chebdomain2=get(cheb2, 'domain');
if chebdomain1~=chebdomain1, error('Intervals differ'), end
ydomain=chebdomain1;
Npts=200; % number of interpolation points

% approximate chebfun at some interpolation points
ychebpts=chebpts(Npts,ydomain); % get interpolation points
chebapprox1=cheb1(ychebpts);    % get values at interpolation points
chebapprox2=cheb2(ychebpts); 
cheb  = chebfun(chebapprox1,ydomain)+chebfun(chebapprox2,ydomain); % add
end