function [u1,u2,V,x,y,q,J,xopt,yopt]=trajplanner(q0)
% TRAJPLANNER
%
% Solve Trajectory Planning Problem by a direct method using fmincon and chebfuns
%
% This example illustrates the solution of Optimal Control Problems (OCP)
% using  a direct method using fmincon and chebfuns
%
%   Inputs
%   q0     : optional initial start point of 13x1 free parameter vector 
%
%   Outputs  
%   u1     : chebfun of optimal control 1 (acceleration)
%   u2     : chebfun of optimal control 2 (turn rate)
%   V      : chebfun optimal airspeed   
%   x      : chebfun optimal position x-direction
%   y      : chebfun optimal position y-direction
%   q      : free parameter vector
%   J      : cost function $$ J = \int_0^T dt $$ (t_f)
%   xopt   : chebfun of Dubins curve
%   yopt   : chebfun of Dubins curve
%
%
%%  DUBINS CURVE SOLUTION
%  minimum distance solution
%  Calculate minimum turn radius
%  $$ Rmin=V*psidotmax=V*u2max $$
%
% x direction distance of path
% $$ R(1-1/\sqrt(2)) + R + R/\sqrt(2) =(x_f-x_0)=1 $$
% => $$R=1/2$$
% y direction distance of path (d2 is straight line section)
% $$ R/\sqrt(2) + d2 +R/\sqrt(2) = 1
% => $$d_2=1-1/\sqrt(2);$$
% Total distance along path
% $$d = R (\pi/4) + d2 + R(3/4\pi) $$
% $$  = \pi/2 + 1 - 1/\sqrt(2)  $$
% $$ T = d/V = d/R $$
% 
%
%% OPTIMAL CONTROL PROBLEM
% The trajectory planning problem can  be posed as an OCP:
%
%  Find $p(t)$ over $t \in [0, t_f]$ to minimize either
%     $$ J = \int_{0}^{t_f} u(t)^2 dt $$
%  or
%     $$ J = \int_{0}^{t_f}  dt $$
%   subject to:
%    $$ u_1=\dot{V} $$
%    $$ u_2=\dot{\psi} $$
%    $$ \dot{x}=V \sin \psi $$
%    $$ \dot{y}=V \cos \ps i$$
%    $$ x(0) = x_0 $$
%    $$ x(t_f) = y_f $$
%    $$ \dot{x}(0) = \dot{x}_0 $$
%    $$ \dot{x}(t_f) = \dot{x}_f $$
%    $$ y(0) = y_0 $$
%    $$ y(t_f) = y_f $$
%    $$ \dot{y}(0) = \dot{y}_0 $$
%    $$ \dot{y}(t_f) = \dot{y}_f $$
%
% Reference  
%    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 


%% set parameters

% initial parameters
if nargin==0
    qx0=randn(6,1); % random start point of free parameter vector
    qy0=randn(6,1); % random start point of free parameter vector
    qtf0=1; %final time
    q0=[qx0;qy0;qtf0]; % vector of free parameters
end

%  boundary   conditions
x0=0;
xf=1;
y0=0;
yf=1;
%V0=Vf=0.5  => 
a=sqrt(2)/4;% V0/sqrt(2)
xdot0=a;
xdotf=a;
ydot0=a;
ydotf=-a;

% Dubin's solution
 
psidotmax=1;
psi0=atan2(xdot0,ydot0);
d2=1-1/sqrt(2);
R=1/2;
V0=R*psidotmax;
Topt=(pi*R+d2)/V0;
t=chebfun(@(t) t,[0,Topt]);
u2opt= chebfun({psidotmax,0,-psidotmax},[0 pi/4*R pi/4*R+d2 Topt*R]/V0);
psiopt=psi0+cumsum(u2opt); 
yopt=y0+cumsum(V0*sin(psiopt)); 
xopt=x0+cumsum(V0*cos(psiopt)); 


%% Solution of OCP using direct method

% Control Function Parametrization:
% Parameterize y(t) by Chebychev orthogonal basis function
%  y(t) = \sum_{n = 0}^\infty a_n T_n(t)               
% where a_n are the coefficients and T_n(t) are the Chebychev polynomials
% 
% Define boundary constraints on chebfuns
% $$  y(t_0)=y_0, \dot{y}(t_0)=\dot{y}_0, \ddot{y}(t_0)=\ddot{y}_0, \cdots $$
% $$  y(t_f)=y_f, \dot{y}(t_f)=\dot{y}_f, \ddot{y}(t_f)=\ddot{y}_f, \cdots $$
% where 
%   $$  y_0,  \dot{y}_0, y_f,  \dot{y}_f $$
% are defined by boundary  constraints and higher  derivatives 
% are free parameters 
% $$ \ddot{y}(x_0)= q(1), \ddot{y}(x_f)=q(2) \cdots $$
%
% Construct $ \mathbf{T}_{\mathrm{bc}}$, the matrix of Chebychev polynomials 
% and their time derivatives evaluated at $t=t_0$ and $t=t_f$ 
% Construct $\mathbf{y}_{\mathrm{bc}}$ is the vector of boundary constraints 
% Hence
%  $$\mathbf{a}= \mathbf{T}_{\mathrm{bc}}^{-1} \mathbf{y}_{\mathrm{bc}}$$
% where
%  $$ \mathbf{a}=[a_0&a_1&a_2&\cdots& a_N]^T $$
% is the vector of Chebychev coefficients,
%
%
%  Solve using non-linear programming with fmincon


% boundary condition  
xb = [x0; xf;xdot0;xdotf];  %  boundary  condition vector
yb = [y0; yf;ydot0;ydotf];  %  boundary  condition vector
pb = [xb;yb]; % position p(t) boundary conditions

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

disp('Start nonlinear programming solver fmincon')
tic % start clock
[q] = fmincon(@(q) trajplannerfun(q),...
    q0,[],[],[],[],[],[],@(q) trajplannercon(q,pb),options);
toc % stop clock

%% construct solution
xb=pb(1:4);yb=pb(5:8); % boundary conditions
qx=q(1:6);qy=q(7:12);  % free parameters
J=q(end);              % cost function t_f
t_f=J; 

disp(['Dubins path final time is ', num2str(Topt)])
disp(['Optimal path final time is ', num2str(t_f)])

% get trajectory and controls
[u1,u2,V,x,y] = trajplannerinverse(qx,qy,t_f,xb,yb);

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

figure(1)% plot path
plot(xopt,yopt,'--r','linewidth',lw), hold on
plot(x,y,'linewidth',lw)

axis equal
axis([-0.2,1.2,-0.2,1.2])
grid
xlabel('$x$','interpreter','latex','fontsize',fs)
ylabel('$y$','interpreter','latex','fontsize',fs)
title('optimal path')
legend('Dubins path','Optimal path','Location','southeast')
hold off

figure(2) % plot airspeed
plot(V,'linewidth',lw)
grid
xlabel('$t$','interpreter','latex','fontsize',fs)
ylabel('$V$','interpreter','latex','fontsize',fs)
title(' OCP solution - air speed')

figure(3) % plot controls
plot(u2,'linewidth',lw)
hold on
plot(u2opt,'--r','linewidth',lw)
grid
xlabel('$t$','interpreter','latex','fontsize',fs)
ylabel('$u_2$','interpreter','latex','fontsize',fs)
title(' OCP solution - controls')
legend('Optimal trajectory', 'Dubins path')
hold off

return


%% 
function [ J ] = trajplannerfun(q)
%  trajectory planning problem cost function
%
%   q - free parameter vector
%   J = tf 

J=q(end);  % cost function is final time
return

%%
function [ c,ceq ] = trajplannercon(q,pb)
% trajectory planning problem constraint function
%
%%  inequality constraints
Vmin=0.4;
Vmax=2;
u1max=2;
u2max=1;
tfmin=0.01;

xb=pb(1:4);yb=pb(5:8); % boundary conditions
qx=q(1:6);qy=q(7:12);  % free parameters
t_f=max(q(end),tfmin); % final time

% get trajectory and controls
[u1,u2,V] = trajplannerinverse(qx,qy,t_f,xb,yb);

c = [-min(V)+Vmin;        % speed between Vmin and Vmax
    max(V)-Vmax;
    -min(u1)-u1max;       % |u1|<u1max
    max(u1)-u1max;
    -min(u2)-u2max;       % |u2|<u2max
    max(u2)-u2max;
    -t_f+tfmin];          % ensure t_f>0

%% equality constraints
ceq = []; % we do not have any equality constraints (say)
return


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

function [u1,u2,V,x,y] = trajplannerinverse(qx,qy,t_f,xb,yb)
% trajectory planning problem inverse dynamics

tdomain=[0,t_f]; % time axis domain interval
N=length(qx)+length(xb); % number of boundary conditions
Tcheb=bcmatrix( N,tdomain ); %  Construct $ \mathbf{T}_{\mathrm{bc}}$

%% calculate x trajectory
cx =[xb;qx]; % boundary conditions
ax= Tcheb\[cx];% Chebychev polynomial coefficients to meet boundary conditions
x=chebfun(ax,tdomain,'coeffs'); % solution 
%x=chebfun(ax,tdomain,'coeffs', 'chebkind', 1); % solution

%% calculate y trajectory
cy =[yb;qy]; % boundary conditions
ay=Tcheb\[cy];% Chebychev polynomial coefficients to meet boundary conditions
y=chebfun(ay,tdomain,'coeffs'); % solution 
%y=chebfun(ay,tdomain,'coeffs', 'chebkind', 1); % solution

% calculate remaining chebfuns
xdot=diff(x);                               % x rate
ydot=diff(y);                               % y rate
V2 = xdot.^2 + ydot.^2;                     % square of airspeed  
V = sqrt(V2);                               % airspeed
u1=diff(V);                                 % control 1 (acceleration) 
u2=(diff(ydot).*xdot-diff(xdot).*ydot)./V2; % control 2 (turn rate)
return

%%
function [ TA ] = bcmatrix( nbc,tdomain )
% BCMATRIX
% Construct Boundary Condition Matrix
%
% Construct Chebychev (first kind) polynomial  matrix to evaluate
% boundary conditions  i.e Tn(x) and the n derivatives at x=[-1,1]
% mapped to [t0,tf]
%
% Inputs
%  nbc - number of boundary conditions (even number)
%  tdomain - time domain interval [t0,tf]
%
% Output
%  TA -  $ \mathbf{T}_{\mathrm{bc}}$ - boundary condition matrix
%
% derivative recursion formula (Wikipedia)
% \frac{d^p T_n}{d x^p} \Bigg|_{x = \pm 1}   = (\pm 1)^{n+p}\prod_{k=0}^{p-1}\frac{n^2-k^2}{2k+1}
%

nT=nbc-1;   % Chebychev order
np=nbc/2-1; % max derivative number

% calculate T_n at [-1,1] for n=0:nbc-1 (in columns) and derivatives (in the rows)

% first calculate T_n matrix at x=+1
Tn1=zeros(nbc/2,nbc);
Tn1(1,1:nbc)=1; % zeroth derivative

% calculate d^p T_n/dx^n
for n=1:nT  %   T_n
    for p=1:np % p is derivative number
        if(n>=p) % if derivative exists
            P=1;
            for k=0:p-1
                if n>=k% now do recursion
                    P=(n^2-k^2)/(2*k+1)*P;
                end
            end
            Tn1(p+1,n+1)=P;
        end
    end
end

%  T_n matrix at x=-1
nn=[0:nbc-1];
signcol=diag((-1).^nn);%
signrow=diag((-1).^nn(1:nbc/2));
Tn2=signrow*Tn1*signcol;

% now map Tn matrices from [-1,1] domain interval to [t0,tf] domain interval to get A
st=2/(tdomain(2)-tdomain(1)); % time mapping
A = [diag(st.^nn(1:nbc/2))*Tn2;
    diag(st.^nn(1:nbc/2))*Tn1];

% now reorder rows
TA=0*A;
TA(1:2:nbc-1,:)=A(1:nbc/2,:);
TA(2:2:nbc,:)=A(nbc/2+1:nbc,:);
return